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 1ddf02f25c..84c9234894 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 @@ -51,16 +51,14 @@ AuxGlobalPosition::AuxGlobalPosition() : ModuleParams(nullptr) AuxGlobalPosition::~AuxGlobalPosition() { for (int i = 0; i < MAX_AGP_IDS; i++) { - if (_id_param_values[i] != 0) { - delete _sources[i]; - } + delete _sources[i]; } } void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed) { for (int i = 0; i < MAX_AGP_IDS; i++) { - if (_id_param_values[i] != 0) { + if (_sources[i]) { _sources[i]->checkAndBufferData(imu_delayed); _sources[i]->update(ekf, imu_delayed); } @@ -70,7 +68,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed void AuxGlobalPosition::paramsUpdated() { for (int i = 0; i < MAX_AGP_IDS; i++) { - if (_id_param_values[i] != 0) { + if (_sources[i]) { _sources[i]->updateParams(); } } @@ -81,7 +79,7 @@ float AuxGlobalPosition::test_ratio_filtered() const float max_ratio = 0.f; for (int i = 0; i < MAX_AGP_IDS; i++) { - if (_id_param_values[i] != 0 && _sources[i]->isFusing()) { + if (_sources[i] && _sources[i]->isFusing()) { max_ratio = math::max(max_ratio, _sources[i]->getTestRatioFiltered()); } } @@ -92,7 +90,7 @@ float AuxGlobalPosition::test_ratio_filtered() const bool AuxGlobalPosition::anySourceFusing() const { for (int i = 0; i < MAX_AGP_IDS; i++) { - if (_id_param_values[i] != 0 && _sources[i]->isFusing()) { + if (_sources[i] && _sources[i]->isFusing()) { return true; } } 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 1a916ee3c9..cda09c4f25 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 @@ -68,7 +68,7 @@ public: void paramsUpdated(); private: - AgpSource *_sources[MAX_AGP_IDS]; + AgpSource *_sources[MAX_AGP_IDS] {}; int32_t getAgpParamInt32(const char *param_suffix, int instance) const; bool setAgpParamInt32(const char *param_suffix, int instance, int32_t value); 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 0212d97694..c6a57dce70 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 @@ -51,34 +51,35 @@ 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); + _param_handles.ctrl = param_find(param_name); snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_MODE", _instance_id); - _param_mode = param_find(param_name); + _param_handles.mode = param_find(param_name); snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_DELAY", _instance_id); - _param_delay = param_find(param_name); + _param_handles.delay = param_find(param_name); snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_NOISE", _instance_id); - _param_noise = param_find(param_name); + _param_handles.noise = param_find(param_name); snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_GATE", _instance_id); - _param_gate = param_find(param_name); + _param_handles.gate = param_find(param_name); updateParams(); } void AgpSource::updateParams() { - if (_param_ctrl == PARAM_INVALID) { + if (_param_handles.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); + param_get(_param_handles.id, &_params.id); + param_get(_param_handles.ctrl, &_params.ctrl); + param_get(_param_handles.mode, &_params.mode); + param_get(_param_handles.delay, &_params.delay); + param_get(_param_handles.noise, &_params.noise); + param_get(_param_handles.gate, &_params.gate); } void AgpSource::checkAndBufferData(const estimator::imuSample &imu_delayed) @@ -102,7 +103,7 @@ void AgpSource::checkAndBufferData(const estimator::imuSample &imu_delayed) } const int64_t time_us = aux_global_position.timestamp_sample - - static_cast(_delay * 1000); + - static_cast(_params.delay * 1000); AuxGlobalPositionSample sample{}; sample.time_us = time_us; @@ -125,14 +126,14 @@ void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed) if (_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) { - if (!(_ctrl & static_cast(Ctrl::kHPos))) { + if (!(_params.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, _noise, 0.01f); + float pos_noise = math::max(sample.eph, _params.noise, 0.01f); const float pos_var = sq(pos_noise); const Vector2f pos_obs_var(pos_var, pos_var); @@ -142,7 +143,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(_gate, 1.f)); // innovation gate + math::max(_params.gate, 1.f)); // innovation gate const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude) && ekf.control_status_flags().yaw_align; @@ -242,9 +243,9 @@ void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed) bool AgpSource::isResetAllowed(const Ekf &ekf) const { - return ((static_cast(_mode) == Mode::kAuto) + return ((static_cast(_params.mode) == Mode::kAuto) && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) - || ((static_cast(_mode) == Mode::kDeadReckoning) + || ((static_cast(_params.mode) == Mode::kDeadReckoning) && !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos)); } 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 331032d5b4..b3667b2c8d 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 @@ -107,17 +107,23 @@ 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}; + struct ParamHandles { + param_t id{PARAM_INVALID}; + param_t ctrl{PARAM_INVALID}; + param_t mode{PARAM_INVALID}; + param_t delay{PARAM_INVALID}; + param_t noise{PARAM_INVALID}; + param_t gate{PARAM_INVALID}; + } _param_handles; - int32_t _ctrl{0}; - int32_t _mode{0}; - float _delay{0.f}; - float _noise{10.f}; - float _gate{3.f}; + struct Params { + int32_t id{0}; + int32_t ctrl{0}; + int32_t mode{0}; + float delay{0.f}; + float noise{10.f}; + float gate{3.f}; + } _params; bool isResetAllowed(const Ekf &ekf) const; bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const;