Copter: fix threading issues with motor test and the fast rate thread

Author: andyp1per

ArduCopter/motor_test.cpp

@@ -159,15 +159,9 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t
         } else {
             // start test
             gcs().send_text(MAV_SEVERITY_INFO, "starting motor test");
-            ap.motor_test = true;
 
             EXPECT_DELAY_MS(3000);
 
-            // wait for rate thread to stop running due to motor test
-            while (using_rate_thread) {
-                hal.scheduler->delay(1);
-            }
-
             // enable and arm motors
             if (!motors->armed()) {
                 motors->output_min();  // output lowest possible value to motors
@@ -182,6 +176,7 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t
 
             // turn on notify leds
             AP_Notify::flags.esc_calibration = true;
+            ap.motor_test = true;
         }
     }
 
@@ -213,9 +208,6 @@ void Copter::motor_test_stop()
 
     gcs().send_text(MAV_SEVERITY_INFO, "finished motor test");    
 
-    // flag test is complete
-    ap.motor_test = false;
-
     // disarm motors
     motors->armed(false);
     hal.util->set_soft_armed(false);
@@ -235,4 +227,7 @@ void Copter::motor_test_stop()
 
     // turn off notify leds
     AP_Notify::flags.esc_calibration = false;
+
+    // flag test is complete
+    ap.motor_test = false;
 }

ArduCopter/rate_thread.cpp

@@ -215,7 +215,7 @@ void Copter::rate_controller_thread()
 #endif
 
         // allow changing option at runtime
-        if (get_fast_rate_type() == FastRateType::FAST_RATE_DISABLED || ap.motor_test) {
+        if (get_fast_rate_type() == FastRateType::FAST_RATE_DISABLED) {
             if (was_using_rate_thread) {
                 disable_fast_rate_loop(rates);
                 was_using_rate_thread = false;