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

EKF: Add persistence criteria to GPS fail check

Browse Source
This commit is contained in:
Paul Riseborough
2018-02-26 14:56:20 +11:00
parent 67d71ca406
commit 4ab78230e6
3 changed files with 5 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+2 -1
View File
@@ -461,6 +461,7 @@ 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); bool gps_checks_passing = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6);
bool gps_checks_failing = (_time_last_imu - _last_gps_pass_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 && gps_checks_passing) { if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) {
// 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
@@ -500,7 +501,7 @@ void Ekf::controlGpsFusion()
} }
// Handle the case where we are using GPS and another source of aiding and GPS is failing checks // 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)) { if (_control_status.flags.gps && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) {
_control_status.flags.gps = false; _control_status.flags.gps = false;
ECL_WARN("EKF GPS data quality poor - stopping use"); ECL_WARN("EKF GPS data quality poor - stopping use");
} }
EKF/ekf.h
+1
View File
@@ -372,6 +372,7 @@ private:
float _gps_velN_filt{0.0f}; ///< GPS filtered North velocity (m/sec) float _gps_velN_filt{0.0f}; ///< GPS filtered North velocity (m/sec)
float _gps_velE_filt{0.0f}; ///< GPS filtered East velocity (m/sec) float _gps_velE_filt{0.0f}; ///< GPS filtered East velocity (m/sec)
uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks
uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks
// Variables used to publish the WGS-84 location of the EKF local NED origin // Variables used to publish the WGS-84 location of the EKF local NED origin
uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec) uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec)
EKF/gps_checks.cpp
+2
View File
@@ -228,6 +228,8 @@ bool Ekf::gps_is_good(struct gps_message *gps)
(_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD)) (_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD))
) { ) {
_last_gps_fail_us = _time_last_imu; _last_gps_fail_us = _time_last_imu;
} else {
_last_gps_pass_us = _time_last_imu;
} }
// continuous period without fail of 10 seconds required to return a healthy status // continuous period without fail of 10 seconds required to return a healthy status