AP_MotorsHeli: Create Servo Test functionality

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -148,6 +148,14 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("CYC_MAX", 16, AP_MotorsHeli, _cyclic_max, AP_MOTORS_HELI_SWASH_CYCLIC_MAX),
 
+    // @Param: SV_TEST
+    // @DisplayName: Boot-up Servo Test Cycles
+    // @Description: Number of cycles to run servo test on boot-up
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("SV_TEST",  17, AP_MotorsHeli, _servo_test, 0),
+
     AP_GROUPEND
 };
 
@@ -161,6 +169,9 @@ void AP_MotorsHeli::Init()
     // set update rate
     set_update_rate(_speed_hz);
 
+    // load boot-up servo test cycles into counter to be consumed
+    _servo_test_cycle_counter = _servo_test;
+
     // ensure inputs are not passed through to servos on start-up
     _servo_mode = SERVO_CONTROL_MODE_AUTOMATED;
 
@@ -247,43 +258,48 @@ void AP_MotorsHeli::output_armed_zero_throttle()
 // output_disarmed - sends commands to the motors
 void AP_MotorsHeli::output_disarmed()
 {
-    // manual override (i.e. when setting up swash)
-    switch (_servo_mode) {
-        case SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH:
-            // pass pilot commands straight through to swash
-            _roll_control_input = _roll_radio_passthrough;
-            _pitch_control_input = _pitch_radio_passthrough;
-            _throttle_control_input = _throttle_radio_passthrough;
-            _yaw_control_input = _yaw_radio_passthrough;
-            break;
-        case SERVO_CONTROL_MODE_MANUAL_CENTER:
-            // fixate mid collective
-            _roll_control_input = 0;
-            _pitch_control_input = 0;
-            _throttle_control_input = _collective_mid_pwm;
-            _yaw_control_input = 0;
-            break;
-        case SERVO_CONTROL_MODE_MANUAL_MAX:
-            // fixate max collective
-            _roll_control_input = 0;
-            _pitch_control_input = 0;
-            _throttle_control_input = 1000;
-            _yaw_control_input = 4500;
-            break;
-        case SERVO_CONTROL_MODE_MANUAL_MIN:
-            // fixate min collective
-            _roll_control_input = 0;
-            _pitch_control_input = 0;
-            _throttle_control_input = 0;
-            _yaw_control_input = -4500;
-            break;
-        case SERVO_CONTROL_MODE_MANUAL_OSCILLATE:
-            // use servo_test function from child classes
-            servo_test();
-            break;
-        default:
-            // no manual override
-            break;
+    if (_servo_test_cycle_counter > 0){
+        // perform boot-up servo test cycle if enabled
+        servo_test();
+    } else {
+        // manual override (i.e. when setting up swash)
+        switch (_servo_mode) {
+            case SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH:
+                // pass pilot commands straight through to swash
+                _roll_control_input = _roll_radio_passthrough;
+                _pitch_control_input = _pitch_radio_passthrough;
+                _throttle_control_input = _throttle_radio_passthrough;
+                _yaw_control_input = _yaw_radio_passthrough;
+                break;
+            case SERVO_CONTROL_MODE_MANUAL_CENTER:
+                // fixate mid collective
+                _roll_control_input = 0;
+                _pitch_control_input = 0;
+                _throttle_control_input = _collective_mid_pwm;
+                _yaw_control_input = 0;
+                break;
+            case SERVO_CONTROL_MODE_MANUAL_MAX:
+                // fixate max collective
+                _roll_control_input = 0;
+                _pitch_control_input = 0;
+                _throttle_control_input = 1000;
+                _yaw_control_input = 4500;
+                break;
+            case SERVO_CONTROL_MODE_MANUAL_MIN:
+                // fixate min collective
+                _roll_control_input = 0;
+                _pitch_control_input = 0;
+                _throttle_control_input = 0;
+                _yaw_control_input = -4500;
+                break;
+            case SERVO_CONTROL_MODE_MANUAL_OSCILLATE:
+                // use servo_test function from child classes
+                servo_test();
+                break;
+            default:
+                // no manual override
+                break;
+        }
     }
 
     // ensure swash servo endpoints haven't been moved

libraries/AP_Motors/AP_MotorsHeli.h

@@ -223,6 +223,7 @@ protected:
     AP_Int16        _rsc_idle_output;           // Rotor control output while at idle
     AP_Int16        _rsc_power_low;             // throttle value sent to throttle servo at zero collective pitch
     AP_Int16        _rsc_power_high;            // throttle value sent to throttle servo at maximum collective pitch
+    AP_Int8         _servo_test;                // sets number of cycles to test servo movement on bootup
 
     // internal variables
     float           _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
@@ -240,6 +241,7 @@ protected:
     int16_t         _throttle_radio_passthrough = 0; // throttle control PWM direct from radio, used for manual control
     int16_t         _yaw_radio_passthrough = 0;      // yaw control PWM direct from radio, used for manual control
     int16_t         _collective_range = 0;           // maximum absolute collective pitch range (500 - 1000)
+    uint8_t         _servo_test_cycle_counter = 0;   // number of test cycles left to run after bootup
 };
 
 #endif  // AP_MOTORSHELI

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -522,6 +522,10 @@ void AP_MotorsHeli_Single::servo_test()
         roll_test = 0.0f;
         pitch_test = 0.0f;
         yaw_test = 0.0f;
+        // decrement servo test cycle counter at the end of the cycle
+        if (_servo_test_cycle_counter > 0){
+            _servo_test_cycle_counter--;
+        }
     }
 
     // over-ride servo commands to move servos through defined ranges