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 a205a8bb94..dca1bf8536 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 @@ -39,11 +39,19 @@ AuxGlobalPosition::AuxGlobalPosition() : ModuleParams(nullptr) { - for (int i = 0; i < MAX_AGP_IDS; i++) { - _id_param_values[i] = getAgpParamInt32("ID", i); + for (int slot = 0; slot < MAX_AGP_IDS; slot++) { + _id_param_values[slot] = getAgpParamInt32("ID", slot); - if (_id_param_values[i] != 0) { - _sources[i] = new AgpSource(i, this); + if (_id_param_values[slot] != 0) { + _sources[slot] = new AgpSource(slot, this); + _n_sources++; + } + } + + // Only subscribe to uORB instances if there are configured sources + if (_n_sources > 0) { + for (int i = 0; i < MAX_AGP_IDS; i++) { + _agp_sub[i] = uORB::Subscription(ORB_ID(aux_global_position), i); } } } @@ -57,10 +65,36 @@ AuxGlobalPosition::~AuxGlobalPosition() void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed) { - for (int i = 0; i < MAX_AGP_IDS; i++) { - if (_sources[i]) { - _sources[i]->checkAndBufferData(imu_delayed); - _sources[i]->update(ekf, imu_delayed); + // If there are no sources configured, we also don't subscribe to any uORB topic + // and skip the update completely. + if (_n_sources == 0) { + return; + } + + for (int instance = 0; instance < MAX_AGP_IDS; instance++) { + if (_agp_sub[instance].updated()) { + aux_global_position_s msg{}; + _agp_sub[instance].copy(&msg); + + int slot = _instance_slot_map[instance]; + + if (slot < 0) { + slot = mapSensorIdToSlot(msg.id); + _instance_slot_map[instance] = static_cast(slot); + } + + if (slot >= 0 && _sources[slot]) { + _sources[slot]->bufferData(msg, imu_delayed); + } + } + } + + for (int slot = 0; slot < MAX_AGP_IDS; slot++) { + if (_sources[slot]) { + if (_sources[slot]->update(ekf, imu_delayed)) { + // Only update one source per update cycle + break; + } } } } @@ -146,7 +180,7 @@ int AuxGlobalPosition::mapSensorIdToSlot(int32_t sensor_id) } } - return MAX_AGP_IDS; + return -1; } #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME 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 26ad5be09c..4b30f5d9d7 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 @@ -41,6 +41,8 @@ #include #include #include +#include +#include #include class Ekf; @@ -72,6 +74,9 @@ public: private: AgpSource *_sources[MAX_AGP_IDS] {}; + uORB::Subscription _agp_sub[MAX_AGP_IDS]; + int8_t _instance_slot_map[MAX_AGP_IDS] {-1, -1, -1, -1}; + uint8_t _n_sources{0}; 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 84e5aa062c..6ebce26a76 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 @@ -37,10 +37,9 @@ #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) -AgpSource::AgpSource(int instance_id, AuxGlobalPosition *manager) - : _agp_sub(ORB_ID(aux_global_position), instance_id) - , _manager(manager) - , _instance_id(instance_id) +AgpSource::AgpSource(int slot, AuxGlobalPosition *manager) + : _manager(manager) + , _slot(slot) { initParams(); advertise(); @@ -50,19 +49,19 @@ void AgpSource::initParams() { char param_name[20] {}; - snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_CTRL", _instance_id); + snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_CTRL", _slot); _param_handles.ctrl = param_find(param_name); - snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_MODE", _instance_id); + snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_MODE", _slot); _param_handles.mode = param_find(param_name); - snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_DELAY", _instance_id); + snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_DELAY", _slot); _param_handles.delay = param_find(param_name); - snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_NOISE", _instance_id); + snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_NOISE", _slot); _param_handles.noise = param_find(param_name); - snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_GATE", _instance_id); + snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_GATE", _slot); _param_handles.gate = param_find(param_name); updateParams(); @@ -74,7 +73,6 @@ void AgpSource::updateParams() return; } - 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); @@ -82,52 +80,33 @@ void AgpSource::updateParams() param_get(_param_handles.gate, &_params.gate); } -void AgpSource::checkAndBufferData(const estimator::imuSample &imu_delayed) +void AgpSource::bufferData(const aux_global_position_s &msg, const estimator::imuSample &imu_delayed) { - if (_agp_sub.updated()) { + const int64_t time_us = msg.timestamp_sample + - static_cast(_params.delay * 1000); - aux_global_position_s aux_global_position{}; - _agp_sub.copy(&aux_global_position); + AuxGlobalPositionSample sample{}; + sample.time_us = time_us; + sample.id = msg.id; + sample.latitude = msg.lat; + sample.longitude = msg.lon; + sample.altitude_amsl = msg.alt; + sample.eph = msg.eph; + sample.epv = msg.epv; + sample.lat_lon_reset_counter = msg.lat_lon_reset_counter; - const uint8_t sensor_id = aux_global_position.id; - const int slot = _manager->mapSensorIdToSlot(sensor_id); - - if (slot >= AuxGlobalPosition::MAX_AGP_IDS) { - // All parameter slots are full, cannot handle this sensor - return; - } - - if (slot != _instance_id) { - // This sensor is mapped to a different instance - return; - } - - const int64_t time_us = aux_global_position.timestamp_sample - - static_cast(_params.delay * 1000); - - AuxGlobalPositionSample sample{}; - sample.time_us = time_us; - sample.id = sensor_id; - 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; - - _buffer.push(sample); - _time_last_buffer_push = imu_delayed.time_us; - } + _buffer.push(sample); + _time_last_buffer_push = imu_delayed.time_us; } -void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed) +bool AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed) { AuxGlobalPositionSample sample; if (_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) { if (!(_params.ctrl & static_cast(Ctrl::kHPos))) { - return; + return true; } const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl); @@ -230,13 +209,19 @@ void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed) _test_ratio_filtered = math::max(fabsf(_aid_src.test_ratio_filtered[0]), fabsf(_aid_src.test_ratio_filtered[1])); + return true; + } else if ((_state != State::kStopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) { _state = State::kStopped; if (!_manager->anySourceFusing()) { ekf.disableControlStatusAuxGpos(); } + + ECL_INFO("Aux global position data stopped for slot %d", _slot); } + + return false; } bool AgpSource::isResetAllowed(const Ekf &ekf) const 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 6e81dbf072..ad8e332170 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 @@ -40,7 +40,6 @@ #include #include -#include #include #include @@ -50,11 +49,11 @@ class AuxGlobalPosition; class AgpSource { public: - AgpSource(int instance_id, AuxGlobalPosition *manager); + AgpSource(int slot, AuxGlobalPosition *manager); ~AgpSource() = default; - void checkAndBufferData(const estimator::imuSample &imu_delayed); - void update(Ekf &ekf, const estimator::imuSample &imu_delayed); + void bufferData(const aux_global_position_s &msg, const estimator::imuSample &imu_delayed); + bool update(Ekf &ekf, const estimator::imuSample &imu_delayed); void advertise() { _aid_src_pub.advertise(); } void initParams(); void updateParams(); @@ -64,8 +63,8 @@ public: private: struct AuxGlobalPositionSample { - uint64_t time_us{}; ///< timestamp of the measurement (uSec) - uint8_t id{}; ///< source identifier + uint64_t time_us{}; + uint8_t id{}; double latitude{}; double longitude{}; float altitude_amsl{}; @@ -94,7 +93,6 @@ private: uint8_t lat_lon{}; }; - uORB::Subscription _agp_sub; uORB::PublicationMulti _aid_src_pub{ORB_ID(estimator_aid_src_aux_global_position)}; TimestampedRingBuffer _buffer{20}; @@ -105,10 +103,9 @@ private: reset_counters_s _reset_counters{}; AuxGlobalPosition *_manager; - const int _instance_id; + int _slot; struct ParamHandles { - param_t id{PARAM_INVALID}; param_t ctrl{PARAM_INVALID}; param_t mode{PARAM_INVALID}; param_t delay{PARAM_INVALID}; @@ -117,7 +114,6 @@ private: } _param_handles; struct Params { - int32_t id{0}; int32_t ctrl{0}; int32_t mode{0}; float delay{0.f};