Line data    Source code

       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      759512 : 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      759512 :     Location temp_loc;
      37      759512 :     if (!ahrs.get_origin(temp_loc)) {
      38      172047 :         return;
      39             :     }
      40             : 
      41             :     // return immediately if ekf check is disabled
      42      664974 :     if (g.fs_ekf_thresh <= 0.0f) {
      43           0 :         ekf_check_state.fail_count = 0;
      44           0 :         ekf_check_state.bad_variance = false;
      45           0 :         AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
      46           0 :         failsafe_ekf_off_event();   // clear failsafe
      47           0 :         return;
      48             :     }
      49             : 
      50             :     // compare compass and velocity variance vs threshold and also check
      51             :     // if we has a position estimate
      52      664974 :     const bool over_threshold = ekf_over_threshold();
      53      664974 :     const bool has_position = ekf_has_relative_position() || ekf_has_absolute_position();
      54      664974 :     const bool checks_passed = !over_threshold && has_position;
      55             : 
      56             :     // return if ekf checks have never passed
      57      664974 :     ekf_check_state.has_ever_passed |= checks_passed;
      58      664974 :     if (!ekf_check_state.has_ever_passed) {
      59       77509 :         return;
      60             :     }
      61             : 
      62             :     // increment or decrement counters and take action
      63      587465 :     if (!checks_passed) {
      64             :         // if variances are not yet flagged as bad
      65        4605 :         if (!ekf_check_state.bad_variance) {
      66             :             // increase counter
      67         466 :             ekf_check_state.fail_count++;
      68         466 :             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           6 :                 ahrs.request_yaw_reset();
      72             :             }
      73         466 :             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          39 :                 ahrs.check_lane_switch();
      77             :             }
      78             :             // if counter above max then trigger failsafe
      79         466 :             if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
      80             :                 // limit count from climbing too high
      81          39 :                 ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
      82          39 :                 ekf_check_state.bad_variance = true;
      83          39 :                 LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE);
      84             :                 // send message to gcs
      85          39 :                 if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
      86          32 :                     gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance: %s", over_threshold ? "over thresholds" : "position lost");
      87          32 :                     ekf_check_state.last_warn_time = AP_HAL::millis();
      88             :                 }
      89          39 :                 failsafe_ekf_event();
      90             :             }
      91             :         }
      92             :     } else {
      93             :         // reduce counter
      94      582860 :         if (ekf_check_state.fail_count > 0) {
      95         258 :             ekf_check_state.fail_count--;
      96             : 
      97             :             // if variances are flagged as bad and the counter reaches zero then clear flag
      98         258 :             if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
      99          19 :                 ekf_check_state.bad_variance = false;
     100          19 :                 LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED);
     101             :                 // clear failsafe
     102          19 :                 failsafe_ekf_off_event();
     103             :             }
     104             :         }
     105             :     }
     106             : 
     107             :     // set AP_Notify flags
     108      587465 :     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      664974 : bool Copter::ekf_over_threshold()
     115             : {
     116             :     // use EKF to get variance
     117             :     float position_var, vel_var, height_var, tas_variance;
     118      664974 :     Vector3f mag_variance;
     119      664974 :     variances_valid = ahrs.get_variances(vel_var, position_var, height_var, mag_variance, tas_variance);
     120             : 
     121      664974 :     if (!variances_valid) {
     122         131 :         return false;
     123             :     }
     124             : 
     125      664843 :     uint32_t now_us = AP_HAL::micros();
     126      664843 :     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      664843 :     position_var = pos_variance_filt.apply(position_var, dt);
     130      664843 :     vel_var = vel_variance_filt.apply(vel_var, dt);
     131             : 
     132      664843 :     last_ekf_check_us = now_us;
     133             : 
     134             :     // return false if disabled
     135      664843 :     if (g.fs_ekf_thresh <= 0.0f) {
     136           0 :         return false;
     137             :     }
     138             : 
     139      664843 :     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      664843 :     uint8_t over_thresh_count = 0;
     143      664843 :     if (mag_max >= g.fs_ekf_thresh) {
     144        1369 :         over_thresh_count++;
     145             :     }
     146             : 
     147      664843 :     bool optflow_healthy = false;
     148             : #if AP_OPTICALFLOW_ENABLED
     149      664843 :     optflow_healthy = optflow.healthy();
     150             : #endif
     151      664843 :     if (!optflow_healthy && (vel_var >= (2.0f * g.fs_ekf_thresh))) {
     152         105 :         over_thresh_count += 2;
     153      664738 :     } else if (vel_var >= g.fs_ekf_thresh) {
     154          84 :         over_thresh_count++;
     155             :     }
     156             : 
     157      664843 :     if ((position_var >= g.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) {
     158         129 :         return true;
     159             :     }
     160             : 
     161      664714 :     return false;
     162             : }
     163             : 
     164             : 
     165             : // failsafe_ekf_event - perform ekf failsafe
     166          39 : void Copter::failsafe_ekf_event()
     167             : {
     168             :     // EKF failsafe event has occurred
     169          39 :     failsafe.ekf = true;
     170          39 :     LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED);
     171             : 
     172             :     // if disarmed take no action
     173          39 :     if (!motors->armed()) {
     174          29 :         return;
     175             :     }
     176             : 
     177             :     // set true if ekf failsafe is triggered
     178          10 :     AP_Notify::flags.failsafe_ekf = true;
     179             : 
     180             :     // True if no action should be taken
     181          10 :     const bool report_only = g.fs_ekf_action == FS_EKF_Action::REPORT_ONLY;
     182             : 
     183             :     // sometimes LAND *does* require GPS so ensure we are in non-GPS land
     184          10 :     const bool landing_with_position = landing_with_GPS();
     185          10 :     if (landing_with_position && !report_only) {
     186           0 :         mode_land.do_not_use_GPS();
     187             :     }
     188             : 
     189             :     // does this mode require position?
     190          10 :     const bool no_action_in_current_mode = !copter.flightmode->requires_position() && (g.fs_ekf_action != FS_EKF_Action::LAND_EVEN_STABILIZE);
     191             : 
     192          10 :     if (report_only || landing_with_position || no_action_in_current_mode) {
     193           5 :         gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF Failsafe");
     194           5 :         return;
     195             :     }
     196             : 
     197             :     // take action based on fs_ekf_action parameter
     198           5 :     switch ((FS_EKF_Action)g.fs_ekf_action) {
     199           0 :         case FS_EKF_Action::REPORT_ONLY:
     200             :             // Should have early returned above
     201           0 :             break;
     202           0 :         case FS_EKF_Action::ALTHOLD:
     203             :             // AltHold
     204           0 :             if (failsafe.radio || !set_mode(Mode::Number::ALT_HOLD, ModeReason::EKF_FAILSAFE)) {
     205           0 :                 set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
     206             :             }
     207           0 :             break;
     208           5 :         case FS_EKF_Action::LAND:
     209             :         case FS_EKF_Action::LAND_EVEN_STABILIZE:
     210             :         default:
     211           5 :             set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
     212           5 :             break;
     213             :     }
     214             : 
     215           5 :     gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF Failsafe: changed to %s Mode", flightmode->name());
     216             : }
     217             : 
     218             : // failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
     219          19 : void Copter::failsafe_ekf_off_event(void)
     220             : {
     221             :     // return immediately if not in ekf failsafe
     222          19 :     if (!failsafe.ekf) {
     223           0 :         return;
     224             :     }
     225             : 
     226          19 :     failsafe.ekf = false;
     227          19 :     if (AP_Notify::flags.failsafe_ekf) {
     228           7 :         AP_Notify::flags.failsafe_ekf = false;
     229           7 :         gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF Failsafe Cleared");
     230             :     }
     231          19 :     LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED);
     232             : }
     233             : 
     234             : // re-check if the flight mode requires GPS but EKF failsafe is active
     235             : // 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
     236           0 : void Copter::failsafe_ekf_recheck()
     237             : {
     238             :     // return immediately if not in ekf failsafe
     239           0 :     if (!failsafe.ekf) {
     240           0 :         return;
     241             :     }
     242             : 
     243             :     // trigger EKF failsafe action
     244           0 :     failsafe_ekf_event();
     245             : }
     246             : 
     247             : // check for ekf yaw reset and adjust target heading, also log position reset
     248    30412448 : void Copter::check_ekf_reset()
     249             : {
     250             :     // check for yaw reset
     251    30412448 :     const uint16_t new_yaw_reset_count = ahrs.get_yaw_reset_count();
     252    30412448 :     if (new_yaw_reset_count != ahrs_yaw_reset_count) {
     253        2087 :         attitude_control->inertial_frame_reset();
     254        2087 :         ahrs_yaw_reset_count = new_yaw_reset_count;
     255             :     }
     256             : 
     257             :     // check for change in primary EKF, reset attitude target and log.  AC_PosControl handles position target adjustment
     258    30412448 :     const auto new_reset_count = ahrs.get_last_attitude_reset_count();
     259    30412448 :     if (new_reset_count != attitude_reset_count) {
     260         952 :         attitude_control->inertial_frame_reset();
     261         952 :         attitude_reset_count = new_reset_count;
     262             :     }
     263    30412448 : }
     264             : 
     265             : // check for high vibrations affecting altitude control
     266      759512 : void Copter::check_vibration()
     267             : {
     268      759512 :     uint32_t now = AP_HAL::millis();
     269             : 
     270             :     // assume checks will succeed
     271      759512 :     bool innovation_checks_valid = true;
     272             : 
     273             :     // check if vertical velocity and position innovations are positive (NKF3.IVD & NKF3.IPD are both positive)
     274      759512 :     Vector3f vel_innovation;
     275      759512 :     Vector3f pos_innovation;
     276      759512 :     Vector3f mag_innovation;
     277             :     float tas_innovation;
     278             :     float yaw_innovation;
     279      759512 :     if (!ahrs.get_innovations(vel_innovation, pos_innovation, mag_innovation, tas_innovation, yaw_innovation)) {
     280        9819 :         innovation_checks_valid = false;
     281             :     }
     282      759512 :     const bool innov_velD_posD_positive = is_positive(vel_innovation.z) && is_positive(pos_innovation.z);
     283             : 
     284             :     // check if vertical velocity variance is at least 1 (NK4.SV >= 1.0)
     285             :     // filtered variances are updated in ekf_check() which runs at the same rate (10Hz) as this check
     286      759512 :     if (!variances_valid) {
     287       94669 :         innovation_checks_valid = false;
     288             :     }
     289      759512 :     const bool is_vibration_affected = ahrs.is_vibration_affected();
     290      759512 :     const bool bad_vibe_detected = (innovation_checks_valid && innov_velD_posD_positive && (vel_variance_filt.get() > 1.0f)) || is_vibration_affected;
     291      759512 :     const bool do_bad_vibe_actions = (g2.fs_vibe_enabled == 1) && bad_vibe_detected && motors->armed() && !flightmode->has_manual_throttle();
     292             : 
     293      759512 :     if (!vibration_check.high_vibes) {
     294             :         // initialise timers
     295      759219 :         if (!do_bad_vibe_actions) {
     296      759208 :             vibration_check.start_ms = now;
     297             :         }
     298             :         // check if failure has persisted for at least 1 second
     299      759219 :         if (now - vibration_check.start_ms > 1000) {
     300             :             // switch position controller to use resistant gains
     301           1 :             vibration_check.clear_ms = 0;
     302           1 :             vibration_check.high_vibes = true;
     303           1 :             pos_control->set_vibe_comp(true);
     304           1 :             LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED);
     305           1 :             gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON");
     306             :         }
     307             :     } else {
     308             :         // initialise timer
     309         293 :         if (do_bad_vibe_actions) {
     310         217 :             vibration_check.clear_ms = now;
     311             :         }
     312             :         // turn off vibration compensation after 15 seconds
     313         293 :         if (now - vibration_check.clear_ms > 15000) {
     314             :             // restore position controller gains, reset timers and update user
     315           0 :             vibration_check.start_ms = 0;
     316           0 :             vibration_check.high_vibes = false;
     317           0 :             pos_control->set_vibe_comp(false);
     318           0 :             vibration_check.clear_ms = 0;
     319           0 :             LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED);
     320           0 :             gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF");
     321             :         }
     322             :     }
     323             : 
     324      759512 :     return;
     325             : }