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

ekf2: Enable user defeatible saving of declination for next startup

Browse Source
This commit is contained in:
Paul Riseborough
2016-02-13 19:12:47 +11:00
committed by tumbili
parent 2ca48037fd
commit af5bbc916c
1 changed files with 11 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -131,6 +131,8 @@ private:
int _control_mode_sub = -1;
int _vehicle_status_sub = -1;
bool _prev_motors_armed = false; // motors armed status from the previous frame
orb_advert_t _att_pub;
orb_advert_t _lpos_pub;
orb_advert_t _control_state_pub;
@@ -563,6 +565,15 @@ void Ekf2::task_main()
orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
}
// save the declination to the EKF2_MAG_DECL parameter when a dis-arm event is detected
if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !vehicle_control_mode.flag_armed) {
float decl_deg;
_ekf->copy_mag_decl_deg(&decl_deg);
param_set(param_find("EKF2_MAG_DECL"), &decl_deg);
}
_prev_motors_armed = vehicle_control_mode.flag_armed;
}
delete ekf2::instance;