diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index f023d56fde..1ddf02f25c 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/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) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp index 1772c61aac..1a916ee3c9 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp +++ b/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] {}; }; diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp index a9b1bdc379..0212d97694 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp +++ b/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(getDelayParam() * 1000); + - static_cast(_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(Ctrl::kHPos))) { + if (!(_ctrl & static_cast(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(getModeParam()) == Mode::kAuto) + return ((static_cast(_mode) == Mode::kAuto) && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) - || ((static_cast(getModeParam()) == Mode::kDeadReckoning) + || ((static_cast(_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 diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp index d6843e0410..331032d5b4 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp +++ b/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 +#include #include #include #include @@ -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 diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 0f5cb823ad..5ba1b5d3cc 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/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 }