fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-15 13:17:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Improve code clarity - non functional change

Browse Source
This commit is contained in:
Paul Riseborough
2020-10-06 19:21:50 +11:00
committed by Paul Riseborough
parent 1a7c68ea72
commit e82d0af6d2
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+5 -5
View File
@@ -581,16 +581,17 @@ void Ekf::controlGpsFusion()
_time_last_on_ground_us = _time_last_imu; _time_last_on_ground_us = _time_last_imu;
} }
bool recent_takeoff_nav_failure = _control_status.flags.in_air && const bool recent_takeoff_nav_failure = _control_status.flags.in_air &&
!isTimedOut(_time_last_on_ground_us, 30000000) && !isTimedOut(_time_last_on_ground_us, 30000000) &&
isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) &&
(_time_last_hor_vel_fuse > _time_last_on_ground_us); (_time_last_hor_vel_fuse > _time_last_on_ground_us);
bool inflight_nav_failure = _control_status.flags.in_air && const bool inflight_nav_failure = _control_status.flags.in_air &&
do_vel_pos_reset && do_vel_pos_reset &&
(_time_last_hor_vel_fuse > _time_last_on_ground_us) && (_time_last_hor_vel_fuse > _time_last_on_ground_us) &&
(_time_last_hor_pos_fuse > _time_last_on_ground_us); (_time_last_hor_pos_fuse > _time_last_on_ground_us);
bool is_yaw_failure = false;
if ((recent_takeoff_nav_failure || inflight_nav_failure) && _time_last_hor_vel_fuse > 0) { if ((recent_takeoff_nav_failure || inflight_nav_failure) && _time_last_hor_vel_fuse > 0) {
// Do sanity check to see if the innovation failures is likely caused by a yaw angle error // Do sanity check to see if the innovation failures is likely caused by a yaw angle error
// A 180 deg yaw error would (in the absence of other errors) generate a velocity innovation // A 180 deg yaw error would (in the absence of other errors) generate a velocity innovation
@@ -598,14 +599,13 @@ void Ekf::controlGpsFusion()
const float hvel_obs_sq = sq(_last_vel_obs(0)) + sq(_last_vel_obs(1)); const float hvel_obs_sq = sq(_last_vel_obs(0)) + sq(_last_vel_obs(1));
const float hvel_innov_sq = sq(_last_fail_hvel_innov(0)) + sq(_last_fail_hvel_innov(1)); const float hvel_innov_sq = sq(_last_fail_hvel_innov(0)) + sq(_last_fail_hvel_innov(1));
if (hvel_innov_sq > 9.0f * hvel_obs_sq) { if (hvel_innov_sq > 9.0f * hvel_obs_sq) {
recent_takeoff_nav_failure = false; is_yaw_failure = true;
inflight_nav_failure = false;
} }
} }
// Detect if coming back after significant time without GPS data // Detect if coming back after significant time without GPS data
const bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000); const bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000);
const bool do_yaw_vel_pos_reset = (_do_ekfgsf_yaw_reset || recent_takeoff_nav_failure || inflight_nav_failure) && const bool do_yaw_vel_pos_reset = (_do_ekfgsf_yaw_reset || is_yaw_failure) &&
_ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit &&
isTimedOut(_ekfgsf_yaw_reset_time, 5000000) && isTimedOut(_ekfgsf_yaw_reset_time, 5000000) &&
!gps_signal_was_lost; !gps_signal_was_lost;