ekf2: cache AGP parameters per instance. only create AgpSource for slots

with set ID on boot!

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

@@ -40,47 +40,39 @@
 AuxGlobalPosition::AuxGlobalPosition() : ModuleParams(nullptr)
 {
 	for (int i = 0; i < MAX_AGP_IDS; i++) {
-		_sources[i] = new AgpSource(i, this);
-	}
+		_id_param_values[i] = getAgpParamInt32("ID", i);
 
-	touchActiveAgpParams();
+		if (_id_param_values[i] != 0) {
+			_sources[i] = new AgpSource(i, this);
+		}
+	}
 }
 
 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;
+		if (_id_param_values[i] != 0) {
+			delete _sources[i];
 		}
 	}
 }
 
 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);
+		if (_id_param_values[i] != 0) {
+			_sources[i]->checkAndBufferData(imu_delayed);
+			_sources[i]->update(ekf, imu_delayed);
+		}
+	}
+}
+
+void AuxGlobalPosition::paramsUpdated()
+{
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		if (_id_param_values[i] != 0) {
+			_sources[i]->updateParams();
+		}
 	}
 }
 
@@ -89,7 +81,7 @@ float AuxGlobalPosition::test_ratio_filtered() const
 	float max_ratio = 0.f;
 
 	for (int i = 0; i < MAX_AGP_IDS; i++) {
-		if (_sources[i]->isFusing()) {
+		if (_id_param_values[i] != 0 && _sources[i]->isFusing()) {
 			max_ratio = math::max(max_ratio, _sources[i]->getTestRatioFiltered());
 		}
 	}
@@ -100,7 +92,7 @@ float AuxGlobalPosition::test_ratio_filtered() const
 bool AuxGlobalPosition::anySourceFusing() const
 {
 	for (int i = 0; i < MAX_AGP_IDS; i++) {
-		if (_sources[i]->isFusing()) {
+		if (_id_param_values[i] != 0 && _sources[i]->isFusing()) {
 			return true;
 		}
 	}
@@ -122,20 +114,6 @@ int32_t AuxGlobalPosition::getAgpParamInt32(const char *param_suffix, int instan
 	return value;
 }
 
-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);
-
-	float value = NAN;
-
-	if (param_get(param_find(param_name), &value) != 0) {
-		PX4_ERR("failed to get %s", param_name);
-	}
-
-	return value;
-}
-
 bool AuxGlobalPosition::setAgpParamInt32(const char *param_suffix, int instance, int32_t value)
 {
 	char param_name[20] {};
@@ -144,65 +122,15 @@ bool AuxGlobalPosition::setAgpParamInt32(const char *param_suffix, int instance,
 	return param_set_no_notification(param_find(param_name), &value) == PX4_OK;
 }
 
-int32_t AuxGlobalPosition::getCtrlParam(int instance)
-{
-	if (_params_updated) {
-		_param_values[instance].ctrl = getAgpParamInt32("CTRL", instance);
-	}
-
-	return _param_values[instance].ctrl;
-}
-
-int32_t AuxGlobalPosition::getModeParam(int instance)
-{
-	if (_params_updated) {
-		_param_values[instance].mode = getAgpParamInt32("MODE", instance);
-	}
-
-	return _param_values[instance].mode;
-}
-
-float AuxGlobalPosition::getDelayParam(int instance)
-{
-	if (_params_updated) {
-		_param_values[instance].delay = getAgpParamFloat("DELAY", instance);
-	}
-
-	return _param_values[instance].delay;
-}
-
-float AuxGlobalPosition::getNoiseParam(int instance)
-{
-	if (_params_updated) {
-		_param_values[instance].noise = getAgpParamFloat("NOISE", instance);
-	}
-
-	return _param_values[instance].noise;
-}
-
-float AuxGlobalPosition::getGateParam(int instance)
-{
-	if (_params_updated) {
-		_param_values[instance].gate = getAgpParamFloat("GATE", instance);
-	}
-
-	return _param_values[instance].gate;
-}
-
 int32_t AuxGlobalPosition::getIdParam(int instance)
 {
-	if (_params_updated) {
-		_param_values[instance].id = getAgpParamInt32("ID", instance);
-	}
-
-	return _param_values[instance].id;
+	return _id_param_values[instance];
 }
 
 void AuxGlobalPosition::setIdParam(int instance, int32_t sensor_id)
 {
 	setAgpParamInt32("ID", instance, sensor_id);
-	_param_values[instance].id = sensor_id;
-	_params_updated = true;
+	_id_param_values[instance] = sensor_id;
 }
 
 int AuxGlobalPosition::mapSensorIdToSlot(int32_t sensor_id)

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

@@ -62,37 +62,18 @@ public:
 
 	float test_ratio_filtered() const;
 	bool anySourceFusing() const;
-	int32_t getCtrlParam(int instance);
-	int32_t getModeParam(int instance);
-	float getDelayParam(int instance);
-	float getNoiseParam(int instance);
-	float getGateParam(int instance);
 	int32_t getIdParam(int instance);
 	void setIdParam(int instance, int32_t sensor_id);
 	int mapSensorIdToSlot(int32_t sensor_id);
-	void params_updated() { _params_updated = true; }
+	void paramsUpdated();
 
 private:
-	friend class AgpSource;
-
 	AgpSource *_sources[MAX_AGP_IDS];
-	bool _params_updated{true};
 
 	int32_t getAgpParamInt32(const char *param_suffix, int instance) const;
-	float getAgpParamFloat(const char *param_suffix, int instance) const;
 	bool setAgpParamInt32(const char *param_suffix, int instance, int32_t value);
 
-	struct ParamValues {
-		int32_t ctrl{0};
-		int32_t mode{0};
-		float delay{0.f};
-		float noise{10.f};
-		float gate{3.f};
-		int32_t id{0};
-	};
-
-	void touchActiveAgpParams();
-	ParamValues _param_values[MAX_AGP_IDS] {};
+	int32_t _id_param_values[MAX_AGP_IDS] {};
 
 };
 

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

@@ -40,7 +40,46 @@
 AgpSource::AgpSource(int instance_id, AuxGlobalPosition *manager)
 	: _agp_sub(ORB_ID(aux_global_position), instance_id)
 	, _manager(manager)
-	, _instance_id(instance_id) {}
+	, _instance_id(instance_id)
+{
+	initParams();
+	advertise();
+}
+
+void AgpSource::initParams()
+{
+	char param_name[20] {};
+
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_CTRL", _instance_id);
+	_param_ctrl = param_find(param_name);
+
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_MODE", _instance_id);
+	_param_mode = param_find(param_name);
+
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_DELAY", _instance_id);
+	_param_delay = param_find(param_name);
+
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_NOISE", _instance_id);
+	_param_noise = param_find(param_name);
+
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_GATE", _instance_id);
+	_param_gate = param_find(param_name);
+
+	updateParams();
+}
+
+void AgpSource::updateParams()
+{
+	if (_param_ctrl == PARAM_INVALID) {
+		return;
+	}
+
+	param_get(_param_ctrl, &_ctrl);
+	param_get(_param_mode, &_mode);
+	param_get(_param_delay, &_delay);
+	param_get(_param_noise, &_noise);
+	param_get(_param_gate, &_gate);
+}
 
 void AgpSource::checkAndBufferData(const estimator::imuSample &imu_delayed)
 {
@@ -63,7 +102,7 @@ void AgpSource::checkAndBufferData(const estimator::imuSample &imu_delayed)
 		}
 
 		const int64_t time_us = aux_global_position.timestamp_sample
-					- static_cast<int64_t>(getDelayParam() * 1000);
+					- static_cast<int64_t>(_delay * 1000);
 
 		AuxGlobalPositionSample sample{};
 		sample.time_us = time_us;
@@ -86,14 +125,14 @@ void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 
 	if (_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
 
-		if (!(getCtrlParam() & static_cast<int32_t>(Ctrl::kHPos))) {
+		if (!(_ctrl & static_cast<int32_t>(Ctrl::kHPos))) {
 			return;
 		}
 
 		const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
 		const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
 
-		float pos_noise = math::max(sample.eph, getNoiseParam(), 0.01f);
+		float pos_noise = math::max(sample.eph, _noise, 0.01f);
 		const float pos_var = sq(pos_noise);
 		const Vector2f pos_obs_var(pos_var, pos_var);
 
@@ -103,7 +142,7 @@ void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 					  pos_obs_var,                                         // observation variance
 					  innovation,                                          // innovation
 					  Vector2f(ekf.getPositionVariance()) + pos_obs_var,   // innovation variance
-					  math::max(getGateParam(), 1.f));                     // innovation gate
+					  math::max(_gate, 1.f));                              // innovation gate
 
 		const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
 						 && ekf.control_status_flags().yaw_align;
@@ -203,9 +242,9 @@ void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 
 bool AgpSource::isResetAllowed(const Ekf &ekf) const
 {
-	return ((static_cast<Mode>(getModeParam()) == Mode::kAuto)
+	return ((static_cast<Mode>(_mode) == Mode::kAuto)
 		&& !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos))
-	       || ((static_cast<Mode>(getModeParam()) == Mode::kDeadReckoning)
+	       || ((static_cast<Mode>(_mode) == Mode::kDeadReckoning)
 		   && !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos));
 }
 
@@ -214,29 +253,4 @@ bool AgpSource::isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed
 	return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < time_delayed_us);
 }
 
