feat(ekf2): Fusion-Control of sensors over MAVLink by haumarco · Pull Request #26739 · PX4/PX4-Autopilot

haumarco · 2026-03-13T16:32:46Z

Solved Problem

There is no way to enable or disable individual EKF2 sensor fusion sources at runtime without changing parameters. This leads to the issue that the GCS needs to know or cache the default values of a sensor with different options.

Solution

Add a MAVLink command and status message so a GCS can enable/disable sensor fusion sources with a simple on/off command, without needing to know the underlying parameter values.

MAV_CMD_SET_ESTIMATOR_SENSOR_FUSION (43005): Enable or disable a sensor source (GPS, OF, EV, AGP, BARO, RNG, DRAG, MAG, ASPD) at runtime. The CTRL param values are preserved and applied when the source is re-enabled.
ESTIMATOR_SENSOR_FUSION_STATUS: MAVLink message reporting which sources are intended (enabled) and which are actively fusing, streamed at 1 Hz.
EKF2_SENS_EN: New bitmask parameter tracking which sources are enabled. Updated automatically by the MAVLink command, persists across reboots.

Internally, a FusionControl struct sits between the parameters and the EKF core. The EKF reads _fc.<sensor>.intended instead of the CTRL params directly. When a source is disabled, its intended value is forced to the disabled state while the actual CTRL param remains unchanged.

The message format is prepared for dual-GPS support (gps_intended[2], gps_active bitmask) even though only one GPS instance is wired internally for now.

Changelog Entry

For release notes:

Feature: Runtime EKF2 sensor fusion control via MAVLink
  - New MAVLink command MAV_CMD_SET_ESTIMATOR_SENSOR_FUSION (43005) to enable/disable
    individual sensor fusion sources (GPS, OF, EV, AGP, BARO, RNG, DRAG, MAG, ASPD)
  - New MAVLink message ESTIMATOR_SENSOR_FUSION_STATUS reporting intended and active fusion state
  - New parameter EKF2_SENS_EN (bitmask) controlling which fusion sources are enabled
  - EKF2_ARSP_THR parameter type changed from float to int32
Documentation: Need to clarify pages for EKF2_SENS_EN parameter and MAVLink command usage

Test coverage

Not yet covered by automated tests
Only tested locally in SITL with testing Python scripts.

Context

New MAVLink definitions (not yet merged):

MAV_CMD_SET_ESTIMATOR_SENSOR_FUSION (43005)
ESTIMATOR_SENSOR_FUSION_SOURCE enum (values 1–9) + Ranging-Beacon (todo)
ESTIMATOR_SENSOR_FUSION_STATUS message (id 514)

TODO:

Add support for ranging beacons as soon as sensor is approved in new MAVLink message (id 513)

github-actions · 2026-03-13T16:32:59Z

💡 Commit messages could be improved

Not blocking, but these commit messages could use some cleanup.

Commit	Message	Suggestion
`f20d119e1f`	submodule(mavlink): update to latest mavlink/main to include ESTIMATOR_SENSOR_FUSION_STATUS, MAV_CMD_ESTIMATOR_SENSOR_ENABLE	Missing conventional commit format (e.g. "feat(ekf2): add something")

See the commit message convention for details.

This comment will be automatically removed once the issues are resolved.

mrpollo · 2026-03-13T17:56:15Z

Rebased to clean up an error picked up from main

