Advanced Time Synchronisation

msg/CMakeLists.txt

@@ -103,7 +103,7 @@ set(msg_files
 	tecs_status.msg
 	telemetry_status.msg
 	test_motor.msg
-	time_offset.msg
+	timesync_status.msg
 	transponder_report.msg
 	tune_control.msg
 	uavcan_parameter_request.msg

msg/timesync_status.msg

@@ -0,0 +1,4 @@
+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)

src/modules/logger/logger.cpp

@@ -628,6 +628,7 @@ void Logger::add_default_topics()
 	add_topic("vehicle_vision_position");
 	add_topic("vtol_vehicle_status", 200);
 	add_topic("wind_estimate", 200);
+	add_topic("timesync_status");
 
 #ifdef CONFIG_ARCH_BOARD_SITL
 	add_topic("actuator_armed");

src/modules/mavlink/CMakeLists.txt

@@ -60,6 +60,7 @@ px4_add_module(
 		mavlink_simple_analyzer.cpp
 		mavlink_stream.cpp
 		mavlink_ulog.cpp
+		mavlink_timesync.cpp
 	DEPENDS
 		git_mavlink_v2
 		conversion

src/modules/mavlink/mavlink_receiver.cpp

@@ -96,6 +96,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_parameters_manager(parent),
 	_mavlink_ftp(parent),
 	_mavlink_log_handler(parent),
+	_mavlink_timesync(parent),
 	_status{},
 	_hil_local_pos{},
 	_hil_land_detector{},
@@ -128,7 +129,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_manual_pub(nullptr),
 	_obstacle_distance_pub(nullptr),
 	_land_detector_pub(nullptr),
-	_time_offset_pub(nullptr),
 	_follow_target_pub(nullptr),
 	_landing_target_pose_pub(nullptr),
 	_transponder_report_pub(nullptr),
@@ -147,8 +147,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_hil_local_alt0(0.0f),
 	_hil_local_proj_ref{},
 	_offboard_control_mode{},
-	_time_offset_avg_alpha(0.8),
-	_time_offset(0),
 	_orb_class_instance(-1),
 	_mom_switch_pos{},
 	_mom_switch_state(0),
@@ -285,14 +283,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_heartbeat(msg);
 		break;
 
-	case MAVLINK_MSG_ID_SYSTEM_TIME:
-		handle_message_system_time(msg);
-		break;
-
-	case MAVLINK_MSG_ID_TIMESYNC:
-		handle_message_timesync(msg);
-		break;
-
 	case MAVLINK_MSG_ID_DISTANCE_SENSOR:
 		handle_message_distance_sensor(msg);
 		break;
@@ -812,7 +802,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
 	// Use the component ID to identify the mocap system
 	att_pos_mocap.id = msg->compid;
 
-	att_pos_mocap.timestamp = sync_stamp(mocap.time_usec);
+	att_pos_mocap.timestamp = _mavlink_timesync.sync_stamp(mocap.time_usec);
 	att_pos_mocap.timestamp_received = hrt_absolute_time();
 
 	att_pos_mocap.q[0] = mocap.q[0];
@@ -1106,7 +1096,7 @@ MavlinkReceiver::handle_message_attitude_quaternion_cov(mavlink_message_t *msg)
 
 	struct vehicle_attitude_s vision_attitude = {};
 
-	vision_attitude.timestamp = sync_stamp(att.time_usec);
+	vision_attitude.timestamp = _mavlink_timesync.sync_stamp(att.time_usec);
 
 	vision_attitude.q[0] = att.q[0];
 	vision_attitude.q[1] = att.q[1];
@@ -1132,7 +1122,7 @@ MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg)
 
 	struct vehicle_local_position_s vision_position = {};
 
-	vision_position.timestamp = sync_stamp(pos.time_usec);
+	vision_position.timestamp = _mavlink_timesync.sync_stamp(pos.time_usec);
 
 	vision_position.xy_valid = true;
 	vision_position.z_valid = true;
