fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 06:18:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(ekf2): EKF2_SENS_EN param, SensEnBit enum, MAVLink fusion command

Browse Source 
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.
This commit is contained in:
Marco Hauswirth
2026-04-02 16:20:19 +02:00
committed by Marco Hauswirth
parent b9a1c429b3
commit 6306c78f79
6 changed files with 176 additions and 232 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/EstimatorFusionControl.msg
+20 -10
View File
@@ -1,13 +1,23 @@
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)
# sensor intended for fusion (enabled via EKF2_SENS_EN AND CTRL param != disabled)
bool[2] gps_intended
bool of_intended
bool ev_intended
bool[4] agp_intended
bool baro_intended
bool rng_intended
bool mag_intended
bool aspd_intended
bool rngbcn_intended
uint8 agp_active # bitmask of AGP instances actively fusing (bit i = instance i)
# whether the estimator is actively fusing data from each source
bool[2] gps_active
bool of_active
bool ev_active
bool[4] agp_active
bool baro_active
bool rng_active
bool mag_active
bool aspd_active
bool rngbcn_active
msg/versioned/VehicleCommand.msg
+11 -11
View File
@@ -110,17 +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)
uint16 VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE = 43006 # Enable/disable estimator sensor fusion. |Source (FUSION_SOURCE_*)|Sensor instance (0-based)|Enable (1) or Disable (0)|Estimator Instance (NaN: not used)|Empty|Empty|Empty|
# Sensor fusion source types for VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE
uint8 FUSION_SOURCE_GPS = 0 # GNSS (EKF2_GPS{i}_CTRL, use instance param)
uint8 FUSION_SOURCE_OF = 1 # Optical Flow (EKF2_OF_CTRL)
uint8 FUSION_SOURCE_EV = 2 # External Vision (EKF2_EV_CTRL)
uint8 FUSION_SOURCE_AGP = 3 # Auxiliary Global Position (EKF2_AGP{i}_CTRL, use instance param)
uint8 FUSION_SOURCE_BARO = 4 # Barometer (EKF2_BARO_CTRL)
uint8 FUSION_SOURCE_RNG = 5 # Range Finder (EKF2_RNG_CTRL)
uint8 FUSION_SOURCE_MAG = 6 # Magnetometer (EKF2_MAG_TYPE)
uint8 FUSION_SOURCE_ASPD = 7 # Airspeed (EKF2_ARSP_THR)
uint8 FUSION_SOURCE_RNGBCN = 8 # Ranging Beacon
# PX4 vehicle commands (beyond 16 bit MAVLink commands).
uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # Start of PX4 internal only vehicle commands (> UINT16_MAX).