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

EKF: update mag LPF immediately

Browse Source
This commit is contained in:
Daniel Agar
2020-12-17 10:46:04 +01:00
committed by Mathieu Bresciani
parent 398fe159ce
commit a8e0e82858
3 changed files with 2 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+2
View File
@@ -97,6 +97,8 @@ void Ekf::controlFusionModes()
_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed); _mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
if (_mag_data_ready) { if (_mag_data_ready) {
_mag_lpf.update(_mag_sample_delayed.mag);
// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value. // 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. // 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 || ISFINITE(_mag_declination_gps))) { if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) && (_NED_origin_initialised || ISFINITE(_mag_declination_gps))) {
EKF/ekf.h
-1
View File
@@ -762,7 +762,6 @@ private:
// control fusion of magnetometer observations // control fusion of magnetometer observations
void controlMagFusion(); void controlMagFusion();
void updateMagFilter();
bool noOtherYawAidingThanMag() const; bool noOtherYawAidingThanMag() const;
EKF/mag_control.cpp
-8
View File
@@ -58,7 +58,6 @@ void Ekf::controlMagFusion()
return; return;
} }
updateMagFilter();
checkMagFieldStrength(); checkMagFieldStrength();
// If we are on ground, reset the flight alignment flag so that the mag fields will be // If we are on ground, reset the flight alignment flag so that the mag fields will be
@@ -112,13 +111,6 @@ void Ekf::controlMagFusion()
} }
} }
void Ekf::updateMagFilter()
{
if (_mag_data_ready) {
_mag_lpf.update(_mag_sample_delayed.mag);
}
}
bool Ekf::noOtherYawAidingThanMag() const bool Ekf::noOtherYawAidingThanMag() const
{ {
// If we are using external vision data or GPS-heading for heading then no magnetometer fusion is used // If we are using external vision data or GPS-heading for heading then no magnetometer fusion is used