uxrce_dds: support multi-instance uORB topics

msg/CMakeLists.txt

@@ -247,6 +247,7 @@ set(msg_files
 	versioned/AirspeedValidated.msg
 	versioned/ArmingCheckReply.msg
 	versioned/ArmingCheckRequest.msg
+	versioned/AuxGlobalPosition.msg
 	versioned/BatteryStatus.msg
 	versioned/ConfigOverrides.msg
 	versioned/FixedWingLateralSetpoint.msg

msg/versioned/AuxGlobalPosition.msg

@@ -0,0 +1,23 @@
+# Auxiliary global position
+
+# This message provides global position data from an external source such as
+# pseudolites, viusal navigation, or other positioning system.
+
+uint32 MESSAGE_VERSION = 0
+
+uint64 timestamp                # [us] Time since system start
+uint64 timestamp_sample         # [us] Timestamp of the raw data
+
+uint8 id                        # Unique identifier for the AGP source
+uint8 source			# 0: Unkown, 1: GNSS, 2: Vision, 3: Pseudolites, 4: Terrain, 5: Magnetic, 6: Estimator
+
+float64 lat                     # [deg] Latitude in WGS84
+float64 lon                     # [deg] Longitude in WGS84
+float32 alt                     # [m] Altitude above mean sea level (AMSL)
+
+float32 eph                     # [m] Standard deviation of horizontal position error
+float32 epv                     # [m] Standard deviation of vertical position error
+
+uint8 lat_lon_reset_counter     # Counter for reset events on horizontal position coordinates
+
+# TOPICS aux_global_position

msg/versioned/VehicleGlobalPosition.msg

@@ -34,4 +34,3 @@ bool dead_reckoning		# True if this position is estimated through dead-reckoning
 
 # TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position
 # TOPICS estimator_global_position
-# TOPICS aux_global_position

src/modules/ekf2/CMakeLists.txt

@@ -141,7 +141,10 @@ if(CONFIG_EKF2_AIRSPEED)
 endif()
 
 if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	list(APPEND EKF_SRCS EKF/aid_sources/aux_global_position/aux_global_position.cpp)
+	list(APPEND EKF_SRCS
+		EKF/aid_sources/aux_global_position/aux_global_position.cpp
+		EKF/aid_sources/aux_global_position/aux_global_position_control.cpp
+	)
 	list(APPEND EKF_MODULE_PARAMS params_aux_global_position.yaml)
 endif()
 

src/modules/ekf2/EKF/CMakeLists.txt

@@ -60,10 +60,6 @@ if(CONFIG_EKF2_AIRSPEED)
 	list(APPEND EKF_SRCS aid_sources/airspeed/airspeed_fusion.cpp)
 endif()
 
-if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	list(APPEND EKF_SRCS aid_sources/aux_global_position/aux_global_position.cpp)
-endif()
-
 if(CONFIG_EKF2_AUXVEL)
 	list(APPEND EKF_SRCS aid_sources/auxvel/auxvel_fusion.cpp)
 endif()

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp

@@ -32,158 +32,195 @@
  ****************************************************************************/
 
 #include "ekf.h"
-
-#include "aid_sources/aux_global_position/aux_global_position.hpp"
+#include <aid_sources/aux_global_position/aux_global_position.hpp>
+#include <aid_sources/aux_global_position/aux_global_position_control.hpp>
 
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
 
+AuxGlobalPosition::AuxGlobalPosition() : ModuleParams(nullptr)
+{
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		_sources[i] = new AgpSource(i, this);
+	}
+
+	touchActiveAgpParams();
+}
+
+AuxGlobalPosition::~AuxGlobalPosition()
+{
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		delete _sources[i];
+		_sources[i] = nullptr;
+	}
+}
+
+void AuxGlobalPosition::touchActiveAgpParams()
+{
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		if (getIdParam(i) != 0) {
+			getCtrlParam(i);
+			getModeParam(i);
+			getDelayParam(i);
+			getNoiseParam(i);
+			getGateParam(i);
+			_sources[i]->advertise();
+
+		} else {
+			break;
+		}
+	}
+}
+
 void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 {
+	if (_params_updated) {
+		touchActiveAgpParams();
+		_params_updated = false;
+	}
+
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		_sources[i]->checkAndBufferData(imu_delayed);
+		_sources[i]->update(ekf, imu_delayed);
+	}
+}
+
+float AuxGlobalPosition::test_ratio_filtered() const
+{
+	float max_ratio = 0.f;
 
-#if defined(MODULE_NAME)
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		if (_sources[i]->isFusing()) {
+			max_ratio = math::max(max_ratio, _sources[i]->getTestRatioFiltered());
+		}
+	}
 
