ArduPlane/ekf_check.cpp

   1 #include "Plane.h"
   2
   3 /**
   4  *
   5  * Detects failures of the ekf or inertial nav system triggers an alert
   6  * to the pilot and helps take countermeasures
   7  *
   8  */
   9
  10 #ifndef EKF_CHECK_ITERATIONS_MAX
  11  # define EKF_CHECK_ITERATIONS_MAX          10      // 1 second (ie. 10 iterations at 10hz) of bad variances signals a failure
  12 #endif
  13
  14 #ifndef EKF_CHECK_WARNING_TIME
  15  # define EKF_CHECK_WARNING_TIME            (30*1000)   // warning text messages are sent to ground no more than every 30 seconds
  16 #endif
  17
  18 ////////////////////////////////////////////////////////////////////////////////
  19 // EKF_check structure
  20 ////////////////////////////////////////////////////////////////////////////////
  21 static struct {
  22     uint8_t fail_count;         // number of iterations ekf or dcm have been out of tolerances
  23     bool bad_variance;          // true if ekf should be considered untrusted (fail_count has exceeded EKF_CHECK_ITERATIONS_MAX)
  24     uint32_t last_warn_time;    // system time of last warning in milliseconds.  Used to throttle text warnings sent to GCS
  25     bool failsafe_on;           // true when the loss of navigation failsafe is on
  26 } ekf_check_state;
  27
  28 // ekf_check - detects if ekf variance are out of tolerance and triggers failsafe
  29 // should be called at 10hz
  30 void Plane::ekf_check()
  31 {
  32     // ensure EKF_CHECK_ITERATIONS_MAX is at least 7
  33     static_assert(EKF_CHECK_ITERATIONS_MAX >= 7, "EKF_CHECK_ITERATIONS_MAX must be at least 7");
  34
  35     // exit immediately if ekf has no origin yet - this assumes the origin can never become unset
  36     Location temp_loc;
  37     if (!ahrs.get_origin(temp_loc)) {
  38         return;
  39     }
  40
  41     // return immediately if motors are not armed, or ekf check is disabled
  42     bool ekf_check_disabled = !plane.arming.is_armed() || (g2.fs_ekf_thresh <= 0.0f);
  43 #if HAL_QUADPLANE_ENABLED
  44     if (!quadplane.in_vtol_posvel_mode()) {
  45         ekf_check_disabled = true;
  46     }
  47 #endif
  48     if (ekf_check_disabled) {
  49         ekf_check_state.fail_count = 0;
  50         ekf_check_state.bad_variance = false;
  51         AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
  52         failsafe_ekf_off_event();   // clear failsafe
  53         return;
  54     }
  55
  56     // compare compass and velocity variance vs threshold
  57     if (ekf_over_threshold()) {
  58         // if compass is not yet flagged as bad
  59         if (!ekf_check_state.bad_variance) {
  60             // increase counter
  61             ekf_check_state.fail_count++;
  62             if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-2)) {
  63                 // we are two iterations away from declaring an EKF failsafe, ask the EKF if we can reset
  64                 // yaw to resolve the issue
  65                 ahrs.request_yaw_reset();
  66             }
  67             if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-1)) {
  68                 // we are just about to declare a EKF failsafe, ask the EKF if we can
  69                 // change lanes to resolve the issue
  70                 ahrs.check_lane_switch();
  71             }
  72             // if counter above max then trigger failsafe
  73             if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
  74                 // limit count from climbing too high
  75                 ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
  76                 ekf_check_state.bad_variance = true;
  77                 LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE);
  78                 // send message to gcs
  79                 if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
  80                     gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance");
  81                     ekf_check_state.last_warn_time = AP_HAL::millis();
  82                 }
  83                 failsafe_ekf_event();
  84             }
  85         }
  86     } else {
  87         // reduce counter
  88         if (ekf_check_state.fail_count > 0) {
  89             ekf_check_state.fail_count--;
  90
  91             // if compass is flagged as bad and the counter reaches zero then clear flag
  92             if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
  93                 ekf_check_state.bad_variance = false;
  94                 LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED);
  95                 // clear failsafe
  96                 failsafe_ekf_off_event();
  97             }
  98         }
  99     }
 100
 101     // set AP_Notify flags
 102     AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
 103
 104     // To-Do: add ekf variances to extended status
 105 }
 106
 107 // ekf_over_threshold - returns true if the ekf's variance are over the tolerance
 108 bool Plane::ekf_over_threshold()
 109 {
 110     // return false immediately if disabled
 111     if (g2.fs_ekf_thresh <= 0.0f) {
 112         return false;
 113     }
 114
 115     // Get EKF innovations normalised wrt the innovation test limits.
 116     // A value above 1.0 means the EKF has rejected that sensor data
 117     float position_variance, vel_variance, height_variance, tas_variance;
 118     Vector3f mag_variance;
 119     if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance)) {
 120         return false;
 121     };
 122
 123     // The EKF rejects all magnetometer axes if any single axis exceeds limits
 124     // so take the maximum of all axes
 125     const float mag_max = fmaxf(fmaxf(mag_variance.x,mag_variance.y),mag_variance.z);
 126
 127     // Assign a score to each over threshold based on severity
 128     uint8_t over_thresh_count = 0;
 129     if (mag_max >= g2.fs_ekf_thresh) {
 130         over_thresh_count++;
 131     }
 132
 133     if (vel_variance >= (2.0f * g2.fs_ekf_thresh)) {
 134         over_thresh_count += 2;
 135     } else if (vel_variance >= g2.fs_ekf_thresh) {
 136         over_thresh_count++;
 137     }
 138
 139     // Position is the most important so accept a lower score from other sensors if position failed
 140     if ((position_variance >= g2.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) {
 141         return true;
 142     }
 143
 144     return false;
 145 }
 146
 147
 148 // failsafe_ekf_event - perform ekf failsafe
 149 void Plane::failsafe_ekf_event()
 150 {
 151     // return immediately if ekf failsafe already triggered
 152     if (ekf_check_state.failsafe_on) {
 153         return;
 154     }
 155
 156     // EKF failsafe event has occurred
 157     ekf_check_state.failsafe_on = true;
 158     LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED);
 159
 160     // if not in a VTOL mode requiring position, then nothing needs to be done
 161 #if HAL_QUADPLANE_ENABLED
 162     if (!quadplane.in_vtol_posvel_mode()) {
 163         return;
 164     }
 165
 166     if (quadplane.in_vtol_auto()) {
 167         // the pilot is not controlling via sticks so switch to QLAND
 168         plane.set_mode(mode_qland, ModeReason::EKF_FAILSAFE);
 169     } else {
 170         // the pilot is controlling via sticks so fallbacl to QHOVER
 171         plane.set_mode(mode_qhover, ModeReason::EKF_FAILSAFE);
 172     }
 173 #endif
 174 }
 175
 176 // failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
 177 void Plane::failsafe_ekf_off_event(void)
 178 {
 179     // return immediately if not in ekf failsafe
 180     if (!ekf_check_state.failsafe_on) {
 181         return;
 182     }
 183
 184     ekf_check_state.failsafe_on = false;
 185     LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED);
 186 }