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

EKF: use GPS to lookup declination from WMM before full GPS checks pass

Browse Source
This commit is contained in:
Daniel Agar
2020-09-28 08:43:32 +02:00
committed by Mathieu Bresciani
parent 6b99ea2a60
commit 4c2355a638
7 changed files with 777 additions and 740 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+2 -1
View File
@@ -99,10 +99,11 @@ void Ekf::controlFusionModes()
if (_mag_data_ready) {
// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
// this is useful if there is a lot of interference on the sensor measurement.
if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) &&_NED_origin_initialised) {
if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) && (_NED_origin_initialised || ISFINITE(_mag_declination_gps))) {
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
_mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred);
_control_status.flags.synthetic_mag_z = true;
} else {
_control_status.flags.synthetic_mag_z = false;
}