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

mag fusion: improve on ground forced mag fusion logic

Browse Source 
While a yaw test ration > 1 rejects the mag data in air, the fusion is
forced with a limited innovation on ground to fix gyro bias and local
magnetic effects problems pre-takeoff. However, the same logic is
applied after landing and immediately after the land detector detects
landing. In some cases, this can disturb the other EKF states which can
lead to a state change of the land detector.
To avoid such a loop, the start of this forced yaw fusion is delayed by
5 seconds after landing.
This commit is contained in:
bresch 2021-02-16 15:01:25 +01:00 committed by Mathieu Bresciani
parent 310f415175
commit b00d4a9237
3 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/control.cpp
View File

@ -601,6 +601,9 @@ void Ekf::controlGpsFusion()
if (!_control_status.flags.in_air) {
_time_last_on_ground_us = _time_last_imu;
} else {
_time_last_in_air = _time_last_imu;
}
const bool recent_takeoff_nav_failure = _control_status.flags.in_air &&

3
EKF/ekf.h
View File

@ -959,7 +959,8 @@ private:
EKFGSF_yaw _yawEstimator;
int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec)
uint64_t _time_last_on_ground_us{0}; ///< last tine we were on the ground (uSec)
uint64_t _time_last_on_ground_us{0}; ///< last time we were on the ground (uSec)
uint64_t _time_last_in_air{0}; ///< last time we were in air (uSec)
bool _do_ekfgsf_yaw_reset{false}; // true when an emergency yaw reset has been requested
uint8_t _ekfgsf_yaw_reset_count{0}; // number of times the yaw has been reset to the EKF-GSF estimate

10
EKF/mag_fusion.cpp
View File

@ -646,13 +646,15 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
// if we are in air we don't want to fuse the measurement
// we allow to use it when on the ground because the large innovation could be caused
// by interference or a large initial gyro bias
if (_control_status.flags.in_air) {
return;
} else {
if (!_control_status.flags.in_air && isTimedOut(_time_last_in_air, (uint64_t)5e6)) {
// constrain the innovation to the maximum set by the gate
// we need to delay this forced fusion to avoid starting it
// immediately after touchdown, when the drone is still armed
float gate_limit = sqrtf((sq(gate_sigma) * _heading_innov_var));
_heading_innov = math::constrain(innovation, -gate_limit, gate_limit);
} else {
return;
}
} else {