github-actions · 2026-03-13T18:00:24Z

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 1896 byte (0.09 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.1% +1.85Ki  +0.1% +1.85Ki    .text
  [NEW]    +424  [NEW]    +424    EKF2::PublishFusionControl()
  [NEW]    +336  [NEW]    +336    EKF2::handleSensorFusionCommand()
  +0.1%    +188  +0.1%    +188    g_cromfs_image
  [NEW]    +144  [NEW]    +144    EKF2::initFusionControl()
  +0.6%    +124  +0.6%    +124    uORB::compressed_fields
  [NEW]    +120  [NEW]    +120    EKF2::syncSensEnParam()
 -99.8%    +112 -99.8%    +112    [33 Others]
  +3.1%     +84  +3.1%     +84    Ekf::controlRangeHaglFusion()
  +1.1%     +64  +1.1%     +64    EKF2::EKF2()
  +1.6%     +56  +1.6%     +56    EKF2::Run()
  +4.1%     +40  +4.1%     +40    AgpSource::update()
  +2.7%     +40  +2.7%     +40    Ekf::controlBaroHeightFusion()
  +0.0%     +40  +0.0%     +40    [section .text]
  +0.9%     +32  +0.9%     +32    Ekf::Ekf()
  +2.8%     +28  +2.8%     +28    Ekf::controlAirDataFusion()
  +1.6%     +28  +1.6%     +28    Ekf::controlEvHeightFusion()
  +1.4%     +28  +1.4%     +28    Ekf::controlOpticalFlowFusion()
  +1.6%     +24  +1.6%     +24    EKF2::~EKF2()
  +1.2%     +20  +1.2%     +20    Ekf::controlExternalVisionFusion()
  -2.5%     -16  -2.5%     -16    EKF2::UpdateGpsSample()
  -0.7%     -20  -0.7%     -20    EKF2::PublishAidSourceStatus()
+0.0%    +927  [ = ]       0    .debug_abbrev
+0.0%     +80  [ = ]       0    .debug_aranges
+0.0%    +208  [ = ]       0    .debug_frame
+0.1% +29.1Ki  [ = ]       0    .debug_info
+0.1% +5.13Ki  [ = ]       0    .debug_line
 -33.3%      -1  [ = ]       0    [Unmapped]
  +0.1% +5.14Ki  [ = ]       0    [section .debug_line]
+0.1% +3.83Ki  [ = ]       0    .debug_loclists
+0.1%    +481  [ = ]       0    .debug_rnglists
  [DEL]      -2  [ = ]       0    [Unmapped]
  +0.1%    +483  [ = ]       0    [section .debug_rnglists]
+0.1% +3.28Ki  [ = ]       0    .debug_str
-0.8%      -2  [ = ]       0    .shstrtab
+0.0%    +242  [ = ]       0    .strtab
  [NEW]     +35  [ = ]       0    EKF2::PublishFusionControl()
  [NEW]     +82  [ = ]       0    EKF2::handleSensorFusionCommand()
  [NEW]     +37  [ = ]       0    EKF2::initFusionControl()
  [NEW]     +28  [ = ]       0    EKF2::syncSensEnParam()
  +0.1%     +29  [ = ]       0    [section .strtab]
   +36%     +16  [ = ]       0    ___ZL19param_get_cplusplustPf.isra.0_veneer
 -12.8%     -16  [ = ]       0    ___ZN39ControlAllocationSequentialDesaturation23computeDesaturationGainERKN6matrix6VectorIfLj16EEES4__veneer
  [NEW]     +31  [ = ]       0    __orb_estimator_fusion_control
+0.0%    +272  [ = ]       0    .symtab
 -50.0%     -16  [ = ]       0    EKF2::PublishAidSourceStatus()
   +33%     +16  [ = ]       0    EKF2::PublishEventFlags()
  [NEW]     +48  [ = ]       0    EKF2::PublishFusionControl()
 -50.0%     -16  [ = ]       0    EKF2::PublishGpsStatus()
 -18.2%     -32  [ = ]       0    EKF2::Run()
  [NEW]    +160  [ = ]       0    EKF2::handleSensorFusionCommand()
  [NEW]     +32  [ = ]       0    EKF2::initFusionControl()
  [NEW]     +32  [ = ]       0    EKF2::syncSensEnParam()
   +17%     +16  [ = ]       0    Ekf::controlRangeHaglFusion()
  -0.3%     -32  [ = ]       0    [section .symtab]
   +13%     +32  [ = ]       0    ___ZL19param_get_cplusplustPf.isra.0_veneer
 -40.0%     -32  [ = ]       0    ___ZN39ControlAllocationSequentialDesaturation23computeDesaturationGainERKN6matrix6VectorIfLj16EEES4__veneer
   +50%     +16  [ = ]       0    ___ZN4ListIP13MavlinkStreamE8IteratorppEv.isra.0_veneer
  [NEW]     +48  [ = ]       0    __orb_estimator_fusion_control
 -33.3%     -16  [ = ]       0    __stm32_dmastop_veneer
 -25.0%     -16  [ = ]       0    __stm32_ep0out_ctrlsetup.isra.0_veneer
   +33%     +16  [ = ]       0    __stm32_i2c_modifyreg32_veneer
   +50%     +16  [ = ]       0    estimator::State::quat_nominal
 -20.0%     -16  [ = ]       0    matrix::Matrix<>::operator+=()
   +33%     +16  [ = ]       0    matrix::Matrix<>::operator/()
 +23% +2.15Ki  [ = ]       0    [Unmapped]
+0.1% +47.5Ki  +0.1% +1.85Ki    TOTAL

px4_fmu-v6x [Total VM Diff: 1928 byte (0.1 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.1% +1.88Ki  +0.1% +1.88Ki    .text
  [NEW]    +424  [NEW]    +424    EKF2::PublishFusionControl()
  [NEW]    +336  [NEW]    +336    EKF2::handleSensorFusionCommand()
  +0.1%    +180  +0.1%    +180    g_cromfs_image
  [NEW]    +144  [NEW]    +144    EKF2::initFusionControl()
  +0.6%    +124  +0.6%    +124    uORB::compressed_fields
  [NEW]    +120  [NEW]    +120    EKF2::syncSensEnParam()
 -99.8%    +104 -99.8%    +104    [44 Others]
  +3.1%     +84  +3.1%     +84    Ekf::controlRangeHaglFusion()
  +1.1%     +64  +1.1%     +64    EKF2::EKF2()
  +1.6%     +56  +1.6%     +56    EKF2::Run()
  +0.0%     +52  +0.0%     +52    [section .text]
  +4.1%     +40  +4.1%     +40    AgpSource::update()
  +2.7%     +40  +2.7%     +40    Ekf::controlBaroHeightFusion()
  +0.9%     +32  +0.9%     +32    Ekf::Ekf()
  +2.8%     +28  +2.8%     +28    Ekf::controlAirDataFusion()
  +1.6%     +28  +1.6%     +28    Ekf::controlEvHeightFusion()
  +1.4%     +28  +1.4%     +28    Ekf::controlOpticalFlowFusion()
  +1.6%     +24  +1.6%     +24    EKF2::~EKF2()
  +1.2%     +20  +1.2%     +20    Ekf::controlExternalVisionFusion()
  +4.7%     +20  +4.7%     +20    param_reset_specific
  -0.7%     -20  -0.7%     -20    EKF2::PublishAidSourceStatus()
+0.0%    +935  [ = ]       0    .debug_abbrev
+0.0%     +80  [ = ]       0    .debug_aranges
+0.0%    +228  [ = ]       0    .debug_frame
+0.1% +28.7Ki  [ = ]       0    .debug_info
+0.1% +5.20Ki  [ = ]       0    .debug_line
  +200%      +2  [ = ]       0    [Unmapped]
  +0.1% +5.20Ki  [ = ]       0    [section .debug_line]
+0.1% +4.00Ki  [ = ]       0    .debug_loclists
+0.1%    +490  [ = ]       0    .debug_rnglists
 -66.7%      -2  [ = ]       0    [Unmapped]
  +0.1%    +492  [ = ]       0    [section .debug_rnglists]
+0.1% +3.29Ki  [ = ]       0    .debug_str
+0.4%      +1  [ = ]       0    .shstrtab
+0.0%    +243  [ = ]       0    .strtab
  [NEW]     +35  [ = ]       0    EKF2::PublishFusionControl()
  [NEW]     +82  [ = ]       0    EKF2::handleSensorFusionCommand()
  [NEW]     +37  [ = ]       0    EKF2::initFusionControl()
  [NEW]     +28  [ = ]       0    EKF2::syncSensEnParam()
  +0.1%     +29  [ = ]       0    [section .strtab]
  [NEW]     +31  [ = ]       0    __orb_estimator_fusion_control
  +0.0%      +1  [ = ]       0    do_not_explicitly_use_this_namespace::Param<>::update()
+0.0%    +272  [ = ]       0    .symtab
 -50.0%     -16  [ = ]       0    ConstLayer::containedAsBitset()
 -50.0%     -16  [ = ]       0    EKF2::PublishAidSourceStatus()
   +33%     +16  [ = ]       0    EKF2::PublishEventFlags()
  [NEW]     +48  [ = ]       0    EKF2::PublishFusionControl()
 -50.0%     -16  [ = ]       0    EKF2::PublishGpsStatus()
 -18.2%     -32  [ = ]       0    EKF2::Run()
  [NEW]    +160  [ = ]       0    EKF2::handleSensorFusionCommand()
  [NEW]     +32  [ = ]       0    EKF2::initFusionControl()
  [NEW]     +32  [ = ]       0    EKF2::syncSensEnParam()
   +17%     +16  [ = ]       0    Ekf::controlRangeHaglFusion()
  -0.6%     -64  [ = ]       0    [section .symtab]
  [NEW]     +48  [ = ]       0    __orb_estimator_fusion_control
   +50%     +16  [ = ]       0    estimator::State::quat_nominal
  +100%     +32  [ = ]       0    fmodf
 -20.0%     -16  [ = ]       0    matrix::Matrix<>::operator+=()
   +33%     +16  [ = ]       0    matrix::Matrix<>::operator/()
   +33%     +16  [ = ]       0    param_import_internal()
 +44% +2.12Ki  [ = ]       0    [Unmapped]
+0.1% +47.4Ki  +0.1% +1.88Ki    TOTAL

Updated: 2026-04-02T14:06:00

sfuhrer

I would consider removing all the _CTRL parameters that are simple bools. They are now redundant with the new bitmask you introduce here. You'd though need to add parameter translations from the _CTRL to the bitmask.

src/modules/ekf2/params_airspeed.yaml

msg/versioned/VehicleCommand.msg

src/modules/ekf2/EKF2.cpp

bresch

Why not simply: if enabled, then use _param_ekf2_xxx_ctrl, otherwise stop the fusion?
For example, if baro is disabled by the manufacturer, it should not be possible to enable it through that mavlink command.
And why storing the fusion control bitmask in a parameter? I would argue that it should start as configured by the _ctrl parameter and then it could be disabled and re-enabled (if it was configured in _ctrl) using mavlink

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

src/modules/ekf2/EKF2.cpp

src/modules/ekf2/EKF2.hpp

src/modules/ekf2/EKF2.cpp

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

…t only as reference

sfuhrer

@haumarco pleas address/close the remaining comments, then this is good to go in!

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.

Add EKF2_SENS_EN bitmask parameter (replaces EKF2_EN_BOOT) with per-sensor enable bits. initFusionControl reads SENS_EN while disarmed. handleSensorFusionCommand sets FusionSensor.enabled via VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE. syncSensEnParam writes back to param on disarm. Update EstimatorFusionControl.msg to bool intended/active fields. Update VehicleCommand.msg FUSION_SOURCE enum.

Add MAVLink stream that maps EstimatorFusionControl uORB message to ESTIMATOR_SENSOR_FUSION_STATUS, exposing per-sensor intended/active bitmasks to the GCS.

EkfWrapper now holds a FusionControl pointer and enables all sensors by default. Sensor-specific enable methods also set fc.enabled = true.

…R_SENSOR_FUSION_STATUS, MAV_CMD_ESTIMATOR_SENSOR_ENABLE

src/modules/ekf2/EKF2.cpp

src/modules/ekf2/module.yaml

haumarco mentioned this pull request Mar 13, 2026

development: Add estimator sensor fusion control command, enum, and status message mavlink/mavlink#2439

Merged

mrpollo force-pushed the ekf2_mavlink_fusion_ctrl branch from 3d7df89 to b01d6c8 Compare March 13, 2026 17:52

haumarco requested a review from sfuhrer March 17, 2026 09:08

haumarco force-pushed the ekf2_mavlink_fusion_ctrl branch from b01d6c8 to 17b0435 Compare March 17, 2026 09:14

haumarco marked this pull request as ready for review March 17, 2026 09:18

haumarco force-pushed the ekf2_mavlink_fusion_ctrl branch from 17b0435 to 8d53692 Compare March 17, 2026 13:31

sfuhrer reviewed Mar 18, 2026

View reviewed changes

src/modules/ekf2/params_airspeed.yaml Outdated Show resolved Hide resolved

msg/versioned/VehicleCommand.msg Outdated Show resolved Hide resolved

src/modules/ekf2/EKF2.cpp Outdated Show resolved Hide resolved

src/modules/ekf2/EKF2.cpp Outdated Show resolved Hide resolved

haumarco force-pushed the ekf2_mavlink_fusion_ctrl branch 2 times, most recently from ab41d53 to 6686413 Compare March 19, 2026 12:33

bresch reviewed Mar 19, 2026

View reviewed changes

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp Outdated Show resolved Hide resolved

src/modules/ekf2/EKF2.cpp Outdated Show resolved Hide resolved

src/modules/ekf2/EKF2.hpp Outdated Show resolved Hide resolved

src/modules/ekf2/EKF2.hpp Outdated Show resolved Hide resolved

haumarco force-pushed the ekf2_mavlink_fusion_ctrl branch 2 times, most recently from ac3810a to 8ad8d34 Compare March 20, 2026 15:16

bresch requested changes Mar 20, 2026

View reviewed changes

src/modules/ekf2/EKF2.cpp Outdated Show resolved Hide resolved

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp Outdated Show resolved Hide resolved

feat(ekf2): enable fusion-ctrl toggle over mavlink cmd, CTRL param ac…

1e05b4e

…t only as reference

haumarco force-pushed the ekf2_mavlink_fusion_ctrl branch from abd0345 to 8d0fa65 Compare April 1, 2026 15:27

sfuhrer previously approved these changes Apr 1, 2026

View reviewed changes

haumarco dismissed sfuhrer’s stale review via d8ca7e7 April 2, 2026 10:46

haumarco enabled auto-merge (rebase) April 2, 2026 12:28

sfuhrer previously approved these changes Apr 2, 2026

View reviewed changes

haumarco disabled auto-merge April 2, 2026 13:12

haumarco added 6 commits April 2, 2026 15:27

feat(mavlink): ESTIMATOR_SENSOR_FUSION_STATUS stream

677fc76

Add MAVLink stream that maps EstimatorFusionControl uORB message to ESTIMATOR_SENSOR_FUSION_STATUS, exposing per-sensor intended/active bitmasks to the GCS.

test(ekf2): sync EkfWrapper with FusionControl enabled flags

efe76f1

EkfWrapper now holds a FusionControl pointer and enables all sensors by default. Sensor-specific enable methods also set fc.enabled = true.

submodule(mavlink): update to latest mavlink/main to include ESTIMATO…

f20d119

…R_SENSOR_FUSION_STATUS, MAV_CMD_ESTIMATOR_SENSOR_ENABLE

chore(boards): remove optical flow from holybro-kakutef7 build

bd2990e

haumarco dismissed sfuhrer’s stale review via bd2990e April 2, 2026 13:28

haumarco force-pushed the ekf2_mavlink_fusion_ctrl branch from d8ca7e7 to bd2990e Compare April 2, 2026 13:28

bresch self-requested a review April 2, 2026 13:41

bresch previously approved these changes Apr 2, 2026

View reviewed changes

src/modules/ekf2/EKF2.cpp Outdated Show resolved Hide resolved

src/modules/ekf2/module.yaml Outdated Show resolved Hide resolved

style(ekf2): minor style fixes

c536d52

haumarco dismissed bresch’s stale review via c536d52 April 2, 2026 13:57

bresch approved these changes Apr 2, 2026

View reviewed changes

haumarco merged commit c8a1a38 into main Apr 2, 2026
78 checks passed

haumarco deleted the ekf2_mavlink_fusion_ctrl branch April 2, 2026 14:20

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(ekf2): Fusion-Control of sensors over MAVLink#26739

feat(ekf2): Fusion-Control of sensors over MAVLink#26739
haumarco merged 8 commits intomainfrom
ekf2_mavlink_fusion_ctrl

haumarco commented Mar 13, 2026

Uh oh!

github-actions bot commented Mar 13, 2026 •

edited

Loading

Uh oh!

mrpollo commented Mar 13, 2026

Uh oh!

github-actions bot commented Mar 13, 2026 •

edited

Loading

Uh oh!

sfuhrer left a comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

bresch left a comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

sfuhrer left a comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

Conversation

haumarco commented Mar 13, 2026

Solved Problem

Solution

Changelog Entry

Test coverage

Context

TODO:

Uh oh!

github-actions bot commented Mar 13, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

💡 Commit messages could be improved

Uh oh!

mrpollo commented Mar 13, 2026

Uh oh!

github-actions bot commented Mar 13, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

🔎 FLASH Analysis

Uh oh!

sfuhrer left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

bresch left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

sfuhrer left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

github-actions bot commented Mar 13, 2026 •

edited

Loading

github-actions bot commented Mar 13, 2026 •

edited

Loading