From a61b7fefc086e094b2c50a54902333046eefb538 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Mon, 9 Mar 2020 13:23:38 +0000 Subject: [PATCH] mavlink: timesync: readd timesync_status uORB to report Mavlink timesync --- msg/templates/urtps/RtpsTopics.cpp.em | 2 +- msg/timesync.msg | 15 +++++---------- msg/timesync_status.msg | 5 +++++ msg/tools/uorb_rtps_message_ids.yaml | 2 ++ src/modules/mavlink/mavlink_timesync.cpp | 4 ++-- src/modules/mavlink/mavlink_timesync.h | 4 ++-- 6 files changed, 17 insertions(+), 15 deletions(-) create mode 100644 msg/timesync_status.msg diff --git a/msg/templates/urtps/RtpsTopics.cpp.em b/msg/templates/urtps/RtpsTopics.cpp.em index d643bcd6a03a..afd19513818f 100644 --- a/msg/templates/urtps/RtpsTopics.cpp.em +++ b/msg/templates/urtps/RtpsTopics.cpp.em @@ -169,10 +169,10 @@ bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr) // apply timestamp offset _timesync->addOffset(msg.timestamp()); msg.serialize(scdr); + ret = true; @[ if topic == 'Timesync' or topic == 'timesync']@ } @[ end if]@ - ret = true; _@(topic)_sub.unlockMsg(); } break; diff --git a/msg/timesync.msg b/msg/timesync.msg index 1104bd287cdb..45c92f27e7b6 100644 --- a/msg/timesync.msg +++ b/msg/timesync.msg @@ -1,10 +1,5 @@ -uint64 timestamp # time since system start (microseconds) -uint8 sys_id # id of the origin system -uint8 seq # timesync msg sequence -int64 tc1 # time sync timestamp 1 -int64 ts1 # time sync timestamp 2 - -uint64 remote_timestamp # remote system timestamp (microseconds) -int64 observed_offset # raw time offset directly observed from this timesync packet (microseconds) -int64 estimated_offset # smoothed time offset between companion system and PX4 (microseconds) -uint32 round_trip_time # round trip time of this timesync packet (microseconds) +uint64 timestamp # time since system start (microseconds) +uint8 sys_id # id of the origin system +uint8 seq # timesync msg sequence +int64 tc1 # time sync timestamp 1 +int64 ts1 # time sync timestamp 2 diff --git a/msg/timesync_status.msg b/msg/timesync_status.msg new file mode 100644 index 000000000000..38ff31db7542 --- /dev/null +++ b/msg/timesync_status.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) +uint64 remote_timestamp # remote system timestamp (microseconds) +int64 observed_offset # raw time offset directly observed from this timesync packet (microseconds) +int64 estimated_offset # smoothed time offset between companion system and PX4 (microseconds) +uint32 round_trip_time # round trip time of this timesync packet (microseconds) diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index f8f4197c92b3..ea45f437cf38 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -291,6 +291,8 @@ rtps: id: 129 - msg: vehicle_trajectory_bezier id: 130 + - msg: timesync_status + id: 131 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/src/modules/mavlink/mavlink_timesync.cpp b/src/modules/mavlink/mavlink_timesync.cpp index 501726c6c6f5..48714ec16f4b 100644 --- a/src/modules/mavlink/mavlink_timesync.cpp +++ b/src/modules/mavlink/mavlink_timesync.cpp @@ -138,7 +138,7 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg) } // Publish status message - timesync_s tsync_status{}; + timesync_status_s tsync_status{}; tsync_status.timestamp = hrt_absolute_time(); tsync_status.remote_timestamp = tsync.tc1 / 1000ULL; @@ -146,7 +146,7 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg) tsync_status.estimated_offset = (int64_t)_time_offset; tsync_status.round_trip_time = rtt_us; - _timesync_pub.publish(tsync_status); + _timesync_status_pub.publish(tsync_status); } break; diff --git a/src/modules/mavlink/mavlink_timesync.h b/src/modules/mavlink/mavlink_timesync.h index bdb0c11ee4af..3204696f4260 100644 --- a/src/modules/mavlink/mavlink_timesync.h +++ b/src/modules/mavlink/mavlink_timesync.h @@ -43,7 +43,7 @@ #include "mavlink_bridge_header.h" #include -#include +#include #include @@ -132,7 +132,7 @@ class MavlinkTimesync */ void reset_filter(); - uORB::PublicationMulti _timesync_pub{ORB_ID(timesync)}; + uORB::PublicationMulti _timesync_status_pub{ORB_ID(timesync_status)}; uint32_t _sequence{0};