diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index dbdaf79b7f2d..66a9f7e599ca 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -248,6 +248,7 @@ set(msg_files versioned/AirspeedValidated.msg versioned/ArmingCheckReply.msg versioned/ArmingCheckRequest.msg + versioned/AuxGlobalPosition.msg versioned/BatteryStatus.msg versioned/ConfigOverrides.msg versioned/FixedWingLateralSetpoint.msg diff --git a/msg/px4_msgs_old/msg/VehicleGlobalPositionV0.msg b/msg/px4_msgs_old/msg/VehicleGlobalPositionV0.msg new file mode 100644 index 000000000000..eae8a489a470 --- /dev/null +++ b/msg/px4_msgs_old/msg/VehicleGlobalPositionV0.msg @@ -0,0 +1,36 @@ +# Fused global position in WGS84. +# This struct contains global position estimation. It is not the raw GPS +# measurement (@see vehicle_gps_position). This topic is usually published by the position +# estimator, which will take more sources of information into account than just GPS, +# e.g. control inputs of the vehicle in a Kalman-filter implementation. +# + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float64 lat # Latitude, (degrees) +float64 lon # Longitude, (degrees) +float32 alt # Altitude AMSL, (meters) +float32 alt_ellipsoid # Altitude above ellipsoid, (meters) + +bool lat_lon_valid +bool alt_valid + +float32 delta_alt # Reset delta for altitude +float32 delta_terrain # Reset delta for terrain +uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates +uint8 alt_reset_counter # Counter for reset events on altitude +uint8 terrain_reset_counter # Counter for reset events on terrain + +float32 eph # Standard deviation of horizontal position error, (metres) +float32 epv # Standard deviation of vertical position error, (metres) + +float32 terrain_alt # Terrain altitude WGS84, (metres) +bool terrain_alt_valid # Terrain altitude estimate is valid + +bool dead_reckoning # True if this position is estimated through dead-reckoning + +# TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position +# TOPICS estimator_global_position diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 78d30fc7feef..31342547f96d 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -7,6 +7,7 @@ #include #include "translation_airspeed_validated_v1.h" +#include "translation_aux_global_position_v1.h" #include "translation_arming_check_reply_v1.h" #include "translation_arming_check_request_v1.h" #include "translation_battery_status_v1.h" diff --git a/msg/translation_node/translations/translation_aux_global_position_v1.h b/msg/translation_node/translations/translation_aux_global_position_v1.h new file mode 100644 index 000000000000..153f89b395bd --- /dev/null +++ b/msg/translation_node/translations/translation_aux_global_position_v1.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * Copyright (c) 2026 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include + +// Translate aux_global_position: VehicleGlobalPosition v0 <--> AuxGlobalPosition v1 +#include +#include + +class AuxGlobalPositionV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::VehicleGlobalPositionV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::AuxGlobalPosition; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/in/aux_global_position"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + msg_newer.timestamp = msg_older.timestamp; + msg_newer.timestamp_sample = msg_older.timestamp_sample; + msg_newer.id = 1; + msg_newer.source = MessageNewer::SOURCE_VISION; + msg_newer.lat = msg_older.lat; + msg_newer.lon = msg_older.lon; + msg_newer.alt = msg_older.alt_valid ? msg_older.alt : NAN; + msg_newer.eph = msg_older.eph; + msg_newer.epv = msg_older.epv; + msg_newer.lat_lon_reset_counter = msg_older.lat_lon_reset_counter; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + msg_older.timestamp = msg_newer.timestamp; + msg_older.timestamp_sample = msg_newer.timestamp_sample; + msg_older.lat = msg_newer.lat; + msg_older.lon = msg_newer.lon; + msg_older.alt = std::isnan(msg_newer.alt) ? 0.0f : msg_newer.alt; + msg_older.alt_ellipsoid = 0.0f; + msg_older.lat_lon_valid = true; + msg_older.alt_valid = !std::isnan(msg_newer.alt); + msg_older.delta_alt = 0.0f; + msg_older.delta_terrain = 0.0f; + msg_older.lat_lon_reset_counter = msg_newer.lat_lon_reset_counter; + msg_older.alt_reset_counter = 0; + msg_older.terrain_reset_counter = 0; + msg_older.eph = std::isnan(msg_newer.eph) ? 0.0f : msg_newer.eph; + msg_older.epv = std::isnan(msg_newer.epv) ? 0.0f : msg_newer.epv; + msg_older.terrain_alt = 0.0f; + msg_older.terrain_alt_valid = false; + msg_older.dead_reckoning = false; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(AuxGlobalPositionV1Translation); diff --git a/msg/versioned/AuxGlobalPosition.msg b/msg/versioned/AuxGlobalPosition.msg new file mode 100644 index 000000000000..d5716ba4a184 --- /dev/null +++ b/msg/versioned/AuxGlobalPosition.msg @@ -0,0 +1,31 @@ +# Auxiliary global position +# +# This message provides global position data from an external source such as +# pseudolites, visual navigation, or other positioning system. + +uint32 MESSAGE_VERSION = 1 + +uint64 timestamp # [us] Time since system start +uint64 timestamp_sample # [us] Timestamp of the raw data + +uint8 id # [-] Unique identifier for the AGP source +uint8 source # [@enum SOURCE] Source type of the position data (based on mavlink::GLOBAL_POSITION_SRC) +uint8 SOURCE_UNKNOWN = 0 # Unknown source +uint8 SOURCE_GNSS = 1 # GNSS +uint8 SOURCE_VISION = 2 # Vision +uint8 SOURCE_PSEUDOLITES = 3 # Pseudolites +uint8 SOURCE_TERRAIN = 4 # Terrain +uint8 SOURCE_MAGNETIC = 5 # Magnetic +uint8 SOURCE_ESTIMATOR = 6 # Estimator + +# lat, lon: required for horizontal position fusion, alt: required for vertical position fusion +float64 lat # [deg] Latitude in WGS84 +float64 lon # [deg] Longitude in WGS84 +float32 alt # [m] [@invalid NaN] Altitude above mean sea level (AMSL) + +float32 eph # [m] [@invalid NaN] Std dev of horizontal position, lower bounded by NOISE param +float32 epv # [m] [@invalid NaN] Std dev of vertical position, lower bounded by NOISE param + +uint8 lat_lon_reset_counter # [-] Counter for reset events on horizontal position coordinates + +# TOPICS aux_global_position diff --git a/msg/versioned/VehicleGlobalPosition.msg b/msg/versioned/VehicleGlobalPosition.msg index 387576d8c792..eae8a489a470 100644 --- a/msg/versioned/VehicleGlobalPosition.msg +++ b/msg/versioned/VehicleGlobalPosition.msg @@ -34,4 +34,3 @@ bool dead_reckoning # True if this position is estimated through dead-reckoning # TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position # TOPICS estimator_global_position -# TOPICS aux_global_position diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index a7b8e18f8aa5..c12d7657abe7 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 381e265380e1..7ef54d5995f0 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 90f6adfdd439..ec3525c09172 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) -void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed) +AuxGlobalPosition::AuxGlobalPosition() : ModuleParams(nullptr) { + for (int slot = 0; slot < MAX_AGP_IDS; slot++) { + _id_param_values[slot] = getAgpParamInt32("ID", slot); -#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); + if (_id_param_values[slot] != 0) { + _sources[slot] = new AgpSource(slot); + _n_sources++; + } + } - 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; + // 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); + } + } +} - _aux_global_position_buffer.push(sample); +AuxGlobalPosition::~AuxGlobalPosition() +{ + for (int i = 0; i < MAX_AGP_IDS; i++) { + delete _sources[i]; + } +} - _time_last_buffer_push = imu_delayed.time_us; +void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &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; } -#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 (slot < 0) { + slot = mapSensorIdToSlot(msg.id); + _instance_slot_map[instance] = static_cast(slot); + } - if (!(_param_ekf2_agp_ctrl.get() & static_cast(Ctrl::kHPos))) { - return; + if (slot >= 0 && _sources[slot]) { + _sources[slot]->bufferData(msg, imu_delayed); + } } + } - 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; - } - } + 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; } + } + } +} - break; +void AuxGlobalPosition::paramsUpdated() +{ + for (int i = 0; i < MAX_AGP_IDS; i++) { + if (_sources[i]) { + _sources[i]->updateParams(); + } + } +} - case State::kActive: - if (continuing_conditions) { - ekf.fuseHorizontalPosition(aid_src); +float AuxGlobalPosition::testRatioFiltered() const +{ + float max_ratio = 0.f; - 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)) { + for (int i = 0; i < MAX_AGP_IDS; i++) { + if (_sources[i] && _sources[i]->isFusing()) { + max_ratio = math::max(max_ratio, _sources[i]->getTestRatioFiltered()); + } + } - ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); + return max_ratio; +} - ekf.resetAidSourceStatusZeroInnovation(aid_src); +bool AuxGlobalPosition::anySourceFusing() const +{ + for (int i = 0; i < MAX_AGP_IDS; i++) { + if (_sources[i] && _sources[i]->isFusing()) { + return true; + } + } - _reset_counters.lat_lon = sample.lat_lon_reset_counter; + return false; +} - } else { - ekf.disableControlStatusAuxGpos(); - _state = State::kStopped; - } - } +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); - } else { - ekf.disableControlStatusAuxGpos(); - _state = State::kStopped; - } + int32_t value = 0; - break; + if (param_get(param_find(param_name), &value) != 0) { + PX4_ERR("failed to get %s", param_name); + } - default: - break; - } + return value; +} -#if defined(MODULE_NAME) - aid_src.timestamp = hrt_absolute_time(); - _estimator_aid_src_aux_global_position_pub.publish(aid_src); +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); - _test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1])); -#endif // MODULE_NAME + return param_set_no_notification(param_find(param_name), &value) == PX4_OK; +} - } 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"); - } +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; } -bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf) const +int AuxGlobalPosition::mapSensorIdToSlot(int32_t sensor_id) { - 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 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 +#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 8777be409abd..4b30f5d9d75a 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 @@ class AuxGlobalPosition : public ModuleParams 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}; + + int32_t getAgpParamInt32(const char *param_suffix, int instance) const; + bool setAgpParamInt32(const char *param_suffix, int instance, int32_t value); + + int32_t _id_param_values[MAX_AGP_IDS] {}; - bool isResetAllowed(const Ekf &ekf) const; - - 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{}; - }; - - 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 000000000000..3a790cd8f03a --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2023-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. + * + ****************************************************************************/ + +#include "ekf.h" +#include + +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) + +AgpSource::AgpSource(int slot) + : _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) { + _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); + _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; + } + } + + } else { + _state = State::kStopped; + } + + 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; + 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 000000000000..e601b86e32fe --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * 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 AgpSource +{ +public: + AgpSource(int slot); + ~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{}; + 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/control.cpp b/src/modules/ekf2/EKF/control.cpp index 95ee21c01468..caf3e0108a96 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -118,6 +118,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) _aux_global_position.update(*this, imu_delayed); + _control_status.flags.aux_gpos = _aux_global_position.anySourceFusing(); #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION #if defined(CONFIG_EKF2_AIRSPEED) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index ac04d5e9c3e9..5ba1b5d3cc96 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 80ef94b9fc6d..ef70ff3613d7 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -435,6 +435,7 @@ class Ekf final : public EstimatorInterface 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 d568487da65b..f5b8703c0de1 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 diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index f7f6e559c9d7..55148e2fd536 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -303,9 +303,6 @@ class EstimatorInterface const filter_control_status_u &control_status_prev() const { return _control_status_prev; } const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; } - void enableControlStatusAuxGpos() { _control_status.flags.aux_gpos = true; } - void disableControlStatusAuxGpos() { _control_status.flags.aux_gpos = false; } - // get EKF internal fault status const fault_status_u &fault_status() const { return _fault_status; } const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; } diff --git a/src/modules/ekf2/params_aux_global_position.yaml b/src/modules/ekf2/params_aux_global_position.yaml index 93c71652268a..fc8cff63c32d 100644 --- a/src/modules/ekf2/params_aux_global_position.yaml +++ b/src/modules/ekf2/params_aux_global_position.yaml @@ -1,10 +1,22 @@ +__max_num_agp_instances: &max_num_agp_instances 4 + module_name: ekf2 parameters: - group: EKF2 definitions: - EKF2_AGP_CTRL: + EKF2_AGP${i}_ID: description: - short: Aux global position (AGP) sensor aiding + short: Auxiliary global position sensor ${i} ID + long: 'Sensor ID for slot ${i}. Set to 0 to disable this slot.' + type: int32 + default: 0 + min: 0 + max: 255 + num_instances: *max_num_agp_instances + instance_start: 0 + EKF2_AGP${i}_CTRL: + description: + short: Auxiliary global position sensor ${i} aiding long: 'Set bits in the following positions to enable: 0 : Horizontal position fusion 1 : Vertical position fusion' type: bitmask @@ -14,9 +26,11 @@ parameters: default: 0 min: 0 max: 3 - EKF2_AGP_MODE: + num_instances: *max_num_agp_instances + instance_start: 0 + EKF2_AGP${i}_MODE: description: - short: Fusion reset mode + short: Fusion reset mode for sensor ${i} long: 'Automatic: reset on fusion timeout if no other source of position is available Dead-reckoning: reset on fusion timeout if no source of velocity is available' type: enum @@ -24,28 +38,34 @@ parameters: 0: Automatic 1: Dead-reckoning default: 0 - EKF2_AGP_DELAY: + num_instances: *max_num_agp_instances + instance_start: 0 + EKF2_AGP${i}_DELAY: description: - short: Aux global position estimator delay relative to IMU measurements + short: Auxiliary global position sensor ${i} delay (to IMU) type: float default: 0 min: 0 - max: 300 + max: 1000 unit: ms reboot_required: true decimal: 1 - EKF2_AGP_NOISE: + num_instances: *max_num_agp_instances + instance_start: 0 + EKF2_AGP${i}_NOISE: description: - short: Measurement noise for aux global position measurements + short: Measurement noise for auxiliary global position sensor ${i} long: Used to lower bound or replace the uncertainty included in the message type: float - default: 0.9 + default: 1.0 min: 0.01 unit: m decimal: 2 - EKF2_AGP_GATE: + num_instances: *max_num_agp_instances + instance_start: 0 + EKF2_AGP${i}_GATE: description: - short: Gate size for aux global position fusion + short: Gate size for auxiliary global position sensor ${i} fusion long: Sets the number of standard deviations used by the innovation consistency test. type: float @@ -53,3 +73,5 @@ parameters: min: 1.0 unit: SD decimal: 1 + num_instances: *max_num_agp_instances + instance_start: 0 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 5e366273d373..2f28932af7b9 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -210,7 +210,7 @@ void LoggedTopics::add_default_topics() add_topic_multi("vehicle_imu_status", 1000, 4); add_optional_topic_multi("vehicle_magnetometer", 500, 4); add_topic("vehicle_optical_flow", 500); - add_topic("aux_global_position", 500); + add_topic_multi("aux_global_position", 500); add_optional_topic("pps_capture"); // additional control allocation logging @@ -319,7 +319,7 @@ void LoggedTopics::add_estimator_replay_topics() add_topic("vehicle_magnetometer"); add_topic("vehicle_status"); add_topic("vehicle_visual_odometry"); - add_topic("aux_global_position"); + add_topic_multi("aux_global_position"); add_topic_multi("distance_sensor"); } diff --git a/src/modules/mavlink/streams/GLOBAL_POSITION.hpp b/src/modules/mavlink/streams/GLOBAL_POSITION.hpp index 198d8b26e8e6..ed8a43cfdf61 100644 --- a/src/modules/mavlink/streams/GLOBAL_POSITION.hpp +++ b/src/modules/mavlink/streams/GLOBAL_POSITION.hpp @@ -36,7 +36,7 @@ #include -#include +#include class MavlinkStreamGLobalPosition : public MavlinkStream { diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 3d3eab6bd718..fd334c24e819 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -54,6 +54,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 70241950fcd9..d50ecab63223 100644 --- a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp +++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp @@ -114,7 +114,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)) { @@ -131,17 +131,17 @@ 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 = 123; + sample.source = aux_global_position_s::SOURCE_VISION; 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 cc9cae13303b..8f92182bfde0 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; @@ -85,7 +86,7 @@ class SensorAgpSim : public ModuleBase, public ModuleParams, public px4::Schedul 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")}; diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index dc6cc1f2cfcf..19270aa623cf 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -52,7 +52,7 @@ publications: - topic: /fmu/out/transponder_report type: px4_msgs::msg::TransponderReport - + # - topic: /fmu/out/vehicle_angular_velocity # type: px4_msgs::msg::VehicleAngularVelocity @@ -191,7 +191,7 @@ subscriptions: type: px4_msgs::msg::ActuatorServos - topic: /fmu/in/aux_global_position - type: px4_msgs::msg::VehicleGlobalPosition + type: px4_msgs::msg::AuxGlobalPosition - topic: /fmu/in/fixed_wing_longitudinal_setpoint type: px4_msgs::msg::FixedWingLongitudinalSetpoint diff --git a/src/modules/zenoh/dds_topics.yaml b/src/modules/zenoh/dds_topics.yaml index 2c592d714b51..6f6f3dc00a33 100644 --- a/src/modules/zenoh/dds_topics.yaml +++ b/src/modules/zenoh/dds_topics.yaml @@ -149,7 +149,7 @@ subscriptions: type: px4_msgs::msg::ActuatorServos - topic: /fmu/in/aux_global_position - type: px4_msgs::msg::VehicleGlobalPosition + type: px4_msgs::msg::AuxGlobalPosition # Create uORB::PublicationMulti subscriptions_multi: diff --git a/test/ros_tests/config.json b/test/ros_tests/config.json index 071bceff20b0..43a464bac746 100644 --- a/test/ros_tests/config.json +++ b/test/ros_tests/config.json @@ -24,7 +24,8 @@ "test_filter": "GlobalPositionInterfaceTest.*", "timeout_min": 10, "env": { - "PX4_PARAM_EKF2_AGP_CTRL": 1 + "PX4_PARAM_EKF2_AGP0_ID": 1, + "PX4_PARAM_EKF2_AGP0_CTRL": 1 } } ]