TradHeli: output_test for individual motors

Based on original work by Nils Hogberg

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -274,60 +274,45 @@ void AP_MotorsHeli::output_min()
 }
 
 
-// output_test - spin each motor for a moment to allow the user to confirm the motor order and spin direction
-void AP_MotorsHeli::output_test()
+// output_test - spin a motor at the pwm value specified
+//  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
+//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
+void AP_MotorsHeli::output_test(uint8_t motor_seq, int16_t pwm)
 {
-    int16_t i;
-    // Send minimum values to all motors
-    output_min();
-
-    // servo 1
-    for( i=0; i<5; i++ ) {
-        hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo_1.radio_trim + 100);
-        hal.scheduler->delay(300);
-        hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo_1.radio_trim - 100);
-        hal.scheduler->delay(300);
-        hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo_1.radio_trim + 0);
-        hal.scheduler->delay(300);
-    }
-
-    // servo 2
-    for( i=0; i<5; i++ ) {
-        hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_trim + 100);
-        hal.scheduler->delay(300);
-        hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_trim - 100);
-        hal.scheduler->delay(300);
-        hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_trim + 0);
-        hal.scheduler->delay(300);
-    }
-
-    // servo 3
-    for( i=0; i<5; i++ ) {
-        hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo_3.radio_trim + 100);
-        hal.scheduler->delay(300);
-        hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo_3.radio_trim - 100);
-        hal.scheduler->delay(300);
-        hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo_3.radio_trim + 0);
-        hal.scheduler->delay(300);
-    }
-
-    // external gyro
-    if (_tail_type == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) {
-        write_aux(_ext_gyro_gain);
+    // exit immediately if not armed
+    if (!_flags.armed) {
+        return;
     }
 
-    // servo 4
-    for( i=0; i<5; i++ ) {
-        hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo_4.radio_trim + 100);
-        hal.scheduler->delay(300);
-        hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo_4.radio_trim - 100);
-        hal.scheduler->delay(300);
-        hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo_4.radio_trim + 0);
-        hal.scheduler->delay(300);
+    // output to motors and servos
+    switch (motor_seq) {
+        case 1:
+            // swash servo 1
+            hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), pwm);
+            break;
+        case 2:
+            // swash servo 2
+            hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), pwm);
+            break;
+        case 3:
+            // swash servo 3
+            hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), pwm);
+            break;
+        case 4:
+            // external gyro & tail servo
+            if (_tail_type == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) {
+                write_aux(_ext_gyro_gain);
+            }
+            hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), pwm);
+            break;
+        case 5:
+            // main rotor
+            hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_HELI_RSC]), pwm);
+            break;
+        default:
+            // do nothing
+            break;
     }
-
-    // Send minimum values to all motors
-    output_min();
 }
 
 // allow_arming - returns true if main rotor is spinning and it is ok to arm

libraries/AP_Motors/AP_MotorsHeli.h

@@ -138,8 +138,10 @@ public:
     // output_min - sends minimum values out to the motors
     void output_min();
 
-    // output_test - wiggle servos in order to show connections are correct
-    void output_test();
+    // output_test - spin a motor at the pwm value specified
+    //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
+    //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
+    virtual void        output_test(uint8_t motor_seq, int16_t pwm);
 
     //
     // heli specific methods