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..f023d56fde 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,195 @@ ****************************************************************************/ #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 i = 0; i < MAX_AGP_IDS; i++) { + _sources[i] = new AgpSource(i, this); + } + + touchActiveAgpParams(); +} + +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; + } + } +} + 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 (_params_updated) { + touchActiveAgpParams(); + _params_updated = false; } -#endif // MODULE_NAME - - AuxGlobalPositionSample sample; - - 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; - } - } - } - - 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; - } - - 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 i = 0; i < MAX_AGP_IDS; i++) { + _sources[i]->checkAndBufferData(imu_delayed); + _sources[i]->update(ekf, imu_delayed); } } -bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf) const +float AuxGlobalPosition::test_ratio_filtered() const { - 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)); + float max_ratio = 0.f; + + for (int i = 0; i < MAX_AGP_IDS; i++) { + if (_sources[i]->isFusing()) { + max_ratio = math::max(max_ratio, _sources[i]->getTestRatioFiltered()); + } + } + + return max_ratio; } -#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION +bool AuxGlobalPosition::anySourceFusing() const +{ + for (int i = 0; i < MAX_AGP_IDS; i++) { + if (_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; +} + +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] {}; + 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::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; +} + +void AuxGlobalPosition::setIdParam(int instance, int32_t sensor_id) +{ + setAgpParamInt32("ID", instance, sensor_id); + _param_values[instance].id = sensor_id; + _params_updated = true; +} + +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 MAX_AGP_IDS; +} + +#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..1772c61aac 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,24 @@ #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 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 +60,42 @@ public: updateParams(); } - float test_ratio_filtered() const { return _test_ratio_filtered; } + 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; } 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); - } + friend class AgpSource; - bool isResetAllowed(const Ekf &ekf) const; + AgpSource *_sources[MAX_AGP_IDS]; + bool _params_updated{true}; - 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 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}; }; - 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}; + void touchActiveAgpParams(); + ParamValues _param_values[MAX_AGP_IDS] {}; - 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..a9b1bdc379 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp @@ -0,0 +1,242 @@ +/**************************************************************************** + * + * 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 instance_id, AuxGlobalPosition *manager) + : _agp_sub(ORB_ID(aux_global_position), instance_id) + , _manager(manager) + , _instance_id(instance_id) {} + +void AgpSource::checkAndBufferData(const estimator::imuSample &imu_delayed) +{ + if (_agp_sub.updated()) { + + aux_global_position_s aux_global_position{}; + _agp_sub.copy(&aux_global_position); + + 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(getDelayParam() * 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; + } +} + +void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed) +{ + AuxGlobalPositionSample sample; + + if (_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) { + + if (!(getCtrlParam() & 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); + 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(getGateParam(), 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])); + + } 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_WARN("Aux global position data stopped for instance %d", _instance_id); + } +} + +bool AgpSource::isResetAllowed(const Ekf &ekf) const +{ + return ((static_cast(getModeParam()) == Mode::kAuto) + && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) + || ((static_cast(getModeParam()) == 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); +} + +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 new file mode 100644 index 0000000000..d6843e0410 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#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 +#include + +class Ekf; +class AuxGlobalPosition; + +class AgpSource +{ +public: + AgpSource(int instance_id, AuxGlobalPosition *manager); + ~AgpSource() = default; + + void checkAndBufferData(const estimator::imuSample &imu_delayed); + void update(Ekf &ekf, const estimator::imuSample &imu_delayed); + void advertise() { _aid_src_pub.advertise(); } + + float getTestRatioFiltered() const { return _test_ratio_filtered; } + bool isFusing() const { return _state == State::kActive; } + +private: + struct AuxGlobalPositionSample { + uint64_t time_us{}; ///< timestamp of the measurement (uSec) + uint8_t id{}; ///< source identifier + 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::Subscription _agp_sub; + 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; + const int _instance_id; + + 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 + +#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..0f5cb823ad 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.params_updated(); #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/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index c9785b5ef0..96e66fbe68 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -53,6 +53,7 @@ #include #include #include +#include #include "ReplayEkf2.hpp" diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp index 4de5d46c2d..c8174c1019 100644 --- a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp +++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp @@ -112,7 +112,7 @@ void SensorAgpSim::Run() _time_last_update = now; if (!(_param_sim_agp_fail.get() & static_cast(FailureMode::Stuck))) { - _measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid); + _measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt); } if (_param_sim_agp_fail.get() & static_cast(FailureMode::Drift)) { @@ -129,17 +129,16 @@ void SensorAgpSim::Run() CONSTANTS_RADIUS_OF_EARTH); const double altitude = (double)(_measured_lla.altitude() + (generate_wgn() * 0.5f)); - vehicle_global_position_s sample{}; + aux_global_position_s sample{}; + sample.id = 33; sample.timestamp_sample = gpos.timestamp_sample; sample.lat = latitude; sample.lon = longitude; sample.alt = altitude; - sample.lat_lon_valid = true; - sample.alt_ellipsoid = altitude; - sample.alt_valid = true; sample.eph = 20.f; sample.epv = 5.f; + sample.lat_lon_reset_counter = 0; sample.timestamp = hrt_absolute_time(); _aux_global_position_pub.publish(sample); diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp index 9700780adb..6fff8cbeb6 100644 --- a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp +++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp @@ -44,6 +44,7 @@ #include #include #include +#include using namespace time_literals; @@ -83,7 +84,7 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; - uORB::PublicationMulti _aux_global_position_pub{ORB_ID(aux_global_position)}; + uORB::PublicationMulti _aux_global_position_pub{ORB_ID(aux_global_position)}; perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};