Skip to content

Commit

Permalink
AP_GPS: fragments_received is a bitmask not a count
Browse files Browse the repository at this point in the history
  • Loading branch information
andyp1per authored and peterbarker committed Feb 25, 2024
1 parent 870c6c5 commit 6d6c790
Showing 1 changed file with 2 additions and 2 deletions.
4 changes: 2 additions & 2 deletions libraries/AP_GPS/AP_GPS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1633,7 +1633,7 @@ void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_
// we have one or more partial fragments already received
// which conflict with the new fragment, discard previous fragments
rtcm_buffer->fragment_count = 0;
rtcm_stats.fragments_discarded += rtcm_buffer->fragments_received;
rtcm_stats.fragments_discarded += __builtin_popcount(rtcm_buffer->fragments_received);
rtcm_buffer->fragments_received = 0;
}

Expand Down Expand Up @@ -1662,7 +1662,7 @@ void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_
if (rtcm_buffer->fragment_count != 0 &&
rtcm_buffer->fragments_received == (1U << rtcm_buffer->fragment_count) - 1) {
// we have them all, inject
rtcm_stats.fragments_used += rtcm_buffer->fragments_received;
rtcm_stats.fragments_used += __builtin_popcount(rtcm_buffer->fragments_received);
inject_data(rtcm_buffer->buffer, rtcm_buffer->total_length);
rtcm_buffer->fragment_count = 0;
rtcm_buffer->fragments_received = 0;
Expand Down

0 comments on commit 6d6c790

Please sign in to comment.