From 9e796daee8d03a90d3cf88178f35195738bd552c Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Tue, 17 Feb 2026 11:30:55 +0100 Subject: [PATCH] add translation for aux_global_position --- .../msg/VehicleGlobalPositionV0.msg | 36 ++++++++++++ .../translations/all_translations.h | 1 + .../translation_aux_global_position_v1.h | 58 +++++++++++++++++++ msg/versioned/AuxGlobalPosition.msg | 2 +- 4 files changed, 96 insertions(+), 1 deletion(-) create mode 100644 msg/px4_msgs_old/msg/VehicleGlobalPositionV0.msg create mode 100644 msg/translation_node/translations/translation_aux_global_position_v1.h diff --git a/msg/px4_msgs_old/msg/VehicleGlobalPositionV0.msg b/msg/px4_msgs_old/msg/VehicleGlobalPositionV0.msg new file mode 100644 index 0000000000..eae8a489a4 --- /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 78d30fc7fe..31342547f9 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 0000000000..153f89b395 --- /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 index b8e6f1f9ad..d5716ba4a1 100644 --- a/msg/versioned/AuxGlobalPosition.msg +++ b/msg/versioned/AuxGlobalPosition.msg @@ -3,7 +3,7 @@ # This message provides global position data from an external source such as # pseudolites, visual navigation, or other positioning system. -uint32 MESSAGE_VERSION = 0 +uint32 MESSAGE_VERSION = 1 uint64 timestamp # [us] Time since system start uint64 timestamp_sample # [us] Timestamp of the raw data