Copter: desync_check() logic

ArduCopter/Copter.h

@@ -348,6 +348,9 @@ class Copter : public AP_Vehicle {
     // takeoff check
     uint32_t takeoff_check_warning_ms;  // system time user was last warned of takeoff check failure
 
+    // desync check
+    uint32_t desync_check_warning_ms;  // system time user was last warned of desync check failure
+
     // GCS selection
     GCS_Copter _gcs; // avoid using this; use gcs()
     GCS_Copter &gcs() { return _gcs; }
@@ -754,6 +757,7 @@ class Copter : public AP_Vehicle {
     void crash_check();
     void thrust_loss_check();
     void yaw_imbalance_check();
+    void desync_check();
     LowPassFilterFloat yaw_I_filt{0.05f};
     uint32_t last_yaw_warn_ms;
     void parachute_check();

ArduCopter/crash_check.cpp

@@ -229,6 +229,36 @@ void Copter::yaw_imbalance_check()
     }
 }
 
+// check for motor desync
+void Copter::desync_check()
+{
+#if HAL_WITH_ESC_TELEM && FRAME_CONFIG != HELI_FRAME
+    // If we are on the ground do nothing
+    if (!motors->armed() || ap.land_complete) {
+        return;
+    }
+
+    // check ESCs are sending RPM at expected level
+    uint32_t motor_mask = motors->get_motor_mask();
+    const bool telem_active = AP::esc_telem().is_telemetry_active(motor_mask);
+    // check that all motors are running
+    const uint32_t failed_motors = AP::esc_telem().get_motors_not_running(motor_mask);
+
+    // all good
+    if ((telem_active && !failed_motors) || !telem_active) {
+        return;
+    }
+
+    // warn user telem inactive or rpm is inadequate every 5 seconds
+    uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - desync_check_warning_ms > 5000) {
+        desync_check_warning_ms = now_ms;
+        const char* prefix_str = "Motor desync:";
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "%s ESC RPM out of range", prefix_str);
+    }
+#endif
+}
+
 #if PARACHUTE == ENABLED
 
 // Code to detect a crash main ArduCopter code

ArduCopter/land_detector.cpp

@@ -28,6 +28,7 @@ void Copter::update_land_and_crash_detectors()
     crash_check();
     thrust_loss_check();
     yaw_imbalance_check();
+    desync_check();
 }
 
 // update_land_detector - checks if we have landed and updates the ap.land_complete flag