Skip to content

Commit 9e796da

Browse files
committed
add translation for aux_global_position
1 parent 17242bc commit 9e796da

File tree

4 files changed

+96
-1
lines changed

4 files changed

+96
-1
lines changed
Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
# Fused global position in WGS84.
2+
# This struct contains global position estimation. It is not the raw GPS
3+
# measurement (@see vehicle_gps_position). This topic is usually published by the position
4+
# estimator, which will take more sources of information into account than just GPS,
5+
# e.g. control inputs of the vehicle in a Kalman-filter implementation.
6+
#
7+
8+
uint32 MESSAGE_VERSION = 0
9+
10+
uint64 timestamp # time since system start (microseconds)
11+
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
12+
13+
float64 lat # Latitude, (degrees)
14+
float64 lon # Longitude, (degrees)
15+
float32 alt # Altitude AMSL, (meters)
16+
float32 alt_ellipsoid # Altitude above ellipsoid, (meters)
17+
18+
bool lat_lon_valid
19+
bool alt_valid
20+
21+
float32 delta_alt # Reset delta for altitude
22+
float32 delta_terrain # Reset delta for terrain
23+
uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates
24+
uint8 alt_reset_counter # Counter for reset events on altitude
25+
uint8 terrain_reset_counter # Counter for reset events on terrain
26+
27+
float32 eph # Standard deviation of horizontal position error, (metres)
28+
float32 epv # Standard deviation of vertical position error, (metres)
29+
30+
float32 terrain_alt # Terrain altitude WGS84, (metres)
31+
bool terrain_alt_valid # Terrain altitude estimate is valid
32+
33+
bool dead_reckoning # True if this position is estimated through dead-reckoning
34+
35+
# TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position
36+
# TOPICS estimator_global_position

msg/translation_node/translations/all_translations.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
#include <translation_util.h>
88

99
#include "translation_airspeed_validated_v1.h"
10+
#include "translation_aux_global_position_v1.h"
1011
#include "translation_arming_check_reply_v1.h"
1112
#include "translation_arming_check_request_v1.h"
1213
#include "translation_battery_status_v1.h"
Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
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);

msg/versioned/AuxGlobalPosition.msg

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
# This message provides global position data from an external source such as
44
# pseudolites, visual navigation, or other positioning system.
55

6-
uint32 MESSAGE_VERSION = 0
6+
uint32 MESSAGE_VERSION = 1
77

88
uint64 timestamp # [us] Time since system start
99
uint64 timestamp_sample # [us] Timestamp of the raw data

0 commit comments

Comments
 (0)