ArduCopter/ekf_check.cpp

   1 #include "Copter.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     bool has_ever_passed;       // true if the ekf checks have ever passed
  25     uint32_t last_warn_time;    // system time of last warning in milliseconds.  Used to throttle text warnings sent to GCS
  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 Copter::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 ekf check is disabled
  42     if (g.fs_ekf_thresh <= 0.0f) {
  43         ekf_check_state.fail_count = 0;
  44         ekf_check_state.bad_variance = false;
  45         AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
  46         failsafe_ekf_off_event();   // clear failsafe
  47         return;
  48     }
  49
  50     // compare compass and velocity variance vs threshold and also check
  51     // if we has a position estimate
  52     const bool over_threshold = ekf_over_threshold();
  53     const bool has_position = ekf_has_relative_position() || ekf_has_absolute_position();
  54     const bool checks_passed = !over_threshold && has_position;
  55
  56     // return if ekf checks have never passed
  57     ekf_check_state.has_ever_passed |= checks_passed;
  58     if (!ekf_check_state.has_ever_passed) {
  59         return;
  60     }
  61
  62     // increment or decrement counters and take action
  63     if (!checks_passed) {
  64         // if variances are not yet flagged as bad
  65         if (!ekf_check_state.bad_variance) {
  66             // increase counter
  67             ekf_check_state.fail_count++;
  68             if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-2) && over_threshold) {
  69                 // we are two iterations away from declaring an EKF failsafe, ask the EKF if we can reset
  70                 // yaw to resolve the issue
  71                 ahrs.request_yaw_reset();
  72             }
  73             if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-1)) {
  74                 // we are just about to declare a EKF failsafe, ask the EKF if we can
  75                 // change lanes to resolve the issue
  76                 ahrs.check_lane_switch();
  77             }
  78             // if counter above max then trigger failsafe
  79             if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
  80                 // limit count from climbing too high
  81                 ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
  82                 ekf_check_state.bad_variance = true;
  83                 LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE);
  84                 // send message to gcs
  85                 if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
  86                     gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance");
  87                     ekf_check_state.last_warn_time = AP_HAL::millis();
  88                 }
  89                 failsafe_ekf_event();
  90             }
  91         }
  92     } else {
  93         // reduce counter
  94         if (ekf_check_state.fail_count > 0) {
  95             ekf_check_state.fail_count--;
  96
  97             // if variances are flagged as bad and the counter reaches zero then clear flag
  98             if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
  99                 ekf_check_state.bad_variance = false;
 100                 LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED);
 101                 // clear failsafe
 102                 failsafe_ekf_off_event();
 103             }
 104         }
 105     }
 106
 107     // set AP_Notify flags
 108     AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
 109
 110     // To-Do: add ekf variances to extended status
 111 }
 112
 113 // ekf_over_threshold - returns true if the ekf's variance are over the tolerance
 114 bool Copter::ekf_over_threshold()
 115 {
 116     // use EKF to get variance
 117     float position_var, vel_var, height_var, tas_variance;
 118     Vector3f mag_variance;
 119     variances_valid = ahrs.get_variances(vel_var, position_var, height_var, mag_variance, tas_variance);
 120
 121     if (!variances_valid) {
 122         return false;
 123     }
 124
 125     uint32_t now_us = AP_HAL::micros();
 126     float dt = (now_us - last_ekf_check_us) * 1e-6f;
 127
 128     // always update filtered values as this serves the vibration check as well
 129     position_var = pos_variance_filt.apply(position_var, dt);
 130     vel_var = vel_variance_filt.apply(vel_var, dt);
 131
 132     last_ekf_check_us = now_us;
 133
 134     // return false if disabled
 135     if (g.fs_ekf_thresh <= 0.0f) {
 136         return false;
 137     }
 138
 139     const float mag_max = fmaxf(fmaxf(mag_variance.x,mag_variance.y),mag_variance.z);
 140
 141     // return true if two of compass, velocity and position variances are over the threshold OR velocity variance is twice the threshold
 142     uint8_t over_thresh_count = 0;
 143     if (mag_max >= g.fs_ekf_thresh) {
 144         over_thresh_count++;
 145     }
 146
 147     bool optflow_healthy = false;
 148 #if AP_OPTICALFLOW_ENABLED
 149     optflow_healthy = optflow.healthy();
 150 #endif
 151     if (!optflow_healthy && (vel_var >= (2.0f * g.fs_ekf_thresh))) {
 152         over_thresh_count += 2;
 153     } else if (vel_var >= g.fs_ekf_thresh) {
 154         over_thresh_count++;
 155     }
 156
 157     if ((position_var >= g.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) {
 158         return true;
 159     }
 160
 161     return false;
 162 }
 163
 164
 165 // failsafe_ekf_event - perform ekf failsafe
 166 void Copter::failsafe_ekf_event()
 167 {
 168     // EKF failsafe event has occurred
 169     failsafe.ekf = true;
 170     LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED);
 171
 172     // if disarmed take no action
 173     if (!motors->armed()) {
 174         return;
 175     }
 176
 177     // sometimes LAND *does* require GPS so ensure we are in non-GPS land
 178     if (flightmode->mode_number() == Mode::Number::LAND && landing_with_GPS()) {
 179         mode_land.do_not_use_GPS();
 180         return;
 181     }
 182
 183     // does this mode require position?
 184     if (!copter.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) {
 185         return;
 186     }
 187
 188     // take action based on fs_ekf_action parameter
 189     switch (g.fs_ekf_action) {
 190         case FS_EKF_ACTION_ALTHOLD:
 191             // AltHold
 192             if (failsafe.radio || !set_mode(Mode::Number::ALT_HOLD, ModeReason::EKF_FAILSAFE)) {
 193                 set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
 194             }
 195             break;
 196         case FS_EKF_ACTION_LAND:
 197         case FS_EKF_ACTION_LAND_EVEN_STABILIZE:
 198         default:
 199             set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
 200             break;
 201     }
 202
 203     // set true if ekf action is triggered
 204     AP_Notify::flags.failsafe_ekf = true;
 205     gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF Failsafe: changed to %s Mode", flightmode->name());
 206 }
 207
 208 // failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
 209 void Copter::failsafe_ekf_off_event(void)
 210 {
 211     // return immediately if not in ekf failsafe
 212     if (!failsafe.ekf) {
 213         return;
 214     }
 215
 216     failsafe.ekf = false;
 217     if (AP_Notify::flags.failsafe_ekf) {
 218         AP_Notify::flags.failsafe_ekf = false;
 219         gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF Failsafe Cleared");
 220     }
 221     LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED);
 222 }
 223
 224 // re-check if the flight mode requires GPS but EKF failsafe is active
 225 // this should be called by flight modes that are changing their submode from one that does NOT require a position estimate to one that does
 226 void Copter::failsafe_ekf_recheck()
 227 {
 228     // return immediately if not in ekf failsafe
 229     if (!failsafe.ekf) {
 230         return;
 231     }
 232
 233     // trigger EKF failsafe action
 234     failsafe_ekf_event();
 235 }
 236
 237 // check for ekf yaw reset and adjust target heading, also log position reset
 238 void Copter::check_ekf_reset()
 239 {
 240     // check for yaw reset
 241     float yaw_angle_change_rad;
 242     uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
 243     if (new_ekfYawReset_ms != ekfYawReset_ms) {
 244         attitude_control->inertial_frame_reset();
 245         ekfYawReset_ms = new_ekfYawReset_ms;
 246         LOGGER_WRITE_EVENT(LogEvent::EKF_YAW_RESET);
 247     }
 248
 249     // check for change in primary EKF, reset attitude target and log.  AC_PosControl handles position target adjustment
 250     if ((ahrs.get_primary_core_index() != ekf_primary_core) && (ahrs.get_primary_core_index() != -1)) {
 251         attitude_control->inertial_frame_reset();
 252         ekf_primary_core = ahrs.get_primary_core_index();
 253         LOGGER_WRITE_ERROR(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core));
 254         gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core);
 255     }
 256 }
 257
 258 // check for high vibrations affecting altitude control
 259 void Copter::check_vibration()
 260 {
 261     uint32_t now = AP_HAL::millis();
 262
 263     // assume checks will succeed
 264     bool innovation_checks_valid = true;
 265
 266     // check if vertical velocity and position innovations are positive (NKF3.IVD & NKF3.IPD are both positive)
 267     Vector3f vel_innovation;
 268     Vector3f pos_innovation;
 269     Vector3f mag_innovation;
 270     float tas_innovation;
 271     float yaw_innovation;
 272     if (!ahrs.get_innovations(vel_innovation, pos_innovation, mag_innovation, tas_innovation, yaw_innovation)) {
 273         innovation_checks_valid = false;
 274     }
 275     const bool innov_velD_posD_positive = is_positive(vel_innovation.z) && is_positive(pos_innovation.z);
 276
 277     // check if vertical velocity variance is at least 1 (NK4.SV >= 1.0)
 278     // filtered variances are updated in ekf_check() which runs at the same rate (10Hz) as this check
 279     if (!variances_valid) {
 280         innovation_checks_valid = false;
 281     }
 282     const bool is_vibration_affected = ahrs.is_vibration_affected();
 283     const bool bad_vibe_detected = (innovation_checks_valid && innov_velD_posD_positive && (vel_variance_filt.get() > 1.0f)) || is_vibration_affected;
 284     const bool do_bad_vibe_actions = (g2.fs_vibe_enabled == 1) && bad_vibe_detected && motors->armed() && !flightmode->has_manual_throttle();
 285
 286     if (!vibration_check.high_vibes) {
 287         // initialise timers
 288         if (!do_bad_vibe_actions) {
 289             vibration_check.start_ms = now;
 290         }
 291         // check if failure has persisted for at least 1 second
 292         if (now - vibration_check.start_ms > 1000) {
 293             // switch position controller to use resistant gains
 294             vibration_check.clear_ms = 0;
 295             vibration_check.high_vibes = true;
 296             pos_control->set_vibe_comp(true);
 297             LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED);
 298             gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON");
 299         }
 300     } else {
 301         // initialise timer
 302         if (do_bad_vibe_actions) {
 303             vibration_check.clear_ms = now;
 304         }
 305         // turn off vibration compensation after 15 seconds
 306         if (now - vibration_check.clear_ms > 15000) {
 307             // restore position controller gains, reset timers and update user
 308             vibration_check.start_ms = 0;
 309             vibration_check.high_vibes = false;
 310             pos_control->set_vibe_comp(false);
 311             vibration_check.clear_ms = 0;
 312             LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED);
 313             gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF");
 314         }
 315     }
 316
 317     return;
 318 }