From 729ce34ce30eaf693fe508de03d74eed2dd6d779 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 9 Mar 2017 09:47:40 +0900
Subject: [PATCH] AP_GPS: add all_consistent and blend health for use as
 pre-arm check

---
 libraries/AP_GPS/AP_GPS.cpp | 30 ++++++++++++++++++++++++++++++
 libraries/AP_GPS/AP_GPS.h   |  7 +++++++
 2 files changed, 37 insertions(+)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 8b436c2931..0a6163d7d3 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -41,6 +41,7 @@
 #define BLEND_MASK_USE_HPOS_ACC     1
 #define BLEND_MASK_USE_VPOS_ACC     2
 #define BLEND_MASK_USE_SPD_ACC      4
+#define BLEND_COUNTER_FAILURE_INCREMENT 10
 
 extern const AP_HAL::HAL &hal;
 
@@ -632,6 +633,12 @@ AP_GPS::update(void)
     // if blending is requested, attempt to calculate weighting for each GPS
     if (_auto_switch == 2) {
         _output_is_blended = calc_blend_weights();
+        // adjust blend health counter
+        if (!_output_is_blended) {
+            _blend_health_counter = MIN(_blend_health_counter+BLEND_COUNTER_FAILURE_INCREMENT, 100);
+        } else if (_blend_health_counter > 0) {
+            _blend_health_counter--;
+        }
     } else {
         _output_is_blended = false;
     }
@@ -915,6 +922,29 @@ AP_GPS::broadcast_first_configuration_failure_reason(void) const {
     }
 }
 
+// pre-arm check that all GPSs are close to each other.  farthest distance between GPSs (in meters) is returned
+bool AP_GPS::all_consistent(float &distance) const
+{
+    // return true immediately if only one valid receiver
+    if (num_instances <= 1 ||
+        drivers[0] == nullptr || _type[0] == GPS_TYPE_NONE ||
+        drivers[1] == nullptr || _type[1] == GPS_TYPE_NONE) {
+        distance = 0;
+        return true;
+    }
+
+    // calculate distance
+    distance = location_3d_diff_NED(state[0].location, state[1].location).length();
+    // success if distance is within 50m
+    return (distance < 50);
+}
+
+// pre-arm check of GPS blending.  True means healthy or that blending is not being used
+bool AP_GPS::blend_healthy() const
+{
+    return (_blend_health_counter < 50);
+}
+
 void
 AP_GPS::_broadcast_gps_type(const char *type, uint8_t instance, int8_t baud_index)
 {
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index 50921f1232..f2d6d952c0 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -341,6 +341,12 @@ public:
         return first_unconfigured_gps() == GPS_ALL_CONFIGURED;
     }
 
+    // pre-arm check that all GPSs are close to each other.  farthest distance between GPSs (in meters) is returned
+    bool all_consistent(float &distance) const;
+
+    // pre-arm check of GPS blending.  True means healthy or that blending is not being used
+    bool blend_healthy() const;
+
     // handle sending of initialisation strings to the GPS - only used by backends
     void send_blob_start(uint8_t instance, const char *_blob, uint16_t size);
     void send_blob_update(uint8_t instance);
@@ -469,6 +475,7 @@ private:
     uint32_t _last_time_updated[GPS_MAX_RECEIVERS]; // the last value of state.last_gps_time_ms read for that GPS instance - used to detect new data.
     float _omega_lpf; // cutoff frequency in rad/sec of LPF applied to position offsets
     bool _output_is_blended; // true when a blended GPS solution being output
+    uint8_t _blend_health_counter;  // 0 = perfectly health, 100 = very unhealthy
 
     // calculate the blend weight.  Returns true if blend could be calculated, false if not
     bool calc_blend_weights(void);