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

AP_GPS: Don't reset the entire buffer when handling RTCM data

Browse Source 
This is a micro optimization intended to just make handling GPS
corrections a bit faster.
This commit is contained in:
Michael du Breuil 2020-10-05 11:55:23 -07:00 committed by Andrew Tridgell
parent acdc939946
commit a61444cec0
1 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_GPS/AP_GPS.cpp
View File

@ -1282,16 +1282,17 @@ void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_
}
}
uint8_t fragment = (flags >> 1U) & 0x03;
uint8_t sequence = (flags >> 3U) & 0x1F;
const uint8_t fragment = (flags >> 1U) & 0x03;
const uint8_t sequence = (flags >> 3U) & 0x1F;
// see if this fragment is consistent with existing fragments
if (rtcm_buffer->fragments_received &&
(rtcm_buffer->sequence != sequence ||
(rtcm_buffer->fragments_received & (1U<<fragment)))) {
(rtcm_buffer->fragments_received & (1U<<fragment)))) {
// we have one or more partial fragments already received
// which conflict with the new fragment, discard previous fragments
memset(rtcm_buffer, 0, sizeof(*rtcm_buffer));
rtcm_buffer->fragment_count = 0;
rtcm_buffer->fragments_received = 0;
}
// add this fragment
@ -1320,7 +1321,8 @@ void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_
rtcm_buffer->fragments_received == (1U << rtcm_buffer->fragment_count) - 1) {
// we have them all, inject
inject_data(rtcm_buffer->buffer, rtcm_buffer->total_length);
memset(rtcm_buffer, 0, sizeof(*rtcm_buffer));
rtcm_buffer->fragment_count = 0;
rtcm_buffer->fragments_received = 0;
}
}