|
| 1 | +/**************************************************************************** |
| 2 | + * Copyright (c) 2026 PX4 Development Team. |
| 3 | + * SPDX-License-Identifier: BSD-3-Clause |
| 4 | + ****************************************************************************/ |
| 5 | +#pragma once |
| 6 | + |
| 7 | +#include <cmath> |
| 8 | + |
| 9 | +// Translate aux_global_position: VehicleGlobalPosition v0 <--> AuxGlobalPosition v1 |
| 10 | +#include <px4_msgs_old/msg/vehicle_global_position_v0.hpp> |
| 11 | +#include <px4_msgs/msg/aux_global_position.hpp> |
| 12 | + |
| 13 | +class AuxGlobalPositionV1Translation { |
| 14 | +public: |
| 15 | + using MessageOlder = px4_msgs_old::msg::VehicleGlobalPositionV0; |
| 16 | + static_assert(MessageOlder::MESSAGE_VERSION == 0); |
| 17 | + |
| 18 | + using MessageNewer = px4_msgs::msg::AuxGlobalPosition; |
| 19 | + static_assert(MessageNewer::MESSAGE_VERSION == 1); |
| 20 | + |
| 21 | + static constexpr const char* kTopic = "fmu/in/aux_global_position"; |
| 22 | + |
| 23 | + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { |
| 24 | + msg_newer.timestamp = msg_older.timestamp; |
| 25 | + msg_newer.timestamp_sample = msg_older.timestamp_sample; |
| 26 | + msg_newer.id = 1; |
| 27 | + msg_newer.source = MessageNewer::SOURCE_VISION; |
| 28 | + msg_newer.lat = msg_older.lat; |
| 29 | + msg_newer.lon = msg_older.lon; |
| 30 | + msg_newer.alt = msg_older.alt_valid ? msg_older.alt : NAN; |
| 31 | + msg_newer.eph = msg_older.eph; |
| 32 | + msg_newer.epv = msg_older.epv; |
| 33 | + msg_newer.lat_lon_reset_counter = msg_older.lat_lon_reset_counter; |
| 34 | + } |
| 35 | + |
| 36 | + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { |
| 37 | + msg_older.timestamp = msg_newer.timestamp; |
| 38 | + msg_older.timestamp_sample = msg_newer.timestamp_sample; |
| 39 | + msg_older.lat = msg_newer.lat; |
| 40 | + msg_older.lon = msg_newer.lon; |
| 41 | + msg_older.alt = std::isnan(msg_newer.alt) ? 0.0f : msg_newer.alt; |
| 42 | + msg_older.alt_ellipsoid = 0.0f; |
| 43 | + msg_older.lat_lon_valid = true; |
| 44 | + msg_older.alt_valid = !std::isnan(msg_newer.alt); |
| 45 | + msg_older.delta_alt = 0.0f; |
| 46 | + msg_older.delta_terrain = 0.0f; |
| 47 | + msg_older.lat_lon_reset_counter = msg_newer.lat_lon_reset_counter; |
| 48 | + msg_older.alt_reset_counter = 0; |
| 49 | + msg_older.terrain_reset_counter = 0; |
| 50 | + msg_older.eph = std::isnan(msg_newer.eph) ? 0.0f : msg_newer.eph; |
| 51 | + msg_older.epv = std::isnan(msg_newer.epv) ? 0.0f : msg_newer.epv; |
| 52 | + msg_older.terrain_alt = 0.0f; |
| 53 | + msg_older.terrain_alt_valid = false; |
| 54 | + msg_older.dead_reckoning = false; |
| 55 | + } |
| 56 | +}; |
| 57 | + |
| 58 | +REGISTER_TOPIC_TRANSLATION_DIRECT(AuxGlobalPositionV1Translation); |
0 commit comments