Handle TIMESYNC mavlink messages

[fnoop]

PR to support #5884
TIMESYNC mavlink message is used to synchronise the flight controller with systems connected over mavlink. It can be used in one of two ways:

• Connected system sends a TIMESYNC message with a local timestamp
• Flight Controller sends a TIMESYNC message back with the timestamp echoed and the local timestamp of the flight controller.
  Once the connected system receives the returned TIMESYNC message, it can calculate the roundtrip time as well as receiving the local time since boot in usec of the flight controller.

• Connected system sends a TIMESYNC message with the estimated time of the flight controller, and a local timestamp.
• Flight controller sends a TIMESYNC message back with the difference or offset between the estimated FC time and the actual FC time, and the original timestamp echoed.
  Once the connected system receives the returned TIMESYNC message, it can accurately calculate the timestamp of the flight controller, and can continue to estimate it against the local clock of the connected system. If TIMESYNC messages are exchanged at 10hz, time sync is typically as accurate as <1ms (fcoffset below is the difference between the two systems in microseconds):

fcoffset: 756 , fclocalfctime: 1090965029 , fctimecorrected: 1090964273
fcoffset: 846 , fclocalfctime: 1091065079 , fctimecorrected: 1091064233
fcoffset: 679 , fclocalfctime: 1091164872 , fctimecorrected: 1091164193
fcoffset: 647 , fclocalfctime: 1091264800 , fctimecorrected: 1091264153
fcoffset: 689 , fclocalfctime: 1091364802 , fctimecorrected: 1091364113
fcoffset: 676 , fclocalfctime: 1091464749 , fctimecorrected: 1091464073
fcoffset: 743 , fclocalfctime: 1091564776 , fctimecorrected: 1091564033
fcoffset: 683 , fclocalfctime: 1091664676 , fctimecorrected: 1091663993

Sample python/dronekit code:

class TrackTimer:
    def __init__(self, vehicle):
        self.fctimecorrected = None
        self.fcoffset = None
        self.fclocalstamp = None
        self.fclocalfctime = None
        self.debug = False
        self.vehicle = vehicle
        @self.vehicle.on_message("SYSTEM_TIME")
        def listener_systemtime(subself, name, message):
            # If the fctime is not set, set it from system_time.time_boot_ms for an initial sync.
            if not self.fclocalfctime:
                self.fclocalfctime = int(message.time_boot_ms*1000)
                self.fclocalstamp = int(time() * 1000000)
        @self.vehicle.on_message("TIMESYNC")
        def listener_timesync(subself, name, message):
            if message.ts1 == self.fclocalstamp:
                self.fcoffset = message.tc1
                self.fctimecorrected = self.fclocalfctime - self.fcoffset
                if self.debug:
                    print "fcoffset:",self.fcoffset,", fclocalfctime:",self.fclocalfctime,", fctimecorrected:",self.fctimecorrected
    def send_timesync(self, tc, ts):
        msg = self.vehicle.message_factory.timesync_encode(
            tc,          # tc1
            ts          # ts1
        )
        self.vehicle.send_mavlink(msg)
        self.vehicle.flush()
    def update(self):
        if self.fcoffset and self.fctimecorrected:
            oldlocalstamp = self.fclocalstamp
            self.fclocalstamp = int(time() * 1000000)
            newdiff = self.fclocalstamp - oldlocalstamp
            self.fclocalfctime = self.fctimecorrected+newdiff
            self.send_timesync(self.fclocalfctime, self.fclocalstamp)
        elif self.fclocalfctime:
            self.send_timesync(self.fclocalfctime, self.fclocalstamp)
    def actual(self):
        return self.fctimecorrected
    def estimate(self):
        if self.fctimecorrected:
            newdiff = int(time() * 1000000) - self.fclocalstamp
            return self.fctimecorrected + newdiff
        else:
            return None

To use the above class:

vehicle = connect(connectstr, wait_ready=True)
track_time = TrackTimer(vehicle)
track_time.debug = True
while True:
    track_time.update()
    sleep(0.1)

Note: This works quite differently to how PX4 processes the TIMESYNC request. PX4 returns nanoseconds instead of microseconds, and sending a non-zero timesync.tc1 field does not return an offset (instead it does something different internally in PX4 firmware).

[WickedShell]

Not normally a time guy, but wouldn't the best numbers for timing come from capturing the micros64() number as soon as you enter the function, given that you call it in 2 places? (I get that this is a tiny tiny tiny tiny amount of jitter but stil).

[fnoop]

Thanks, good point :) I've addressed it in a subsequent commit, even though I've also removed the second call to micros64.

[meee1]

I don't believe this is correct, tc1 should be the absolute time, not a delta. this could also lead to a timesync flood.
tsync.ts1 = time at send
tsync.tc1 = 0 > time at client (unknown)
rsync.ts1 = copy of tsync.ts1 (time of the origin)
rsync.tx1 = out local absolute time

if tsync.tc1 > 0, then this is a time reply that originated from ourself, so there is no need to resend the message at all. plus we now have the info of the local time we sent the request (stored as we asked for it), and the time we acquired the response. (ie calc the offset)
please refer to here https://pixhawk.ethz.ch/docs/mavlink__receiver_8cpp_source.html

[fnoop]

@meee1 Yes this is correct if using this message to try to sync the absolute time of the flight controller, but my intent was to return the time since boot as that is a more reliable and consistent timestamp and is more useful for correlating log timestamps, EKF etc. The absolute time changes according to quality or absence of GPS lock.
The two fields of the TIMESYNC message don't allow for the same type of synchronisation as NTP which uses 4 fields. With the two fields of TIMESYNC the best we can achieve is (roundtrip/2) unless using tc1>0 to estimate the incoming offset and return the offset to the remote end. I've removed that logic in a subsequent commit to clarify the main purpose of the message response.
The px4 code uses tc1>0 to publish a smoothed offset to the remote timestamp within the flight controller, which is probably not what we want to do.

So thanks for raising this important question :) Which is, do we want to return the time since boot, or the absolute time?

[rmackay9]

I've submitted an updated PR so closing this one. #5944