diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index a7b8e18f8a..c12d7657ab 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/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() diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 381e265380..7ef54d5995 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/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() 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 90f6adfdd4..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 @@ -32,158 +32,155 @@ ****************************************************************************/ #include "ekf.h" - -#include "aid_sources/aux_global_position/aux_global_position.hpp" +#include +#include #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) +AuxGlobalPosition::AuxGlobalPosition() : ModuleParams(nullptr) +{ + for (int slot = 0; slot < MAX_AGP_IDS; slot++) { + _id_param_values[slot] = getAgpParamInt32("ID", slot); + + 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); + } + } +} + +AuxGlobalPosition::~AuxGlobalPosition() +{ + for (int i = 0; i < MAX_AGP_IDS; i++) { + delete _sources[i]; + } +} + void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed) { - -#if defined(MODULE_NAME) - - if (_aux_global_position_sub.updated()) { - - vehicle_global_position_s aux_global_position{}; - _aux_global_position_sub.copy(&aux_global_position); - - const int64_t time_us = aux_global_position.timestamp_sample - static_cast(_param_ekf2_agp_delay.get() * 1000); - - 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; - - _aux_global_position_buffer.push(sample); - - _time_last_buffer_push = imu_delayed.time_us; + // 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; } -#endif // MODULE_NAME + for (int instance = 0; instance < MAX_AGP_IDS; instance++) { + if (_agp_sub[instance].updated()) { + aux_global_position_s msg{}; + _agp_sub[instance].copy(&msg); - AuxGlobalPositionSample sample; + int slot = _instance_slot_map[instance]; - if (_aux_global_position_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) { - - if (!(_param_ekf2_agp_ctrl.get() & static_cast(Ctrl::kHPos))) { - return; - } - - 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; - } - } + if (slot < 0) { + slot = mapSensorIdToSlot(msg.id); + _instance_slot_map[instance] = static_cast(slot); } - break; - - case State::kActive: - if (continuing_conditions) { - ekf.fuseHorizontalPosition(aid_src); - - 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)) { - - ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); - - ekf.resetAidSourceStatusZeroInnovation(aid_src); - - _reset_counters.lat_lon = sample.lat_lon_reset_counter; - - } else { - ekf.disableControlStatusAuxGpos(); - _state = State::kStopped; - } - } - - } else { - ekf.disableControlStatusAuxGpos(); - _state = State::kStopped; + if (slot >= 0 && _sources[slot]) { + _sources[slot]->bufferData(msg, imu_delayed); } - - break; - - default: - break; } + } -#if defined(MODULE_NAME) - aid_src.timestamp = hrt_absolute_time(); - _estimator_aid_src_aux_global_position_pub.publish(aid_src); - - _test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1])); -#endif // MODULE_NAME - - } 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"); + 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; + } + } } } -bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf) const +void AuxGlobalPosition::paramsUpdated() { - return ((static_cast(_param_ekf2_agp_mode.get()) == Mode::kAuto) - && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) - || ((static_cast(_param_ekf2_agp_mode.get()) == Mode::kDeadReckoning) - && !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos)); + for (int i = 0; i < MAX_AGP_IDS; i++) { + if (_sources[i]) { + _sources[i]->updateParams(); + } + } } -#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION +float AuxGlobalPosition::testRatioFiltered() const +{ + float max_ratio = 0.f; + + for (int i = 0; i < MAX_AGP_IDS; i++) { + if (_sources[i] && _sources[i]->isFusing()) { + max_ratio = math::max(max_ratio, _sources[i]->getTestRatioFiltered()); + } + } + + return max_ratio; +} + +bool AuxGlobalPosition::anySourceFusing() const +{ + for (int i = 0; i < MAX_AGP_IDS; i++) { + if (_sources[i] && _sources[i]->isFusing()) { + return true; + } + } + + return false; +} + +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); + + int32_t value = 0; + + 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] {}; + snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_%s", instance, param_suffix); + + return param_set_no_notification(param_find(param_name), &value) == PX4_OK; +} + +int32_t AuxGlobalPosition::getIdParam(int instance) +{ + return _id_param_values[instance]; +} + +void AuxGlobalPosition::setIdParam(int instance, int32_t sensor_id) +{ + setAgpParamInt32("ID", instance, sensor_id); + _id_param_values[instance] = sensor_id; +} + +int AuxGlobalPosition::mapSensorIdToSlot(int32_t sensor_id) +{ + 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 -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 8777be409a..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 @@ -34,38 +34,26 @@ #ifndef EKF_AUX_GLOBAL_POSITION_HPP #define EKF_AUX_GLOBAL_POSITION_HPP -// interface? -// - ModuleParams -// - Base class EKF -// - bool update(imu) -// how to get delay? -// WelfordMean for init? -// WelfordMean for rate - #include "../../common.h" -#include #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) -#if defined(MODULE_NAME) -# include -# include -# include -# include -# include -#endif // MODULE_NAME +#include +#include +#include +#include +#include +#include class Ekf; class AuxGlobalPosition : public ModuleParams { public: - AuxGlobalPosition() : ModuleParams(nullptr) - { - _estimator_aid_src_aux_global_position_pub.advertise(); - } + static constexpr uint8_t MAX_AGP_IDS = 4; - ~AuxGlobalPosition() = default; + AuxGlobalPosition(); + ~AuxGlobalPosition(); void update(Ekf &ekf, const estimator::imuSample &imu_delayed); @@ -74,70 +62,29 @@ public: updateParams(); } - float test_ratio_filtered() const { return _test_ratio_filtered; } + /** + * Returns the maximum filtered test ratio across all active AGP sources. + */ + float testRatioFiltered() const; + bool anySourceFusing() const; + int32_t getIdParam(int instance); + void setIdParam(int instance, int32_t sensor_id); + int mapSensorIdToSlot(int32_t sensor_id); + void paramsUpdated(); private: - bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const - { - return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < time_delayed_us); - } + 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}; - bool isResetAllowed(const Ekf &ekf) const; + int32_t getAgpParamInt32(const char *param_suffix, int instance) const; + bool setAgpParamInt32(const char *param_suffix, int instance, int32_t value); - struct AuxGlobalPositionSample { - uint64_t time_us{}; ///< timestamp of the measurement (uSec) - double latitude{}; - double longitude{}; - float altitude_amsl{}; - float eph{}; - float epv{}; - uint8_t lat_lon_reset_counter{}; - }; + int32_t _id_param_values[MAX_AGP_IDS] {}; - estimator_aid_source2d_s _aid_src_aux_global_position{}; - TimestampedRingBuffer _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate - uint64_t _time_last_buffer_push{0}; - - enum class Ctrl : uint8_t { - kHPos = (1 << 0), - kVPos = (1 << 1) - }; - - enum class Mode : uint8_t { - kAuto = 0, ///< Reset on fusion timeout if no other source of position is available - kDeadReckoning = 1 ///< Reset on fusion timeout if no source of velocity is availabl - }; - - enum class State { - kStopped, - kStarting, - kActive, - }; - - State _state{State::kStopped}; - - float _test_ratio_filtered{INFINITY}; - -#if defined(MODULE_NAME) - struct reset_counters_s { - uint8_t lat_lon{}; - }; - reset_counters_s _reset_counters{}; - - uORB::PublicationMulti _estimator_aid_src_aux_global_position_pub{ORB_ID(estimator_aid_src_aux_global_position)}; - uORB::Subscription _aux_global_position_sub{ORB_ID(aux_global_position)}; - - DEFINE_PARAMETERS( - (ParamInt) _param_ekf2_agp_ctrl, - (ParamInt) _param_ekf2_agp_mode, - (ParamFloat) _param_ekf2_agp_delay, - (ParamFloat) _param_ekf2_agp_noise, - (ParamFloat) _param_ekf2_agp_gate - ) - -#endif // MODULE_NAME }; -#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION +#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME #endif // !EKF_AUX_GLOBAL_POSITION_HPP 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 new file mode 100644 index 0000000000..6ebce26a76 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp @@ -0,0 +1,240 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ekf.h" +#include +#include + +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) + +AgpSource::AgpSource(int slot, AuxGlobalPosition *manager) + : _manager(manager) + , _slot(slot) +{ + initParams(); + advertise(); +} + +void AgpSource::initParams() +{ + char param_name[20] {}; + + 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", _slot); + _param_handles.mode = param_find(param_name); + + 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", _slot); + _param_handles.noise = param_find(param_name); + + snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_GATE", _slot); + _param_handles.gate = param_find(param_name); + + updateParams(); +} + +void AgpSource::updateParams() +{ + if (_param_handles.ctrl == PARAM_INVALID) { + return; + } + + 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::bufferData(const aux_global_position_s &msg, const estimator::imuSample &imu_delayed) +{ + const int64_t time_us = msg.timestamp_sample + - static_cast(_params.delay * 1000); + + 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; + + _buffer.push(sample); + _time_last_buffer_push = imu_delayed.time_us; +} + +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 true; + } + + 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, _params.noise, 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(_params.gate, 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; + } + } + } + + break; + + case State::kActive: + if (continuing_conditions) { + ekf.fuseHorizontalPosition(_aid_src); + + 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)) { + + ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(_aid_src.observation_variance)); + + ekf.resetAidSourceStatusZeroInnovation(_aid_src); + + _reset_counters.lat_lon = sample.lat_lon_reset_counter; + + } else { + _state = State::kStopped; + + if (!_manager->anySourceFusing()) { + ekf.disableControlStatusAuxGpos(); + } + } + } + + } else { + _state = State::kStopped; + + if (!_manager->anySourceFusing()) { + ekf.disableControlStatusAuxGpos(); + } + } + + break; + + default: + break; + } + + _aid_src.timestamp = hrt_absolute_time(); + _aid_src_pub.publish(_aid_src); + + _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 +{ + return ((static_cast(_params.mode) == Mode::kAuto) + && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) + || ((static_cast(_params.mode) == Mode::kDeadReckoning) + && !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos)); +} + +bool AgpSource::isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const +{ + return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < time_delayed_us); +} + +#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 new file mode 100644 index 0000000000..ad8e332170 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef EKF_AUX_GLOBAL_POSITION_CONTROL_HPP +#define EKF_AUX_GLOBAL_POSITION_CONTROL_HPP + +#include "../../common.h" + +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) + +#include +#include +#include +#include + +class Ekf; +class AuxGlobalPosition; + +class AgpSource +{ +public: + AgpSource(int slot, AuxGlobalPosition *manager); + ~AgpSource() = default; + + 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(); + + float getTestRatioFiltered() const { return _test_ratio_filtered; } + bool isFusing() const { return _state == State::kActive; } + +private: + struct AuxGlobalPositionSample { + uint64_t time_us{}; + uint8_t id{}; + double latitude{}; + double longitude{}; + float altitude_amsl{}; + float eph{}; + float epv{}; + uint8_t lat_lon_reset_counter{}; + }; + + enum class Ctrl : uint8_t { + kHPos = (1 << 0), + kVPos = (1 << 1) + }; + + enum class Mode : uint8_t { + kAuto = 0, ///< Reset on fusion timeout if no other source of position is available + kDeadReckoning = 1 ///< Reset on fusion timeout if no source of velocity is available + }; + + enum class State { + kStopped, + kStarting, + kActive, + }; + + struct reset_counters_s { + uint8_t lat_lon{}; + }; + + uORB::PublicationMulti _aid_src_pub{ORB_ID(estimator_aid_src_aux_global_position)}; + + TimestampedRingBuffer _buffer{20}; + State _state{State::kStopped}; + estimator_aid_source2d_s _aid_src{}; + float _test_ratio_filtered{0.f}; + uint64_t _time_last_buffer_push{0}; + reset_counters_s _reset_counters{}; + + AuxGlobalPosition *_manager; + int _slot; + + struct ParamHandles { + 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; + + struct Params { + 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; +}; + +#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME + +#endif // !EKF_AUX_GLOBAL_POSITION_CONTROL_HPP diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index ac04d5e9c3..5ba1b5d3cc 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -419,7 +419,8 @@ void Ekf::updateParameters() #endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) - _aux_global_position.updateParameters(); + + _aux_global_position.paramsUpdated(); #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 80ef94b9fc..ef70ff3613 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -435,6 +435,7 @@ public: void updateParameters(); friend class AuxGlobalPosition; + friend class AgpSource; private: diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index d568487da6..f5b8703c0d 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -541,7 +541,7 @@ float Ekf::getHorizontalPositionInnovationTestRatio() const #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) if (_control_status.flags.aux_gpos) { - test_ratio = math::max(test_ratio, fabsf(_aux_global_position.test_ratio_filtered())); + test_ratio = math::max(test_ratio, fabsf(_aux_global_position.testRatioFiltered())); } #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION