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

feat(ekf2): enable fusion-ctrl toggle over mavlink cmd, CTRL param act only as reference

Browse Source
This commit is contained in:
Marco Hauswirth 2026-02-13 16:10:51 +01:00 committed by Marco Hauswirth
parent 2828162f72
commit 0dd1640a54
27 changed files with 355 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -79,6 +79,7 @@ set(msg_files
EstimatorBias.msg
EstimatorBias3d.msg
EstimatorEventFlags.msg
EstimatorFusionControl.msg
EstimatorGpsStatus.msg
EstimatorInnovations.msg
EstimatorSelectorStatus.msg

13
msg/EstimatorFusionControl.msg Normal file
View File

@ -0,0 +1,13 @@
uint64 timestamp # time since system start (microseconds)
int32 gps_intended # effective EKF2_GPS_CTRL value (0 = disabled)
int32 of_intended # effective EKF2_OF_CTRL value (0 = disabled)
int32 ev_intended # effective EKF2_EV_CTRL value (0 = disabled)
int32[4] agp_intended # effective EKF2_AGPx_CTRL values (0 = disabled)
int32 baro_intended # effective EKF2_BARO_CTRL value (0 = disabled)
int32 rng_intended # effective EKF2_RNG_CTRL value (0 = disabled)
int32 drag_intended # effective EKF2_DRAG_CTRL value (0 = disabled)
int32 mag_intended # effective EKF2_MAG_TYPE value (5 = disabled/NONE)
int32 imu_intended # effective EKF2_IMU_CTRL value (0 = disabled)
uint8 agp_active # bitmask of AGP instances actively fusing (bit i = instance i)

11
msg/versioned/VehicleCommand.msg
View File

@ -110,6 +110,17 @@ uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch.
uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # External reset of estimator global position when dead reckoning.
uint16 VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE = 43004
uint16 VEHICLE_CMD_SET_EKF_SENSOR_FUSION = 43005 # Enable/disable EKF sensor fusion. |sensor (FUSION_SOURCE_*)|instance (0-based, for multi-instance types)|action (0=disable, 1=enable)|Unused|Unused|Unused|Unused|
# Sensor fusion source types for VEHICLE_CMD_SET_EKF_SENSOR_FUSION
uint8 FUSION_SOURCE_GPS = 1 # GNSS (EKF2_GPS_CTRL)
uint8 FUSION_SOURCE_OF = 2 # Optical Flow (EKF2_OF_CTRL)
uint8 FUSION_SOURCE_EV = 3 # External Vision (EKF2_EV_CTRL)
uint8 FUSION_SOURCE_AGP = 4 # Auxiliary Global Position (EKF2_AGP{i}_CTRL, use instance param)
uint8 FUSION_SOURCE_BARO = 5 # Barometer (EKF2_BARO_CTRL)
uint8 FUSION_SOURCE_RNG = 6 # Range Finder (EKF2_RNG_CTRL)
uint8 FUSION_SOURCE_DRAG = 7 # Drag (EKF2_DRAG_CTRL)
uint8 FUSION_SOURCE_MAG = 8 # Magnetometer (EKF2_MAG_TYPE)
uint8 FUSION_SOURCE_IMU = 9 # IMU (EKF2_IMU_CTRL)
# PX4 vehicle commands (beyond 16 bit MAVLink commands).
uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # Start of PX4 internal only vehicle commands (> UINT16_MAX).

1
src/modules/commander/Commander.cpp
View File

@ -1613,6 +1613,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_REQUEST_CAMERA_INFORMATION:
case vehicle_command_s::VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE:
case vehicle_command_s::VEHICLE_CMD_DO_AUTOTUNE_ENABLE:
case vehicle_command_s::VEHICLE_CMD_SET_EKF_SENSOR_FUSION:
/* ignore commands that are handled by other parts of the system */
break;

2
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 && (_params.ekf2_drag_ctrl == 0))) {
if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_fc.drag.intended == 0))) {
_control_status.flags.wind = false;
}

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

