fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 12:23:33 +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-13 08:14:23 +11:00
committed by Paul Riseborough
parent 2b17ced405
commit 8d6e8ae078
2 changed files with 8 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
-3
View File
@@ -777,17 +777,14 @@ float Ekf::getMagDeclination()
} else if (_params.mag_declination_source & MASK_USE_GEO_DECL) { } else if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
// use parameter value until GPS is available, then use value returned by geo library // use parameter value until GPS is available, then use value returned by geo library
if (_NED_origin_initialised) { if (_NED_origin_initialised) {
_mag_declination_to_save_deg = math::degrees(_mag_declination_gps);
return _mag_declination_gps; return _mag_declination_gps;
} else { } else {
_mag_declination_to_save_deg = _params.mag_declination_deg;
return math::radians(_params.mag_declination_deg); return math::radians(_params.mag_declination_deg);
} }
} else { } else {
// always use the parameter value // always use the parameter value
_mag_declination_to_save_deg = _params.mag_declination_deg;
return math::radians(_params.mag_declination_deg); return math::radians(_params.mag_declination_deg);
} }
} }
EKF/estimator_interface.h
+8 -3
View File
@@ -335,8 +335,14 @@ public:
// At the next startup, set param.mag_declination_deg to the value saved // At the next startup, set param.mag_declination_deg to the value saved
bool get_mag_decl_deg(float *val) bool get_mag_decl_deg(float *val)
{ {
*val = _mag_declination_to_save_deg; *val = 0.0f;
return _NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL); 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; virtual void get_accel_bias(float bias[3]) = 0;
@@ -549,7 +555,6 @@ protected:
void unallocate_buffers(); 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_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_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) float _mag_strength_gps{0.0f}; // magnetic strength returned by the geo library using the last valid GPS position (T)