diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 6f0b6c90e4..1e94fa810c 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1584,6 +1584,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_buffer->fragments_received = 0; } @@ -1612,6 +1613,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; inject_data(rtcm_buffer->buffer, rtcm_buffer->total_length); rtcm_buffer->fragment_count = 0; rtcm_buffer->fragments_received = 0; @@ -2252,6 +2254,8 @@ void AP_GPS::Write_GPS(uint8_t i) sample_ms : last_message_time_ms(i), delta_ms : last_message_delta_time_ms(i), undulation : undulation, + rtcm_fragments_used: rtcm_stats.fragments_used, + rtcm_fragments_discarded: rtcm_stats.fragments_discarded }; AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 73cb0836bf..41ae0c35fa 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -717,6 +717,11 @@ private: uint8_t buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*4]; } *rtcm_buffer; + struct { + uint16_t fragments_used; + uint16_t fragments_discarded; + } rtcm_stats; + // re-assemble GPS_RTCM_DATA message void handle_gps_rtcm_data(const mavlink_message_t &msg); void handle_gps_inject(const mavlink_message_t &msg); diff --git a/libraries/AP_GPS/LogStructure.h b/libraries/AP_GPS/LogStructure.h index e34bed923f..ffdbd73db5 100644 --- a/libraries/AP_GPS/LogStructure.h +++ b/libraries/AP_GPS/LogStructure.h @@ -63,6 +63,8 @@ struct PACKED log_GPS { // @Field: SMS: time since system startup this sample was taken // @Field: Delta: system time delta between the last two reported positions // @Field: Und: Undulation +// @Field: RTCMFU: RTCM fragments used +// @Field: RTCMFD: RTCM fragments discarded struct PACKED log_GPA { LOG_PACKET_HEADER; uint64_t time_us; @@ -76,6 +78,8 @@ struct PACKED log_GPA { uint32_t sample_ms; uint16_t delta_ms; float undulation; + uint16_t rtcm_fragments_used; + uint16_t rtcm_fragments_discarded; }; /* @@ -202,7 +206,7 @@ struct PACKED log_GPS_RAWS { { LOG_GPS_MSG, sizeof(log_GPS), \ "GPS", "QBBIHBcLLeffffB", "TimeUS,I,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,Yaw,U", "s#---SmDUmnhnh-", "F----0BGGB000--" , true }, \ { LOG_GPA_MSG, sizeof(log_GPA), \ - "GPA", "QBCCCCfBIHf", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und", "s#mmmnd-ssm", "F-BBBB0-CC0" , true }, \ + "GPA", "QBCCCCfBIHfHH", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und,RTCMFU,RTCMFD", "s#mmmnd-ssm--", "F-BBBB0-CC0--" , true }, \ { LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \ "UBX1", "QBHBBHI", "TimeUS,Instance,noisePerMS,jamInd,aPower,agcCnt,config", "s#-----", "F------" , true }, \ { LOG_GPS_UBX2_MSG, sizeof(log_Ubx2), \