@@ -1192,7 +1182,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 
 	struct vehicle_local_position_s vision_position = {};
 
-	vision_position.timestamp = sync_stamp(pos.usec);
+	vision_position.timestamp = _mavlink_timesync.sync_stamp(pos.usec);
 	vision_position.x = pos.x;
 	vision_position.y = pos.y;
 	vision_position.z = pos.z;
@@ -1204,7 +1194,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 
 	struct vehicle_attitude_s vision_attitude = {};
 
-	vision_attitude.timestamp = sync_stamp(pos.usec);
+	vision_attitude.timestamp = _mavlink_timesync.sync_stamp(pos.usec);
 
 	matrix::Quatf q(matrix::Eulerf(pos.roll, pos.pitch, pos.yaw));
 	q.copyTo(vision_attitude.q);
@@ -1793,80 +1783,6 @@ MavlinkReceiver::get_message_interval(int msgId)
 	mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval);
 }
 
-void
-MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
-{
-	mavlink_system_time_t time;
-	mavlink_msg_system_time_decode(msg, &time);
-
-	timespec tv = {};
-	px4_clock_gettime(CLOCK_REALTIME, &tv);
-
-	// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
-	bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS;
-	bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL;
-
-	if (!onb_unix_valid && ofb_unix_valid) {
-		tv.tv_sec = time.time_unix_usec / 1000000ULL;
-		tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL;
-
-		if (px4_clock_settime(CLOCK_REALTIME, &tv)) {
-			PX4_ERR("failed setting clock");
-		}
-	}
-
-}
-
-void
-MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
-{
-	mavlink_timesync_t tsync = {};
-	mavlink_msg_timesync_decode(msg, &tsync);
-
-	struct time_offset_s tsync_offset = {};
-
-	uint64_t now_ns = hrt_absolute_time() * 1000LL ;
-
-	if (tsync.tc1 == 0) {
-
-		mavlink_timesync_t rsync; // return timestamped sync message
-
-		rsync.tc1 = now_ns;
-		rsync.ts1 = tsync.ts1;
-
-		mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync);
-
-		return;
-
-	} else if (tsync.tc1 > 0) {
-
-		int64_t offset_ns = (int64_t)(tsync.ts1 + now_ns - tsync.tc1 * 2) / 2 ;
-		int64_t dt = _time_offset - offset_ns;
-
-		if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew
-			_time_offset = offset_ns;
-
-			// Provide a warning only if not syncing initially
-			if (_time_offset != 0) {
-				PX4_ERR("[timesync] Hard setting offset.");
-			}
-
-		} else {
-			smooth_time_offset(offset_ns);
-		}
-	}
-
-	tsync_offset.offset_ns = _time_offset ;
-
-	if (_time_offset_pub == nullptr) {
-		_time_offset_pub = orb_advertise(ORB_ID(time_offset), &tsync_offset);
-
-	} else {
-		orb_publish(ORB_ID(time_offset), _time_offset_pub, &tsync_offset);
-	}
-
-}
-
 void
 MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 {
@@ -2074,7 +1990,7 @@ void MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg)
 
 	if (landing_target.position_valid && landing_target.frame == MAV_FRAME_LOCAL_NED) {
 		landing_target_pose_s landing_target_pose = {};
-		landing_target_pose.timestamp = sync_stamp(landing_target.time_usec);
+		landing_target_pose.timestamp = _mavlink_timesync.sync_stamp(landing_target.time_usec);
 		landing_target_pose.abs_pos_valid = true;
 		landing_target_pose.x_abs = landing_target.x;
 		landing_target_pose.y_abs = landing_target.y;
@@ -2547,6 +2463,9 @@ MavlinkReceiver::receive_thread(void *arg)
 						/* handle packet with log component */
 						_mavlink_log_handler.handle_message(&msg);
 
+						/* handle packet with timesync component */
+						_mavlink_timesync.handle_message(&msg);
+
 						/* handle packet with parent object */
 						_mavlink->handle_message(&msg);
 					}
