yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: Fix failure to save mag declination

Browse Source
This commit is contained in:
Paul Riseborough
2017-05-02 15:55:44 +10:00
committed by Lorenz Meier
parent 0d7f475bd0
commit 5bfe6d7fec
1 changed files with 13 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/ekf2/ekf2_main.cpp
View File

@@ -153,9 +153,8 @@ private:
uint32_t _balt_time_ms_last_used = uint32_t _balt_time_ms_last_used =
0; // time stamp in msec of the last averaged barometric altitude measurement used by the EKF 0; // time stamp in msec of the last averaged barometric altitude measurement used by the EKF
bool _prev_landed = true; // landed status from the previous frame
float _acc_hor_filt = 0.0f; // low-pass filtered horizontal acceleration float _acc_hor_filt = 0.0f; // low-pass filtered horizontal acceleration
bool _mag_decl_saved = false; // true when the declination for the current position has been saved
// Used to check, save and use learned magnetometer biases // Used to check, save and use learned magnetometer biases
hrt_abstime _last_invalid_magcal_us = hrt_abstime _last_invalid_magcal_us =
@@ -1056,6 +1055,18 @@ void Ekf2::task_main()
// reset the timer to prevent possible race condition causing data to be saved too frequently // reset the timer to prevent possible race condition causing data to be saved too frequently
_last_invalid_magcal_us = now; _last_invalid_magcal_us = now;
// save the declination to the EKF2_MAG_DECL parameter when transitioning into ARMING_STATE_STANDBY
// for the first time when the EKF is using GPS
if (!_mag_decl_saved && // this is the first transition into standby
(_params->mag_declination_source & (1 << 1)) && // saving the declination is allowed by the parameter setting
(status.control_mode_flags & (1 << 2))) { // the EKF is using GPS
float decl_deg;
_ekf.copy_mag_decl_deg(&decl_deg);
_mag_declination_deg.set(decl_deg);
_mag_declination_deg.commit_no_notification();
_mag_decl_saved = true;
}
} }
// Publish wind estimate // Publish wind estimate
@@ -1119,15 +1130,6 @@ void Ekf2::task_main()
} }
} }
// save the declination to the EKF2_MAG_DECL parameter when a land event is detected
if ((_params->mag_declination_source & (1 << 1)) && !_prev_landed && vehicle_land_detected.landed) {
float decl_deg;
_ekf.copy_mag_decl_deg(&decl_deg);
_mag_declination_deg.set(decl_deg);
}
_prev_landed = vehicle_land_detected.landed;
// publish ekf2_timestamps (using 0.1 ms relative timestamps) // publish ekf2_timestamps (using 0.1 ms relative timestamps)
{ {
ekf2_timestamps_s ekf2_timestamps; ekf2_timestamps_s ekf2_timestamps;