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

ekf2-mag: never skip post-takeoff mag yaw reset

Browse Source
This commit is contained in:
bresch
2025-05-06 11:24:31 -04:00
committed by Daniel Agar
parent 4e17c5496c
commit 6b8bf80423
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
+5 -3
View File
@@ -210,9 +210,11 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
if (continuing_conditions_passing && _control_status.flags.yaw_align) {
if (checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_not_moving)) {
if ((checkHaglYawResetReq() && (_control_status.flags.mag_hdg || _control_status.flags.mag_3D))
|| (wmm_updated && no_ne_aiding_or_not_moving)) {
ECL_INFO("reset to %s", AID_SRC_NAME);
resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
const bool reset_heading = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
resetMagStates(_mag_lpf.getState(), reset_heading);
aid_src.time_last_fuse = imu_sample.time_us;
} else {
@@ -446,7 +448,7 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
}
// record the start time for the magnetic field alignment
if (_control_status.flags.in_air) {
if (_control_status.flags.in_air && reset_heading) {
_control_status.flags.mag_aligned_in_flight = true;
_flt_mag_align_start_time = _time_delayed_us;
}