-	if (_aux_global_position_sub.updated()) {
+	return max_ratio;
+}
 
-		vehicle_global_position_s aux_global_position{};
-		_aux_global_position_sub.copy(&aux_global_position);
+bool AuxGlobalPosition::anySourceFusing() const
+{
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		if (_sources[i]->isFusing()) {
+			return true;
+		}
+	}
 
-		const int64_t time_us = aux_global_position.timestamp_sample - static_cast<int64_t>(_param_ekf2_agp_delay.get() * 1000);
+	return false;
+}
 
-		AuxGlobalPositionSample sample{};
-		sample.time_us = time_us;
-		sample.latitude = aux_global_position.lat;
-		sample.longitude = aux_global_position.lon;
-		sample.altitude_amsl = aux_global_position.alt;
-		sample.eph = aux_global_position.eph;
-		sample.epv = aux_global_position.epv;
-		sample.lat_lon_reset_counter = aux_global_position.lat_lon_reset_counter;
+int32_t AuxGlobalPosition::getAgpParamInt32(const char *param_suffix, int instance) const
+{
+	char param_name[20] {};
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_%s", instance, param_suffix);
 
-		_aux_global_position_buffer.push(sample);
+	int32_t value = 0;
 
-		_time_last_buffer_push = imu_delayed.time_us;
+	if (param_get(param_find(param_name), &value) != 0) {
+		PX4_ERR("failed to get %s", param_name);
 	}
 
-#endif // MODULE_NAME
+	return value;
+}
 
-	AuxGlobalPositionSample sample;
+float AuxGlobalPosition::getAgpParamFloat(const char *param_suffix, int instance) const
+{
+	char param_name[20] {};
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_%s", instance, param_suffix);
 
-	if (_aux_global_position_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
+	float value = NAN;
 
-		if (!(_param_ekf2_agp_ctrl.get() & static_cast<int32_t>(Ctrl::kHPos))) {
-			return;
-		}
+	if (param_get(param_find(param_name), &value) != 0) {
+		PX4_ERR("failed to get %s", param_name);
+	}
 
-		estimator_aid_source2d_s &aid_src = _aid_src_aux_global_position;
-		const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
-		const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
-
-		// relax the upper observation noise limit which prevents bad measurements perturbing the position estimate
-		float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f);
-		const float pos_var = sq(pos_noise);
-		const Vector2f pos_obs_var(pos_var, pos_var);
-
-		ekf.updateAidSourceStatus(aid_src,
-					  sample.time_us,                                      // sample timestamp
-					  matrix::Vector2d(sample.latitude, sample.longitude), // observation
-					  pos_obs_var,                                         // observation variance
-					  innovation,                                          // innovation
-					  Vector2f(ekf.getPositionVariance()) + pos_obs_var,   // innovation variance
-					  math::max(_param_ekf2_agp_gate.get(), 1.f));         // innovation gate
-
-		const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
-						 && ekf.control_status_flags().yaw_align;
-		const bool continuing_conditions = starting_conditions
-						   && ekf.global_origin_valid();
-
-		switch (_state) {
-		case State::kStopped:
-
-		/* FALLTHROUGH */
-		case State::kStarting:
-			if (starting_conditions) {
-				_state = State::kStarting;
-
-				if (ekf.global_origin_valid()) {
-					const bool fused = ekf.fuseHorizontalPosition(aid_src);
-					bool reset = false;
-
-					if (!fused && isResetAllowed(ekf)) {
-						ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
-						ekf.resetAidSourceStatusZeroInnovation(aid_src);
-						reset = true;
-					}
-
-					if (fused || reset) {
-						ekf.enableControlStatusAuxGpos();
-						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
-						_state = State::kActive;
-					}
-
-				} else {
-					// Try to initialize using measurement
-					if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
-								      sq(sample.epv))) {
-						ekf.resetAidSourceStatusZeroInnovation(aid_src);
-						ekf.enableControlStatusAuxGpos();
-						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
-						_state = State::kActive;
-					}
-				}
-			}
+	return value;
+}
 
