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..0bced259df 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 @@ -42,147 +42,159 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed #if defined(MODULE_NAME) - if (_aux_global_position_sub.updated()) { + for (int instance = 0; instance < MAX_AGP_IDS; instance++) { + if (_aux_global_position_subs[instance].updated()) { - vehicle_global_position_s aux_global_position{}; - _aux_global_position_sub.copy(&aux_global_position); + aux_global_position_s aux_global_position{}; + _aux_global_position_subs[instance].copy(&aux_global_position); - const int64_t time_us = aux_global_position.timestamp_sample - static_cast(_param_ekf2_agp_delay.get() * 1000); + const uint8_t sensor_id = aux_global_position.id; + const int slot = mapSensorIdToSlot(sensor_id); - 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; + if (slot >= MAX_AGP_IDS) { + // All parameter slots are full, cannot handle this sensor. Set ID of unused slot to 0 + continue; + } - _aux_global_position_buffer.push(sample); + const int64_t time_us = aux_global_position.timestamp_sample - static_cast(getDelayParam(slot) * 1000); - _time_last_buffer_push = imu_delayed.time_us; + 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; + + _sources[slot].buffer.push(sample); + _sources[slot].time_last_buffer_push = imu_delayed.time_us; + } } #endif // MODULE_NAME - AuxGlobalPositionSample sample; + for (int slot = 0; slot < MAX_AGP_IDS; slot++) { + SourceData &source = _sources[slot]; + AuxGlobalPositionSample sample; - if (_aux_global_position_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) { + if (source.buffer.pop_first_older_than(imu_delayed.time_us, &sample)) { - if (!(_param_ekf2_agp_ctrl.get() & static_cast(Ctrl::kHPos))) { - return; - } + if (!(getCtrlParam(slot) & static_cast(Ctrl::kHPos))) { + continue; + } - 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 + estimator_aid_source2d_s &aid_src = source.aid_src; + 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); + float pos_noise = math::max(sample.eph, getNoiseParam(slot), 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 + 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(slot), 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(); + 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: + switch (source.state) { + case State::kStopped: - /* FALLTHROUGH */ - case State::kStarting: - if (starting_conditions) { - _state = State::kStarting; + /* FALLTHROUGH */ + case State::kStarting: + if (starting_conditions) { + source.state = State::kStarting; - if (ekf.global_origin_valid()) { - const bool fused = ekf.fuseHorizontalPosition(aid_src); - bool reset = false; + 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 && isResetAllowed(ekf, slot)) { + ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); + ekf.resetAidSourceStatusZeroInnovation(aid_src); + reset = true; + } + + if (fused || reset) { + ekf.enableControlStatusAuxGpos(); + source.reset_counters.lat_lon = sample.lat_lon_reset_counter; + source.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(); + source.reset_counters.lat_lon = sample.lat_lon_reset_counter; + source.state = State::kActive; + } } + } - if (fused || reset) { - 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) + || (source.reset_counters.lat_lon != sample.lat_lon_reset_counter)) { + if (isResetAllowed(ekf, slot)) { + + ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); + + ekf.resetAidSourceStatusZeroInnovation(aid_src); + + source.reset_counters.lat_lon = sample.lat_lon_reset_counter; + + } else { + ekf.disableControlStatusAuxGpos(); + source.state = State::kStopped; + } } } 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; - } + ekf.disableControlStatusAuxGpos(); + source.state = State::kStopped; } - } else { - ekf.disableControlStatusAuxGpos(); - _state = State::kStopped; + break; + + default: + break; } - break; - - default: - break; - } - #if defined(MODULE_NAME) - aid_src.timestamp = hrt_absolute_time(); - _estimator_aid_src_aux_global_position_pub.publish(aid_src); + aid_src.timestamp = hrt_absolute_time(); + source.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])); + source.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"); + } else if ((source.state != State::kStopped) && isTimedOut(source.time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) { + ekf.disableControlStatusAuxGpos(); + source.state = State::kStopped; + ECL_WARN("Aux global position data stopped for slot %d (sensor ID %d)", slot, getIdParam(slot)); + } } } -bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf) const +bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf, int slot) const { - return ((static_cast(_param_ekf2_agp_mode.get()) == Mode::kAuto) + return ((static_cast(getModeParam(slot)) == Mode::kAuto) && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) - || ((static_cast(_param_ekf2_agp_mode.get()) == Mode::kDeadReckoning) + || ((static_cast(getModeParam(slot)) == 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.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp index 8740c2851e..3972ded99f 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 @@ -49,10 +49,12 @@ #if defined(MODULE_NAME) # include +# include +# include # include # include # include -# include +# include #endif // MODULE_NAME class Ekf; @@ -60,9 +62,28 @@ class Ekf; class AuxGlobalPosition : public ModuleParams { public: + static constexpr uint8_t MAX_AGP_IDS = 4; + AuxGlobalPosition() : ModuleParams(nullptr) { - _estimator_aid_src_aux_global_position_pub.advertise(); + for (int i = 0; i < MAX_AGP_IDS; i++) { + _sources[i].aid_src_pub.advertise(); + + } + + // check existing ID to initialize parameters + for (int i = 0; i < MAX_AGP_IDS; i++) { + if (getIdParam(i) != 0) { + getCtrlParam(i); + getModeParam(i); + getDelayParam(i); + getNoiseParam(i); + getGateParam(i); + + } else { + break; + } + } } ~AuxGlobalPosition() = default; @@ -74,7 +95,14 @@ public: updateParams(); } - float test_ratio_filtered() const { return _test_ratio_filtered; } + float test_ratio_filtered(uint8_t id = 0) const + { + if (id < MAX_AGP_IDS) { + return _sources[id].test_ratio_filtered; + } + + return INFINITY; + } private: bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const @@ -82,10 +110,11 @@ private: return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < time_delayed_us); } - bool isResetAllowed(const Ekf &ekf) const; + bool isResetAllowed(const Ekf &ekf, int slot) const; struct AuxGlobalPositionSample { uint64_t time_us{}; ///< timestamp of the measurement (uSec) + uint8_t id{}; ///< source identifier double latitude{}; double longitude{}; float altitude_amsl{}; @@ -94,10 +123,6 @@ private: uint8_t lat_lon_reset_counter{}; }; - estimator_aid_source2d_s _aid_src_aux_global_position{}; - RingBuffer _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) @@ -114,26 +139,120 @@ private: 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)}; + struct SourceData { + estimator_aid_source2d_s aid_src{}; + RingBuffer buffer{20}; + uint64_t time_last_buffer_push{0}; + State state{State::kStopped}; + float test_ratio_filtered{0.f}; + reset_counters_s reset_counters{}; + uORB::PublicationMulti aid_src_pub{ORB_ID(estimator_aid_src_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 - ) + SourceData _sources[MAX_AGP_IDS]; + + uORB::Subscription _aux_global_position_subs[MAX_AGP_IDS] { + {ORB_ID(aux_global_position), 0}, + {ORB_ID(aux_global_position), 1}, + {ORB_ID(aux_global_position), 2}, + {ORB_ID(aux_global_position), 3} + }; + + int32_t 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 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 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 getCtrlParam(int instance) const + { + return getAgpParamInt32("CTRL", instance); + } + + int32_t getModeParam(int instance) const + { + return getAgpParamInt32("MODE", instance); + } + + float getDelayParam(int instance) const + { + return getAgpParamFloat("DELAY", instance); + } + + float getNoiseParam(int instance) const + { + return getAgpParamFloat("NOISE", instance); + } + + float getGateParam(int instance) const + { + return getAgpParamFloat("GATE", instance); + } + + int32_t getIdParam(int instance) const + { + return getAgpParamInt32("ID", instance); + } + + void setIdParam(int instance, int32_t sensor_id) + { + setAgpParamInt32("ID", instance, sensor_id); + } + + int mapSensorIdToSlot(int32_t sensor_id) + { + for (int slot = 0; slot < MAX_AGP_IDS; slot++) { + if (getIdParam(slot) == sensor_id) { + return slot; + } + } + + // Not found, try to assign to first available slot + for (int slot = 0; slot < MAX_AGP_IDS; slot++) { + if (getIdParam(slot) == 0) { + setIdParam(slot, sensor_id); + return slot; + } + } + + // All slots are full + return MAX_AGP_IDS; + } #endif // MODULE_NAME }; 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..d1c8bf5f46 100644 --- a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp +++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp @@ -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")};