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

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
+20 -17
View File
@@ -585,33 +585,36 @@ void Ekf::updateDeadReckoningStatus()
void Ekf::updateHorizontalDeadReckoningstatus() 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) // velocity or position aiding
&& (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) 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_vel_fuse, _params.no_aid_timeout_max)); && (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) #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 #endif // CONFIG_EKF2_OPTICAL_FLOW
bool airDataAiding = false; // air data
bool air_data_aiding = false;
#if defined(CONFIG_EKF2_AIRSPEED) #if defined(CONFIG_EKF2_AIRSPEED)
airDataAiding = _control_status.flags.wind && air_data_aiding = _control_status.flags.wind && _control_status.flags.in_air
isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) && && _control_status.flags.fuse_aspd && 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.fuse_beta && 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;
#endif // CONFIG_EKF2_AIRSPEED #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 (!_control_status.flags.inertial_dead_reckoning) {
if (_time_delayed_us > _params.no_aid_timeout_max) { _time_last_horizontal_aiding = _time_delayed_us;
_time_last_horizontal_aiding = _time_delayed_us - _params.no_aid_timeout_max;
}
} }
// report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present // report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present
+1 -1
View File
@@ -105,7 +105,7 @@ TEST_F(EkfGpsTest, gpsFixLoss)
_sensor_simulator._gps.setFixType(0); _sensor_simulator._gps.setFixType(0);
// THEN: after dead-reconing for a couple of seconds, the local position gets invalidated // 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_TRUE(_ekf->control_status_flags().inertial_dead_reckoning);
EXPECT_FALSE(_ekf->local_position_is_valid()); EXPECT_FALSE(_ekf->local_position_is_valid());