diff --git a/src/lib/ecl b/src/lib/ecl index 4d59c834eb..bcda061bae 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 4d59c834ebb181b78294ec3981c3d32b470e6f15 +Subproject commit bcda061baeed08b667e9f8ab7017c8c83c6e992c diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index deab06f862..924bf7fd57 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -119,7 +119,8 @@ private: template void update_mag_bias(Param &mag_bias_param, int axis_index); - + template + bool update_mag_decl(Param &mag_decl_param); bool publish_attitude(const sensor_combined_s &sensors); bool publish_wind_estimate(const hrt_abstime ×tamp); @@ -191,6 +192,9 @@ private: bool _valid_cal_available[3] = {}; ///< true when an unsaved valid calibration for the XYZ magnetometer bias is available float _last_valid_variance[3] = {}; ///< variances for the last valid magnetometer XYZ bias estimates (mGauss**2) + // Used to control saving of mag declination to be used on next startup + bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved + // Used to filter velocity innovations during pre-flight checks bool _preflt_horiz_fail = false; ///< true if preflight horizontal innovation checks are failed bool _preflt_vert_fail = false; ///< true if preflight vertical innovation checks are failed @@ -648,6 +652,21 @@ void Ekf2::update_mag_bias(Param &mag_bias_param, int axis_index) } } +template +bool Ekf2::update_mag_decl(Param &mag_decl_param) +{ + // update stored declination value + float declination_deg; + + if (_ekf.get_mag_decl_deg(&declination_deg)) { + mag_decl_param.set(declination_deg); + mag_decl_param.commit_no_notification(); + return true; + } + + return false; +} + void Ekf2::run() { bool imu_bias_reset_request = false; @@ -1488,7 +1507,12 @@ void Ekf2::run() _total_cal_time_us = 0; } - publish_wind_estimate(now); + } + + publish_wind_estimate(now); + + if (!_mag_decl_saved && (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) { + _mag_decl_saved = update_mag_decl(_mag_declination_deg); } {