fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 01:33:34 +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/estimator_interface.h
+22 -1
View File
@@ -140,16 +140,34 @@ public:
void set_in_air_status(bool in_air)
{
if (!in_air) {
if (_control_status.flags.in_air) {
ECL_DEBUG("no longer in air");
}
_time_last_on_ground_us = _time_delayed_us;
} else {
if (!_control_status.flags.in_air) {
ECL_DEBUG("in air");
}
_time_last_in_air = _time_delayed_us;
}
_control_status.flags.in_air = in_air;
}
void set_vehicle_at_rest(bool at_rest) { _control_status.flags.vehicle_at_rest = at_rest; }
void set_vehicle_at_rest(bool at_rest)
{
if (!_control_status.flags.vehicle_at_rest && at_rest) {
ECL_DEBUG("at rest");
} else if (_control_status.flags.vehicle_at_rest && !at_rest) {
ECL_DEBUG("no longer at rest");
}
_control_status.flags.vehicle_at_rest = at_rest;
}
// return true if the attitude is usable
bool attitude_valid() const { return _control_status.flags.tilt_align; }
@@ -420,9 +438,12 @@ protected:
// allocate data buffers and initialize interface variables
bool initialise_interface(uint64_t timestamp);
uint64_t _wmm_gps_time_last_checked{0}; // time WMM last checked
uint64_t _wmm_gps_time_last_set{0}; // time WMM last set
float _mag_declination_gps{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad)
float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T)
float _mag_inclination{NAN};
float _mag_strength{NAN};