Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: Don't request EKF yaw reset unless innovations are large

Browse Source 
This prevents unwanted resets if GPS fails.
This commit is contained in:
Paul Riseborough 2020-10-28 20:55:21 +11:00 committed by Randy Mackay
parent fd0e822074
commit 379506b718
4 changed files with 16 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Copter.h
View File

@ -859,8 +859,8 @@ private:
void startup_INS_ground(); void startup_INS_ground();
void update_dynamic_notch() override; void update_dynamic_notch() override;
bool position_ok() const; bool position_ok() const;
bool ekf_position_ok() const; bool ekf_has_absolute_position() const;
bool optflow_position_ok() const; bool ekf_has_relative_position() const;
bool ekf_alt_ok() const; bool ekf_alt_ok() const;
void update_auto_armed(); void update_auto_armed();
bool should_log(uint32_t mask); bool should_log(uint32_t mask);

6
ArduCopter/baro_ground_effect.cpp
View File

@ -23,7 +23,7 @@ void Copter::update_ground_effect_detector(void)
xy_des_speed_cms = vel_target.length(); xy_des_speed_cms = vel_target.length();
} }
if (position_ok() || optflow_position_ok()) { if (position_ok() || ekf_has_relative_position()) {
Vector3f vel = inertial_nav.get_velocity(); Vector3f vel = inertial_nav.get_velocity();
vel.z = 0.0f; vel.z = 0.0f;
xy_speed_cms = vel.length(); xy_speed_cms = vel.length();
@ -52,7 +52,7 @@ void Copter::update_ground_effect_detector(void)
// landing logic // landing logic
Vector3f angle_target_rad = attitude_control->get_att_target_euler_cd() * radians(0.01f); Vector3f angle_target_rad = attitude_control->get_att_target_euler_cd() * radians(0.01f);
bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f)); bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f));
bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f; bool xy_speed_low = (position_ok() || ekf_has_relative_position()) && xy_speed_cms <= 125.0f;
bool xy_speed_demand_low = pos_control->is_active_xy() && xy_des_speed_cms <= 125.0f; bool xy_speed_demand_low = pos_control->is_active_xy() && xy_des_speed_cms <= 125.0f;
bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control->is_active_xy()) || (control_mode == Mode::Number::ALT_HOLD && small_angle_request); bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control->is_active_xy()) || (control_mode == Mode::Number::ALT_HOLD && small_angle_request);
@ -74,7 +74,7 @@ void Copter::update_ground_effect_detector(void)
void Copter::update_ekf_terrain_height_stable() void Copter::update_ekf_terrain_height_stable()
{ {
// set to false if no position estimate // set to false if no position estimate
if (!position_ok() && !optflow_position_ok()) { if (!position_ok() && !ekf_has_relative_position()) {
ahrs.set_terrain_hgt_stable(false); ahrs.set_terrain_hgt_stable(false);
} }

14
ArduCopter/ekf_check.cpp
View File

@ -46,13 +46,15 @@ void Copter::ekf_check()
return; return;
} }
// compare compass and velocity variance vs threshold // compare compass and velocity variance vs threshold and also check
if (ekf_over_threshold()) { // if we are still navigating
bool is_navigating = ekf_has_relative_position() || ekf_has_absolute_position();
if (ekf_over_threshold() || !is_navigating) {
// if compass is not yet flagged as bad // if compass is not yet flagged as bad
if (!ekf_check_state.bad_variance) { if (!ekf_check_state.bad_variance) {
// increase counter // increase counter
ekf_check_state.fail_count++; ekf_check_state.fail_count++;
if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-2)) { if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-2) && ekf_over_threshold()) {
// we are two iterations away from declaring an EKF failsafe, ask the EKF if we can reset // we are two iterations away from declaring an EKF failsafe, ask the EKF if we can reset
// yaw to resolve the issue // yaw to resolve the issue
ahrs.request_yaw_reset(); ahrs.request_yaw_reset();
@ -132,11 +134,7 @@ bool Copter::ekf_over_threshold()
return true; return true;
} }
// either optflow relative or absolute position estimate OK return false;
if (optflow_position_ok() || ekf_position_ok()) {
return false;
}
return true;
} }

10
ArduCopter/system.cpp
View File

@ -325,11 +325,11 @@ bool Copter::position_ok() const
} }
// check ekf position estimate // check ekf position estimate
return (ekf_position_ok() || optflow_position_ok()); return (ekf_has_absolute_position() || ekf_has_relative_position());
} }
// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set // ekf_has_absolute_position - returns true if the EKF can provide an absolute WGS-84 position estimate
bool Copter::ekf_position_ok() const bool Copter::ekf_has_absolute_position() const
{ {
if (!ahrs.have_inertial_nav()) { if (!ahrs.have_inertial_nav()) {
// do not allow navigation with dcm position // do not allow navigation with dcm position
@ -348,8 +348,8 @@ bool Copter::ekf_position_ok() const
} }
} }
// optflow_position_ok - returns true if optical flow based position estimate is ok // ekf_has_relative_position - returns true if the EKF can provide a position estimate relative to it's starting position
bool Copter::optflow_position_ok() const bool Copter::ekf_has_relative_position() const
{ {
// return immediately if EKF not used // return immediately if EKF not used
if (!ahrs.have_inertial_nav()) { if (!ahrs.have_inertial_nav()) {