fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 02: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/common.h
+1
View File
@@ -416,6 +416,7 @@ union filter_control_status_u {
uint32_t update_mag_states_only : 1; ///< 16 - true when only the magnetometer states are updated by the magnetometer
uint32_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
uint32_t mag_fault : 1; ///< 18 - true when the magnetomer has been declared faulty and is no longer being used
uint32_t fuse_aspd : 1; ///< 19 - true when airspedd measurements are being fused
} flags;
uint32_t value;
};
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;
EKF/ekf_helper.cpp
+15 -6
View File
@@ -902,7 +902,10 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
// TODO - allow for baro drift in vertical position error
float hpos_err;
float vpos_err;
bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
bool vel_pos_aiding = (_control_status.flags.gps ||
_control_status.flags.opt_flow ||
_control_status.flags.ev_pos ||
(_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd));
if (vel_pos_aiding && _NED_origin_initialised) {
hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph));
@@ -933,7 +936,10 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
// TODO - allow for baro drift in vertical position error
float hpos_err;
float vpos_err;
bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
bool vel_pos_aiding = (_control_status.flags.gps ||
_control_status.flags.opt_flow ||
_control_status.flags.ev_pos ||
(_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd));
if (vel_pos_aiding && _NED_origin_initialised) {
hpos_err = sqrtf(P[7][7] + P[8][8]);
@@ -963,7 +969,10 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon
{
float hvel_err;
float vvel_err;
bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
bool vel_pos_aiding = (_control_status.flags.gps ||
_control_status.flags.opt_flow ||
_control_status.flags.ev_pos ||
(_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd));
if (vel_pos_aiding && _NED_origin_initialised) {
hvel_err = sqrtf(P[4][4] + P[5][5]);
@@ -1058,9 +1067,9 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
{
ekf_solution_status soln_status{};
soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow ) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
soln_status.flags.pos_vert_agl = get_terrain_valid();
@@ -1146,7 +1155,7 @@ bool Ekf::inertial_dead_reckoning()
bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos)
&& ((_time_last_imu - _time_last_pos_fuse <= 1E6) || (_time_last_imu - _time_last_vel_fuse <= 1E6));
bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= 1E6);
bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6);
bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6) && (_time_last_imu - _time_last_beta_fuse <= 1E6);
return !velPosAiding && !optFlowAiding && !airDataAiding;
}
EKF/estimator_interface.h
+1 -1
View File
@@ -199,7 +199,7 @@ public:
void set_is_fixed_wing(bool is_fixed_wing) {_control_status.flags.fixed_wing = is_fixed_wing;}
// set flag if synthetic sideslip measurement should be fused
void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = fuse_beta;}
void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air);}
// set flag if only only mag states should be updated by the magnetometer
void set_update_mag_states_only_flag(bool update_mag_states_only) {_control_status.flags.update_mag_states_only = update_mag_states_only;}