dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Add function to calculate the magnetic declination to use

This commit is contained in:
Paul Riseborough 2016-02-12 14:09:23 +11:00 committed by Roman
parent 9017e077f8
commit 83945581ed
3 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/ekf.h
View File

@ -201,6 +201,9 @@ private:
// return true if successful
bool resetMagHeading(Vector3f &mag_init);
// calculate the magnetic declination to be used by the alignment and fusion processing
void calcMagDeclination();
// reset position states of the ekf (only vertical position)
void resetPosition();

22
EKF/ekf_helper.cpp
View File

@ -124,6 +124,28 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
return true;
}
// Calculate the magnetic declination to be used by the alignment and fusion processing
void Ekf::calcMagDeclination()
{
// set source of magnetic declination for internal use
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;
_mag_declination_to_save_deg = math::degrees(_mag_declination);
} else {
_mag_declination = math::radians(_params.mag_declination_deg);
_mag_declination_to_save_deg = _params.mag_declination_deg;
}
} else {
// always use the parameter value
_mag_declination = math::radians(_params.mag_declination_deg);
_mag_declination_to_save_deg = _params.mag_declination_deg;
}
}
// This function forces the covariance matrix to be symmetric
void Ekf::makeSymmetrical()
{

3
EKF/gps_checks.cpp
View File

@ -40,6 +40,7 @@
*/
#include "ekf.h"
#include <mathlib/mathlib.h>
// GPS pre-flight check bit locations
#define MASK_GPS_NSATS (1<<0)
@ -67,6 +68,8 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
_gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2);
_NED_origin_initialised = true;
_last_gps_origin_time_us = _time_last_imu;
// set the magnetic declination returned by the geo library using the current GPS position
_mag_declination_gps = math::radians(get_mag_declination(lat, lon));
}
}