Line data    Source code

       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      547097 : 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      547097 :     Location temp_loc;
      37      547097 :     if (!ahrs.get_origin(temp_loc)) {
      38      470729 :         return;
      39             :     }
      40             : 
      41             :     // return immediately if motors are not armed, or ekf check is disabled
      42      488395 :     bool ekf_check_disabled = !plane.arming.is_armed() || (g2.fs_ekf_thresh <= 0.0f);
      43             : #if HAL_QUADPLANE_ENABLED
      44      488395 :     if (!quadplane.in_vtol_posvel_mode()) {
      45      405913 :         ekf_check_disabled = true;
      46             :     }
      47             : #endif
      48      488395 :     if (ekf_check_disabled) {
      49      412027 :         ekf_check_state.fail_count = 0;
      50      412027 :         ekf_check_state.bad_variance = false;
      51      412027 :         AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
      52      412027 :         failsafe_ekf_off_event();   // clear failsafe
      53      412027 :         return;
      54             :     }
      55             : 
      56             :     // compare compass and velocity variance vs threshold
      57       76368 :     if (ekf_over_threshold()) {
      58             :         // if compass is not yet flagged as bad
      59           0 :         if (!ekf_check_state.bad_variance) {
      60             :             // increase counter
      61           0 :             ekf_check_state.fail_count++;
      62           0 :             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           0 :                 ahrs.request_yaw_reset();
      66             :             }
      67           0 :             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           0 :                 ahrs.check_lane_switch();
      71             :             }
      72             :             // if counter above max then trigger failsafe
      73           0 :             if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
      74             :                 // limit count from climbing too high
      75           0 :                 ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
      76           0 :                 ekf_check_state.bad_variance = true;
      77           0 :                 LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE);
      78             :                 // send message to gcs
      79           0 :                 if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
      80           0 :                     gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance");
      81           0 :                     ekf_check_state.last_warn_time = AP_HAL::millis();
      82             :                 }
      83           0 :                 failsafe_ekf_event();
      84             :             }
      85             :         }
      86             :     } else {
      87             :         // reduce counter
      88       76368 :         if (ekf_check_state.fail_count > 0) {
      89           0 :             ekf_check_state.fail_count--;
      90             : 
      91             :             // if compass is flagged as bad and the counter reaches zero then clear flag
      92           0 :             if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
      93           0 :                 ekf_check_state.bad_variance = false;
      94           0 :                 LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED);
      95             :                 // clear failsafe
      96           0 :                 failsafe_ekf_off_event();
      97             :             }
      98             :         }
      99             :     }
     100             : 
     101             :     // set AP_Notify flags
     102       76368 :     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       76368 : bool Plane::ekf_over_threshold()
     109             : {
     110             :     // return false immediately if disabled
     111       76368 :     if (g2.fs_ekf_thresh <= 0.0f) {
     112           0 :         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       76368 :     Vector3f mag_variance;
     119       76368 :     if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance)) {
     120         558 :         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       75810 :     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       75810 :     uint8_t over_thresh_count = 0;
     129       75810 :     if (mag_max >= g2.fs_ekf_thresh) {
     130           3 :         over_thresh_count++;
     131             :     }
     132             : 
     133       75810 :     if (vel_variance >= (2.0f * g2.fs_ekf_thresh)) {
     134           0 :         over_thresh_count += 2;
     135       75810 :     } else if (vel_variance >= g2.fs_ekf_thresh) {
     136           0 :         over_thresh_count++;
     137             :     }
     138             : 
     139             :     // Position is the most important so accept a lower score from other sensors if position failed
     140       75810 :     if ((position_variance >= g2.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) {
     141           0 :         return true;
     142             :     }
     143             : 
     144       75810 :     return false;
     145             : }
     146             : 
     147             : 
     148             : // failsafe_ekf_event - perform ekf failsafe
     149           0 : void Plane::failsafe_ekf_event()
     150             : {
     151             :     // return immediately if ekf failsafe already triggered
     152           0 :     if (ekf_check_state.failsafe_on) {
     153           0 :         return;
     154             :     }
     155             : 
     156             :     // EKF failsafe event has occurred
     157           0 :     ekf_check_state.failsafe_on = true;
     158           0 :     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           0 :     if (!quadplane.in_vtol_posvel_mode()) {
     163           0 :         return;
     164             :     }
     165             : 
     166           0 :     if (quadplane.in_vtol_auto()) {
     167             :         // the pilot is not controlling via sticks so switch to QLAND
     168           0 :         plane.set_mode(mode_qland, ModeReason::EKF_FAILSAFE);
     169             :     } else {
     170             :         // the pilot is controlling via sticks so fallback to QHOVER
     171           0 :         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      412027 : void Plane::failsafe_ekf_off_event(void)
     178             : {
     179             :     // return immediately if not in ekf failsafe
     180      412027 :     if (!ekf_check_state.failsafe_on) {
     181      412027 :         return;
     182             :     }
     183             : 
     184           0 :     ekf_check_state.failsafe_on = false;
     185           0 :     LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED);
     186             : }