Skip to content

Commit 21a73eb

Browse files
committed
AP_GPS: AP_GPS_DroneCAN: limit rate of base-message warnings
1 parent cae92d8 commit 21a73eb

File tree

2 files changed

+6
-1
lines changed

2 files changed

+6
-1
lines changed

libraries/AP_GPS/AP_GPS_DroneCAN.cpp

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -549,7 +549,11 @@ void AP_GPS_DroneCAN::handle_moving_baseline_msg(const ardupilot_gnss_MovingBase
549549
{
550550
WITH_SEMAPHORE(sem);
551551
if (role != AP_GPS::GPS_ROLE_MB_BASE) {
552-
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Incorrect Role set for DroneCAN GPS, %d should be Base", node_id);
552+
const uint32_t now_ms = AP_HAL::millis();
553+
if (now_ms - last_base_warning_ms > 5000) {
554+
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Incorrect Role set for DroneCAN GPS, %d should be Base", node_id);
555+
last_base_warning_ms = now_ms;
556+
}
553557
return;
554558
}
555559

libraries/AP_GPS/AP_GPS_DroneCAN.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -132,6 +132,7 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
132132
#if GPS_MOVING_BASELINE
133133
// RTCM3 parser for when in moving baseline base mode
134134
RTCM3_Parser *rtcm3_parser;
135+
uint32_t last_base_warning_ms;
135136
#endif
136137
// the role set from GPS_TYPE
137138
AP_GPS::GPS_Role role;

0 commit comments

Comments
 (0)