fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 11:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

GPS Yaw: move isfinite checks in control.cpp

Browse Source
This commit is contained in:
bresch
2020-06-23 08:43:48 +02:00
committed by Mathieu Bresciani
parent 3c6790f5d5
commit fe2a9d3018
3 changed files with 100 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+19 -16
View File
@@ -518,28 +518,31 @@ void Ekf::controlGpsFusion()
// Detect if coming back after significant time without GPS data
bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000);
// GPS yaw aiding selection logic
if ((_params.fusion_mode & MASK_USE_GPSYAW)
&& ISFINITE(_gps_sample_delayed.yaw)
&& _control_status.flags.tilt_align
&& !_control_status.flags.gps_yaw
&& !_gps_hgt_intermittent) {
if (!(_params.fusion_mode & MASK_USE_GPSYAW)) {
stopGpsYawFusion();
if (resetGpsAntYaw()) {
// flag the yaw as aligned
_control_status.flags.yaw_align = true;
} else {
if (ISFINITE(_gps_sample_delayed.yaw)) {
if (!_control_status.flags.gps_yaw
&& _control_status.flags.tilt_align
&& !_gps_hgt_intermittent) {
startGpsYawFusion();
// Activate GPS yaw fusion
if (resetGpsAntYaw()) {
_control_status.flags.yaw_align = true;
ECL_INFO_TIMESTAMPED("starting GPS yaw fusion");
startGpsYawFusion();
ECL_INFO_TIMESTAMPED("starting GPS yaw fusion");
}
}
if (_control_status.flags.gps_yaw) {
fuseGpsAntYaw();
}
}
}
// fuse the yaw observation
if (_control_status.flags.gps_yaw) {
fuseGpsAntYaw();
}
// Determine if we should use GPS aiding for velocity and horizontal position
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);