Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: move to using last_corrected_gps_time_us instead of uart_timestamp_us

This commit is contained in:
bugobliterator 2022-01-18 16:12:57 +05:30 committed by Andrew Tridgell
parent 9834304525
commit 49bf4fa27b
4 changed files with 12 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_GPS/AP_GPS.cpp
View File

@ -879,11 +879,12 @@ void AP_GPS::update_instance(uint8_t instance)
data_should_be_logged = true;
}
} else {
if (state[instance].uart_timestamp_ms != 0) {
if (state[instance].corrected_timestamp_updated) {
// set the timestamp for this messages based on
// set_uart_timestamp() in backend, if available
tnow = state[instance].uart_timestamp_ms;
state[instance].uart_timestamp_ms = 0;
// set_uart_timestamp() or per specific transport in backend
// , if available
tnow = state[instance].last_corrected_gps_time_us/1000U;
state[instance].corrected_timestamp_updated = false;
}
// delta will only be correct after parsing two messages
timing[instance].delta_time_ms = tnow - timing[instance].last_message_time_ms;

2
libraries/AP_GPS/AP_GPS.h
View File

@ -194,8 +194,8 @@ public:
bool have_gps_yaw; ///< does GPS give yaw? Set to true only once available.
bool have_gps_yaw_accuracy; ///< does the GPS give a heading accuracy estimate? Set to true only once available
uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds
uint32_t uart_timestamp_ms; ///< optional timestamp from set_uart_timestamp()
uint64_t last_corrected_gps_time_us;///< the system time we got the last corrected GPS timestamp, microseconds
bool corrected_timestamp_updated; ///< true if the corrected timestamp has been updated
uint32_t lagged_sample_count; ///< number of samples with 50ms more lag than expected
// all the following fields must only all be filled by RTK capable backend drivers

3
libraries/AP_GPS/AP_GPS_MAV.cpp
View File

@ -121,7 +121,8 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
}
uint32_t timestamp_ms = (packet.time_week - first_week) * AP_MSEC_PER_WEEK + packet.time_week_ms;
uint32_t corrected_ms = jitter.correct_offboard_timestamp_msec(timestamp_ms, now_ms);
state.uart_timestamp_ms = corrected_ms;
state.last_corrected_gps_time_us = (corrected_ms * 1000ULL);
state.corrected_timestamp_updated = true;
if (have_yaw) {
state.gps_yaw_time_ms = corrected_ms;
}

7
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -227,7 +227,8 @@ void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes)
{
if (port) {
state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U;
state.last_corrected_gps_time_us = port->receive_time_constraint_us(nbytes);
state.corrected_timestamp_updated = true;
}
}
@ -291,14 +292,14 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
// use msg arrival time, and correct for jitter
uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us);
state.last_corrected_gps_time_us = local_us;
state.uart_timestamp_ms = local_us / 1000U;
state.corrected_timestamp_updated = true;
// look for lagged data from the GPS. This is meant to detect
// the case that the GPS is trying to push more data into the
// UART than can fit (eg. with GPS_RAW_DATA at 115200).
float expected_lag;
if (gps.get_lag(state.instance, expected_lag)) {
float lag_s = (now - state.uart_timestamp_ms) * 0.001;
float lag_s = (now - (state.last_corrected_gps_time_us/1000U)) * 0.001;
if (lag_s > expected_lag+0.05) {
// more than 50ms over expected lag, increment lag counter
state.lagged_sample_count++;