mirror of https://github.com/ArduPilot/ardupilot
ArduCopter: rework ekf check to use separate yaw reset request
ArduCopter: Request lane or yaw reset switch sooner ArduCopter: rework ekf check to use separate yaw reset request ArduCopter: Rework EKF failsafe During flight operations it is the deviation in position of the vehicle that matters during most scenarios. t patch ensures that an EKF fail-safe will be triggered when the position inconsistency exceeds check limits in addition to either a bad mag or velocity innovation. This will stop velocity noise triggering an early fail-safe when electromagnetic interference is present.
This commit is contained in:
parent
0efdb2b66b
commit
36031c01a9
|
@ -49,10 +49,17 @@ void Copter::ekf_check()
|
|||
if (!ekf_check_state.bad_variance) {
|
||||
// increase counter
|
||||
ekf_check_state.fail_count++;
|
||||
#if EKF_CHECK_ITERATIONS_MAX > 3
|
||||
if (ekf_check_state.fail_count == MIN((EKF_CHECK_ITERATIONS_MAX-2), 5)) {
|
||||
// we are just about to declare a EKF failsafe, ask the EKF if we can reset
|
||||
// yaw to resolve the issue
|
||||
ahrs.request_yaw_reset();
|
||||
}
|
||||
#endif
|
||||
#if EKF_CHECK_ITERATIONS_MAX > 2
|
||||
if (ekf_check_state.fail_count == EKF_CHECK_ITERATIONS_MAX-1) {
|
||||
// we are just about to declare a EKF failsafe, ask the EKF if we can change lanes
|
||||
// to resolve the issue
|
||||
// we are just about to declare a EKF failsafe, ask the EKF if we can
|
||||
// change lanes to resolve the issue
|
||||
ahrs.check_lane_switch();
|
||||
}
|
||||
#endif
|
||||
|
@ -112,6 +119,7 @@ bool Copter::ekf_over_threshold()
|
|||
if (mag_max >= g.fs_ekf_thresh) {
|
||||
over_thresh_count++;
|
||||
}
|
||||
|
||||
bool optflow_healthy = false;
|
||||
#if OPTFLOW == ENABLED
|
||||
optflow_healthy = optflow.healthy();
|
||||
|
@ -121,11 +129,8 @@ bool Copter::ekf_over_threshold()
|
|||
} else if (vel_variance >= g.fs_ekf_thresh) {
|
||||
over_thresh_count++;
|
||||
}
|
||||
if (position_variance >= g.fs_ekf_thresh) {
|
||||
over_thresh_count++;
|
||||
}
|
||||
|
||||
if (over_thresh_count >= 2) {
|
||||
if (position_variance >= g.fs_ekf_thresh && over_thresh_count >= 1) {
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue