AP_GPS: Always send GPS2_RAW after we've seen the second GPS

This significantly improves the GCS's ability to correctly display whats
happening with the second GPS.
This commit is contained in:
Michael du Breuil 2019-10-28 10:14:52 -07:00 committed by Andrew Tridgell
parent 4838ee6f58
commit d623da0de6
2 changed files with 8 additions and 1 deletions

View File

@ -602,6 +602,9 @@ found_gps:
timing[instance].last_message_time_ms = now;
timing[instance].delta_time_ms = GPS_TIMEOUT_MS;
new_gps->broadcast_gps_type();
if (instance == 1) {
has_had_second_instance = true;
}
}
}
@ -989,7 +992,8 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
#if GPS_MAX_RECEIVERS > 1
void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
{
if (num_instances < 2 || status(1) <= AP_GPS::NO_GPS) {
// always send the message once we've ever seen the GPS
if (!has_had_second_instance) {
return;
}

View File

@ -579,6 +579,9 @@ private:
// used for flight testing with GPS loss
bool _force_disable_gps;
// used to ensure we continue sending status messages if we ever detected the second GPS
bool has_had_second_instance;
};
namespace AP {