mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
add translation for aux_global_position
This commit is contained in:
parent
17242bc1a4
commit
9e796daee8
36
msg/px4_msgs_old/msg/VehicleGlobalPositionV0.msg
Normal file
36
msg/px4_msgs_old/msg/VehicleGlobalPositionV0.msg
Normal file
@ -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
|
||||||
@ -7,6 +7,7 @@
|
|||||||
#include <translation_util.h>
|
#include <translation_util.h>
|
||||||
|
|
||||||
#include "translation_airspeed_validated_v1.h"
|
#include "translation_airspeed_validated_v1.h"
|
||||||
|
#include "translation_aux_global_position_v1.h"
|
||||||
#include "translation_arming_check_reply_v1.h"
|
#include "translation_arming_check_reply_v1.h"
|
||||||
#include "translation_arming_check_request_v1.h"
|
#include "translation_arming_check_request_v1.h"
|
||||||
#include "translation_battery_status_v1.h"
|
#include "translation_battery_status_v1.h"
|
||||||
|
|||||||
@ -0,0 +1,58 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
* Copyright (c) 2026 PX4 Development Team.
|
||||||
|
* SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
****************************************************************************/
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
// Translate aux_global_position: VehicleGlobalPosition v0 <--> AuxGlobalPosition v1
|
||||||
|
#include <px4_msgs_old/msg/vehicle_global_position_v0.hpp>
|
||||||
|
#include <px4_msgs/msg/aux_global_position.hpp>
|
||||||
|
|
||||||
|
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);
|
||||||
@ -3,7 +3,7 @@
|
|||||||
# This message provides global position data from an external source such as
|
# This message provides global position data from an external source such as
|
||||||
# pseudolites, visual navigation, or other positioning system.
|
# pseudolites, visual navigation, or other positioning system.
|
||||||
|
|
||||||
uint32 MESSAGE_VERSION = 0
|
uint32 MESSAGE_VERSION = 1
|
||||||
|
|
||||||
uint64 timestamp # [us] Time since system start
|
uint64 timestamp # [us] Time since system start
|
||||||
uint64 timestamp_sample # [us] Timestamp of the raw data
|
uint64 timestamp_sample # [us] Timestamp of the raw data
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user