-int32_t AgpSource::getCtrlParam() const
-{
-	return _manager->getCtrlParam(_instance_id);
-}
-
-int32_t AgpSource::getModeParam() const
-{
-	return _manager->getModeParam(_instance_id);
-}
-
-float AgpSource::getDelayParam() const
-{
-	return _manager->getDelayParam(_instance_id);
-}
-
-float AgpSource::getNoiseParam() const
-{
-	return _manager->getNoiseParam(_instance_id);
-}
-
-float AgpSource::getGateParam() const
-{
-	return _manager->getGateParam(_instance_id);
-}
-
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp

@@ -38,7 +38,7 @@
 
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
 
-#include <lib/ringbuffer/TimestampedRingBuffer.hpp>
+#include <lib/parameters/param.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/estimator_aid_source2d.h>
@@ -56,6 +56,8 @@ public:
 	void checkAndBufferData(const estimator::imuSample &imu_delayed);
 	void update(Ekf &ekf, const estimator::imuSample &imu_delayed);
 	void advertise() { _aid_src_pub.advertise(); }
+	void initParams();
+	void updateParams();
 
 	float getTestRatioFiltered() const { return _test_ratio_filtered; }
 	bool isFusing() const { return _state == State::kActive; }
@@ -105,14 +107,20 @@ private:
 	AuxGlobalPosition *_manager;
 	const int _instance_id;
 
+	param_t _param_ctrl{PARAM_INVALID};
+	param_t _param_mode{PARAM_INVALID};
+	param_t _param_delay{PARAM_INVALID};
+	param_t _param_noise{PARAM_INVALID};
+	param_t _param_gate{PARAM_INVALID};
+
+	int32_t _ctrl{0};
+	int32_t _mode{0};
+	float _delay{0.f};
+	float _noise{10.f};
+	float _gate{3.f};
+
 	bool isResetAllowed(const Ekf &ekf) const;
 	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const;
-
-	int32_t getCtrlParam() const;
-	int32_t getModeParam() const;
-	float getDelayParam() const;
-	float getNoiseParam() const;
-	float getGateParam() const;
 };
 
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME

src/modules/ekf2/EKF/ekf.cpp

@@ -420,7 +420,7 @@ void Ekf::updateParameters()
 
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
 
-	_aux_global_position.params_updated();
+	_aux_global_position.paramsUpdated();
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
 }