@ -103,7 +103,7 @@ bool AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
if (_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
if (!(_params.ctrl & static_cast<int32_t>(Ctrl::kHPos))) {
if (!(ekf._fc.agp[_slot].intended & static_cast<int32_t>(Ctrl::kHPos))) {
return true;
}

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

@ -111,7 +111,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
}
// determine if we should use height aiding
const bool continuing_conditions_passing = (_params.ekf2_baro_ctrl == 1)
const bool continuing_conditions_passing = (_fc.baro.intended == 1)
&& 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 ((_params.ekf2_drag_ctrl > 0) && _drag_buffer) {
if ((_fc.drag.intended > 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_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 = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
const bool continuing_conditions_passing = (_fc.ev.intended & 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 = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
bool continuing_conditions_passing = (_fc.ev.intended & 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 = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
bool continuing_conditions_passing = (_fc.ev.intended & 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 = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
bool continuing_conditions_passing = (_fc.ev.intended & 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 = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
const bool continuing_conditions_passing = (_fc.gps.intended & 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 (!(_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
if (!(_fc.gps.intended & static_cast<int32_t>(GnssCtrl::YAW))
|| _control_status.flags.gnss_yaw_fault) {
stopGnssYawFusion();

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

@ -41,7 +41,7 @@
void Ekf::controlGpsFusion(const imuSample &imu_delayed)
{
if (!_gps_buffer || (_params.ekf2_gps_ctrl == 0)) {
if (!_gps_buffer || (_fc.gps.intended == 0)) {
stopGnssFusion();
return;
}
@ -126,7 +126,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 = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
const bool continuing_conditions_passing = (_fc.gps.intended & static_cast<int32_t>(GnssCtrl::VEL))
&& _control_status.flags.tilt_align
&& _control_status.flags.yaw_align
&& !_control_status.flags.gnss_fault
@ -182,7 +182,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 = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
const bool gnss_pos_enabled = (_fc.gps.intended & static_cast<int32_t>(GnssCtrl::HPOS));
const bool continuing_conditions_passing = gnss_pos_enabled
&& _control_status.flags.tilt_align
@ -385,8 +385,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 (((_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
|| (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)))
if (((_fc.gps.intended & static_cast<int32_t>(GnssCtrl::VEL))
|| (_fc.gps.intended & 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 = (_params.ekf2_imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
_control_status.flags.gravity_vector = (_fc.imu.intended & 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)

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

@ -54,7 +54,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
_control_status.flags.mag_aligned_in_flight = false;
}
if (_params.ekf2_mag_type == MagFuseType::NONE) {
if (_fc.mag.intended == MagFuseType::NONE) {
stopMagFusion();
return;
}
@ -150,9 +150,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 = ((_params.ekf2_mag_type == MagFuseType::INIT)
|| (_params.ekf2_mag_type == MagFuseType::AUTO)
|| (_params.ekf2_mag_type == MagFuseType::HEADING))
bool continuing_conditions_passing = ((_fc.mag.intended == MagFuseType::INIT)
|| (_fc.mag.intended == MagFuseType::AUTO)
|| (_fc.mag.intended == 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 +187,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
&& (_params.ekf2_mag_type == MagFuseType::AUTO)
&& (_fc.mag.intended == MagFuseType::AUTO)
&& _control_status.flags.mag_aligned_in_flight;
_control_status.flags.mag_hdg = common_conditions_passing
&& ((_params.ekf2_mag_type == MagFuseType::HEADING)
|| (_params.ekf2_mag_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
&& ((_fc.mag.intended == MagFuseType::HEADING)
|| (_fc.mag.intended == MagFuseType::AUTO && !_control_status.flags.mag_3D));
}
if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {

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

@ -42,7 +42,7 @@
void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
{
if (!_flow_buffer || (_params.ekf2_of_ctrl != 1)) {
if (!_flow_buffer || (_fc.of.intended != 1)) {
stopFlowFusion();
return;
}
@ -146,7 +146,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 = (_params.ekf2_of_ctrl == 1)
const bool continuing_conditions_passing = (_fc.of.intended == 1)
&& _control_status.flags.tilt_align
&& is_within_sensor_dist;

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

@ -127,8 +127,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
aid_src.innovation_rejected = false;
}
const bool continuing_conditions_passing = ((_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
|| (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
const bool continuing_conditions_passing = ((_fc.rng.intended == static_cast<int32_t>(RngCtrl::ENABLED))
|| (_fc.rng.intended == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
&& _control_status.flags.tilt_align
&& measurement_valid;
@ -138,11 +138,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)
&& (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
&& (_fc.rng.intended == static_cast<int32_t>(RngCtrl::CONDITIONAL))
&& isConditionalRangeAidSuitable();
const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
&& (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
&& (_fc.rng.intended == static_cast<int32_t>(RngCtrl::ENABLED));
if (_control_status.flags.rng_hgt) {
if (!(do_conditional_range_aid || do_range_aid)) {

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

@ -284,6 +284,27 @@ struct systemFlagUpdate {
bool in_transition{false};
};
// 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};
};
struct FusionControl {
FusionSensor gps;
FusionSensor of;
FusionSensor ev;
FusionSensor agp[MAX_AGP_INSTANCES];
FusionSensor baro;
FusionSensor rng;
FusionSensor drag;
FusionSensor mag{false, 5}; // MagFuseType::NONE
FusionSensor imu;
};
struct parameters {
int32_t ekf2_predict_us{10000}; ///< filter update interval in microseconds

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 (_params.ekf2_imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias)) {
if (_fc.imu.intended & static_cast<int32_t>(ImuCtrl::GyroBias)) {
_zero_gyro_update.update(*this, imu_delayed);
}

6
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 && (_params.ekf2_of_ctrl == 1)
if (!_control_status.flags.in_air && (_fc.of.intended == 1)
&& isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max)
) {
// currently landed, but optical flow aiding should be possible once in air
@ -997,7 +997,7 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
// gyro bias inhibit
const bool do_inhibit_all_gyro_axes = !(_params.ekf2_imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias));
const bool do_inhibit_all_gyro_axes = !(_fc.imu.intended & 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 = !(_params.ekf2_imu_ctrl & static_cast<int32_t>(ImuCtrl::AccelBias))
const bool do_inhibit_all_accel_axes = !(_fc.imu.intended & 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 (_params.ekf2_drag_ctrl > 0) {
if (_fc.drag.intended > 0) {
// Allocate the required buffer size if not previously done
if (_drag_buffer == nullptr) {

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

@ -155,6 +155,7 @@ public:
// return a address to the parameters struct
// in order to give access to the application
parameters *getParamHandle() { return &_params; }
FusionControl *getFusionControlHandle() { return &_fc; }
// set vehicle landed status data
void set_in_air_status(bool in_air)
@ -336,6 +337,7 @@ protected:
virtual bool init(uint64_t timestamp) = 0;
parameters _params{}; // filter parameters
FusionControl _fc{};
/*
OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer

232
src/modules/ekf2/EKF2.cpp
View File

@ -62,6 +62,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)),
#endif // CONFIG_EKF2_WIND
_params(_ekf.getParamHandle()),
_fc(*_ekf.getFusionControlHandle()),
_param_ekf2_predict_us(_params->ekf2_predict_us),
_param_ekf2_delay_max(_params->ekf2_delay_max),
_param_ekf2_imu_ctrl(_params->ekf2_imu_ctrl),
@ -218,6 +219,17 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_abl_tau(_params->ekf2_abl_tau),
_param_ekf2_gyr_b_lim(_params->ekf2_gyr_b_lim)
{
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
char param_name[20] {};
for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) {
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_CTRL", i);
_param_ekf2_agp_ctrl[i] = param_find(param_name);
}
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
initFusionControl();
AdvertiseTopics();
}
@ -458,6 +470,7 @@ void EKF2::Run()
// update parameters from storage
updateParams();
updateFusionIntended();
VerifyParams();
@ -607,6 +620,12 @@ void EKF2::Run()
command_ack.timestamp = hrt_absolute_time();
_vehicle_command_ack_pub.publish(command_ack);
}
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_EKF_SENSOR_FUSION) {
handleSensorFusionCommand(vehicle_command, command_ack);
command_ack.timestamp = hrt_absolute_time();
_vehicle_command_ack_pub.publish(command_ack);
}
}
}
@ -828,6 +847,7 @@ void EKF2::Run()
PublishEventFlags(now);
PublishStatus(now);
PublishStatusFlags(now);
PublishFusionControl(now);
if (_param_ekf2_log_verbose.get()) {
PublishAidSourceStatus(now);
@ -984,6 +1004,198 @@ void EKF2::VerifyParams()
}
}
void EKF2::initFusionControl()
{
_boot_param = param_find("EKF2_EN_BOOT");
int32_t boot_mask = 4095; // default: all enabled
if (_boot_param != PARAM_INVALID) {
param_get(_boot_param, &boot_mask);
}
// bit layout: 0=GPS 1=OF 2=EV 3..6=AGP0..3 7=BARO 8=RNG 9=DRAG 10=MAG 11=IMU
#if defined(CONFIG_EKF2_GNSS)
_fc.gps.exists = boot_mask & (1 << 0);
_fc.gps.intended = _fc.gps.exists ? _params->ekf2_gps_ctrl : 0;
#endif
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_fc.of.exists = boot_mask & (1 << 1);
_fc.of.intended = _fc.of.exists ? _params->ekf2_of_ctrl : 0;
#endif
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_fc.ev.exists = boot_mask & (1 << 2);
_fc.ev.intended = _fc.ev.exists ? _params->ekf2_ev_ctrl : 0;
#endif
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) {
_fc.agp[i].exists = boot_mask & (1 << (3 + i));
int32_t agp_param_val = 0;
if (_param_ekf2_agp_ctrl[i] != PARAM_INVALID) {
param_get(_param_ekf2_agp_ctrl[i], &agp_param_val);
}
_fc.agp[i].intended = _fc.agp[i].exists ? agp_param_val : 0;
}
#endif
#if defined(CONFIG_EKF2_BAROMETER)
_fc.baro.exists = boot_mask & (1 << 7);
_fc.baro.intended = _fc.baro.exists ? _params->ekf2_baro_ctrl : 0;
#endif
#if defined(CONFIG_EKF2_RANGE_FINDER)
_fc.rng.exists = boot_mask & (1 << 8);
_fc.rng.intended = _fc.rng.exists ? _params->ekf2_rng_ctrl : 0;
#endif
#if defined(CONFIG_EKF2_DRAG_FUSION)
_fc.drag.exists = boot_mask & (1 << 9);
_fc.drag.intended = _fc.drag.exists ? _params->ekf2_drag_ctrl : 0;
#endif
#if defined(CONFIG_EKF2_MAGNETOMETER)
_fc.mag.exists = boot_mask & (1 << 10);
_fc.mag.intended = _fc.mag.exists ? _params->ekf2_mag_type : 5;
#endif
_fc.imu.exists = boot_mask & (1 << 11);
_fc.imu.intended = _fc.imu.exists ? _params->ekf2_imu_ctrl : 0;
}
void EKF2::updateFusionIntended()
{
if (_fc.gps.intended != 0) {
_fc.gps.intended = _params->ekf2_gps_ctrl;
}
if (_fc.of.intended != 0) {
_fc.of.intended = _params->ekf2_of_ctrl;
}
if (_fc.ev.intended != 0) {
_fc.ev.intended = _params->ekf2_ev_ctrl;
}
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) {
if (_fc.agp[i].intended != 0) {
int32_t agp_param_val = 0;
if (_param_ekf2_agp_ctrl[i] != PARAM_INVALID) {
param_get(_param_ekf2_agp_ctrl[i], &agp_param_val);
}
_fc.agp[i].intended = agp_param_val;
}
}
#endif
if (_fc.baro.intended != 0) {
_fc.baro.intended = _params->ekf2_baro_ctrl;
}
if (_fc.rng.intended != 0) {
_fc.rng.intended = _params->ekf2_rng_ctrl;
}
if (_fc.drag.intended != 0) {
_fc.drag.intended = _params->ekf2_drag_ctrl;
}
if (_fc.mag.intended != 5) {
_fc.mag.intended = _params->ekf2_mag_type;
}
if (_fc.imu.intended != 0) {
_fc.imu.intended = _params->ekf2_imu_ctrl;
}
}
void EKF2::handleSensorFusionCommand(const vehicle_command_s &cmd, vehicle_command_ack_s &ack)
{
const uint8_t sensor = static_cast<uint8_t>(cmd.param1);
const uint8_t instance = PX4_ISFINITE(cmd.param2) ? static_cast<uint8_t>(cmd.param2) : 0;
const int32_t action = static_cast<int32_t>(cmd.param3); // 0 = disable, 1 = enable
const bool enable = (action == 1);
bool supported = false;
switch (sensor) {
case vehicle_command_s::FUSION_SOURCE_GPS:
_fc.gps.exists = enable;
_fc.gps.intended = enable ? _param_ekf2_gps_ctrl.get() : 0;
updateBootParam(0, enable);
supported = true;
break;
case vehicle_command_s::FUSION_SOURCE_OF:
_fc.of.exists = enable;
_fc.of.intended = enable ? _param_ekf2_of_ctrl.get() : 0;
updateBootParam(1, enable);
supported = true;
break;
case vehicle_command_s::FUSION_SOURCE_EV:
_fc.ev.exists = enable;
_fc.ev.intended = enable ? _param_ekf2_ev_ctrl.get() : 0;
updateBootParam(2, enable);
supported = true;
break;
case vehicle_command_s::FUSION_SOURCE_AGP:
if (instance < MAX_AGP_INSTANCES) {
_fc.agp[instance].exists = enable;
int32_t agp_param_val = 0;
if (_param_ekf2_agp_ctrl[instance] != PARAM_INVALID) {
param_get(_param_ekf2_agp_ctrl[instance], &agp_param_val);
}
_fc.agp[instance].intended = enable ? agp_param_val : 0;
updateBootParam(3 + instance, enable);
supported = true;
}
break;
case vehicle_command_s::FUSION_SOURCE_BARO:
_fc.baro.exists = enable;
_fc.baro.intended = enable ? _param_ekf2_baro_ctrl.get() : 0;
updateBootParam(7, enable);
supported = true;
break;
case vehicle_command_s::FUSION_SOURCE_RNG:
_fc.rng.exists = enable;
_fc.rng.intended = enable ? _param_ekf2_rng_ctrl.get() : 0;
updateBootParam(8, enable);
supported = true;
break;
case vehicle_command_s::FUSION_SOURCE_DRAG:
_fc.drag.exists = enable;
_fc.drag.intended = enable ? _param_ekf2_drag_ctrl.get() : 0;
updateBootParam(9, enable);
supported = true;
break;
case vehicle_command_s::FUSION_SOURCE_MAG:
_fc.mag.exists = enable;
_fc.mag.intended = enable ? _param_ekf2_mag_type.get() : 5;
updateBootParam(10, enable);
supported = true;
break;
case vehicle_command_s::FUSION_SOURCE_IMU:
_fc.imu.exists = enable;
_fc.imu.intended = enable ? _param_ekf2_imu_ctrl.get() : 0;
updateBootParam(11, enable);
supported = true;
break;
}
ack.result = supported ? vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
}
void EKF2::updateBootParam(uint8_t bit, bool enable)
{
if (_boot_param == PARAM_INVALID) {
return;
}
int32_t boot_mask = 0;
param_get(_boot_param, &boot_mask);
if (enable) {
boot_mask |= (1 << bit);
} else {
boot_mask &= ~(1 << bit);
}
param_set_no_notification(_boot_param, &boot_mask);
}
void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
{
#if defined(CONFIG_EKF2_AIRSPEED)
@ -1903,6 +2115,26 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
_estimator_status_pub.publish(status);
}
void EKF2::PublishFusionControl(const hrt_abstime &timestamp)
{
estimator_fusion_control_s msg{};
msg.gps_intended = _fc.gps.intended;
msg.of_intended = _fc.of.intended;
msg.ev_intended = _fc.ev.intended;
for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) {
msg.agp_intended[i] = _fc.agp[i].intended;
}
msg.baro_intended = _fc.baro.intended;
msg.rng_intended = _fc.rng.intended;
msg.drag_intended = _fc.drag.intended;
msg.mag_intended = _fc.mag.intended;
msg.imu_intended = _fc.imu.intended;
msg.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_fc_pub.publish(msg);
}
void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
{
// publish at ~ 1 Hz (or immediately if filter control status or fault status changes)

15
src/modules/ekf2/EKF2.hpp
View File

@ -73,6 +73,7 @@
#include <uORB/topics/estimator_states.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/estimator_status_flags.h>
#include <uORB/topics/estimator_fusion_control.h>
#include <uORB/topics/launch_detection_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>
@ -198,6 +199,7 @@ private:
void PublishStates(const hrt_abstime &timestamp);
void PublishStatus(const hrt_abstime &timestamp);
void PublishStatusFlags(const hrt_abstime &timestamp);
void PublishFusionControl(const hrt_abstime &timestamp);
#if defined(CONFIG_EKF2_WIND)
void PublishWindEstimate(const hrt_abstime &timestamp);
#endif // CONFIG_EKF2_WIND
@ -405,6 +407,17 @@ private:
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
param_t _param_ekf2_agp_ctrl[MAX_AGP_INSTANCES] {PARAM_INVALID, PARAM_INVALID, PARAM_INVALID, PARAM_INVALID};
#endif
param_t _boot_param{PARAM_INVALID};
void initFusionControl();
void updateFusionIntended();
void handleSensorFusionCommand(const vehicle_command_s &cmd, vehicle_command_ack_s &ack);
void updateBootParam(uint8_t bit, bool enable);
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
@ -443,6 +456,7 @@ private:
uORB::PublicationMulti<estimator_sensor_bias_s> _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
uORB::PublicationMulti<estimator_status_flags_s> _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
uORB::PublicationMulti<estimator_fusion_control_s> _estimator_fc_pub{ORB_ID(estimator_fusion_control)};
uORB::PublicationMulti<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)};
@ -496,6 +510,7 @@ private:
Ekf _ekf;
parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance)
FusionControl &_fc;
DEFINE_PARAMETERS(
(ParamBool<px4::params::EKF2_LOG_VERBOSE>) _param_ekf2_log_verbose,

25
src/modules/ekf2/module.yaml
View File

@ -192,3 +192,28 @@ parameters:
long: When enabled, constant position fusion is enabled when the vehicle is landeded if position has been initialized but has currently no vel/pos aiding.
type: boolean
default: 0
EKF2_EN_BOOT:
description:
short: Sensor fusion enable at boot
long: 'Bitmask to control which sensor fusion sources are active at boot.
Sources whose bit is cleared will be disabled at startup.
They can be toggled at runtime via MAVLink command
(VEHICLE_CMD_SET_EKF_SENSOR_FUSION), which also updates this param.'
type: bitmask
bit:
0: GNSS
1: Optical flow
2: External vision
3: Aux global position 0
4: Aux global position 1
5: Aux global position 2
6: Aux global position 3
7: Barometer
8: Range finder
9: Drag
10: Magnetometer
11: IMU
default: 4095
min: 0
max: 4095