Copter: fixed motor test with rate thread

avoids race condition that leads to motors being unable to switch off
ArduPilot · Feb 17, 2024 · 3e9a51b · 3e9a51b
1 parent d9d3377
commit 3e9a51b
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 3 deletions.
diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp
@@ -21,7 +21,8 @@ void Copter::rate_controller_thread()
 
     while (true) {
         // allow changing option at runtime
-        if (!flight_option_is_set(FlightOptions::USE_RATE_LOOP_THREAD)) {
+        if (!flight_option_is_set(FlightOptions::USE_RATE_LOOP_THREAD) ||
+            ap.motor_test) {
             using_rate_thread = false;
             if (was_using_rate_thread) {
                 // if we were using the rate thread, we need to
@@ -38,6 +39,10 @@ void Copter::rate_controller_thread()
 
         // wait for an IMU sample
         rate_sem.wait_blocking();
+        if (ap.motor_test) {
+            continue;
+        }
+
         const uint32_t now_us = AP_HAL::micros();
         const uint32_t dt_us = now_us - last_run_us;
         const float dt = dt_us * 1.0e-6;
@@ -82,9 +87,9 @@ void Copter::rate_controller_thread()
             attitude_control->set_notch_sample_rate(1.0 / dt_avg);
         }
 
-        if (now_ms - last_report_ms >= 2000) {
+        if (now_ms - last_report_ms >= 200) {
             last_report_ms = now_ms;
-            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Rate: %.2fHz", 1.0/dt_avg);
+            gcs().send_named_float("LRATE", 1.0/dt_avg);
         }
 
         was_using_rate_thread = true;

diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp
@@ -158,6 +158,12 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t
             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