aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: make GPS packet timestamps more accurate

Browse Source 
this reduces the effect of processing time and uart transmit time
This commit is contained in:
Andrew Tridgell 2018-05-16 11:16:01 +10:00
parent 62c26ec5ff
commit 3fe4b9c0f8
11 changed files with 61 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_GPS/AP_GPS.cpp
View File

@ -603,7 +603,7 @@ void AP_GPS::update_instance(uint8_t instance)
// we have an active driver for this instance
bool result = drivers[instance]->read();
const uint32_t tnow = AP_HAL::millis();
uint32_t tnow = AP_HAL::millis();
// if we did not get a message, and the idle timer of 2 seconds
// has expired, re-initialise the GPS. This will cause GPS
@ -632,6 +632,12 @@ void AP_GPS::update_instance(uint8_t instance)
data_should_be_logged = true;
}
} else {
if (state[instance].uart_timestamp_ms != 0) {
// 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;
}
// delta will only be correct after parsing two messages
timing[instance].delta_time_ms = tnow - timing[instance].last_message_time_ms;
timing[instance].last_message_time_ms = tnow;

1
libraries/AP_GPS/AP_GPS.h
View File

@ -148,6 +148,7 @@ public:
bool have_horizontal_accuracy; ///< does GPS give horizontal position accuracy? Set to true only once available.
bool have_vertical_accuracy; ///< does GPS give vertical position accuracy? 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()
// all the following fields must only all be filled by RTK capable backend drivers
uint32_t rtk_time_week_ms; ///< GPS Time of Week of last baseline in milliseconds

2
libraries/AP_GPS/AP_GPS_ERB.cpp
View File

@ -178,6 +178,8 @@ AP_GPS_ERB::_parse_gps(void)
}
state.num_sats = _buffer.stat.satellites;
if (next_fix >= AP_GPS::GPS_OK_FIX_3D) {
// use the uart receive time to make packet timestamps more accurate
set_uart_timestamp(_payload_length + sizeof(erb_header) + 2);
state.last_gps_time_ms = AP_HAL::millis();
state.time_week_ms = _buffer.stat.time;
state.time_week = _buffer.stat.week;

5
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -99,6 +99,7 @@ bool AP_GPS_NMEA::_decode(char c)
++_term_number;
_term_offset = 0;
_is_checksum_term = c == '*';
_sentence_length++;
return valid_sentence;
case '$': // sentence begin
@ -107,6 +108,7 @@ bool AP_GPS_NMEA::_decode(char c)
_sentence_type = _GPS_SENTENCE_OTHER;
_is_checksum_term = false;
_gps_data_good = false;
_sentence_length = 0;
return valid_sentence;
}
@ -116,6 +118,8 @@ bool AP_GPS_NMEA::_decode(char c)
if (!_is_checksum_term)
_parity ^= c;
_sentence_length++;
return valid_sentence;
}
@ -252,6 +256,7 @@ bool AP_GPS_NMEA::_term_complete()
state.ground_speed = _new_speed*0.01f;
state.ground_course = wrap_360(_new_course*0.01f);
make_gps_time(_new_date, _new_time * 10);
set_uart_timestamp(_sentence_length);
state.last_gps_time_ms = now;
fill_3d_velocity();
break;

1
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -125,6 +125,7 @@ private:
uint8_t _sentence_type; ///< the sentence type currently being processed
uint8_t _term_number; ///< term index within the current sentence
uint8_t _term_offset; ///< character offset with the term being received
uint16_t _sentence_length;
bool _gps_data_good; ///< set when the sentence indicates data is good
// The result of parsing terms within a message is stored temporarily until

1
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -284,6 +284,7 @@ AP_GPS_SBF::process_message(void)
state.time_week_ms = (uint32_t)(temp.TOW);
}
set_uart_timestamp(sbf_msg.length);
state.last_gps_time_ms = AP_HAL::millis();
// Update velocity state (don't use −2·10^10)

1
libraries/AP_GPS/AP_GPS_SBP2.cpp
View File

@ -288,6 +288,7 @@ AP_GPS_SBP2::_attempt_state_update()
state.time_week_ms = last_vel_ned.tow;
state.hdop = last_dops.hdop;
state.vdop = last_dops.vdop;
set_uart_timestamp(parser_state.msg_len);
state.last_gps_time_ms = now;
//

