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

ekf2: separate mag and mag heading control logic (#21212)

Browse Source 
- split mag_3d into new standalone mag fusion and mag fusion allowed to update all states (full mag_3d)
 - new dedicated control logic for mag/mag_3d fusion and standalone mag heading fusion
 - if WMM available use for mag_I and mag_B init
 - mag states reset if external yaw reset (yaw estimator, GPS yaw, etc)
 - mag reset if declination changed (eliminate _mag_yaw_reset_req)
 - mag fusion (but not mag_hdg or mag_3d) can be active during gps_yaw or ev_yaw (if yaw aligned north)

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2023-08-04 10:39:16 -04:00
committed by GitHub
co-authored by bresch
parent 7db667558e
commit d75bb62a65
26 changed files with 1650 additions and 1397 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+8 -8
View File
@@ -76,7 +76,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
_control_status.flags.tilt_align = true;
// send alignment status message to the console
const char *height_source = nullptr;
const char *height_source = "unknown";
if (_control_status.flags.baro_hgt) {
height_source = "baro";
@@ -89,16 +89,16 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
} else if (_control_status.flags.rng_hgt) {
height_source = "rng";
} else {
height_source = "unknown";
}
if (height_source) {
ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
(unsigned long long)imu_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
}
ECL_DEBUG("tilt aligned, roll: %.3f, pitch %.3f, yaw: %.3f",
(double)matrix::Eulerf(_state.quat_nominal).phi(),
(double)matrix::Eulerf(_state.quat_nominal).theta(),
(double)matrix::Eulerf(_state.quat_nominal).psi()
);
}
}