Copter: add vibration check

ArduCopter/Copter.cpp

@@ -142,6 +142,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_Notify,            &copter.notify,              update,          50,  90),
     SCHED_TASK(one_hz_loop,            1,    100),
     SCHED_TASK(ekf_check,             10,     75),
+    SCHED_TASK(check_vibration,       10,     50),
     SCHED_TASK(gpsglitch_check,       10,     50),
     SCHED_TASK(landinggear_update,    10,     75),
     SCHED_TASK(standby_update,        100,    75),

ArduCopter/Copter.h

@@ -333,6 +333,13 @@ private:
     uint32_t ekfYawReset_ms;
     int8_t ekf_primary_core;
 
+    // vibration check
+    struct {
+        bool high_vibes;    // true while high vibration are detected
+        uint32_t start_ms;  // system time high vibration were last detected
+        uint32_t clear_ms;  // system time high vibrations stopped
+    } vibration_check;
+
     // GCS selection
     GCS_Copter _gcs; // avoid using this; use gcs()
     GCS_Copter &gcs() { return _gcs; }
@@ -693,6 +700,7 @@ private:
     void failsafe_ekf_event();
     void failsafe_ekf_off_event(void);
     void check_ekf_reset();
+    void check_vibration();
 
     // esc_calibration.cpp
     void esc_calibration_startup_check();

ArduCopter/Parameters.cpp

@@ -940,6 +940,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPPTR(mode_systemid_ptr, "SID", 34, ParametersG2, ModeSystemId),
 #endif
     
+    // @Param: FS_VIBE_ENABLE
+    // @DisplayName: Vibration Failsafe enable
+    // @Description: This enables the vibration failsafe which will use modified altitude estimation and control during high vibrations
+    // @Values: 0:Disabled, 1:Enabled
+    // @User: Standard
+    AP_GROUPINFO("FS_VIBE_ENABLE", 35, ParametersG2, fs_vibe_enabled, 1),
+
     AP_GROUPEND
 };
 

ArduCopter/Parameters.h

@@ -609,6 +609,9 @@ public:
     // we need a pointer to the mode for the G2 table
     void *mode_systemid_ptr;
 #endif
+
+    // vibration failsafe enable/disable
+    AP_Int8 fs_vibe_enabled;
 };
 
 extern const AP_Param::Info        var_info[];

ArduCopter/ekf_check.cpp

@@ -204,3 +204,72 @@ void Copter::check_ekf_reset()
     }
 #endif
 }
+
+// check for high vibrations affecting altitude control
+void Copter::check_vibration()
+{
+    uint32_t now = AP_HAL::millis();
+
+    // assume checks will succeed
+    bool checks_succeeded = true;
+
+    // check if vertical velocity and position innovations are positive (NKF3.IVD & NKF3.IPD are both positive)
+    Vector3f vel_innovation;
+    Vector3f pos_innovation;
+    Vector3f mag_innovation;
+    float tas_innovation;
+    float yaw_innovation;
+    if (!ahrs.get_innovations(vel_innovation, pos_innovation, mag_innovation, tas_innovation, yaw_innovation)) {
+        checks_succeeded = false;
+    }
+    const bool innov_velD_posD_positive = is_positive(vel_innovation.z) && is_positive(pos_innovation.z);
+
+    // check if EKF's NKF4.SH and NK4.SV > 1.0
+    float position_variance, vel_variance, height_variance, tas_variance;
+    Vector3f mag_variance;
+    Vector2f offset;
+    if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset)) {
+        checks_succeeded = false;
+    }
+    const bool var_vel_hgt_high = (vel_variance >= 1.0f);
+
+    // if no failure
+    if ((g2.fs_vibe_enabled == 0) || !checks_succeeded || !motors->armed() || !innov_velD_posD_positive || !var_vel_hgt_high) {
+        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");
+            }
+        }
+        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) {
+            // switch ekf to use resistant gains
+            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");
+        }
+    }
+}

ArduCopter/inertia.cpp

@@ -3,8 +3,8 @@
 // read_inertia - read inertia in from accelerometers
 void Copter::read_inertia()
 {
-    // inertial altitude estimates
-    inertial_nav.update();
+    // inertial altitude estimates. Use barometer climb rate during high vibrations
+    inertial_nav.update(vibration_check.high_vibes);
 
     // pull position from ahrs
     Location loc;