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

EKF: enable gps fusion flag to be false while fusing air data

Browse Source
This commit is contained in:
Paul Riseborough
2017-10-20 14:44:37 +11:00
parent 204a218ee6
commit 929c5c2b37
4 changed files with 42 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+25 -12
View File
@@ -494,14 +494,18 @@ void Ekf::controlGpsFusion()
} else {
// handle the case where we do not have GPS and have not been using it for an extended period, but are still relying on it
bool lost_gps = _control_status.flags.gps && (_time_last_imu - _time_last_gps > 10e6);
bool no_velpos_aiding = !_control_status.flags.opt_flow && !_control_status.flags.ev_pos;
bool no_airdata_aiding = (_time_last_imu - _time_last_arsp_fuse > 10e6) || (_time_last_imu - _time_last_beta_fuse > 10e6);
// If we don't have a source of aiding to constrain attitude drift, then we need to switch
// to the non-aiding mode, zero the velocity states and set the position to the current estimate.
// Air data aiding prevents loss of attitude reference, constrains velocity drift and can assist
// a FW vehicle to recover to the launch area.
if (lost_gps && no_velpos_aiding && no_airdata_aiding) {
if (lost_gps) {
_control_status.flags.gps = false;
ECL_WARN("EKF GPS data stopped");
}
bool no_velpos_aiding = !_control_status.flags.opt_flow && !_control_status.flags.ev_pos;
bool no_airdata_aiding = (_time_last_imu - _time_last_arsp_fuse > 10e6) || (_time_last_imu - _time_last_beta_fuse > 10e6);
if (!_control_status.flags.gps && no_velpos_aiding && no_airdata_aiding) {
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
@@ -1010,9 +1014,18 @@ void Ekf::controlAirDataFusion()
}
// Always try to fuse airspeed data if available and we are in flight and the filter is operating in a normal aiding mode
bool is_aiding = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos;
if (_tas_data_ready && _control_status.flags.in_air && is_aiding) {
if (_control_status.flags.fuse_aspd && airspeed_timed_out) {
_control_status.flags.fuse_aspd = false;
}
// Always try to fuse airspeed data if available and we are in flight
if (_tas_data_ready && _control_status.flags.in_air) {
// always fuse airsped data if we are flying and data is present
if (!_control_status.flags.fuse_aspd) {
_control_status.flags.fuse_aspd = true;
}
// If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) {
// activate the wind states
@@ -1047,10 +1060,7 @@ void Ekf::controlBetaFusion()
// Suffient time has lapsed sice the last fusion
bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_us;
// The filter is operating in a mode where velocity states can be used
bool vel_states_active = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos;
if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air && vel_states_active) {
if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) {
// If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) {
// activate the wind states
@@ -1267,8 +1277,11 @@ void Ekf::controlVelPosFusion()
{
// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
if (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos
&& ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
if (!_control_status.flags.gps &&
!_control_status.flags.opt_flow &&
!_control_status.flags.ev_pos &&
!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta) &&
((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
_fuse_pos = true;
_time_last_fake_gps = _time_last_imu;