@@ -2587,28 +2506,6 @@ void MavlinkReceiver::print_status()
 
 }
 
-uint64_t MavlinkReceiver::sync_stamp(uint64_t usec)
-{
-
-	if (_time_offset != 0) {
-		return usec + (_time_offset / 1000) ;
-
-	} else {
-		return hrt_absolute_time();
-	}
-}
-
-
-void MavlinkReceiver::smooth_time_offset(int64_t offset_ns)
-{
-	/* The closer alpha is to 1.0, the faster the moving
-	 * average updates in response to new offset samples.
-	 */
-
-	_time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset;
-}
-
-
 void *MavlinkReceiver::start_helper(void *context)
 {
 

src/modules/mavlink/mavlink_receiver.h

@@ -71,7 +71,6 @@
 #include <uORB/topics/debug_vect.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
-#include <uORB/topics/time_offset.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/follow_target.h>
 #include <uORB/topics/landing_target_pose.h>
@@ -84,8 +83,7 @@
 #include "mavlink_parameters.h"
 #include "mavlink_ftp.h"
 #include "mavlink_log_handler.h"
-
-#define PX4_EPOCH_SECS 1234567890ULL
+#include "mavlink_timesync.h"
 
 class Mavlink;
 
@@ -143,8 +141,6 @@ class MavlinkReceiver
 	void handle_message_rc_channels_override(mavlink_message_t *msg);
 	void handle_message_heartbeat(mavlink_message_t *msg);
 	void handle_message_ping(mavlink_message_t *msg);
-	void handle_message_system_time(mavlink_message_t *msg);
-	void handle_message_timesync(mavlink_message_t *msg);
 	void handle_message_hil_sensor(mavlink_message_t *msg);
 	void handle_message_hil_gps(mavlink_message_t *msg);
 	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
@@ -178,17 +174,6 @@ class MavlinkReceiver
 	int set_message_interval(int msgId, float interval, int data_rate = -1);
 	void get_message_interval(int msgId);
 
-	/**
-	 * Convert remote timestamp to local hrt time (usec)
-	 * Use timesync if available, monotonic boot time otherwise
-	 */
-	uint64_t sync_stamp(uint64_t usec);
-
-	/**
-	 * Exponential moving average filter to smooth time offset
-	 */
-	void smooth_time_offset(int64_t offset_ns);
-
 	/**
 	 * Decode a switch position from a bitfield
 	 */
@@ -209,6 +194,7 @@ class MavlinkReceiver
 	MavlinkParametersManager	_parameters_manager;
 	MavlinkFTP			_mavlink_ftp;
 	MavlinkLogHandler		_mavlink_log_handler;
+	MavlinkTimesync		_mavlink_timesync;
 
 	mavlink_status_t _status; ///< receiver status, used for mavlink_parse_char()
 	struct vehicle_local_position_s _hil_local_pos;
@@ -242,7 +228,6 @@ class MavlinkReceiver
 	orb_advert_t _manual_pub;
 	orb_advert_t _obstacle_distance_pub;
 	orb_advert_t _land_detector_pub;
-	orb_advert_t _time_offset_pub;
 	orb_advert_t _follow_target_pub;
 	orb_advert_t _landing_target_pose_pub;
 	orb_advert_t _transponder_report_pub;
@@ -262,8 +247,6 @@ class MavlinkReceiver
 	float _hil_local_alt0;
 	struct map_projection_reference_s _hil_local_proj_ref;
 	struct offboard_control_mode_s _offboard_control_mode;
-	double _time_offset_avg_alpha;
-	int64_t _time_offset;
 	int	_orb_class_instance;
 
 	static constexpr unsigned MOM_SWITCH_COUNT = 8;