fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 16:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2-mag_auto: always use mag 3D after takeoff

Browse Source
This commit is contained in:
bresch
2024-02-27 12:33:43 -05:00
committed by Daniel Agar
parent 086656dc7f
commit b80f15f7b5
10 changed files with 8 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_control.cpp
-24
View File
@@ -47,15 +47,9 @@ void Ekf::controlMagFusion()
_control_status.flags.mag_aligned_in_flight = false;
}
// check mag state observability
checkYawAngleObservability();
checkMagBiasObservability();
checkMagHeadingConsistency();
if (_mag_bias_observable || _yaw_angle_observable) {
_time_last_mov_3d_mag_suitable = _time_delayed_us;
}
if (_params.mag_fusion_type == MagFuseType::NONE) {
stopMagFusion();
stopMagHdgFusion();
@@ -245,24 +239,6 @@ void Ekf::checkYawAngleObservability()
}
}
void Ekf::checkMagBiasObservability()
{
// check if there is enough yaw rotation to make the mag bias states observable
if (!_mag_bias_observable && (fabsf(_yaw_rate_lpf_ef) > _params.mag_yaw_rate_gate)) {
// initial yaw motion is detected
_mag_bias_observable = true;
} else if (_mag_bias_observable) {
// require sustained yaw motion of 50% the initial yaw rate threshold
const float yaw_dt = 1e-6f * (float)(_time_delayed_us - _time_yaw_started);
const float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * yaw_dt;
_mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req;
}
_yaw_delta_ef = 0.0f;
_time_yaw_started = _time_delayed_us;
}
void Ekf::checkMagHeadingConsistency()
{
if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {