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

EKF: Don't do magnetic field estimation without earth frame measurements

Browse Source 
Use horizontal acceleration to check if yaw is observable independent of the magnetometer.
Use rotation about the vertical to check if mag raises are observable.
If neither yaw of mag biases are observable, save the magnetic field variances and switch to magnetic yaw fusion.
Use the last learned declination when using magnetic yaw fusion so that the yaw reference remains consistent.
When yaw or biases become observable, reinstate the saved variances and switch back to 3D mag fusion.
This commit is contained in:
Paul Riseborough
2017-05-17 08:59:01 +02:00
committed by Lorenz Meier
parent 20db74eca8
commit 394dd95cba
6 changed files with 105 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+4 -1
View File
@@ -501,7 +501,10 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
void Ekf::calcMagDeclination()
{
// set source of magnetic declination for internal use
if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
if (_flt_mag_align_complete) {
// Use value consistent with earth field state
_mag_declination = atan2f(_state.mag_I(1),_state.mag_I(0));
} else if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
// use parameter value until GPS is available, then use value returned by geo library
if (_NED_origin_initialised) {
_mag_declination = _mag_declination_gps;