Add support for calculating GPS Lag over DroneCAN

[bugobliterator]

• Adds support for GlobalTimeSync to precisely synchronize GPS Time.
• Adds support for calculating GPS Lag.

Following image shows the calculation of GPS message rx lag.

[tridge]

I'm pretty sure this will fail for several cases:

• a GPS module that doesn't have the leap seconds yet when we first get GPS lock and lockin the RTC base time
• a large crystal drift leading to large drift between GNSS time and AP_HAL::micros64()

The fundamental problem is the assumption that the two time domains are equivalent

[bugobliterator]

Explanation of procedure on how the Lag is being Calculated:

Key Time Domains:
Periph Time Domain : Time from micros on AP_Periph
GPS Time Domain: Time from last_message_epoch_usec() or time_epoch_usec(), both these are calculated using PPS signal on Periph side.
Ardupilot Time Domain: Time from micros on Ardupilot

On Periph Side for Time Synchronisation Message:

• DroneCAN Transmit Timestamp[Periph Time Domain]:
  On Tx interrupt time is recorded by message id. Tx Interrupt is only called once the transmission is successfully placed on the wire and ACK is received. This time is recorded in AP_Periph local timescale.

• Ublox Message Timestamp[GPS Time Domain]:
  This is the time for which the solution is calculated by the GPS module, this time does not fluctuate with leapS as shown below.
  This is set using last_message_epoch_usec() == UNIX_OFFSET_MSEC + gps_week * AP_MSEC_PER_WEEK + gps_ms. Its to be noted that that none of these parameters jump arbittrarily once lock is achieved. Concerns regarding leap seconds are not applicable as well. Leap seconds is not added to either gps_week or gps_ms. Its a constant value part of UNIX_OFFSET_MSEC.

• Ublox Message Timestamp[Periph Time Domain]:
  This is the time for which the solution is calculated by the GPS module, just in Periph Time domain
  This is simply set through AP_GPS_Backend with PPS pin timestamp as reference.

Finally:
DroneCAN Transmit Timestamp[GPS Time Domain] = DroneCAN Transmit Timestamp[Periph Time Domain] + Ublox Message Timestamp[GPS Time Domain] - Ublox Message Timestamp[Periph Time Domain]

Ublox Message Timestamp[GPS Time Domain] - Ublox Message Timestamp[Periph Time Domain] is the Conversion Time Delta between Periph Local Time and GPS Time.

On Periph Side for GPS Fix Message and GPS RelPosNed Message:

• Ublox Message Timestamp[GPS Time Domain]:
  Same as above, set using last_message_epoch_usec()

On Ardupilot Side:

• TimeSync Correction:
  Conversion Time Delta between GPS Time Domain and Local Time Domain:
  DroneCAN Transmit Timestamp[GPS Time Domain] - DroneCAN TimeSync Receive Timestamp[Local Time Domain]

• Current Timestamp[GPS Time Domain]:
  TimeSync Correction + Local Timestamp[Local Time Domain]

• Total Lag:
  Current Timestamp[GPS Time Domain] - Ublox Message Timestamp[GPS Time Domain]

Benefit of using Time Synchronisation Message: pkt.timestamp.usec[GPS Time Domain] in message itself contains timestamp recorded at send call time on Periph. Messages are put on queue but may not transmitted for sometime if there's too much higher priority traffic on the bus.
We are getting gps timestamp[GPS Time Domain] when the message was put on the bus and ACKed which is near equivalent to when we record Rx Timestamp[Local Time Domain]. So, we can more precisely synchronise time between GPS module and Ardupilot.
With this type of synchronisation GPS PPS to Ardupilot PPS accuracy of less than 40us was achieved.

Furthermore:

• I have also added code to ensure we handle missed packets properly using transfer id.
• I have also added sanity check to limit the lag calculation to be within 0.05s to 0.3s.

[magicrub]

You say "precisely synchronize GPS Time" But what people really want is PPS. Does this help create a path for that?

[bugobliterator]

You say "precisely synchronize GPS Time" But what people really want is PPS. Does this help create a path for that?

It extends the PPS through dronecan, yes.

[tridge]

@bugobliterator how did the tests code with the artificial clock drift?

[bugobliterator]

Clock drift Estimation using PPM based Time synchronisation over DroneCAN is done by calculating the variation in Delta between GPS Time and Local Time. The result is as follows.

Zero PPM Drift added to micros/millis

1000 PPM Drift added to micros/millis

Above plots show clock drift estimation at Global Time Synchronisation rate which is 5Hz/200ms. 1000PPM drift means we will be drifting by 1000us every second or 200us every 200ms. This is correctly estimated as shown in 2nd Image. In the 1st Image the clock drift comes out to be around 4us/200ms or around 20ppm, which seems like a close enough combined figure for combined clock drift of Crystal on Here4 (+/-20ppm) and CubeOrangePlus (+/-20ppm)

[bugobliterator]

Jitter in sample time:

This is done simply by finding delta between subsequent GPS Times as seen by GPS:

The Values were within 200 +/- 1ms. The Plot is of gps_time_delta_ms from following code:

c5a1f6a#diff-8f617a403545351808c8bf54a6d9fc310593075737558480ca2dcc4f12cb4f46R576-R587