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

refactor(ekf2): FusionSensor available/enabled/intended() data model

Browse Source 
Split FusionSensor into available (CTRL param != disabled) and enabled
(runtime-toggleable). intended() = enabled && available. EKF core aid
sources now set available themselves and use intended() or _params
directly for CTRL-level checks. Remove drag/imu from FusionControl,
add aspd/rngbcn. Add AGP sourceFusingBitmask() for active-status.
This commit is contained in:
Marco Hauswirth 2026-04-01 16:44:00 +02:00 committed by Marco Hauswirth
parent 0dd1640a54
commit b9a1c429b3
24 changed files with 80 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp
View File

@ -58,7 +58,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
const bool sideslip_timed_out = isTimedOut(_aid_src_sideslip.time_last_fuse, (uint64_t)10e6);
if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_fc.drag.intended == 0))) {
if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_params.ekf2_drag_ctrl == 0))) {
_control_status.flags.wind = false;
}
@ -82,7 +82,9 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
#endif // CONFIG_EKF2_GNSS
if (_params.ekf2_arsp_thr <= 0.f) {
_fc.aspd.available = (_params.ekf2_arsp_thr > 0.f);
if (!_fc.aspd.intended()) {
stopAirspeedFusion();
return;
}

13
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp
View File

@ -132,6 +132,19 @@ bool AuxGlobalPosition::anySourceFusing() const
return false;
}
uint8_t AuxGlobalPosition::sourceFusingBitmask() const
{
uint8_t mask = 0;
for (int i = 0; i < MAX_AGP_IDS; i++) {
if (_sources[i] && _sources[i]->isFusing()) {
mask |= (1u << i);
}
}
return mask;
}
int32_t AuxGlobalPosition::getAgpParamInt32(const char *param_suffix, int instance) const
{
char param_name[20] {};

1
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp
View File

@ -67,6 +67,7 @@ public:
*/
float testRatioFiltered() const;
bool anySourceFusing() const;
uint8_t sourceFusingBitmask() const;
int32_t getIdParam(int instance);
void setIdParam(int instance, int32_t sensor_id);
int mapSensorIdToSlot(int32_t sensor_id);

4
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp
View File

@ -99,11 +99,13 @@ void AgpSource::bufferData(const aux_global_position_s &msg, const estimator::im
bool AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
{
ekf._fc.agp[_slot].available = (_params.ctrl != 0);
AuxGlobalPositionSample sample;
if (_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
if (!(ekf._fc.agp[_slot].intended & static_cast<int32_t>(Ctrl::kHPos))) {
if (!ekf._fc.agp[_slot].intended()) {
return true;
}

4
src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
View File

@ -40,6 +40,8 @@
void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
{
_fc.baro.available = (_params.ekf2_baro_ctrl != 0);
static constexpr const char *HGT_SRC_NAME = "baro";
auto &aid_src = _aid_src_baro_hgt;
@ -111,7 +113,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
}
// determine if we should use height aiding
const bool continuing_conditions_passing = (_fc.baro.intended == 1)
const bool continuing_conditions_passing = _fc.baro.intended()
&& measurement_valid
&& (_baro_counter > _obs_buffer_length)
&& !_control_status.flags.baro_fault;

2
src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp
View File

@ -45,7 +45,7 @@
void Ekf::controlDragFusion(const imuSample &imu_delayed)
{
if ((_fc.drag.intended > 0) && _drag_buffer) {
if ((_params.ekf2_drag_ctrl > 0) && _drag_buffer) {
if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) {
_control_status.flags.wind = true;

2
src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp
View File

@ -41,6 +41,8 @@
void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
{
_fc.ev.available = (_params.ekf2_ev_ctrl != 0);
_ev_pos_b_est.predict(_dt_ekf_avg);
_ev_hgt_b_est.predict(_dt_ekf_avg);

2
src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp
View File

@ -102,7 +102,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
bias_est.fuseBias(measurement + _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
}
const bool continuing_conditions_passing = (_fc.ev.intended & static_cast<int32_t>(EvCtrl::VPOS))
const bool continuing_conditions_passing = _fc.ev.enabled && (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
&& measurement_valid;
const bool starting_conditions_passing = common_starting_conditions_passing

2
src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp
View File

@ -49,7 +49,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
// determine if we should use EV position aiding
bool continuing_conditions_passing = (_fc.ev.intended & static_cast<int32_t>(EvCtrl::HPOS))
bool continuing_conditions_passing = _fc.ev.enabled && (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
&& _control_status.flags.tilt_align
&& PX4_ISFINITE(ev_sample.pos(0))
&& PX4_ISFINITE(ev_sample.pos(1));

2
src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp
View File

@ -51,7 +51,7 @@ void Ekf::controlEvVelFusion(ExternalVisionVel &ev, const bool common_starting_c
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
// determine if we should use EV velocity aiding
bool continuing_conditions_passing = (_fc.ev.intended & static_cast<int32_t>(EvCtrl::VEL))
bool continuing_conditions_passing = _fc.ev.enabled && (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
&& _control_status.flags.tilt_align
&& ev._sample.vel.isAllFinite()
&& !ev._sample.vel.longerThan(_params.ekf2_vel_lim);

2
src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp
View File

@ -66,7 +66,7 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample
}
// determine if we should use EV yaw aiding
bool continuing_conditions_passing = (_fc.ev.intended & static_cast<int32_t>(EvCtrl::YAW))
bool continuing_conditions_passing = _fc.ev.enabled && (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
&& _control_status.flags.tilt_align
&& !_control_status.flags.ev_yaw_fault
&& PX4_ISFINITE(aid_src.observation)

2
src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp
View File

@ -82,7 +82,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
&& _gnss_checks.passed()
&& !_control_status.flags.gnss_fault;
const bool continuing_conditions_passing = (_fc.gps.intended & static_cast<int32_t>(GnssCtrl::VPOS))
const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
&& common_conditions_passing;
const bool starting_conditions_passing = continuing_conditions_passing

2
src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp
View File

@ -47,7 +47,7 @@
void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
{
if (!(_fc.gps.intended & static_cast<int32_t>(GnssCtrl::YAW))
if (!(_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
|| _control_status.flags.gnss_yaw_fault) {
stopGnssYawFusion();

12
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
View File

@ -41,7 +41,9 @@
void Ekf::controlGpsFusion(const imuSample &imu_delayed)
{
if (!_gps_buffer || (_fc.gps.intended == 0)) {
_fc.gps.available = (_params.ekf2_gps_ctrl != 0);
if (!_gps_buffer || !_fc.gps.intended()) {
stopGnssFusion();
return;
}
@ -126,7 +128,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool force_reset)
{
const bool continuing_conditions_passing = (_fc.gps.intended & static_cast<int32_t>(GnssCtrl::VEL))
const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
&& _control_status.flags.tilt_align
&& _control_status.flags.yaw_align
&& !_control_status.flags.gnss_fault
@ -182,7 +184,7 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for
void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset)
{
const bool gnss_pos_enabled = (_fc.gps.intended & static_cast<int32_t>(GnssCtrl::HPOS));
const bool gnss_pos_enabled = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
const bool continuing_conditions_passing = gnss_pos_enabled
&& _control_status.flags.tilt_align
@ -385,8 +387,8 @@ void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
_yawEstimator.fuseVelocity(vel_xy, vel_var, _control_status.flags.in_air);
// Try to align yaw using estimate if available
if (((_fc.gps.intended & static_cast<int32_t>(GnssCtrl::VEL))
|| (_fc.gps.intended & static_cast<int32_t>(GnssCtrl::HPOS)))
if (((_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
|| (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)))
&& !_control_status.flags.yaw_align
&& _control_status.flags.tilt_align) {
if (resetYawToEKFGSF()) {

2
src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp
View File

@ -59,7 +59,7 @@ void Ekf::controlGravityFusion(const imuSample &imu)
&& (accel_lpf_norm_sq < sq(upper_accel_limit));
// fuse gravity observation if our overall acceleration isn't too big
_control_status.flags.gravity_vector = (_fc.imu.intended & static_cast<int32_t>(ImuCtrl::GravityVector))
_control_status.flags.gravity_vector = (_params.ekf2_imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
&& (accel_lpf_norm_good || _control_status.flags.vehicle_at_rest)
&& !isHorizontalAidingActive()
&& _control_status.flags.tilt_align; // Let fake position do the initial alignment (more robust before takeoff)

16
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
View File

@ -54,7 +54,9 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
_control_status.flags.mag_aligned_in_flight = false;
}
if (_fc.mag.intended == MagFuseType::NONE) {
_fc.mag.available = _params.ekf2_mag_type != static_cast<int32_t>(MagFuseType::NONE);
if (!_fc.mag.intended()) {
stopMagFusion();
return;
}
@ -150,9 +152,9 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
math::max(_params.ekf2_mag_gate, 1.f)); // innovation gate
// determine if we should use mag fusion
bool continuing_conditions_passing = ((_fc.mag.intended == MagFuseType::INIT)
|| (_fc.mag.intended == MagFuseType::AUTO)
|| (_fc.mag.intended == MagFuseType::HEADING))
bool continuing_conditions_passing = ((_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::INIT))
|| (_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::AUTO))
|| (_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::HEADING)))
&& _control_status.flags.tilt_align
&& (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
&& mag_sample.mag.longerThan(0.f)
@ -187,12 +189,12 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
&& (!_control_status.flags.yaw_manual || _control_status.flags.mag_aligned_in_flight);
_control_status.flags.mag_3D = common_conditions_passing
&& (_fc.mag.intended == MagFuseType::AUTO)
&& (_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::AUTO))
&& _control_status.flags.mag_aligned_in_flight;
_control_status.flags.mag_hdg = common_conditions_passing
&& ((_fc.mag.intended == MagFuseType::HEADING)
|| (_fc.mag.intended == MagFuseType::AUTO && !_control_status.flags.mag_3D));
&& ((_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::HEADING))
|| (_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::AUTO) && !_control_status.flags.mag_3D));
}
if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {

6
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
View File

@ -42,7 +42,9 @@
void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
{
if (!_flow_buffer || (_fc.of.intended != 1)) {
_fc.of.available = (_params.ekf2_of_ctrl != 0);
if (!_flow_buffer || !_fc.of.intended()) {
stopFlowFusion();
return;
}
@ -146,7 +148,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
&& !flow_sample.flow_rate.longerThan(_flow_max_rate)
&& !flow_compensated.longerThan(_flow_max_rate);
const bool continuing_conditions_passing = (_fc.of.intended == 1)
const bool continuing_conditions_passing = _fc.of.intended()
&& _control_status.flags.tilt_align
&& is_within_sensor_dist;

9
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View File

@ -42,6 +42,8 @@
void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
{
_fc.rng.available = (_params.ekf2_rng_ctrl != static_cast<int32_t>(RngCtrl::DISABLED));
static constexpr const char *HGT_SRC_NAME = "RNG";
bool rng_data_ready = false;
@ -127,8 +129,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
aid_src.innovation_rejected = false;
}
const bool continuing_conditions_passing = ((_fc.rng.intended == static_cast<int32_t>(RngCtrl::ENABLED))
|| (_fc.rng.intended == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
const bool continuing_conditions_passing = _fc.rng.intended()
&& _control_status.flags.tilt_align
&& measurement_valid;
@ -138,11 +139,11 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
&& _range_sensor.isDataHealthy();
const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
&& (_fc.rng.intended == static_cast<int32_t>(RngCtrl::CONDITIONAL))
&& (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
&& isConditionalRangeAidSuitable();
const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
&& (_fc.rng.intended == static_cast<int32_t>(RngCtrl::ENABLED));
&& (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
if (_control_status.flags.rng_hgt) {
if (!(do_conditional_range_aid || do_range_aid)) {

4
src/modules/ekf2/EKF/aid_sources/ranging_beacon/ranging_beacon_control.cpp
View File

@ -42,7 +42,9 @@
void Ekf::controlRangingBeaconFusion(const imuSample &imu_delayed)
{
if (!_ranging_beacon_buffer || (_params.ekf2_rngbc_ctrl == 0)) {
_fc.rngbcn.available = (_params.ekf2_rngbc_ctrl != 0);
if (!_ranging_beacon_buffer || !_fc.rngbcn.intended()) {
stopRangingBeaconFusion();
return;
}

13
src/modules/ekf2/EKF/common.h
View File

@ -287,10 +287,10 @@ struct systemFlagUpdate {
// Runtime fusion control. Populated by EKF2 module, read by EKF core.
static constexpr uint8_t MAX_AGP_INSTANCES = 4;
struct FusionSensor {
bool exists{false};
int32_t intended{0};
bool enabled{false}; // runtime toggleable via MAVLink
bool available{false}; // CTRL-param != disabled-value (functions as factory-setting)
bool intended() const { return enabled && available; }
};
struct FusionControl {
@ -300,9 +300,9 @@ struct FusionControl {
FusionSensor agp[MAX_AGP_INSTANCES];
FusionSensor baro;
FusionSensor rng;
FusionSensor drag;
FusionSensor mag{false, 5}; // MagFuseType::NONE
FusionSensor imu;
FusionSensor mag;
FusionSensor aspd;
FusionSensor rngbcn;
};
struct parameters {
@ -314,6 +314,7 @@ struct parameters {
float ekf2_vel_lim{100.f}; ///< velocity state limit (m/s)
// measurement source control
int32_t ekf2_sens_en{8191}; ///< sensor fusion enable bitmask (EKF2_SENS_EN)
int32_t ekf2_hgt_ref{static_cast<int32_t>(HeightSensor::BARO)};
int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};

2
src/modules/ekf2/EKF/control.cpp
View File

@ -160,7 +160,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
_zero_velocity_update.update(*this, imu_delayed);
if (_fc.imu.intended & static_cast<int32_t>(ImuCtrl::GyroBias)) {
if (_params.ekf2_imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias)) {
_zero_gyro_update.update(*this, imu_delayed);
}

4
src/modules/ekf2/EKF/ekf.h
View File

@ -137,6 +137,10 @@ public:
const Vector3f &getFlowRefBodyRate() const { return _ref_body_rate; }
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
uint8_t getAgpFusingBitmask() const { return _aux_global_position.sourceFusingBitmask(); }
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
float getHeadingInnov() const;
float getHeadingInnovVar() const;
float getHeadingInnovRatio() const;

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

@ -826,7 +826,7 @@ void Ekf::updateHorizontalDeadReckoningstatus()
inertial_dead_reckoning = false;
} else {
if (!_control_status.flags.in_air && (_fc.of.intended == 1)
if (!_control_status.flags.in_air && _fc.of.intended()
&& isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max)
) {
// currently landed, but optical flow aiding should be possible once in air
@ -853,7 +853,7 @@ void Ekf::updateHorizontalDeadReckoningstatus()
if (!_control_status.flags.in_air && _control_status.flags.fixed_wing
&& (_params.ekf2_fuse_beta == 1)
&& (_params.ekf2_arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max)
&& _fc.aspd.intended() && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max)
) {
// currently landed, but air data aiding should be possible once in air
aiding_expected_in_air = true;
@ -997,7 +997,7 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
// gyro bias inhibit
const bool do_inhibit_all_gyro_axes = !(_fc.imu.intended & static_cast<int32_t>(ImuCtrl::GyroBias));
const bool do_inhibit_all_gyro_axes = !(_params.ekf2_imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias));
for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
bool is_bias_observable = true; // TODO: gyro bias conditions
@ -1005,7 +1005,7 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
}
// accel bias inhibit
const bool do_inhibit_all_accel_axes = !(_fc.imu.intended & static_cast<int32_t>(ImuCtrl::AccelBias))
const bool do_inhibit_all_accel_axes = !(_params.ekf2_imu_ctrl & static_cast<int32_t>(ImuCtrl::AccelBias))
|| is_manoeuvre_level_high
|| _fault_status.flags.bad_acc_vertical;

2
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -519,7 +519,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
{
// down-sample the drag specific force data by accumulating and calculating the mean when
// sufficient samples have been collected
if (_fc.drag.intended > 0) {
if (_params.ekf2_drag_ctrl > 0) {
// Allocate the required buffer size if not previously done
if (_drag_buffer == nullptr) {