From 6d6c790e379004694d8210c41089cf869f55535d Mon Sep 17 00:00:00 2001 From: Andrew Piper Date: Sat, 24 Feb 2024 18:32:12 +0000 Subject: [PATCH] AP_GPS: fragments_received is a bitmask not a count --- libraries/AP_GPS/AP_GPS.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 94e98f0ac1d83..9b15335435200 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -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; } @@ -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;