Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: added jitter correction to timestamp calculation

Browse Source 
this allows for much more accurate (and consistent) timestamping of GPS
messages where the protocol has a ITOW field.
This commit is contained in:
Andrew Tridgell 2018-06-22 15:28:11 +10:00
parent d761b24b92
commit 7a4b8d76c1
2 changed files with 52 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -233,6 +233,49 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
{
if (itow != _last_itow) {
_last_itow = itow;
set_uart_timestamp(msg_length);
/*
we need to calculate a pseudo-itow, which copes with the
iTow from the GPS changing in unexpected ways. We assume
that timestamps from the GPS are always in multiples of
50ms. That means we can't handle a GPS with an update rate
of more than 20Hz. We could do more, but we'd need the GPS
poll time to be higher
*/
const uint32_t gps_min_period_ms = 50;
uint32_t now = AP_HAL::millis();
uint32_t dt_ms = now - _last_ms;
_last_ms = now;
// round to nearest 50ms period
dt_ms = ((dt_ms + (gps_min_period_ms/2)) / gps_min_period_ms) * gps_min_period_ms;
// work out an actual message rate. If we get 5 messages in a
// row with a new rate we switch rate
if (_last_rate_ms == dt_ms) {
if (_rate_counter < 5) {
_rate_counter++;
} else if (_rate_ms != dt_ms) {
_rate_ms = dt_ms;
}
} else {
_rate_counter = 0;
_last_rate_ms = dt_ms;
}
if (_rate_ms == 0) {
_rate_ms = gps.get_rate_ms(state.instance);
}
// round to calculated message rate
dt_ms = ((dt_ms + (_rate_ms/2)) / _rate_ms) * _rate_ms;
// calculate pseudo-itow
_pseudo_itow += dt_ms * 1000U;
// get msg arrival time, and correct for jitter
uint64_t uart_us = port->receive_time_constraint_us(msg_length);
uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us);
state.uart_timestamp_ms = local_us / 1000U;
}
}

8
libraries/AP_GPS/GPS_Backend.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_RTC/JitterCorrection.h>
#include "AP_GPS.h"
class AP_GPS_Backend
@ -99,4 +100,11 @@ protected:
private:
// itow from previous message
uint32_t _last_itow;
uint64_t _pseudo_itow;
uint32_t _last_ms;
uint32_t _rate_ms;
uint32_t _last_rate_ms;
uint16_t _rate_counter;
JitterCorrection jitter_correction;
};