Copter: rework ekf_check to use the EKF vibration_affected status

ArduCopter/ekf_check.cpp

@@ -242,7 +242,7 @@ void Copter::check_vibration()
     uint32_t now = AP_HAL::millis();
 
     // assume checks will succeed
-    bool checks_succeeded = true;
+    bool innovation_checks_valid = true;
 
     // check if vertical velocity and position innovations are positive (NKF3.IVD & NKF3.IPD are both positive)
     Vector3f vel_innovation;
@@ -251,7 +251,7 @@ void Copter::check_vibration()
     float tas_innovation;
     float yaw_innovation;
     if (!ahrs.get_innovations(vel_innovation, pos_innovation, mag_innovation, tas_innovation, yaw_innovation)) {
-        checks_succeeded = false;
+        innovation_checks_valid = false;
     }
     const bool innov_velD_posD_positive = is_positive(vel_innovation.z) && is_positive(pos_innovation.z);
 
@@ -259,46 +259,59 @@ void Copter::check_vibration()
     float position_variance, vel_variance, height_variance, tas_variance;
     Vector3f mag_variance;
     if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance)) {
-        checks_succeeded = false;
+        innovation_checks_valid = false;
     }
 
-    // if no failure
-    if ((g2.fs_vibe_enabled == 0) || !checks_succeeded || !motors->armed() || !innov_velD_posD_positive || (vel_variance < 1.0f)) {
-        if (vibration_check.high_vibes) {
-            // start clear time
-            if (vibration_check.clear_ms == 0) {
-                vibration_check.clear_ms = now;
-                return;
-            }
-            // turn off vibration compensation after 15 seconds
-            if (now - vibration_check.clear_ms > 15000) {
-                // restore ekf gains, reset timers and update user
-                vibration_check.high_vibes = false;
-                pos_control->set_vibe_comp(false);
-                vibration_check.clear_ms = 0;
-                AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED);
-                gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF");
-            }
+    const bool is_vibration_affected = ahrs.is_vibration_affected();
+    const bool bad_vibe_detected = (innovation_checks_valid && innov_velD_posD_positive && (vel_variance > 1.0f)) || is_vibration_affected;
+    const bool do_bad_vibe_actions = (g2.fs_vibe_enabled == 1) && bad_vibe_detected && motors->armed();
+
+    // static uint32_t frame_count=0;
+    // frame_count++;
+    // if (frame_count > 10) {
+    //     frame_count=0;
+    //     AP::logger().Write(
+    //         "CEVC",
+    //         "TimeUS,icv,ivpp,vv,iva,dbva",
+    //         "Qbbfbb",
+    //         AP_HAL::micros64(),
+    //         innovation_checks_valid,
+    //         innov_velD_posD_positive,
+    //         vel_variance,
+    //         is_vibration_affected,
+    //         do_bad_vibe_actions);
+    // }
+
+    if (!vibration_check.high_vibes) {
+        // initialise timers
+        if (!do_bad_vibe_actions) {
+            vibration_check.start_ms = now;
         }
-        vibration_check.start_ms = 0;
-        return;
-    }
-
-    // start timer
-    if (vibration_check.start_ms == 0) {
-        vibration_check.start_ms = now;
-        vibration_check.clear_ms = 0;
-        return;
-    }
-
-    // check if failure has persisted for at least 1 second
-    if (now - vibration_check.start_ms > 1000) {
-        if (!vibration_check.high_vibes) {
+        // check if failure has persisted for at least 1 second
+        if (now - vibration_check.start_ms > 1000) {
             // switch ekf to use resistant gains
+            vibration_check.clear_ms = 0;
             vibration_check.high_vibes = true;
             pos_control->set_vibe_comp(true);
             AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED);
             gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON");
         }
+    } else {
+        // initialise timer
+        if (do_bad_vibe_actions) {
+            vibration_check.clear_ms = now;
+        }
+        // turn off vibration compensation after 15 seconds
+        if (now - vibration_check.clear_ms > 15000) {
+            // restore ekf gains, reset timers and update user
+            vibration_check.start_ms = 0;
+            vibration_check.high_vibes = false;
+            pos_control->set_vibe_comp(false);
+            vibration_check.clear_ms = 0;
+            AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED);
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF");
+        }
     }
+
+    return;
 }