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

ekf reset: more granular reset / timeout strategy for gps fusion

Browse Source 
- if both gps position and velocity measurements are rejected for 7 seconds
do a reset
- if only gps position measurements are rejected then wait for 14 seconds
as we still have velocity measurements to constrain the drift in position
- introduced ecl internal parameter for the timeout

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2016-10-24 11:40:12 +02:00
parent 65de4962b5
commit c5f750dcca
2 changed files with 24 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+5
View File
@@ -261,6 +261,9 @@ struct parameters {
float vel_Tau; // velocity state correction time constant (1/sec)
float pos_Tau; // postion state correction time constant (1/sec)
unsigned no_gps_timeout_max; // maximum time we allow dead reckoning while both gps position and velocity measurements are being
// rejected
// Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
parameters()
{
@@ -351,6 +354,8 @@ struct parameters {
vel_Tau = 0.25f;
pos_Tau = 0.25f;
no_gps_timeout_max = 7e6; // maximum seven seconds of dead reckoning time for gps
}
};
EKF/control.cpp
+19 -15
View File
@@ -355,9 +355,7 @@ void Ekf::controlGpsFusion()
// handle the case when we are relying on GPS fusion and lose it
if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
// We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something
if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) {
if (_time_last_imu - _time_last_gps > 5e5) {
if (_time_last_imu - _time_last_gps > 5e5) {
// if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states
// and set the synthetic GPS position to the current estimate
_control_status.flags.gps = false;
@@ -365,19 +363,25 @@ void Ekf::controlGpsFusion()
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
ECL_WARN("EKF GPS fusion timout - stopping GPS aiding");
} else {
// Reset states to the last GPS measurement
resetPosition();
resetVelocity();
ECL_WARN("EKF GPS fusion timout - resetting to GPS");
// Reset the timeout counters
_time_last_pos_fuse = _time_last_imu;
_time_last_vel_fuse = _time_last_imu;
}
}
// We are relying on GPS aiding to constrain attitude drift so after 7 seconds without aiding we need to do something
bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max) && (_time_last_imu - _time_last_vel_fuse > _params.no_gps_timeout_max);
// Our position measurments have been rejected for more than 14 seconds
do_reset |= _time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max;
if (do_reset) {
// Reset states to the last GPS measurement
resetPosition();
resetVelocity();
ECL_WARN("EKF GPS fusion timout - resetting to GPS");
// Reset the timeout counters
_time_last_pos_fuse = _time_last_imu;
_time_last_vel_fuse = _time_last_imu;
}
}
// Only use GPS data for position and velocity aiding if enabled