Syncing time between ArduPilot vehicle and ground computers?

Pogo · February 23, 2020, 6:56pm

Hi,

I’ve been examining the ArduPilot code to figure out how I can sync the time as tightly as possible between ArduPlane running on a Matek F765 in my wing, and my laptop upon which I’m running the GCS (I’ve tried MP, QGC and MAVProxy). The reason I’d like to get this working is to get my own antenna tracking software to drive a PT unit (it’s called a PTU-5) to track the drone as accurately as possible by predicting and intercepting its position.

One way I can see is to get GLOBAL_POSITION_INT messages coming down at, say, 10 Hz, from which I can extract vehicle time from the time_boot_ms field. But I need to relate this to some common epoch understood by both vehicle and ground computers. Could some kind person steer me in the right direction?

Many thanks.

– Pogo

aldofad · February 24, 2020, 10:35am

Since you have examined the source code and you are proposing yourself to develop/rewrite some of the AT code, I hope you’ll really get the help you need as looks to me that the Antenna Tracker project is a bit abandoned to itself and dedicated developers are needed.

lucasdemarchi · February 25, 2020, 10:13pm

Look for the MAVLINK_MSG_ID_TIMESYNC. That message was made specifically for that… #5944 has more details

Pogo · February 26, 2020, 4:31am

Thank you Lucas.

I read #5944 and the related threads.

Now, I could do the following:

a. Establish good GPS fix in order to populate the time_unix_usec field.

b. Subtract field 2 from field 1 (having first converted field 2 to microseconds). Let’s call this difference ‘dt’.

c. Each GLOBAL_POSITION_INT message has a time_boot_ms field. Add ‘dt’ to this field. Now, I would have an ‘absolute’ timestamp for the position data in UNIX time (time since 01/01/1970), which would pretty much solve my problem (subject to jitter and drift).

What do you think?

EDIT: I should mention that the ground computer (for the GCS) would itself sync off its own GPS receiver, just as the vehicle computer has.

anbello · February 26, 2020, 9:31am

Hi @lucasdemarchi I read PR #5944 and I see that the TIMESYNC handler has been implemented without the calculation of observed offset and estimated offset as the mavors implementation does.
Don’t you think that having these offset calculated by AP on the FC could be useful to assign the correct timestamp to messages related to external navigation as VISION_POSITION_ESTIMATE?
Maybe @fnoop has something to say here.

fnoop · February 26, 2020, 10:38am

TIMESYNC is intended for a very basic function. The mavros implementation uses this but also builds on it for estimate offset compensation. The AP handling of TIMESYNC would have to be completely refactored and be significantly more complex if it was to do offset compensation for all possible connected devices simultaneously. I think it’s for the other end to do.

Also have a look at:

anbello · February 26, 2020, 11:56am

Thanks for you answer @fnoop but there is something i don’t understand, I try to explain myself:

the OBC (on board computer) capture an image with associated timestamp in UNIX epoch time
the OBC do the processing to have pose estimation and send the messages with the capture timestamp
AP on FC receive and manage this message and should translate the timestamp in the local time domain (ms from boot), it should know how many ms from boot has passed when the image was captured

How this last step could be performed without knowing the offset between the two time domains?

fnoop · February 26, 2020, 12:54pm

@anbello - This is a good example of why the compensation shouldn’t/can’t be on the FC end. Say you’re doing pose estimation on a board like the popular raspberry. It’s a slow board, so by the time you’ve captured the image, extracted it to memory structure, processed it and extracted the pose, quite a long time has passed. You then also have to transmit it to the FC, usually over a serial link. On a non-realtime OS and a slow board like the raspberry, the time for processing each image and extracting the pose can vary (very) widely. How is the FC supposed to know when each image was captured? There’s no way for it to know. Knowing an offset between ‘time domains’ wouldn’t help at all in this case.

The only reliable way of syncing image frames with the FC is for the capture device to know a) the time offset to the FC, and b) the exact time the image is captured, and then to send the offset timestamp back with each mavlink message. This is essentially what mavros does, and also what we do in vision_landing. Here we track the time offset to the FC:

github.com

goodrobots/vision_landing/blob/master/vision_landing#L80


            # self.readline()
            tmpline = self.queue.get(False)
            log.debug("clearing queue: {} : {}".format(self.size(), tmpline))
            self.queue.task_done()
        return
    def stop(self):
        self._stop.set()
    def stopped(self):
        return self._stop.isSet()


# TrackTime class is used to synchronise time between the companion computer and the flight controller.
# This is necessary to ensure that the vision frames are matched as closely as possible with the inertial frames.
# Note: This requires a Craft object to be passed, in order to perform the actual sync. 
class TrackTime:
    def __init__(self, craft):
        self.fctime_nanos = 0
        self.mytime_nanos = 0
        self.local_tracker = {}
        self.delta = None
        self.difference = None
        self.delta_buffer = deque(maxlen=50) # Circular buffer to track timesync deltas

And then for each frame we add the offset to the original time the image was captured (or at least the time it gets to the code which is as good as we can get generically) and send this timestamp back to the FC:

github.com

