fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(mavlink): add ranging beacon parser and uORB message

Browse Source
This commit is contained in:
Marco Hauswirth 2026-03-31 20:43:04 +02:00 committed by Marco Hauswirth
parent 61f08771a7
commit c260794122
6 changed files with 69 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -177,6 +177,7 @@ set(msg_files
QshellReq.msg
QshellRetval.msg
RadioStatus.msg
RangingBeacon.msg
RateCtrlStatus.msg
RcChannels.msg
RcParameterMap.msg

24
msg/RangingBeacon.msg Normal file
View File

@ -0,0 +1,24 @@
# Ranging beacon measurement data (e.g. LoRa, UWB)
uint64 timestamp # [us] time since system start
uint64 timestamp_sample # [us] the timestamp of the raw data
uint8 beacon_id
float32 range # [m] Range measurement
float64 lat # [deg] Latitude
float64 lon # [deg] Longitude
float32 alt # [m] Beacon altitude (frame defined in alt_type)
uint8 alt_type # [@enum ALT_TYPE] Altitude frame for alt field
uint8 ALT_TYPE_WGS84 = 0 # Altitude above WGS84 ellipsoid
uint8 ALT_TYPE_MSL = 1 # Altitude above Mean Sea Level (AMSL)
float32 hacc # [m] Groundbeacon horizontal accuracy
float32 vacc # [m] Groundbeacon vertical accuracy
uint8 sequence_nr
uint8 status
uint16 carrier_freq # [MHz] Carrier frequency
float32 range_accuracy # [m] Range accuracy estimate
# TOPICS ranging_beacon

2
src/modules/logger/logged_topics.cpp
View File

@ -160,6 +160,7 @@ void LoggedTopics::add_default_topics()
add_optional_topic("fixed_wing_lateral_guidance_status", 100);
add_optional_topic("fixed_wing_lateral_status", 100);
add_optional_topic("fixed_wing_runway_control", 100);
add_optional_topic("ranging_beacon", 100);
// multi topics
add_optional_topic_multi("actuator_outputs", 100, 3);
@ -319,6 +320,7 @@ void LoggedTopics::add_estimator_replay_topics()
add_topic("vehicle_magnetometer");
add_topic("vehicle_status");
add_topic("vehicle_visual_odometry");
add_topic("ranging_beacon");
add_topic_multi("aux_global_position");
add_topic_multi("distance_sensor");
}

2
src/modules/mavlink/mavlink

@ -1 +1 @@
Subproject commit 51a47ee9aed1216532f2eefbfe8d260a2e3cabcc
Subproject commit 1cdd61046402743e518326e51c4e3ad989297651

35
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -285,6 +285,13 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_open_drone_id_system(msg);
break;
#if defined(MAVLINK_MSG_ID_RANGING_BEACON)
case MAVLINK_MSG_ID_RANGING_BEACON:
handle_message_ranging_beacon(msg);
break;
#endif // MAVLINK_MSG_ID_RANGING_BEACON
#if !defined(CONSTRAINED_FLASH)
case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
@ -2574,6 +2581,34 @@ MavlinkReceiver::handle_message_global_position_sensor(mavlink_message_t *msg)
_aux_global_position_pub.publish(aux_global_position);
}
#if defined(MAVLINK_MSG_ID_RANGING_BEACON)
void
MavlinkReceiver::handle_message_ranging_beacon(mavlink_message_t *msg)
{
mavlink_ranging_beacon_t beacon_pos;
mavlink_msg_ranging_beacon_decode(msg, &beacon_pos);
ranging_beacon_s ranging_beacon{};
ranging_beacon.timestamp = hrt_absolute_time();
ranging_beacon.timestamp_sample = beacon_pos.time_usec;
ranging_beacon.beacon_id = beacon_pos.beacon_id;
ranging_beacon.range = (beacon_pos.range != UINT32_MAX) ? static_cast<float>(beacon_pos.range) * 1e-3f : NAN;
ranging_beacon.lat = static_cast<double>(beacon_pos.lat) * 1e-7;
ranging_beacon.lon = static_cast<double>(beacon_pos.lon) * 1e-7;
ranging_beacon.alt = beacon_pos.alt;
ranging_beacon.alt_type = beacon_pos.alt_type;
ranging_beacon.hacc = (beacon_pos.hacc_est != UINT32_MAX) ? static_cast<float>(beacon_pos.hacc_est) * 1e-3f : NAN;
ranging_beacon.vacc = (beacon_pos.vacc_est != UINT32_MAX) ? static_cast<float>(beacon_pos.vacc_est) * 1e-3f : NAN;
ranging_beacon.sequence_nr = beacon_pos.sequence;
ranging_beacon.status = beacon_pos.status;
ranging_beacon.carrier_freq = beacon_pos.carrier_freq;
ranging_beacon.range_accuracy = (beacon_pos.range_accuracy != UINT32_MAX)
? static_cast<float>(beacon_pos.range_accuracy) * 1e-3f : NAN;
_ranging_beacon_pub.publish(ranging_beacon);
}
#endif // MAVLINK_MSG_ID_RANGING_BEACON
void
MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
{

6
src/modules/mavlink/mavlink_receiver.h
View File

@ -116,6 +116,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/velocity_limits.h>
#include <uORB/topics/aux_global_position.h>
#include <uORB/topics/ranging_beacon.h>
#if !defined(CONSTRAINED_FLASH)
# include <uORB/topics/debug_array.h>
@ -217,6 +218,10 @@ private:
void handle_message_gimbal_device_information(mavlink_message_t *msg);
void handle_message_gimbal_device_attitude_status(mavlink_message_t *msg);
void handle_message_global_position_sensor(mavlink_message_t *msg);
#if defined(MAVLINK_MSG_ID_RANGING_BEACON)
void handle_message_ranging_beacon(mavlink_message_t *msg);
#endif // MAVLINK_MSG_ID_RANGING_BEACON
#if !defined(CONSTRAINED_FLASH)
void handle_message_debug(mavlink_message_t *msg);
void handle_message_debug_float_array(mavlink_message_t *msg);
@ -337,6 +342,7 @@ private:
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication<vehicle_rates_setpoint_s> _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<ranging_beacon_s> _ranging_beacon_pub{ORB_ID(ranging_beacon)};
#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
uORB::Publication<esc_eeprom_write_s> _esc_eeprom_write_pub {ORB_ID(esc_eeprom_write)};