Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   class to correct an offboard timestamp in microseconds into a local
      17             :   timestamp, removing timing jitter caused by the transport.
      18             : 
      19             :   It takes the offboard_usec timestamp which is a timestamp provided
      20             :   in a protocol from a remote device, and the time of arrival of the
      21             :   message in local microseconds. It returns an improved estimate of
      22             :   the time that the message was generated on the remote system in the
      23             :   local time domain
      24             : 
      25             :   The algorithm assumes two things:
      26             : 
      27             :    1) the data did not come from the future in our local time-domain
      28             :    2) the data is not older than max_lag_ms in our local time-domain
      29             : 
      30             :   It works by estimating the transport lag by looking for the incoming
      31             :   packet that had the least lag, and converging on the offset that is
      32             :   associated with that lag
      33             :  */
      34             : 
      35             : #include <AP_HAL/AP_HAL.h>
      36             : #include "JitterCorrection.h"
      37             : 
      38             : // constructor
      39       11510 : JitterCorrection::JitterCorrection(uint16_t _max_lag_ms, uint16_t _convergence_loops) :
      40       11510 :     max_lag_ms(_max_lag_ms),
      41       11510 :     convergence_loops(_convergence_loops)
      42       11510 : {}
      43             : 
      44             : /*
      45             :   correct an offboard timestamp in microseconds into a local
      46             :   timestamp, removing timing jitter caused by the transport. 
      47             : 
      48             :   Return a value in microseconds since boot in the local time domain
      49             :  */
      50      727454 : uint64_t JitterCorrection::correct_offboard_timestamp_usec(uint64_t offboard_usec, uint64_t local_usec)
      51             : {
      52      727454 :     int64_t diff_us = int64_t(local_usec) - int64_t(offboard_usec);
      53             : 
      54      727454 :     if (!initialised ||
      55      726354 :         diff_us < link_offset_usec) {
      56             :         // this message arrived from the remote system with a
      57             :         // timestamp that would imply the message was from the
      58             :         // future. We know that isn't possible, so we adjust down the
      59             :         // correction value
      60       26891 :         link_offset_usec = diff_us;
      61       26891 :         initialised = true;
      62             :     }
      63             : 
      64      727454 :     int64_t estimate_us = offboard_usec + link_offset_usec;
      65             : 
      66      727454 :     if (estimate_us + (max_lag_ms*1000U) < int64_t(local_usec)) {
      67             :         // this implies the message came from too far in the past. clamp the lag estimate
      68             :         // to assume the message had maximum lag
      69         783 :         estimate_us = local_usec - (max_lag_ms*1000U);
      70         783 :         link_offset_usec = estimate_us - offboard_usec;
      71             :     }
      72             : 
      73      727454 :     if (min_sample_counter == 0) {
      74        7852 :         min_sample_us = diff_us;
      75             :     }
      76      727454 :     min_sample_counter++;
      77      727454 :     if (diff_us < min_sample_us) {
      78       62417 :         min_sample_us = diff_us;
      79             :     }
      80      727454 :     if (min_sample_counter == convergence_loops) {
      81             :         // we have the requested number of samples of the transport
      82             :         // lag for convergence. To account for long term clock drift
      83             :         // we set the diff we will use in future to this value
      84        6753 :         link_offset_usec = min_sample_us;
      85        6753 :         min_sample_counter = 0;
      86             :     }
      87             :     
      88      727454 :     return uint64_t(estimate_us);
      89             : }
      90             : 
      91             : /*
      92             :   correct an offboard timestamp in microseconds into a local
      93             :   timestamp, removing timing jitter caused by the transport. 
      94             : 
      95             :   Return a value in milliseconds since boot in the local time domain
      96             :  */
      97        1727 : uint32_t JitterCorrection::correct_offboard_timestamp_msec(uint32_t offboard_ms, uint32_t local_ms)
      98             : {
      99        1727 :     return correct_offboard_timestamp_usec(offboard_ms*1000ULL, local_ms*1000ULL) / 1000ULL;
     100             : }