AP_RTC: added JitterCorrection class

libraries/AP_RTC/JitterCorrection.cpp

@@ -0,0 +1,89 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  class to correct an offboard timestamp in microseconds into a local
+  timestamp, removing timing jitter caused by the transport.
+
+  It takes the offboard_usec timestamp which is a timestamp provided
+  in a protocol from a remote device, and the time of arrival of the
+  message in local microseconds. It returns an improved estimate of
+  the time that the message was generated on the remote system in the
+  local time domain
+
+  The algorithm assumes two things:
+
+   1) the data did not come from the future in our local time-domain
+   2) the data is not older than max_lag_ms in our local time-domain
+
+  It works by estimating the transport lag by looking for the incoming
+  packet that had the least lag, and converging on the offset that is
+  associated with that lag
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include "JitterCorrection.h"
+
+// constructor
+JitterCorrection::JitterCorrection(uint16_t _max_lag_ms, uint16_t _convergence_loops) :
+    max_lag_ms(_max_lag_ms),
+    convergence_loops(_convergence_loops)
+{}
+
+/*
+  correct an offboard timestamp in microseconds into a local
+  timestamp, removing timing jitter caused by the transport. 
+
+  Return a value in microseconds since boot in the local time domain
+ */
+uint64_t JitterCorrection::correct_offboard_timestamp_usec(uint64_t offboard_usec, uint64_t local_usec)
+{
+    int64_t diff_us = int64_t(local_usec) - int64_t(offboard_usec);
+
+    if (!initialised ||
+        diff_us < link_offset_usec) {
+        // this message arrived from the remote system with a
+        // timestamp that would imply the message was from the
+        // future. We know that isn't possible, so we adjust down the
+        // correction value
+        link_offset_usec = diff_us;
+        initialised = true;
+    }
+
+    int64_t estimate_us = offboard_usec + link_offset_usec;
+
+    if (estimate_us + (max_lag_ms*1000U) < int64_t(local_usec)) {
+        // this implies the message came from too far in the past. clamp the lag estimate
+        // to assume the message had maximum lag
+        estimate_us = local_usec - (max_lag_ms*1000U);
+        link_offset_usec = estimate_us - offboard_usec;
+    }
+
+    if (min_sample_counter == 0) {
+        min_sample_us = diff_us;
+    }
+    min_sample_counter++;
+    if (diff_us < min_sample_us) {
+        min_sample_us = diff_us;
+    }
+    if (min_sample_counter == convergence_loops) {
+        // we have the requested number of samples of the transport
+        // lag for convergence. To account for long term clock drift
+        // we set the diff we will use in future to this value
+        link_offset_usec = min_sample_us;
+        min_sample_counter = 0;
+    }
+    
+    return uint64_t(estimate_us);
+}

libraries/AP_RTC/JitterCorrection.h

@@ -0,0 +1,23 @@
+/*
+  timestamp jitter correction class
+ */
+
+#pragma once
+
+class JitterCorrection {
+public:
+    // constructor
+    JitterCorrection(uint16_t max_lag_ms=500, uint16_t convergence_loops=100);
+
+    // correct an offboard timestamp to a jitter-free local
+    // timestamp. See JitterCorrection.cpp for details
+    uint64_t correct_offboard_timestamp_usec(uint64_t offboard_usec, uint64_t local_usec);
+
+private:
+    const uint16_t max_lag_ms;
+    const uint16_t convergence_loops;
+    int64_t link_offset_usec;
+    int64_t min_sample_us;
+    bool initialised;
+    uint16_t min_sample_counter;
+};