20
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -833,7 +833,8 @@ AP_GPS_UBLOX::_parse_gps(void)
_unconfigured_messages |= CONFIG_RATE_POSLLH;
break;
}
_last_pos_time = _buffer.posllh.time;
_check_new_itow(_buffer.posllh.itow);
_last_pos_time = _buffer.posllh.itow;
state.location.lng = _buffer.posllh.longitude;
state.location.lat = _buffer.posllh.latitude;
state.location.alt = _buffer.posllh.altitude_msl / 10;
@ -883,6 +884,7 @@ AP_GPS_UBLOX::_parse_gps(void)
case MSG_DOP:
Debug("MSG_DOP");
noReceivedHdop = false;
_check_new_itow(_buffer.dop.itow);
state.hdop = _buffer.dop.hDOP;
state.vdop = _buffer.dop.vDOP;
#if UBLOX_FAKE_3DLOCK
@ -894,6 +896,7 @@ AP_GPS_UBLOX::_parse_gps(void)
Debug("MSG_SOL fix_status=%u fix_type=%u",
_buffer.solution.fix_status,
_buffer.solution.fix_type);
_check_new_itow(_buffer.solution.itow);
if (havePvtMsg) {
state.time_week = _buffer.solution.week;
break;
@ -920,7 +923,7 @@ AP_GPS_UBLOX::_parse_gps(void)
state.num_sats = _buffer.solution.satellites;
if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {
state.last_gps_time_ms = AP_HAL::millis();
state.time_week_ms = _buffer.solution.time;
state.time_week_ms = _buffer.solution.itow;
state.time_week = _buffer.solution.week;
}
#if UBLOX_FAKE_3DLOCK
@ -936,6 +939,7 @@ AP_GPS_UBLOX::_parse_gps(void)
Debug("MSG_PVT");
havePvtMsg = true;
// position
_check_new_itow(_buffer.pvt.itow);
_last_pos_time = _buffer.pvt.itow;
state.location.lng = _buffer.pvt.lon;
state.location.lat = _buffer.pvt.lat;
@ -1023,7 +1027,8 @@ AP_GPS_UBLOX::_parse_gps(void)
_unconfigured_messages |= CONFIG_RATE_VELNED;
break;
}
_last_vel_time = _buffer.velned.time;
_check_new_itow(_buffer.velned.itow);
_last_vel_time = _buffer.velned.itow;
state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s
state.ground_course = wrap_360(_buffer.velned.heading_2d * 1.0e-5f); // Heading 2D deg * 100000
state.have_vertical_velocity = true;
@ -1043,6 +1048,7 @@ AP_GPS_UBLOX::_parse_gps(void)
{
Debug("MSG_NAV_SVINFO\n");
static const uint8_t HardwareGenerationMask = 0x07;
_check_new_itow(_buffer.svinfo_header.itow);
_hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
switch (_hardware_generation) {
case UBLOX_5:
@ -1329,3 +1335,11 @@ void AP_GPS_UBLOX::Write_DataFlash_Log_Startup_messages() const
_version.swVersion);
}
}
void AP_GPS_UBLOX::_check_new_itow(uint32_t itow)
{
if (itow != _last_itow) {
_last_itow = itow;
set_uart_timestamp(_payload_length + sizeof(ubx_header) + 2);
}
}

14
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -193,7 +193,7 @@ private:
uint32_t scanmode1;
};
struct PACKED ubx_nav_posllh {
uint32_t time; // GPS msToW
uint32_t itow; // GPS msToW
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
@ -202,7 +202,7 @@ private:
uint32_t vertical_accuracy;
};
struct PACKED ubx_nav_status {
uint32_t time; // GPS msToW
uint32_t itow; // GPS msToW
uint8_t fix_type;
uint8_t fix_status;
uint8_t differential_status;
@ -211,7 +211,7 @@ private:
uint32_t uptime; // milliseconds
};
struct PACKED ubx_nav_dop {
uint32_t time; // GPS msToW
uint32_t itow; // GPS msToW
uint16_t gDOP;
uint16_t pDOP;
uint16_t tDOP;
@ -221,7 +221,7 @@ private:
uint16_t eDOP;
};
struct PACKED ubx_nav_solution {
uint32_t time;
uint32_t itow;
int32_t time_nsec;
uint16_t week;
uint8_t fix_type;
@ -263,7 +263,7 @@ private:
uint8_t reserved2[4];
};
struct PACKED ubx_nav_velned {
uint32_t time; // GPS msToW
uint32_t itow; // GPS msToW
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
@ -549,6 +549,9 @@ private:
bool havePvtMsg;
// itow from previous message
uint32_t _last_itow;
bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void _configure_rate(void);
void _configure_sbas(bool enable);
@ -561,6 +564,7 @@ private:
void _request_version(void);
void _save_cfg(void);
void _verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void _check_new_itow(uint32_t itow);
void unexpected_message(void);
void log_mon_hw(void);

11
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -216,3 +216,14 @@ void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
}
}
/*
set a timestamp based on arrival time on uart at current byte,
assuming the message started nbytes ago
*/
void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes)
{
if (port) {
state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U;
}
}

6
libraries/AP_GPS/GPS_Backend.h
View File

@ -87,4 +87,10 @@ protected:
void _detection_message(char *buffer, uint8_t buflen) const;
bool should_df_log() const;
/*
set a timestamp based on arrival time on uart at current byte,
assuming the message started nbytes ago
*/
void set_uart_timestamp(uint16_t nbytes);
};