Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

ekf2: Reinstate saving of mag declination for use next start #10024

Merged
merged 2 commits into from
Jul 26, 2018
Merged

ekf2: Reinstate saving of mag declination for use next start #10024

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
53ff669
ekf2: Reinstate saving of mag declination for use next start
priseborough Jul 24, 2018
c61ba37
EKF2 set param EKF2_MAG_DECL volatile
dagar Jul 25, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/lib/ecl
Open in desktop
Submodule ecl updated from 4d59c8 to bcda06
28 changes: 26 additions & 2 deletions src/modules/ekf2/ekf2_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,7 +119,8 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams

template<typename Param>
void update_mag_bias(Param &mag_bias_param, int axis_index);

template<typename Param>
bool update_mag_decl(Param &mag_decl_param);
bool publish_attitude(const sensor_combined_s &sensors);
bool publish_wind_estimate(const hrt_abstime &timestamp);

Expand Down Expand Up @@ -191,6 +192,9 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
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
Expand Down Expand Up @@ -648,6 +652,21 @@ void Ekf2::update_mag_bias(Param &mag_bias_param, int axis_index)
}
}

template<typename Param>
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;
Expand Down Expand Up @@ -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);
}

{
Expand Down
1 change: 1 addition & 0 deletions src/modules/ekf2/ekf2_params.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -432,6 +432,7 @@ PARAM_DEFINE_FLOAT(EKF2_BETA_NOISE, 0.3f);
* Magnetic declination
*
* @group EKF2
* @volatile
* @unit deg
* @decimal 1
*/
Expand Down