diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 06feb9a335..fe88726d5f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -177,6 +177,7 @@ set(msg_files QshellReq.msg QshellRetval.msg RadioStatus.msg + RangingBeacon.msg RateCtrlStatus.msg RcChannels.msg RcParameterMap.msg diff --git a/msg/RangingBeacon.msg b/msg/RangingBeacon.msg new file mode 100644 index 0000000000..2ee822d8f6 --- /dev/null +++ b/msg/RangingBeacon.msg @@ -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 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 091dc41171..e3044c1ee7 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -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"); } diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 51a47ee9ae..1cdd610464 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 51a47ee9aed1216532f2eefbfe8d260a2e3cabcc +Subproject commit 1cdd61046402743e518326e51c4e3ad989297651 diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 151a091ef2..afab4987fc 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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(beacon_pos.range) * 1e-3f : NAN; + ranging_beacon.lat = static_cast(beacon_pos.lat) * 1e-7; + ranging_beacon.lon = static_cast(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(beacon_pos.hacc_est) * 1e-3f : NAN; + ranging_beacon.vacc = (beacon_pos.vacc_est != UINT32_MAX) ? static_cast(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(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) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index da10db0c69..fc399ff223 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -116,6 +116,7 @@ #include #include #include +#include #if !defined(CONSTRAINED_FLASH) # include @@ -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 _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _ranging_beacon_pub{ORB_ID(ranging_beacon)}; #if defined(MAVLINK_MSG_ID_ESC_EEPROM) uORB::Publication _esc_eeprom_write_pub {ORB_ID(esc_eeprom_write)};