fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-07 12:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: allow emergency reset in GNSS yaw and EV yaw aiding modes

Browse Source
This commit is contained in:
bresch
2021-08-02 14:43:03 +02:00
committed by Mathieu Bresciani
parent 4ebfbc6eab
commit 11cd51c132
4 changed files with 30 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+4 -3
View File
@@ -566,10 +566,11 @@ void Ekf::controlGpsFusion()
// handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before
// we can start using GPS
const bool align_yaw_using_gsf = !_control_status.flags.gps && _do_ekfgsf_yaw_reset
&& isTimedOut(_ekfgsf_yaw_reset_time, 5000000);
const bool align_yaw_using_gsf = !_control_status.flags.yaw_align
&& (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE);
if (align_yaw_using_gsf) {
if ((align_yaw_using_gsf || _do_ekfgsf_yaw_reset)
&& isTimedOut(_ekfgsf_yaw_reset_time, 5000000)){
if (resetYawToEKFGSF()) {
_ekfgsf_yaw_reset_time = _time_last_imu;
_do_ekfgsf_yaw_reset = false;