AP_GPS: create real AP_GPS_Blended backend

Author: peterbarker

libraries/AP_Arming/AP_Arming.cpp

@@ -649,12 +649,6 @@ bool AP_Arming::gps_checks(bool report)
                          (double)distance_m);
             return false;
         }
-#if AP_GPS_BLENDED_ENABLED
-        if (!gps.blend_health_check()) {
-            check_failed(ARMING_CHECK_GPS, report, "GPS blending unhealthy");
-            return false;
-        }
-#endif
 
         // check AHRS and GPS are within 10m of each other
         if (gps.num_sensors() > 0) {

libraries/AP_GPS/AP_GPS.cpp

@@ -29,6 +29,7 @@
 #include <AP_SerialManager/AP_SerialManager.h>
 
 #include "AP_GPS_NOVA.h"
+#include "AP_GPS_Blended.h"
 #include "AP_GPS_ERB.h"
 #include "AP_GPS_GSOF.h"
 #include "AP_GPS_NMEA.h"
@@ -66,8 +67,6 @@
 #define GPS_BAUD_TIME_MS 1200
 #define GPS_TIMEOUT_MS 4000u
 
-#define BLEND_COUNTER_FAILURE_INCREMENT 10
-
 extern const AP_HAL::HAL &hal;
 
 // baudrates to try to detect GPSes with
@@ -347,6 +346,11 @@ void AP_GPS::init()
             rate_ms.set(GPS_MAX_RATE_MS);
         }
     }
+
+    // create the blended instance if appropriate:
+#if AP_GPS_BLENDED_ENABLED
+    drivers[GPS_BLENDED_INSTANCE] = new AP_GPS_Blended(*this, params[GPS_BLENDED_INSTANCE], state[GPS_BLENDED_INSTANCE], timing[GPS_BLENDED_INSTANCE]);
+#endif
 }
 
 void AP_GPS::convert_parameters()
@@ -1084,25 +1088,15 @@ void AP_GPS::update_primary(void)
     */
     const bool using_moving_base = is_rtk_base(0) || is_rtk_base(1);
     if ((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::BLEND && !using_moving_base) {
-        _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--;
-        }
-        // stop blending if unhealthy
-        if (_blend_health_counter >= 50) {
-            _output_is_blended = false;
-        }
+        _output_is_blended = ((AP_GPS_Blended*)drivers[GPS_BLENDED_INSTANCE])->calc_weights();
     } else {
         _output_is_blended = false;
-        _blend_health_counter = 0;
+        ((AP_GPS_Blended*)drivers[GPS_BLENDED_INSTANCE])->zero_health_counter();
     }
 
     if (_output_is_blended) {
         // Use the weighting to calculate blended GPS states
-        calc_blended_state();
+        ((AP_GPS_Blended*)drivers[GPS_BLENDED_INSTANCE])->calc_state();
         // set primary to the virtual instance
         primary_instance = GPS_BLENDED_INSTANCE;
         return;
@@ -1695,10 +1689,7 @@ bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const
 #if AP_GPS_BLENDED_ENABLED
     // return lag of blended GPS
     if (instance == GPS_BLENDED_INSTANCE) {
-        lag_sec = _blended_lag_sec;
-        // auto switching uses all GPS receivers, so all must be configured
-        uint8_t inst; // we don't actually care what the number is, but must pass it
-        return first_unconfigured_gps(inst);
+        return drivers[instance]->get_lag(lag_sec);
     }
 #endif
 
@@ -1732,7 +1723,7 @@ const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const
 #if AP_GPS_BLENDED_ENABLED
     if (instance == GPS_BLENDED_INSTANCE) {
         // return an offset for the blended GPS solution
-        return _blended_antenna_offset;
+        return ((AP_GPS_Blended*)drivers[instance])->get_antenna_offset();
     }
 #endif
 
@@ -1788,12 +1779,6 @@ bool AP_GPS::is_healthy(uint8_t instance) const
     }
 #endif // HAL_BUILD_AP_PERIPH
 
-#if AP_GPS_BLENDED_ENABLED
-    if (instance == GPS_BLENDED_INSTANCE) {
-        return blend_health_check();
-    }
-#endif
-
     return drivers[instance] != nullptr &&
            drivers[instance]->is_healthy();
 }
@@ -1827,6 +1812,14 @@ bool AP_GPS::backends_healthy(char failure_msg[], uint16_t failure_msg_len) {
             }
         }
     }
+
+#if defined(GPS_BLENDED_INSTANCE)
+    if (!drivers[GPS_BLENDED_INSTANCE]->is_healthy()) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "GPS blending unhealthy");
+        return false;
+    }
+#endif
+
     return true;
 }
 

libraries/AP_GPS/AP_GPS.h

@@ -52,6 +52,7 @@ class RTCM3_Parser;
 /// GPS driver main class
 class AP_GPS
 {
+    friend class AP_GPS_Blended;
     friend class AP_GPS_ERB;
     friend class AP_GPS_GSOF;
     friend class AP_GPS_MAV;
@@ -512,9 +513,6 @@ class AP_GPS
     // 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.  False if blending is unhealthy, True if healthy or blending is not being used
-    bool blend_health_check() const;
-
     // handle sending of initialisation strings to the GPS - only used by backends
     void send_blob_start(uint8_t instance);
     void send_blob_start(uint8_t instance, const char *_blob, uint16_t size);
@@ -606,7 +604,7 @@ class AP_GPS
 protected:
 
     // configuration parameters
-    Params params[GPS_MAX_RECEIVERS];
+    Params params[GPS_MAX_INSTANCES];
     AP_Int8 _navfilter;
     AP_Int8 _auto_switch;
     AP_Int16 _sbp_logmask;
@@ -668,7 +666,7 @@ class AP_GPS
     // Note allowance for an additional instance to contain blended data
     GPS_timing timing[GPS_MAX_INSTANCES];
     GPS_State state[GPS_MAX_INSTANCES];
-    AP_GPS_Backend *drivers[GPS_MAX_RECEIVERS];
+    AP_GPS_Backend *drivers[GPS_MAX_INSTANCES];
     AP_HAL::UARTDriver *_port[GPS_MAX_RECEIVERS];
 
     /// primary GPS instance
@@ -756,18 +754,7 @@ class AP_GPS
     void inject_data(uint8_t instance, const uint8_t *data, uint16_t len);
 
 #if AP_GPS_BLENDED_ENABLED
-    // GPS blending and switching
-    Vector3f _blended_antenna_offset; // blended antenna offset
-    float _blended_lag_sec; // blended receiver lag in seconds
-    float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
     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);
-
-    // calculate the blended state
-    void calc_blended_state(void);
 #endif
 
     bool should_log() const;