feat(ekf2): add RangingBeacon to new mavlink-CTRL logic

Author: haumarco

msg/EstimatorFusionControl.msg

@@ -10,6 +10,7 @@ uint8 rng_intended
 uint8 drag_intended
 uint8 mag_intended
 uint8 aspd_intended
+uint8 rngbcn_intended
 
 # whether the estimator is actively fusing data from each source
 uint8 gps_active
@@ -21,3 +22,4 @@ bool rng_active
 bool drag_active
 bool mag_active
 bool aspd_active
+bool rngbcn_active

msg/versioned/VehicleCommand.msg

@@ -119,7 +119,8 @@ 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_ASPD = 9  # Airspeed (EKF2_ARSP_THR)
+uint8 FUSION_SOURCE_ASPD = 9    # Airspeed (EKF2_ARSP_THR)
+uint8 FUSION_SOURCE_RNGBCN = 10 # Ranging Beacon (EKF2_RNGBC_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).

src/modules/ekf2/EKF/common.h

@@ -289,6 +289,7 @@ struct FusionControl {
 	FusionSensor drag;
 	FusionSensor mag{false, 5};  // MagFuseType::NONE
 	FusionSensor aspd;
+	FusionSensor rngbcn;
 };
 
 struct parameters {

src/modules/ekf2/EKF2.cpp

@@ -978,7 +978,7 @@ void EKF2::VerifyParams()
 void EKF2::initFusionControl()
 {
 	_sens_en_param = param_find("EKF2_SENS_EN");
-	int32_t sens_en = 4095; // default: all enabled
+	int32_t sens_en = 16383; // default: all enabled
 
 	if (_sens_en_param != PARAM_INVALID) {
 		param_get(_sens_en_param, &sens_en);
@@ -1001,7 +1001,7 @@ void EKF2::initFusionControl()
 		}
 	};
 
-	// bit layout: 0..1=GPS0..1 2=OF 3=EV 4..7=AGP0..3 8=BARO 9=RNG 10=DRAG 11=MAG 12=ASPD
+	// bit layout: 0..1=GPS0..1 2=OF 3=EV 4..7=AGP0..3 8=BARO 9=RNG 10=DRAG 11=MAG 12=ASPD 13=RNGBCN
 #if defined(CONFIG_EKF2_GNSS)
 	add(_fc.gps,   0, vehicle_command_s::FUSION_SOURCE_GPS, -1, _param_ekf2_gps_ctrl.handle());
 	// TODO: gps[1] reserved — no param yet, add second GPS instance here when multi-GPS CTRL is implemented
@@ -1041,6 +1041,9 @@ void EKF2::initFusionControl()
 #if defined(CONFIG_EKF2_AIRSPEED)
 	add(_fc.aspd, 12, vehicle_command_s::FUSION_SOURCE_ASPD, -1, _param_ekf2_arsp_thr.handle());
 #endif
+#if defined(CONFIG_EKF2_RANGING_BEACON)
+	add(_fc.rngbcn, 13, vehicle_command_s::FUSION_SOURCE_RNGBCN, -1, _param_ekf2_rngbc_ctrl.handle());
+#endif
 }
 
 void EKF2::updateFusionIntended()
@@ -2027,11 +2030,12 @@ void EKF2::PublishFusionControl(const hrt_abstime &timestamp)
 		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.aspd_intended = _fc.aspd.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.aspd_intended   = _fc.aspd.intended;
+	msg.rngbcn_intended = _fc.rngbcn.intended;
 
 	const auto &cs = _ekf.control_status_flags();
 	msg.gps_active  = (cs.gnss_pos || cs.gps_hgt || cs.gnss_vel || cs.gnss_yaw) ? 1u : 0u; // TODO: per-instance active tracking
@@ -2041,7 +2045,8 @@ void EKF2::PublishFusionControl(const hrt_abstime &timestamp)
 	msg.rng_active  = cs.rng_hgt;
 	msg.drag_active = (_fc.drag.intended > 0) && cs.wind && cs.in_air && !cs.fake_pos;
 	msg.mag_active  = cs.mag;
-	msg.aspd_active = cs.fuse_aspd;
+	msg.aspd_active   = cs.fuse_aspd;
+	// msg.rngbcn_active = cs.rngbcn_fusion; // waiting for RangeBeacon PR
 
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
 	msg.agp_active  = _ekf.getAgpFusingBitmask();

src/modules/ekf2/EKF2.hpp

@@ -405,7 +405,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 		uint8_t disabled_val;
 	};
 
