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

GPS Yaw: Always reset Yaw when GPS Yaw fusion is starting

Browse Source 
Also avoid fusing fake mag data when an other source of yaw aiding
is active, even if in MAG_TYPE_NONE mode.
This commit is contained in:
bresch
2020-06-23 08:43:48 +02:00
committed by Mathieu Bresciani
parent ff8b5ec69d
commit 3c6790f5d5
6 changed files with 45 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/gps_yaw_fusion.cpp
+2 -11
View File
@@ -303,20 +303,11 @@ bool Ekf::resetGpsAntYaw()
return false;
}
const float predicted_yaw = atan2f(ant_vec_ef(1),ant_vec_ef(0));
// get measurement and correct for antenna array yaw offset
const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
// calculate the amount the yaw needs to be rotated by
const float yaw_delta = wrap_pi(measured_yaw - predicted_yaw);
// update the quaternion state estimates and corresponding covariances only if
// the change in angle has been large or the yaw is not yet aligned
if(fabsf(yaw_delta) > math::radians(15.0f) || !_control_status.flags.yaw_align) {
const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
resetQuatStateYaw(measured_yaw, yaw_variance, true);
}
const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
resetQuatStateYaw(measured_yaw, yaw_variance, true);
return true;
}