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

EKF: allow GPS fallback if quality bad and alternative aiding available

Browse Source
This commit is contained in:
Paul Riseborough
2018-02-26 13:49:16 +11:00
parent bd59e381db
commit 67d71ca406
1 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+8 -3
View File
@@ -460,10 +460,9 @@ void Ekf::controlGpsFusion()
// Determine if we should use GPS aiding for velocity and horizontal position // 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 // 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 = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6);
if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
if (_control_status.flags.tilt_align && _NED_origin_initialised if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) {
&& (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6)) {
// If the heading is not aligned, reset the yaw and magnetic field states // If the heading is not aligned, reset the yaw and magnetic field states
if (!_control_status.flags.yaw_align) { if (!_control_status.flags.yaw_align) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
@@ -500,6 +499,12 @@ void Ekf::controlGpsFusion()
} }
// Handle the case where we are using GPS and another source of aiding and GPS is failing checks
if (_control_status.flags.gps && !gps_checks_passing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) {
_control_status.flags.gps = false;
ECL_WARN("EKF GPS data quality poor - stopping use");
}
// handle the case when we now have GPS, but have not been using it for an extended period // handle the case when we now have GPS, but have not been using it for an extended period
if (_control_status.flags.gps) { if (_control_status.flags.gps) {
// We are relying on aiding to constrain drift so after a specified time // We are relying on aiding to constrain drift so after a specified time