AP_GPS: prevent announcing DroneCAN at 5Hz

This commit is contained in:
Andrew Tridgell 2024-02-22 08:00:06 +11:00
parent 642a5e13d0
commit 4fca2ee852
2 changed files with 4 additions and 2 deletions

View File

@ -1000,8 +1000,7 @@ void AP_GPS::update_instance(uint8_t instance)
state[instance].corrected_timestamp_updated = false;
}
// we set delta_time_ms to the timeout value when clearing
// state; use it being zero to mark first message
// announce the GPS type once
if (!state[instance].announced_detection) {
state[instance].announced_detection = true;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: detected %s", instance + 1, drivers[instance]->name());

View File

@ -717,6 +717,9 @@ bool AP_GPS_DroneCAN::read(void)
interim_state.vertical_accuracy = MIN(interim_state.vertical_accuracy, 1000.0);
interim_state.speed_accuracy = MIN(interim_state.speed_accuracy, 1000.0);
// prevent announcing multiple times
interim_state.announced_detection = state.announced_detection;
state = interim_state;
if (interim_state.last_corrected_gps_time_us) {
// If we were able to get a valid last_corrected_gps_time_us