From 0066022a7ff364d377314051a02e5dd947eb6abc Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 8 Mar 2017 18:49:58 +0900
Subject: [PATCH] AP_GPS: check for valid instance in highest_supported_status

highest_supported_status will always return FIX_3D for blended or invalid instance
setHIL_Accuracy checks instance is 2 or less
send_mavlink_gps2_raw uses num_instances variable directly to avoid confusion with num_sensors
---
 libraries/AP_GPS/AP_GPS.cpp | 7 +++++--
 libraries/AP_GPS/AP_GPS.h   | 2 +-
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index b83f3d9951..ad9a3cf9bf 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -501,7 +501,7 @@ found_gps:
 AP_GPS::GPS_Status 
 AP_GPS::highest_supported_status(uint8_t instance) const
 {
-    if (drivers[instance] != nullptr) {
+    if (instance < GPS_MAX_RECEIVERS && drivers[instance] != nullptr) {
         return drivers[instance]->highest_supported_status();
     }
     return AP_GPS::GPS_OK_FIX_3D;
@@ -710,6 +710,9 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
 // set accuracy for HIL
 void AP_GPS::setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc, bool _have_vertical_velocity, uint32_t sample_ms)
 {
+    if (instance >= GPS_MAX_RECEIVERS) {
+        return;
+    }
     GPS_State &istate = state[instance];
     istate.vdop = vdop * 100;
     istate.horizontal_accuracy = hacc;
@@ -803,7 +806,7 @@ void
 AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
 {
     static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS];
-    if (num_sensors() < 2 || status(1) <= AP_GPS::NO_GPS) {
+    if (num_instances < 2 || status(1) <= AP_GPS::NO_GPS) {
         return;
     }
     // when we have a GPS then only send new data
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index 35dca3df50..ed7e74f489 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -166,7 +166,7 @@ public:
         return status(primary_instance);
     }
 
-    // Query the highest status this GPS supports
+    // Query the highest status this GPS supports (always reports GPS_OK_FIX_3D for the blended GPS)
     GPS_Status highest_supported_status(uint8_t instance) const;
 
     // location of last fix