From 8d6e8ae07841e4e4ebfa2ae34f7e0a7a828e4cc5 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Tue, 12 Mar 2019 15:38:30 +0100 Subject: [PATCH] EKF: update get_mag_decl_deg() Signed-off-by: CarlOlsson --- EKF/ekf_helper.cpp | 3 --- EKF/estimator_interface.h | 11 ++++++++--- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 996ee9ee17..ae40bb531a 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -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); } } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 6cc8e76662..cb8da452f9 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -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)