-	static constexpr uint8_t MAX_SENSOR_TABLE = 9 + (MAX_AGP_INSTANCES - 1); // 12
+	static constexpr uint8_t MAX_SENSOR_TABLE = 10 + (MAX_AGP_INSTANCES - 1); // 13
 	FusionEntry _sensor_table[MAX_SENSOR_TABLE] {};
 	uint8_t _num_sensor_table{0};
 

src/modules/ekf2/module.yaml

@@ -216,6 +216,7 @@ parameters:
         10: Drag
         11: Magnetometer
         12: Airspeed
-      default: 8191
+        13: Ranging beacon
+      default: 16383
       min: 0
-      max: 8191
+      max: 16383

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

@@ -23,6 +23,8 @@ EkfWrapper::EkfWrapper(std::shared_ptr<Ekf> ekf):
 	_fc->mag.intended = static_cast<uint8_t>(_ekf_params->ekf2_mag_type);
 	_fc->aspd.enabled = true;
 	_fc->aspd.intended = static_cast<uint8_t>(_ekf_params->ekf2_arsp_thr);
+	_fc->rngbcn.enabled = true;
+	_fc->rngbcn.intended = 0; // wait for RangeBeacon PR
 
 	for (int i = 0; i < MAX_AGP_INSTANCES; i++) {
 		_fc->agp[i].enabled = true;

src/modules/mavlink/streams/ESTIMATOR_SENSOR_FUSION_STATUS.hpp

@@ -40,7 +40,7 @@
 /**
  * Array index = ESTIMATOR_SENSOR_FUSION_SOURCE - 1:
  *   [0] GPS    [1] OF     [2] EV    [3] AGP
- *   [4] BARO   [5] RNG    [6] DRAG  [7] MAG   [8] ASPD
+ *   [4] BARO   [5] RNG    [6] DRAG  [7] MAG   [8] ASPD  [9] RNGBCN
  *
  * Each element is a per-instance bitmask (bit 0 = instance 0, etc.).
  */
@@ -71,7 +71,8 @@ class MavlinkStreamEstimatorSensorFusionStatus : public MavlinkStream
 	static constexpr uint8_t IDX_RNG  = 5;
 	static constexpr uint8_t IDX_DRAG = 6;
 	static constexpr uint8_t IDX_MAG  = 7;
-	static constexpr uint8_t IDX_ASPD = 8;
+	static constexpr uint8_t IDX_ASPD   = 8;
+	static constexpr uint8_t IDX_RNGBCN = 9;
 
 	explicit MavlinkStreamEstimatorSensorFusionStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {}
 
@@ -84,7 +85,7 @@ class MavlinkStreamEstimatorSensorFusionStatus : public MavlinkStream
 		if (_estimator_fusion_control_sub.update(&fc)) {
 			mavlink_estimator_sensor_fusion_status_t msg{};
 
-			for (int i = 0; i < 9; i++) { msg.test_ratio[i] = NAN; }
+			for (int i = 0; i < 10; i++) { msg.test_ratio[i] = NAN; }
 
 			// --- intended: effective CTRL values with runtime overrides ---
 			for (int i = 0; i < 2; i++) {
@@ -103,7 +104,9 @@ class MavlinkStreamEstimatorSensorFusionStatus : public MavlinkStream
 
 			if (fc.mag_intended  != 0) { msg.intended[IDX_MAG]  = 1; }
 
-			if (fc.aspd_intended != 0) { msg.intended[IDX_ASPD] = 1; }
+			if (fc.aspd_intended   != 0) { msg.intended[IDX_ASPD]   = 1; }
+
+			if (fc.rngbcn_intended != 0) { msg.intended[IDX_RNGBCN] = 1; }
 
 			for (int i = 0; i < 4; i++) {
 				if (fc.agp_intended[i] != 0) { msg.intended[IDX_AGP] |= (1u << i); }
@@ -118,7 +121,8 @@ class MavlinkStreamEstimatorSensorFusionStatus : public MavlinkStream
 			msg.active[IDX_RNG]  = fc.rng_active;
 			msg.active[IDX_DRAG] = fc.drag_active;
 			msg.active[IDX_MAG]  = fc.mag_active;
-			msg.active[IDX_ASPD] = fc.aspd_active;
+			msg.active[IDX_ASPD]   = fc.aspd_active;
+			msg.active[IDX_RNGBCN] = fc.rngbcn_active;
 
 			mavlink_msg_estimator_sensor_fusion_status_send_struct(_mavlink->get_channel(), &msg);
 			return true;