From 8a9be9a8f0a626942f7ac019678a3b03e4f58875 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 12 Feb 2026 16:20:23 +0100 Subject: [PATCH] ekf2: update logger, mavlink, DDS, and replay for AuxGlobalPosition Update integrations to use the new AuxGlobalPosition message instead of the VehicleGlobalPosition-based aux_global_position topic. --- .../aux_global_position.cpp | 2 +- .../aux_global_position_control.cpp | 23 +++---------------- .../aux_global_position_control.hpp | 5 +--- src/modules/ekf2/EKF/control.cpp | 1 + src/modules/ekf2/EKF/estimator_interface.h | 3 --- src/modules/logger/logged_topics.cpp | 4 ++-- .../mavlink/streams/GLOBAL_POSITION.hpp | 2 +- src/modules/replay/ReplayEkf2.cpp | 1 + src/modules/uxrce_dds_client/dds_topics.yaml | 4 ++-- src/modules/zenoh/dds_topics.yaml | 2 +- 10 files changed, 13 insertions(+), 34 deletions(-) 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 dca1bf8536..ec3525c091 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 @@ -43,7 +43,7 @@ AuxGlobalPosition::AuxGlobalPosition() : ModuleParams(nullptr) _id_param_values[slot] = getAgpParamInt32("ID", slot); if (_id_param_values[slot] != 0) { - _sources[slot] = new AgpSource(slot, this); + _sources[slot] = new AgpSource(slot); _n_sources++; } } diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp index 6ebce26a76..3a790cd8f0 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * 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 @@ -33,13 +33,11 @@ #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) +AgpSource::AgpSource(int slot) + : _slot(slot) { initParams(); advertise(); @@ -148,7 +146,6 @@ bool AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed) } if (fused || reset) { - ekf.enableControlStatusAuxGpos(); _reset_counters.lat_lon = sample.lat_lon_reset_counter; _state = State::kActive; } @@ -158,7 +155,6 @@ bool AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed) 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; } @@ -183,19 +179,11 @@ bool AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed) } else { _state = State::kStopped; - - if (!_manager->anySourceFusing()) { - ekf.disableControlStatusAuxGpos(); - } } } } else { _state = State::kStopped; - - if (!_manager->anySourceFusing()) { - ekf.disableControlStatusAuxGpos(); - } } break; @@ -213,11 +201,6 @@ bool AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed) } 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); } diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp index ad8e332170..e601b86e32 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp @@ -44,12 +44,11 @@ #include class Ekf; -class AuxGlobalPosition; class AgpSource { public: - AgpSource(int slot, AuxGlobalPosition *manager); + AgpSource(int slot); ~AgpSource() = default; void bufferData(const aux_global_position_s &msg, const estimator::imuSample &imu_delayed); @@ -101,8 +100,6 @@ private: float _test_ratio_filtered{0.f}; uint64_t _time_last_buffer_push{0}; reset_counters_s _reset_counters{}; - - AuxGlobalPosition *_manager; int _slot; struct ParamHandles { diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 95ee21c014..caf3e0108a 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/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index f7f6e559c9..55148e2fd5 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -303,9 +303,6 @@ public: 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/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 5e366273d3..2f28932af7 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 198d8b26e8..ed8a43cfdf 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 3d3eab6bd7..fd334c24e8 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/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index dc6cc1f2cf..19270aa623 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 2c592d714b..6f6f3dc00a 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: