dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

microRTPS: timesync: add getters and setter to generalize interface

This commit is contained in:
TSC21 2020-03-11 13:23:10 +00:00 committed by Nuno Marques
parent 7612879ffd
commit 99f96437c3
4 changed files with 85 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/templates/urtps/RtpsTopics.cpp.em
View File

@ -110,6 +110,7 @@ void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len)
// apply timestamp offset
uint64_t timestamp = getMsgTimestamp(&st);
_timesync->subtractOffset(timestamp);
setMsgTimestamp(&st, timestamp);
_@(topic)_pub.publish(&st);
@[ if topic == 'Timesync' or topic == 'timesync']@
}
@ -141,6 +142,7 @@ bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr)
// apply timestamp offset
uint64_t timestamp = getMsgTimestamp(&msg);
_timesync->addOffset(timestamp);
setMsgTimestamp(&msg, timestamp);
msg.serialize(scdr);
ret = true;
@[ if topic == 'Timesync' or topic == 'timesync']@

45
msg/templates/urtps/RtpsTopics.h.em
View File

@ -118,32 +118,65 @@ public:
private:
@[if send_topics]@
// Publishers
/** Publishers **/
@[for topic in send_topics]@
@(topic)_Publisher _@(topic)_pub;
@[end for]@
@[end if]@
@[if recv_topics]@
// Subscribers
/** Subscribers **/
@[for topic in recv_topics]@
@(topic)_Subscriber _@(topic)_sub;
@[end for]@
@[end if]@
/** Msg metada Getters **/
@[if fastrtps_version <= 1.7 or not ros2_distro]@
template <class T>
uint8_t getMsgSysID(T* msg) { return msg->sys_id_(); }
inline uint64_t getMsgTimestamp(const T* msg) { return msg->timestamp_(); }
template <class T>
uint64_t getMsgTimestamp(T* msg) { return msg->timestamp_(); }
inline uint8_t getMsgSysID(const T* msg) { return msg->sys_id_(); }
template <class T>
inline uint8_t getMsgSeq(const T* msg) { return msg->seq_(); }
@[elif ros2_distro]@
template <class T>
uint8_t getMsgSysID(T* msg) { return msg->sys_id(); }
inline uint64_t getMsgTimestamp(const T* msg) { return msg->timestamp(); }
template <class T>
uint64_t getMsgTimestamp(T* msg) { return msg->timestamp(); }
inline uint8_t getMsgSysID(const T* msg) { return msg->sys_id(); }
template <class T>
inline uint8_t getMsgSeq(const T* msg) { return msg->seq(); }
@[end if]@
/** Msg metadata Getters **/
@[if fastrtps_version <= 1.7 or not ros2_distro]@
template <class T>
inline uint64_t setMsgTimestamp(T* msg, const uint64_t& timestamp) { msg->timestamp_() = timestamp; }
template <class T>
inline uint8_t setMsgSysID(T* msg, const uint8_t& sys_id) { msg->sys_id_() = sys_id; }
template <class T>
inline uint8_t setMsgSeq(T* msg, const uint8_t& seq) { msg->seq_() = seq; }
@[elif ros2_distro]@
template <class T>
inline uint64_t setMsgTimestamp(T* msg, const uint64_t& timestamp) { msg->timestamp() = timestamp; }
template <class T>
inline uint8_t setMsgSysID(T* msg, const uint8_t& sys_id) { msg->sys_id() = sys_id; }
template <class T>
inline uint8_t setMsgSeq(T* msg, const uint8_t& seq) { msg->seq() = seq; }
@[end if]@
/**
* @@brief Timesync object ptr.
* This object is used to compuyte and apply the time offsets to the
* messages timestamps.
*/
std::shared_ptr<TimeSync> _timesync;
};

28
msg/templates/urtps/microRTPS_timesync.cpp.em
View File

@ -163,19 +163,19 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
}
void TimeSync::processTimesyncMsg(timesync_msg_t * msg) {
if (msg->sys_id() == 1 && msg->seq() != _last_remote_msg_seq) {
_last_remote_msg_seq = msg->seq();
if (getMsgSysID(msg) == 1 && getMsgSeq(msg) != _last_remote_msg_seq) {
_last_remote_msg_seq = getMsgSeq(msg);
if (msg->tc1() > 0) {
if (!addMeasurement(msg->ts1(), msg->tc1(), getMonoRawTimeNSec())) {
if (getMsgTC1(msg) > 0) {
if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getMonoRawTimeNSec())) {
std::cerr << "Offset not updated" << std::endl;
}
} else if (msg->tc1() == 0) {
msg->timestamp() = getMonoTimeUSec();
msg->sys_id() = 0;
msg->seq()++;
msg->tc1() = getMonoRawTimeNSec();
} else if (getMsgTC1(msg) == 0) {
setMsgTimestamp(msg, getMonoTimeUSec());
setMsgSysID(msg, 0);
setMsgSeq(msg, getMsgSeq(msg) + 1);
setMsgTC1(msg, getMonoRawTimeNSec());
_timesync_pub.publish(msg);
}
@ -185,11 +185,11 @@ void TimeSync::processTimesyncMsg(timesync_msg_t * msg) {
timesync_msg_t TimeSync::newTimesyncMsg() {
timesync_msg_t msg{};
msg.timestamp() = getMonoTimeUSec();
msg.sys_id() = 0;
msg.seq() = _last_msg_seq;
msg.tc1() = 0;
msg.ts1() = getMonoRawTimeNSec();
setMsgTimestamp(&msg, getMonoTimeUSec());
setMsgSysID(&msg, 0);
setMsgSeq(&msg, _last_msg_seq);
setMsgTC1(&msg, 0);
setMsgTS1(&msg, getMonoRawTimeNSec());
_last_msg_seq++;

30
msg/templates/urtps/microRTPS_timesync.h.em
View File

@ -211,4 +211,34 @@ private:
* @@param[in] offset The value of the offset to update to
*/
inline void updateOffset(const uint64_t& offset) { _offset_ns.store(offset, std::memory_order_relaxed); }
/** Timesync msg Getters **/
@[if fastrtps_version <= 1.7 or not ros2_distro]@
inline uint64_t getMsgTimestamp(const timesync_msg_t* msg) { return msg->timestamp_(); }
inline uint8_t getMsgSysID(const timesync_msg_t* msg) { return msg->sys_id_(); }
inline uint8_t getMsgSeq(const timesync_msg_t* msg) { return msg->seq_(); }
inline int64_t getMsgTC1(const timesync_msg_t* msg) { return msg->tc1_(); }
inline int64_t getMsgTS1(const timesync_msg_t* msg) { return msg->ts1_(); }
@[elif ros2_distro]@
inline uint64_t getMsgTimestamp(const timesync_msg_t* msg) { return msg->timestamp(); }
inline uint8_t getMsgSysID(const timesync_msg_t* msg) { return msg->sys_id(); }
inline uint8_t getMsgSeq(const timesync_msg_t* msg) { return msg->seq(); }
inline int64_t getMsgTC1(const timesync_msg_t* msg) { return msg->tc1(); }
inline int64_t getMsgTS1(const timesync_msg_t* msg) { return msg->ts1(); }
@[end if]@
/** Timesync msg Setters **/
@[if fastrtps_version <= 1.7 or not ros2_distro]@
inline uint64_t setMsgTimestamp(timesync_msg_t* msg, const uint64_t& timestamp) { msg->timestamp_() = timestamp; }
inline uint8_t setMsgSysID(timesync_msg_t* msg, const uint8_t& sys_id) { msg->sys_id_() = sys_id; }
inline uint8_t setMsgSeq(timesync_msg_t* msg, const uint8_t& seq) { msg->seq_() = seq; }
inline int64_t setMsgTC1(timesync_msg_t* msg, const int64_t& tc1) { msg->tc1_() = tc1; }
inline int64_t setMsgTS1(timesync_msg_t* msg, const int64_t& ts1) { msg->ts1_() = ts1; }
@[elif ros2_distro]@
inline uint64_t setMsgTimestamp(timesync_msg_t* msg, const uint64_t& timestamp) { msg->timestamp() = timestamp; }
inline uint8_t setMsgSysID(timesync_msg_t* msg, const uint8_t& sys_id) { msg->sys_id() = sys_id; }
inline uint8_t setMsgSeq(timesync_msg_t* msg, const uint8_t& seq) { msg->seq() = seq; }
inline int64_t setMsgTC1(timesync_msg_t* msg, const int64_t& tc1) { msg->tc1() = tc1; }
inline int64_t setMsgTS1(timesync_msg_t* msg, const int64_t& ts1) { msg->ts1() = ts1; }
@[end if]@
};