goodrobots/vision_landing/blob/master/vision_landing#L252


        return
    # If fake rangefinder distance is required, send it
    if args.fakerangefinder:
        craft.send_distance_message(int(z*100))
    # Send the landing_target message to arducopter
    # NOTE: This sends the current estimated fctime, NOT the time the frame was actually captured.
    # So this does not allow for the CV processing time. This should be sending when the frame is actually captured, before processing.
    nowTime = monotonic_time()
    # If data is less than 250ms since the image shuttr, send the message.
    if nowTime - sensorTimestamp < 250 * 1e+6:
        craft.send_land_message(x, y, z, tracktime.toFcTime(sensorTimestamp) / 1000, id) # Send sensor timestamp adjusted to timesync, in micros
    # Track frame timing to calculate fps, running average over last 10 frames
    end_t = time()
    time_taken = end_t - self.start_t
    self.start_t = end_t
    self.frame_times.append(time_taken)
    self.frame_times = self.frame_times[-10:]
    fps = len(self.frame_times) / sum(self.frame_times)
    log.info("Fctime:{:.0f}:{:.0f}, Fps:{:.2f}, Alt:{}, Rng:{}, Marker:{}, Distance:{}, xOffset:{}, yOffset:{}, processTime:{:.0f}, sensorTime:{:.0f}, sendTime:{:.0f}, receiveTime:{:.0f}, currentTime:{:.0f}, sensorDiff:{}, sendDiff:{}, receiveDiff:{}".format(tracktime.actual(), tracktime.estimate(), fps, craft.vehicle.location.global_relative_frame.alt, craft.vehicle.rangefinder.distance, id, z, x, y, processtime, sensorTimestamp, sendTimestamp, receiveTimestamp, nowTime, nowTime - sensorTimestamp, nowTime - sendTimestamp, nowTime - receiveTimestamp))
def start(self):
    if self.state == "initialised":

It’s then up to the FC to match the timestamp to the place in the EKF or buffer to correlate.
That make sense?

Edit: btw the time code in vision_landing is probably not very good - it came out of a hack and slash session to get it working. The mavros code is probably better to follow!

fnoop · February 26, 2020, 1:00pm

ps - also, the timesync can also vary widely. The clocks on board the FC can be quite poor (the pixhawk sucked, eg) so you have to keep sync quite frequently - as much as 10hz+ for good timekeeping. And there can be a lot of variance in the transport and message processing as well. So there’s quite a bit of inherent jitter to the time, which is why systems like mavros and vision_landing keep a running ‘average’ of the time offset.
It would be really cool if AP could act as an ntp/ptp server

anbello · February 26, 2020, 1:37pm

OBC use timestamp from gstreamer to know when each image was captured and send this timestamp with vision message, so also the FC know this but in the OBC time domain, the offset could be used to transform timestamp in the FC time domain.

Maybe this could be done also in AP, in PX4 they do so:

github.com

PX4/PX4-Autopilot/blob/main/src/modules/mavlink/mavlink_timesync.cpp#L52


      
          #include "mavlink_main.h"
          
          #include <stdlib.h>
          
          MavlinkTimesync::MavlinkTimesync(Mavlink *mavlink) :
          	_mavlink(mavlink)
          {
          }
          
          void
          MavlinkTimesync::handle_message(const mavlink_message_t *msg)
          {
          	switch (msg->msgid) {
          	case MAVLINK_MSG_ID_TIMESYNC: {
          
          			mavlink_timesync_t tsync = {};
          			mavlink_msg_timesync_decode(msg, &tsync);
          
          			const uint64_t now = hrt_absolute_time();
          
          			if (tsync.tc1 == 0) {			// Message originating from remote system, timestamp and return it

Another discussion on the subject:

fnoop · February 26, 2020, 2:20pm

PX4/mavros both have the same implementation from mhkabir. License permitting we could do the same thing in AP (I’m sure PRs will be welcome!), but I’m not convinced personally that the FC is the right place to do it. mavros acts as the ‘client’ and makes sense to do it there, to me, but then if it was done in the FC it would save a lot of client-side code. TIMESYNC itself is a bit limited as noted in #5944 - I’m not sure what would happen if you started firing TIMESYNC requests from multiple attached systems.

Pogo · February 26, 2020, 9:34pm

Is SYSTEM_TIME used to get time or set it?

Does AP set the FC’s time from the GPS receiver (and curb the drift)?

cglusky · February 28, 2020, 8:05pm

here’s a very interesting thread on using PPS pin/signal from GPS for synch of systems in general:

https://forum.sparkfun.com/viewtopic.php?t=25061

certainly not a step by step though. i see the PPS pin show up on more and more gps modules so expect it is being used.

Pogo · February 28, 2020, 10:32pm

@cglusky I’m surprised AP doesn’t use the PPS signal already.

I’d be super grateful if someone could let me know how tightly AP is synced to the GPS using just its serial signals, and about the magnitude of the ‘typical’ drift of the ‘average’ FC during GPS outage.

I’ve got my antenna tracker working now using an averaged estimate of the offset between UTC and boot time (using the TIMESYNC message). As this partly depends on the round trip time of the radio link (I’m using a pair of RFD868x’s), I suspect it’s not a great estimate, but I haven’t got around to quantifying the results yet.

cglusky · February 28, 2020, 10:38pm

Sadly, I am not the person to talk to. @rmackay9 might know if any of the devs are playing with PPS.

I just know it has been a source of pain for anyone trying to sync all this stuff up.

Pogo · March 11, 2020, 9:04am

I’ve been reading around further on this topic, particularly regarding the use of the PPS output of GPS receivers to provide an FC with a reasonably accurate clock (which I’ll define arbitrarily as < 1ms). It appears that, usually, a UART’s DCD line is used to sense the pulse, but many existing FCs used by ArduPilot don’t have one. Instead, might the CTS line be used?

Also, am I correct in stating that the ‘second’ TIMESYNC message mode, where a non-zero tc1 is sent from the OBC/GCS to the FC, has been removed?

ejrobinson · November 19, 2022, 11:14am

Hi Pogo,

I know it’s been a while, but did you ever make any progress with getting the FC to time sync using the PPS output?

Cheers