Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3             : /**
       4             :  *
       5             :  * Detects failures of the ekf and triggers a failsafe
       6             :  *
       7             :  */
       8             : 
       9             : #ifndef EKF_CHECK_ITERATIONS_MAX
      10             :  # define EKF_CHECK_ITERATIONS_MAX          10      // 1 second (ie. 10 iterations at 10hz) of bad variances signals a failure
      11             : #endif
      12             : 
      13             : #ifndef EKF_CHECK_WARNING_TIME
      14             :  # define EKF_CHECK_WARNING_TIME            (30*1000)   // warning text messages are sent to ground no more than every 30 seconds
      15             : #endif
      16             : 
      17             : 
      18             : // EKF_check structure
      19             : static struct {
      20             :     uint8_t fail_count;         // number of iterations ekf or dcm have been out of tolerances
      21             :     uint8_t bad_variance : 1;   // true if ekf should be considered untrusted (fail_count has exceeded EKF_CHECK_ITERATIONS_MAX)
      22             :     bool has_ever_passed;       // true if the ekf checks have ever passed
      23             :     uint32_t last_warn_time;    // system time of last warning in milliseconds.  Used to throttle text warnings sent to GCS
      24             : } ekf_check_state;
      25             : 
      26             : // ekf_check - detects if ekf variance are out of tolerance and triggers failsafe
      27             : // should be called at 10hz
      28      125720 : void Rover::ekf_check()
      29             : {
      30             :     // exit immediately if ekf has no origin yet - this assumes the origin can never become unset
      31      125720 :     Location temp_loc;
      32      125720 :     if (!ahrs.get_origin(temp_loc)) {
      33       75534 :         return;
      34             :     }
      35             : 
      36             :     // return immediately if motors are not armed, or ekf check is disabled
      37      101827 :     if (!arming.is_armed() || (g.fs_ekf_thresh <= 0.0f)) {
      38       51641 :         ekf_check_state.fail_count = 0;
      39       51641 :         ekf_check_state.bad_variance = false;
      40       51641 :         AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
      41       51641 :         failsafe_ekf_off_event();   // clear failsafe
      42       51641 :         return;
      43             :     }
      44             : 
      45             :     // compare compass and velocity variance vs threshold
      46       50186 :     const bool over_threshold = ekf_over_threshold();
      47       50186 :     const bool position_ok = ekf_position_ok();
      48       50186 :     const bool checks_passed = !over_threshold && position_ok;
      49             : 
      50             :     // return if ekf checks have never passed
      51       50186 :     ekf_check_state.has_ever_passed |= checks_passed;
      52       50186 :     if (!ekf_check_state.has_ever_passed) {
      53           0 :         return;
      54             :     }
      55             : 
      56       50186 :     if (over_threshold) {
      57             :         // if compass is not yet flagged as bad
      58          11 :         if (!ekf_check_state.bad_variance) {
      59             :             // increase counter
      60          10 :             ekf_check_state.fail_count++;
      61             :             // if counter above max then trigger failsafe
      62          10 :             if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
      63             :                 // limit count from climbing too high
      64           1 :                 ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
      65           1 :                 ekf_check_state.bad_variance = true;
      66             : 
      67           1 :                 LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK,
      68             :                                          LogErrorCode::EKFCHECK_BAD_VARIANCE);
      69             :                 // send message to gcs
      70           1 :                 if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
      71           1 :                     GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL,"EKF variance");
      72           1 :                     ekf_check_state.last_warn_time = AP_HAL::millis();
      73             :                 }
      74           1 :                 failsafe_ekf_event();
      75             :             }
      76             :         }
      77             :     } else {
      78             :         // reduce counter
      79       50175 :         if (ekf_check_state.fail_count > 0) {
      80           0 :             ekf_check_state.fail_count--;
      81             : 
      82             :             // if variance is flagged as bad and the counter reaches zero then clear flag
      83           0 :             if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
      84           0 :                 ekf_check_state.bad_variance = false;
      85           0 :                 LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK,
      86             :                                          LogErrorCode::EKFCHECK_VARIANCE_CLEARED);
      87             :                 // clear failsafe
      88           0 :                 failsafe_ekf_off_event();
      89             :             }
      90             :         }
      91             :     }
      92             : 
      93             :     // set AP_Notify flags
      94       50186 :     AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
      95             : }
      96             : 
      97             : // returns true if the ekf's variance are over the tolerance
      98       50186 : bool Rover::ekf_over_threshold()
      99             : {
     100             :     // return false immediately if disabled
     101       50186 :     if (g.fs_ekf_thresh <= 0.0f) {
     102           0 :         return false;
     103             :     }
     104             : 
     105             :     // use EKF to get variance
     106             :     float position_variance, vel_variance, height_variance, tas_variance;
     107       50186 :     Vector3f mag_variance;
     108       50186 :     ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance);
     109             : 
     110             :     // return true if two of compass, velocity and position variances are over the threshold
     111       50186 :     uint8_t over_thresh_count = 0;
     112       50186 :     if (mag_variance.length() >= g.fs_ekf_thresh) {
     113           0 :         over_thresh_count++;
     114             :     }
     115       50186 :     if (vel_variance >= g.fs_ekf_thresh) {
     116           0 :         over_thresh_count++;
     117             :     }
     118       50186 :     if (position_variance >= g.fs_ekf_thresh) {
     119           0 :         over_thresh_count++;
     120             :     }
     121             : 
     122       50186 :     bool optflow_healthy = false;
     123             : #if AP_OPTICALFLOW_ENABLED
     124       50186 :     optflow_healthy = optflow.healthy();
     125             : #endif
     126       50186 :     if (!optflow_healthy && (vel_variance >= (2.0f * g.fs_ekf_thresh))) {
     127           0 :         over_thresh_count += 2;
     128       50186 :     } else if (vel_variance >= g.fs_ekf_thresh) {
     129           0 :         over_thresh_count++;
     130             :     }
     131             :     
     132       50186 :     if (over_thresh_count >= 2) {
     133           0 :         return true;
     134             :     }
     135             : 
     136       50186 :     return !ekf_position_ok();
     137             : }
     138             : 
     139             : // ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set
     140      102374 : bool Rover::ekf_position_ok()
     141             : {
     142      102374 :     if (!ahrs.have_inertial_nav()) {
     143             :         // do not allow navigation with dcm position
     144           0 :         return false;
     145             :     }
     146             : 
     147             :     // get EKF filter status
     148             :     nav_filter_status filt_status;
     149      102374 :     rover.ahrs.get_filter_status(filt_status);
     150             : 
     151             :     // if disarmed we accept a predicted horizontal absolute or relative position
     152      102374 :     if (!arming.is_armed()) {
     153        1863 :         return (filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs || filt_status.flags.horiz_pos_rel || filt_status.flags.pred_horiz_pos_rel);
     154             :     } else {
     155             :         // once armed we require a good absolute or relative position and EKF must not be in const_pos_mode
     156      100511 :         return ((filt_status.flags.horiz_pos_abs || filt_status.flags.horiz_pos_rel) && !filt_status.flags.const_pos_mode);
     157             :     }
     158             : }
     159             : 
     160             : // perform ekf failsafe
     161           1 : void Rover::failsafe_ekf_event()
     162             : {
     163             :     // return immediately if ekf failsafe already triggered
     164           1 :     if (failsafe.ekf) {
     165           0 :         return;
     166             :     }
     167             : 
     168             :     // EKF failsafe event has occurred
     169           1 :     failsafe.ekf = true;
     170           1 :     LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV,
     171             :                              LogErrorCode::FAILSAFE_OCCURRED);
     172             : 
     173             :     // does this mode require position?
     174           1 :     if (!control_mode->requires_position()) {
     175           0 :         return;
     176             :     }
     177             : 
     178             :     // take action based on fs_ekf_action parameter
     179           1 :     switch ((enum fs_ekf_action)g.fs_ekf_action.get()) {
     180           0 :         case FS_EKF_DISABLE:
     181             :             // do nothing
     182           0 :             return;
     183           0 :         case FS_EKF_REPORT_ONLY:
     184           0 :             break;
     185           1 :         case FS_EKF_HOLD:
     186             :         default:
     187           1 :             set_mode(mode_hold, ModeReason::EKF_FAILSAFE);
     188           1 :             break;
     189             :     }
     190             : 
     191           1 :     GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL,"EKF failsafe");
     192             : }
     193             : 
     194             : // failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
     195       51641 : void Rover::failsafe_ekf_off_event(void)
     196             : {
     197             :     // return immediately if not in ekf failsafe
     198       51641 :     if (!failsafe.ekf) {
     199       51640 :         return;
     200             :     }
     201             : 
     202           1 :     failsafe.ekf = false;
     203           1 :     LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV,
     204             :                              LogErrorCode::FAILSAFE_RESOLVED);
     205           1 :     GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL,"EKF failsafe cleared");
     206             : }