mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-16 11:01:36 +08:00
ekf2: consider drag fusion as air data aiding (allowing wind dead reckoning)
This commit is contained in:
@@ -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
|
||||||
|
|||||||
@@ -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());
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user