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

EKF: update get_mag_decl_deg()

Browse Source 
Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
This commit is contained in:
CarlOlsson 2019-03-12 15:38:30 +01:00 committed by Paul Riseborough
parent 2b17ced405
commit 8d6e8ae078
2 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/ekf_helper.cpp
View File

@ -777,17 +777,14 @@ float Ekf::getMagDeclination()
} 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_to_save_deg = math::degrees(_mag_declination_gps);
return _mag_declination_gps;
} else {
_mag_declination_to_save_deg = _params.mag_declination_deg;
return math::radians(_params.mag_declination_deg);
}
} else {
// always use the parameter value
_mag_declination_to_save_deg = _params.mag_declination_deg;
return math::radians(_params.mag_declination_deg);
}
}

11
EKF/estimator_interface.h
View File

@ -335,8 +335,14 @@ public:
// At the next startup, set param.mag_declination_deg to the value saved
bool get_mag_decl_deg(float *val)
{
*val = _mag_declination_to_save_deg;
return _NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL);
*val = 0.0f;
if (_NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL)) {
*val = math::degrees(_mag_declination_gps);
return true;
} else {
return false;
}
}
virtual void get_accel_bias(float bias[3]) = 0;
@ -549,7 +555,6 @@ protected:
void unallocate_buffers();
float _mag_declination_gps{0.0f}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
float _mag_declination_to_save_deg{0.0f}; // magnetic declination to save to EKF2_MAG_DECL (deg)
float _mag_inclination_gps{0.0f}; // magnetic inclination returned by the geo library using the last valid GPS position (rad)
float _mag_strength_gps{0.0f}; // magnetic strength returned by the geo library using the last valid GPS position (T)