-			break;
+bool AuxGlobalPosition::setAgpParamInt32(const char *param_suffix, int instance, int32_t value)
+{
+	char param_name[20] {};
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_%s", instance, param_suffix);
 
-		case State::kActive:
-			if (continuing_conditions) {
-				ekf.fuseHorizontalPosition(aid_src);
+	return param_set_no_notification(param_find(param_name), &value) == PX4_OK;
+}
 
-				if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.reset_timeout_max)
-				    || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
-					if (isResetAllowed(ekf)) {
+int32_t AuxGlobalPosition::getCtrlParam(int instance)
+{
+	if (_params_updated) {
+		_param_values[instance].ctrl = getAgpParamInt32("CTRL", instance);
+	}
 
-						ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
+	return _param_values[instance].ctrl;
+}
 
-						ekf.resetAidSourceStatusZeroInnovation(aid_src);
+int32_t AuxGlobalPosition::getModeParam(int instance)
+{
+	if (_params_updated) {
+		_param_values[instance].mode = getAgpParamInt32("MODE", instance);
+	}
 
-						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
+	return _param_values[instance].mode;
+}
 
-					} else {
-						ekf.disableControlStatusAuxGpos();
-						_state = State::kStopped;
-					}
-				}
+float AuxGlobalPosition::getDelayParam(int instance)
+{
+	if (_params_updated) {
+		_param_values[instance].delay = getAgpParamFloat("DELAY", instance);
+	}
 
-			} else {
-				ekf.disableControlStatusAuxGpos();
-				_state = State::kStopped;
-			}
+	return _param_values[instance].delay;
+}
 
-			break;
+float AuxGlobalPosition::getNoiseParam(int instance)
+{
+	if (_params_updated) {
+		_param_values[instance].noise = getAgpParamFloat("NOISE", instance);
+	}
 
-		default:
-			break;
-		}
+	return _param_values[instance].noise;
+}
 
-#if defined(MODULE_NAME)
-		aid_src.timestamp = hrt_absolute_time();
-		_estimator_aid_src_aux_global_position_pub.publish(aid_src);
+float AuxGlobalPosition::getGateParam(int instance)
+{
+	if (_params_updated) {
+		_param_values[instance].gate = getAgpParamFloat("GATE", instance);
+	}
 
-		_test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1]));
-#endif // MODULE_NAME
+	return _param_values[instance].gate;
+}
 
-	} else if ((_state != State::kStopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
-		ekf.disableControlStatusAuxGpos();
-		_state = State::kStopped;
-		ECL_WARN("Aux global position data stopped");
+int32_t AuxGlobalPosition::getIdParam(int instance)
+{
+	if (_params_updated) {
+		_param_values[instance].id = getAgpParamInt32("ID", instance);
 	}
+
+	return _param_values[instance].id;
+}
+
+void AuxGlobalPosition::setIdParam(int instance, int32_t sensor_id)
+{
+	setAgpParamInt32("ID", instance, sensor_id);
+	_param_values[instance].id = sensor_id;
+	_params_updated = true;
 }
 
-bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf) const
+int AuxGlobalPosition::mapSensorIdToSlot(int32_t sensor_id)
 {
-	return ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kAuto)
-		&& !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos))
-	       || ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kDeadReckoning)
-		   && !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos));
+	for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
+		if (getIdParam(slot) == sensor_id) {
+			return slot;
+		}
+	}
+
+	for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
+		if (getIdParam(slot) == 0) {
+			setIdParam(slot, sensor_id);
+			return slot;
+		}
+	}
+
+	return MAX_AGP_IDS;
 }
 
-#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME