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

ekf2: consider drag fusion as air data aiding (allowing wind dead reckoning)

Browse Source
This commit is contained in:
Daniel Agar 2024-04-19 09:28:39 -04:00
parent 0a7689f323
commit b3e4de907a
2 changed files with 21 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -585,33 +585,36 @@ void Ekf::updateDeadReckoningStatus()
void Ekf::updateHorizontalDeadReckoningstatus()
{
const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.aux_gpos)
&& (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
|| isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));
// velocity or position aiding
const bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.aux_gpos)
&& (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));
bool optFlowAiding = false;
// optical flow
bool opt_flow_aiding = false;
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
opt_flow_aiding = _control_status.flags.opt_flow
&& isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
#endif // CONFIG_EKF2_OPTICAL_FLOW
bool airDataAiding = false;
// air data
bool air_data_aiding = false;
#if defined(CONFIG_EKF2_AIRSPEED)
airDataAiding = _control_status.flags.wind &&
isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) &&
isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max);
_control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
#else
_control_status.flags.wind_dead_reckoning = false;
air_data_aiding = _control_status.flags.wind && _control_status.flags.in_air
&& _control_status.flags.fuse_aspd && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max)
&& _control_status.flags.fuse_beta && isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max);
#endif // CONFIG_EKF2_AIRSPEED
_control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
#if defined(CONFIG_EKF2_DRAG_FUSION)
air_data_aiding |= _control_status.flags.wind && _control_status.flags.in_air
&& isRecent(_aid_src_drag.time_last_fuse, _params.no_aid_timeout_max);
#endif // CONFIG_EKF2_DRAG_FUSION
_control_status.flags.inertial_dead_reckoning = !vel_pos_aiding && !opt_flow_aiding && !air_data_aiding;
_control_status.flags.wind_dead_reckoning = !vel_pos_aiding && !opt_flow_aiding && air_data_aiding;
if (!_control_status.flags.inertial_dead_reckoning) {
if (_time_delayed_us > _params.no_aid_timeout_max) {
_time_last_horizontal_aiding = _time_delayed_us - _params.no_aid_timeout_max;
}
_time_last_horizontal_aiding = _time_delayed_us;
}
// report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present

2
src/modules/ekf2/test/test_EKF_gps.cpp
View File

@ -105,7 +105,7 @@ TEST_F(EkfGpsTest, gpsFixLoss)
_sensor_simulator._gps.setFixType(0);
// THEN: after dead-reconing for a couple of seconds, the local position gets invalidated
_sensor_simulator.runSeconds(5);
_sensor_simulator.runSeconds(6);
EXPECT_TRUE(_ekf->control_status_flags().inertial_dead_reckoning);
EXPECT_FALSE(_ekf->local_position_is_valid());