AP_Motors: Tradheli servo_test fix

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -171,6 +171,9 @@ void AP_MotorsHeli::output()
     output_logic();
 
     if (_flags.armed) {
+        //block servo_test from happening at disarm
+        _servo_test_cycle_counter = 0;
+        
         calculate_armed_scalars();
         if (!_flags.interlock) {
             output_armed_zero_throttle();
@@ -211,9 +214,13 @@ void AP_MotorsHeli::output_armed_zero_throttle()
 void AP_MotorsHeli::output_disarmed()
 {
     if (_servo_test_cycle_counter > 0){
+		//set servo_test_flag
+        _heliflags.servo_test_running = true;
         // perform boot-up servo test cycle if enabled
         servo_test();
     } else {
+		//set servo_test flag
+        _heliflags.servo_test_running = false;
         // manual override (i.e. when setting up swash)
         switch (_servo_mode) {
             case SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH:

libraries/AP_Motors/AP_MotorsHeli.h

@@ -130,6 +130,9 @@ public:
     // set_enable_bailout - allows main code to set when RSC can immediately ramp engine instantly
     void set_enable_bailout(bool bailout) { _heliflags.enable_bailout = bailout; }
 
+    // return true if the servo test is still running/pending
+    bool servo_test_running() const { return _heliflags.servo_test_running; }
+    
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -199,6 +202,7 @@ protected:
         uint8_t save_rsc_mode           : 1;    // used to determine the rsc mode needs to be saved while disarmed
         uint8_t in_autorotation         : 1;    // true if aircraft is in autorotation
         uint8_t enable_bailout          : 1;    // true if allowing RSC to quickly ramp up engine
+        uint8_t servo_test_running      : 1;    // true if servo_test is running
     } _heliflags;
 
     // parameters