AP_GPS: Add option to blend multiple GPS receivers

Fix pre-existing bug in hard switch logic
Update GPS_AUTO_SWITCH description
protect against zero accuracy estimates returned by GPS drivers

libraries/AP_GPS/AP_GPS.cpp

@@ -35,6 +35,13 @@
 #include "AP_GPS_MAV.h"
 #include "GPS_Backend.h"
 
+#define GPS_BAUD_TIME_MS 1200
+
+// defines used to specify the mask position for use of different accuracy metrics in the blending algorithm
+#define BLEND_MASK_USE_HPOS_ACC     1
+#define BLEND_MASK_USE_VPOS_ACC     2
+#define BLEND_MASK_USE_SPD_ACC      4
+
 extern const AP_HAL::HAL &hal;
 
 // table of user settable parameters
@@ -65,7 +72,8 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Param: AUTO_SWITCH
     // @DisplayName: Automatic Switchover Setting
     // @Description: Automatic switchover to GPS reporting best lock
-    // @Values: 0:Disabled,1:Enabled
+    // @Values: 0:Disabled,1:UseBest,2:Blend
+    // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, 1),
 
@@ -213,6 +221,22 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Range: 0 250
     // @User: Advanced
     AP_GROUPINFO("DELAY_MS2", 19, AP_GPS, _delay_ms[1], 0),
+
+    // @Param: BLEND_MASK
+    // @DisplayName: Multi GPS Blending Mask
+    // @Description: Determines which of the accuracy measures Horizontal position, Vertical Position and Speed are used to calculate the weighting on each GPS receiver when soft switching has been selected by setting GPS_AUTO_SWITCH to 2
+    // @Bitmask: 0:Horiz Pos,1:Vert Pos,2:Speed
+    // @User: Advanced
+    AP_GROUPINFO("BLEND_MASK", 20, AP_GPS, _blend_mask, 5),
+
+    // @Param: BLEND_TC
+    // @DisplayName: Blending time constant
+    // @Description: Controls the slowest time constant applied to the calculation of GPS position and height offsets used to adjust different GPS receivers for steady state position differences.
+    // @Units: seconds
+    // @Range: 5.0 30.0
+    // @User: Advanced
+    AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f),
+
     AP_GROUPEND
 };
 
@@ -226,6 +250,10 @@ void AP_GPS::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_man
     _port[0] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, 0);
     _port[1] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, 1);
     _last_instance_swap_ms = 0;
+
+    // Initialise class variables used to do GPS blending
+    _omega_lpf = 1.0f / constrain_float(_blend_tc, 5.0f, 30.0f);
+
 }
 
 // baudrates to try to detect GPSes with
@@ -536,51 +564,90 @@ AP_GPS::update_instance(uint8_t instance)
 void
 AP_GPS::update(void)
 {
-    for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
         update_instance(i);
     }
 
-    // work out which GPS is the primary, and how many sensors we have
-    for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
+    // calculate number of instances
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
         if (state[i].status != NO_GPS) {
             num_instances = i+1;
         }
-        if (_auto_switch) {            
-            if (i == primary_instance) {
-                continue;
-            }
-            if (state[i].status > state[primary_instance].status) {
-                // we have a higher status lock, change GPS
-                primary_instance = i;
-                continue;
-            }
-
-            bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);
-
-            if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) {
+    }
 
-                uint32_t now = AP_HAL::millis();
-                bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);
+    // if blending is requested, attempt to calculate weighting for each GPS
+    if (_auto_switch == 2) {
+        _output_is_blended = calc_blend_weights();
+    } else {
+        _output_is_blended = false;
+    }
 
-                if ( (another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
-                     (another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000 ) ) {
-                // this GPS has more satellites than the
-                // current primary, switch primary. Once we switch we will
-                // then tend to stick to the new GPS as primary. We don't
-                // want to switch too often as it will look like a
-                // position shift to the controllers.
-                primary_instance = i;
-                _last_instance_swap_ms = now;
+    if (_output_is_blended) {
+        // Use the weighting to calculate blended GPS states
+        calc_blended_state();
+        // set primary to the virtual instance
+        primary_instance = GPS_MAX_RECEIVERS;
+    } else {
+        // use switch logic to find best GPS
+        uint32_t now = AP_HAL::millis();
+        if (_auto_switch >= 1) {
+            // handling switching away from blended GPS
+            if (primary_instance == GPS_MAX_RECEIVERS) {
+                primary_instance = 0;
+                for (uint8_t i=1; i<GPS_MAX_RECEIVERS; i++) {
+                    // choose GPS with highest state or higher number of satellites
+                    if ((state[i].status > state[primary_instance].status) ||
+                        ((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) {
+                        primary_instance = i;
+                        _last_instance_swap_ms = now;
+                    }
+                }
+            } else {
+                // handle switch between real GPSs
+                for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+                    if (i == primary_instance) {
+                        continue;
+                    }
+                    if (state[i].status > state[primary_instance].status) {
+                        // we have a higher status lock, or primary is set to the blended GPS, change GPS
+                        primary_instance = i;
+                        _last_instance_swap_ms = now;
+                        continue;
+                    }
+
+                    bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);
+
+                    if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) {
+
+                        bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);
+
+                        if ( (another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
+                             (another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000 ) ) {
+                            // this GPS has more satellites than the
+                            // current primary, switch primary. Once we switch we will
+                            // then tend to stick to the new GPS as primary. We don't
+                            // want to switch too often as it will look like a
+                            // position shift to the controllers.
+                            primary_instance = i;
+                            _last_instance_swap_ms = now;
+                        }
+                    }
                 }
             }
         } else {
+            // AUTO_SWITCH is 0 so no switching of GPSs
             primary_instance = 0;
         }
+
+        // copy the primary instance to the blended instance in case it is enabled later
+        state[GPS_MAX_RECEIVERS] = state[primary_instance];
+        _blended_antenna_offset = _antenna_offset[primary_instance];
     }
 
     // update notify with gps status. We always base this on the primary_instance
     AP_Notify::flags.gps_status = state[primary_instance].status;
     AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats;
+
 }
 
 /*
@@ -610,7 +677,7 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
                const Location &_location, const Vector3f &_velocity, uint8_t _num_sats, 
                uint16_t hdop)
 {
-    if (instance >= GPS_MAX_INSTANCES) {
+    if (instance >= GPS_MAX_RECEIVERS) {
         return;
     }
     uint32_t tnow = AP_HAL::millis();
@@ -659,7 +726,7 @@ void
 AP_GPS::lock_port(uint8_t instance, bool lock)
 {
 
-    if (instance >= GPS_MAX_INSTANCES) {
+    if (instance >= GPS_MAX_RECEIVERS) {
         return;
     }
     if (lock) {
@@ -675,7 +742,7 @@ AP_GPS::inject_data(uint8_t *data, uint8_t len)
 {
     //Support broadcasting to all GPSes.
     if (_inject_to == GPS_RTK_INJECT_TO_ALL) {
-        for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
+        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
             inject_data(i, data, len);
         }
     } else {
@@ -686,7 +753,7 @@ AP_GPS::inject_data(uint8_t *data, uint8_t len)
 void 
 AP_GPS::inject_data(uint8_t instance, uint8_t *data, uint8_t len)
 {
-    if (instance < GPS_MAX_INSTANCES && drivers[instance] != nullptr) {
+    if (instance < GPS_MAX_RECEIVERS && drivers[instance] != nullptr) {
         drivers[instance]->inject_data(data, len);
     }
 }  
@@ -773,7 +840,7 @@ AP_GPS::send_mavlink_gps2_rtk(mavlink_channel_t chan)
 uint8_t
 AP_GPS::first_unconfigured_gps(void) const
 {
-    for(int i = 0; i < GPS_MAX_INSTANCES; i++) {
+    for(int i = 0; i < GPS_MAX_RECEIVERS; i++) {
         if(_type[i] != GPS_TYPE_NONE && (drivers[i] == nullptr || !drivers[i]->is_configured())) {
             return i;
         }
@@ -896,10 +963,15 @@ void AP_GPS::inject_data_all(const uint8_t *data, uint16_t len)
 }
 
 /*
-  return expected lag from a GPS
+  return the expected lag (in seconds) in the position and velocity readings from the gps
  */
 float AP_GPS::get_lag(uint8_t instance) const
 {
+    // return lag of blended GPS
+    if (instance == GPS_MAX_RECEIVERS) {
+        return _blended_lag_sec;
+    }
+
     if (_delay_ms[instance] > 0) {
         // if the user has specified a non zero time delay, always return that value
         return 0.001f * (float)_delay_ms[instance];
@@ -912,3 +984,396 @@ float AP_GPS::get_lag(uint8_t instance) const
         return drivers[instance]->get_lag();
     }
 }
+
+/*
+ calculate the weightings used to blend GPs location and velocity data
+*/
+bool AP_GPS::calc_blend_weights(void)
+{
+    // zero the blend weights
+    memset(&_blend_weights, 0, sizeof(_blend_weights));
+
+    // exit immediately if not enough receivers to do blending
+    if (num_instances < 2) {
+        return false;
+    }
+
+    // Use the oldest non-zero time, but if time difference is excessive, use newest to prevent a disconnected receiver from blocking updates
+    uint32_t max_ms = 0; // newest non-zero system time of arrival of a GPS message
+    uint32_t min_ms = -1; // oldest non-zero system time of arrival of a GPS message
+    int16_t max_rate_ms = 0; // largest update interval of a GPS receiver
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+        // Find largest and smallest times
+        if (state[i].last_gps_time_ms > max_ms) {
+            max_ms = state[i].last_gps_time_ms;
+        }
+        if ((state[i].last_gps_time_ms < min_ms) && (state[i].last_gps_time_ms > 0)) {
+            min_ms = state[i].last_gps_time_ms;
+        }
+        if (_rate_ms[i] > max_rate_ms) {
+            max_rate_ms = _rate_ms[i];
+        }
+    }
+    if ((int32_t)(max_ms - min_ms) < (int32_t)(2 * max_rate_ms)) {
+        // data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated
+        state[GPS_MAX_RECEIVERS].last_gps_time_ms = min_ms;
+    } else {
+        // receiver data has timed out so fail out of blending
+        state[GPS_MAX_RECEIVERS].last_gps_time_ms = max_ms;
+        return false;
+    }
+
+    // calculate the sum squared speed accuracy across all GPS sensors
+    float speed_accuracy_sum_sq = 0.0f;
+    if (_blend_mask & BLEND_MASK_USE_SPD_ACC) {
+        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+            if (state[i].status >= GPS_OK_FIX_3D) {
+                if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f) {
+                    speed_accuracy_sum_sq += state[i].speed_accuracy * state[i].speed_accuracy;
+                } else {
+                    // not all receivers support this metric so set it to zero and don't use it
+                    speed_accuracy_sum_sq = 0.0f;
+                    break;
+                }
+            }
+        }
+    }
+
+    // calculate the sum squared horizontal position accuracy across all GPS sensors
+    float horizontal_accuracy_sum_sq = 0.0f;
+    if (_blend_mask & BLEND_MASK_USE_HPOS_ACC) {
+        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+            if (state[i].status >= GPS_OK_FIX_2D) {
+                if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f) {
+                    horizontal_accuracy_sum_sq += state[i].horizontal_accuracy * state[i].horizontal_accuracy;
+                } else {
+                    // not all receivers support this metric so set it to zero and don't use it
+                    horizontal_accuracy_sum_sq = 0.0f;
+                    break;
+                }
+            }
+        }
+    }
+
+    // calculate the sum squared vertical position accuracy across all GPS sensors
+    float vertical_accuracy_sum_sq = 0.0f;
+    if (_blend_mask & BLEND_MASK_USE_VPOS_ACC) {
+        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+            if (state[i].status >= GPS_OK_FIX_3D) {
+                if (state[i].have_vertical_accuracy && state[i].vertical_accuracy > 0.0f) {
+                    vertical_accuracy_sum_sq += state[i].vertical_accuracy * state[i].vertical_accuracy;
+                } else {
+                    // not all receivers support this metric so set it to zero and don't use it
+                    vertical_accuracy_sum_sq = 0.0f;
+                    break;
+                }
+            }
+        }
+    }
+    // Check if we can do blending using reported accuracy
+    bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f || speed_accuracy_sum_sq > 0.0f);
+
+    // if we cant do blending using reported accuracy, return false and hard switch logic will be used instead
+    if (!can_do_blending) {
+        return false;
+    }
+
+    float sum_of_all_weights = 0.0f;
+
+    // calculate a weighting using the reported horizontal position
+    float hpos_blend_weights[GPS_MAX_RECEIVERS] = {};
+    if (horizontal_accuracy_sum_sq > 0.0f && (_blend_mask & BLEND_MASK_USE_HPOS_ACC)) {
+        // calculate the weights using the inverse of the variances
+        float sum_of_hpos_weights = 0.0f;
+        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+            if (state[i].status >= GPS_OK_FIX_2D && state[i].horizontal_accuracy > 0.001f) {
+                hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (state[i].horizontal_accuracy * state[i].horizontal_accuracy);
+                sum_of_hpos_weights += hpos_blend_weights[i];
+            }
+        }
+        // normalise the weights
+        if (sum_of_hpos_weights > 0.0f) {
+            for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+                hpos_blend_weights[i] = hpos_blend_weights[i] / sum_of_hpos_weights;
+            }
+            sum_of_all_weights += 1.0f;
+        }
+    }
+
+    // calculate a weighting using the reported vertical position accuracy
+    float vpos_blend_weights[GPS_MAX_RECEIVERS];
+    if (vertical_accuracy_sum_sq > 0.0f && (_blend_mask & BLEND_MASK_USE_VPOS_ACC)) {
+        // calculate the weights using the inverse of the variances
+        float sum_of_vpos_weights = 0.0f;
+        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+            if (state[i].status >= GPS_OK_FIX_3D && state[i].vertical_accuracy > 0.001f) {
+                vpos_blend_weights[i] = vertical_accuracy_sum_sq / (state[i].vertical_accuracy * state[i].vertical_accuracy);
+                sum_of_vpos_weights += vpos_blend_weights[i];
+            } else {
+                vpos_blend_weights[i] = 0.0f;
+            }
+        }
+        // normalise the weights
+        if (sum_of_vpos_weights > 0.0f) {
+            for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+                vpos_blend_weights[i] = vpos_blend_weights[i] / sum_of_vpos_weights;
+            }
+            sum_of_all_weights += 1.0f;
+        };
+    } else {
+        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+            vpos_blend_weights[i] = 0.0f;
+        }
+    }
+
+    // calculate a weighting using the reported speed accuracy
+    float spd_blend_weights[GPS_MAX_RECEIVERS];
+    if (speed_accuracy_sum_sq > 0.0f && (_blend_mask & BLEND_MASK_USE_SPD_ACC)) {
+        // calculate the weights using the inverse of the variances
+        float sum_of_spd_weights = 0.0f;
+        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+            if (state[i].status >= GPS_OK_FIX_3D && state[i].speed_accuracy > 0.001f) {
+                spd_blend_weights[i] = speed_accuracy_sum_sq / (state[i].speed_accuracy * state[i].speed_accuracy);
+                sum_of_spd_weights += spd_blend_weights[i];
+            } else {
+                spd_blend_weights[i] = 0.0f;
+            }
+        }
+        // normalise the weights
+        if (sum_of_spd_weights > 0.0f) {
+            for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+                spd_blend_weights[i] = spd_blend_weights[i] / sum_of_spd_weights;
+            }
+            sum_of_all_weights += 1.0f;
+        }
+    } else {
+        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+            spd_blend_weights[i] = 0.0f;
+        }
+    }
+
+    // calculate an overall weight
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+        _blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights;
+    }
+
+    return true;
+}
+
+/*
+ calculate a blended GPS state
+*/
+void AP_GPS::calc_blended_state(void)
+{
+    // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver
+    state[GPS_MAX_RECEIVERS].instance = GPS_MAX_RECEIVERS;
+    state[GPS_MAX_RECEIVERS].status = NO_GPS;
+    state[GPS_MAX_RECEIVERS].time_week_ms = 0;
+    state[GPS_MAX_RECEIVERS].time_week = 0;
+    state[GPS_MAX_RECEIVERS].ground_speed = 0.0f;
+    state[GPS_MAX_RECEIVERS].ground_course = 0.0f;
+    state[GPS_MAX_RECEIVERS].hdop = 9999;
+    state[GPS_MAX_RECEIVERS].vdop = 9999;
+    state[GPS_MAX_RECEIVERS].num_sats = 0;
+    state[GPS_MAX_RECEIVERS].velocity.zero();
+    state[GPS_MAX_RECEIVERS].speed_accuracy = 1e6f;
+    state[GPS_MAX_RECEIVERS].horizontal_accuracy = 1e6f;
+    state[GPS_MAX_RECEIVERS].vertical_accuracy = 1e6f;
+    state[GPS_MAX_RECEIVERS].have_vertical_velocity = false;
+    state[GPS_MAX_RECEIVERS].have_speed_accuracy = false;
+    state[GPS_MAX_RECEIVERS].have_horizontal_accuracy = false;
+    state[GPS_MAX_RECEIVERS].have_vertical_accuracy = false;
+    state[GPS_MAX_RECEIVERS].last_gps_time_ms = 0;
+    memset(&state[GPS_MAX_RECEIVERS].location, 0, sizeof(state[GPS_MAX_RECEIVERS].location));
+
+    _blended_antenna_offset.zero();
+    _blended_lag_sec = 0;
+
+    timing[GPS_MAX_RECEIVERS].last_fix_time_ms = 0;
+    timing[GPS_MAX_RECEIVERS].last_message_time_ms = 0;
+
+    // combine the states into a blended solution
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+        // use the highest status
+        if (state[i].status > state[GPS_MAX_RECEIVERS].status) {
+            state[GPS_MAX_RECEIVERS].status = state[i].status;
+        }
+
+        // calculate a blended average velocity
+        state[GPS_MAX_RECEIVERS].velocity += state[i].velocity * _blend_weights[i];
+
+        // report the best valid accuracies and DOP metrics
+
+        if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f && state[i].horizontal_accuracy < state[GPS_MAX_RECEIVERS].horizontal_accuracy) {
+            state[GPS_MAX_RECEIVERS].have_horizontal_accuracy = true;
+            state[GPS_MAX_RECEIVERS].horizontal_accuracy = state[i].horizontal_accuracy;
+        }
+
+        if (state[i].have_vertical_accuracy && state[i].vertical_accuracy > 0.0f && state[i].vertical_accuracy < state[GPS_MAX_RECEIVERS].vertical_accuracy) {
+            state[GPS_MAX_RECEIVERS].have_vertical_accuracy = true;
+            state[GPS_MAX_RECEIVERS].vertical_accuracy = state[i].vertical_accuracy;
+        }
+
+        if (state[i].have_vertical_velocity ) {
+            state[GPS_MAX_RECEIVERS].have_vertical_velocity = true;
+        }
+
+        if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f && state[i].speed_accuracy < state[GPS_MAX_RECEIVERS].speed_accuracy) {
+            state[GPS_MAX_RECEIVERS].have_speed_accuracy = true;
+            state[GPS_MAX_RECEIVERS].speed_accuracy = state[i].speed_accuracy;
+        }
+
+        if (state[i].hdop > 0 && state[i].hdop < state[GPS_MAX_RECEIVERS].hdop) {
+            state[GPS_MAX_RECEIVERS].hdop = state[i].hdop;
+        }
+
+        if (state[i].vdop > 0 && state[i].vdop < state[GPS_MAX_RECEIVERS].vdop) {
+            state[GPS_MAX_RECEIVERS].vdop = state[i].vdop;
+        }
+
+        if (state[i].num_sats > 0 && state[i].num_sats > state[GPS_MAX_RECEIVERS].num_sats) {
+            state[GPS_MAX_RECEIVERS].num_sats = state[i].num_sats;
+        }
+
+        // report a blended average GPS antenna position
+        Vector3f temp_antenna_offset = _antenna_offset[i];
+        temp_antenna_offset *= _blend_weights[i];
+        _blended_antenna_offset += temp_antenna_offset;
+
+        // blend the timing data
+        if (timing[i].last_fix_time_ms > timing[GPS_MAX_RECEIVERS].last_fix_time_ms) {
+            timing[GPS_MAX_RECEIVERS].last_fix_time_ms = timing[i].last_fix_time_ms;
+        }
+        if (timing[i].last_message_time_ms > timing[GPS_MAX_RECEIVERS].last_message_time_ms) {
+            timing[GPS_MAX_RECEIVERS].last_message_time_ms = timing[i].last_message_time_ms;
+        }
+
+    }
+
+    /*
+     * Calculate an instantaneous weighted/blended average location from the available GPS instances and store in the _output_state.
+     * This will be statisticaly the most likely location, but will be not stable enough for direct use by the autopilot.
+    */
+
+    // Use the GPS with the highest weighting as the reference position
+    float best_weight = 0.0f;
+    uint8_t best_index = 0;
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+        if (_blend_weights[i] > best_weight) {
+            best_weight = _blend_weights[i];
+            best_index = i;
+            state[GPS_MAX_RECEIVERS].location = state[i].location;
+        }
+    }
+
+    // Calculate the weighted sum of horizontal and vertical position offsets relative to the reference position
+    Vector2f blended_NE_offset_m;
+    float blended_alt_offset_cm = 0.0f;
+    blended_NE_offset_m.zero();
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+        if (_blend_weights[i] > 0.0f && i != best_index) {
+            blended_NE_offset_m += location_diff(state[GPS_MAX_RECEIVERS].location, state[i].location) * _blend_weights[i];
+            blended_alt_offset_cm += (float)(state[i].location.alt - state[GPS_MAX_RECEIVERS].location.alt) * _blend_weights[i];
+        }
+    }
+
+    // Add the sum of weighted offsets to the reference location to obtain the blended location
+    location_offset(state[GPS_MAX_RECEIVERS].location, blended_NE_offset_m.x, blended_NE_offset_m.y);
+    state[GPS_MAX_RECEIVERS].location.alt += (int)blended_alt_offset_cm;
+
+    // Calculate ground speed and course from blended velocity vector
+    state[GPS_MAX_RECEIVERS].ground_speed = norm(state[GPS_MAX_RECEIVERS].velocity.x, state[GPS_MAX_RECEIVERS].velocity.y);
+    state[GPS_MAX_RECEIVERS].ground_course = wrap_360(degrees(atan2f(state[GPS_MAX_RECEIVERS].velocity.x, state[GPS_MAX_RECEIVERS].velocity.x)));
+
+    /*
+     * The blended location in _output_state.location is not stable enough to be used by the autopilot
+     * as it will move around as the relative accuracy changes. To mitigate this effect a low-pass filtered
+     * offset from each GPS location to the blended location is calculated and the filtered offset is applied
+     * to each receiver.
+    */
+
+    // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset
+    // A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering
+    float alpha[GPS_MAX_RECEIVERS] = {};
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+        if (state[i].last_gps_time_ms - _last_time_updated[i] > 0) {
+            float min_alpha = constrain_float(_omega_lpf * 0.001f * (float)(state[i].last_gps_time_ms - _last_time_updated[i]), 0.0f, 1.0f);
+            if (_blend_weights[i] > min_alpha) {
+                alpha[i] = min_alpha / _blend_weights[i];
+            } else {
+                alpha[i] = 1.0f;
+            }
+            _last_time_updated[i] = state[i].last_gps_time_ms;
+        }
+    }
+
+    // Calculate the offset from each GPS solution to the blended solution
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+        _NE_pos_offset_m[i] = location_diff(state[i].location, state[GPS_MAX_RECEIVERS].location) * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]);
+        _hgt_offset_cm[i] = (float)(state[GPS_MAX_RECEIVERS].location.alt - state[i].location.alt) *  alpha[i] + _hgt_offset_cm[i] * (1.0f - alpha[i]);
+    }
+
+    // Calculate a corrected location for each GPS
+    Location corrected_location[GPS_MAX_RECEIVERS];
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+        corrected_location[i] = state[i].location;
+        location_offset(corrected_location[i], _NE_pos_offset_m[i].x, _NE_pos_offset_m[i].y);
+        corrected_location[i].alt += (int)(_hgt_offset_cm[i]);
+    }
+
+    // Calculate the weighted sum of horizontal and vertical position offsets relative to the blended location
+    blended_alt_offset_cm = 0.0f;
+    blended_NE_offset_m.zero();
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+        if (_blend_weights[i] > 0.0f) {
+            blended_NE_offset_m += location_diff(state[GPS_MAX_RECEIVERS].location, corrected_location[i]) * _blend_weights[i];
+            blended_alt_offset_cm += (float)(corrected_location[i].alt - state[GPS_MAX_RECEIVERS].location.alt) * _blend_weights[i];
+        }
+    }
+
+    // If the GPS week is the same then use a blended time_week_ms
+    // If week is different, then use time stamp from GPS with largest weighting
+    // detect inconsistent week data
+    uint8_t last_week_instance = 0;
+    bool weeks_consistent = true;
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+        if (last_week_instance == 0 && _blend_weights[i] > 0) {
+            // this is our first valid sensor week data
+            last_week_instance = state[i].time_week;
+        } else if (last_week_instance != 0 && _blend_weights[i] > 0 && last_week_instance != state[i].time_week) {
+            // there is valid sensor week data that is inconsistent
+            weeks_consistent = false;
+        }
+    }
+    // calculate output
+    if (!weeks_consistent) {
+        // use data from highest weighted sensor
+        state[GPS_MAX_RECEIVERS].time_week = state[best_index].time_week;
+        state[GPS_MAX_RECEIVERS].time_week_ms = state[best_index].time_week_ms;
+    } else {
+        // use week number from highest weighting GPS (they should all have the same week number)
+        state[GPS_MAX_RECEIVERS].time_week = state[best_index].time_week;
+        // calculate a blended value for the number of ms lapsed in the week
+        double temp_time_0 = 0.0;
+        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+            if (_blend_weights[i] > 0.0f) {
+                temp_time_0 += (double)state[i].time_week_ms * _blend_weights[i];
+            }
+        }
+        state[GPS_MAX_RECEIVERS].time_week_ms = (uint32_t)temp_time_0;
+    }
+
+    // calculate a blended value for the timing data and lag
+    double temp_time_1 = 0.0;
+    double temp_time_2 = 0.0;
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+        if (_blend_weights[i] > 0.0f) {
+            temp_time_1 += (double)timing[i].last_fix_time_ms * _blend_weights[i];
+            temp_time_2 += (double)timing[i].last_message_time_ms * _blend_weights[i];
+            _blended_lag_sec += get_lag(i) * _blended_lag_sec;
+        }
+    }
+    timing[GPS_MAX_RECEIVERS].last_fix_time_ms = (uint32_t)temp_time_1;
+    timing[GPS_MAX_RECEIVERS].last_message_time_ms = (uint32_t)temp_time_2;
+
+}

libraries/AP_GPS/AP_GPS.h

@@ -28,7 +28,8 @@
    maximum number of GPS instances available on this platform. If more
    than 1 then redundant sensors may be available
  */
-#define GPS_MAX_INSTANCES 2
+#define GPS_MAX_RECEIVERS 2 // maximum number of physical GPS sensors allowed - does not include virtual GPS created by blending receiver data
+#define GPS_MAX_INSTANCES  (GPS_MAX_RECEIVERS + 1) // maximumum number of GPs instances including the 'virtual' GPS created by blending receiver data
 #define GPS_RTK_INJECT_TO_ALL 127
 
 // the number of GPS leap seconds
@@ -140,11 +141,18 @@ public:
     // Accessor functions
 
     // return number of active GPS sensors. Note that if the first GPS
-    // is not present but the 2nd is then we return 2
+    // is not present but the 2nd is then we return 2. Note that a blended
+    // GPS solution is treated as an addditional sensor.
     uint8_t num_sensors(void) const {
-        return num_instances;
+        if (!_output_is_blended) {
+            return num_instances;
+        } else {
+            return num_instances+1;
+        }
     }
 
+    // Return the index of the primary sensor which is the index of the sensor contributing to
+    // the output. A blended solution is avalable as an additional instance
     uint8_t primary_sensor(void) const {
         return primary_instance;
     }
@@ -321,7 +329,12 @@ public:
 
     // return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin
     const Vector3f &get_antenna_offset(uint8_t instance) const {
-        return _antenna_offset[instance];
+        if (instance == GPS_MAX_RECEIVERS) {
+            // return an offset for the blended GPS solution
+            return _blended_antenna_offset;
+        } else {
+            return _antenna_offset[instance];
+        }
     }
 
     // set position for HIL
@@ -338,7 +351,7 @@ public:
     DataFlash_Class *_DataFlash;
 
     // configuration parameters
-    AP_Int8 _type[GPS_MAX_INSTANCES];
+    AP_Int8 _type[GPS_MAX_RECEIVERS];
     AP_Int8 _navfilter;
     AP_Int8 _auto_switch;
     AP_Int8 _min_dgps;
@@ -348,12 +361,14 @@ public:
     AP_Int8 _sbas_mode;
     AP_Int8 _min_elevation;
     AP_Int8 _raw_data;
-    AP_Int8 _gnss_mode[2];
-    AP_Int16 _rate_ms[2];
+    AP_Int8 _gnss_mode[GPS_MAX_RECEIVERS];
+    AP_Int16 _rate_ms[GPS_MAX_RECEIVERS];
     AP_Int8 _save_config;
     AP_Int8 _auto_config;
-    AP_Vector3f _antenna_offset[2];
-    AP_Int16 _delay_ms[2];
+    AP_Vector3f _antenna_offset[GPS_MAX_RECEIVERS];
+    AP_Int16 _delay_ms[GPS_MAX_RECEIVERS];
+    AP_Int8 _blend_mask;
+    AP_Float _blend_tc;
 
     // handle sending of initialisation strings to the GPS
     void send_blob_start(uint8_t instance, const char *_blob, uint16_t size);
@@ -390,10 +405,11 @@ private:
         // the time we got our last fix in system milliseconds
         uint32_t last_message_time_ms;
     };
-    GPS_timing timing[GPS_MAX_INSTANCES];
-    GPS_State state[GPS_MAX_INSTANCES];
-    AP_GPS_Backend *drivers[GPS_MAX_INSTANCES];
-    AP_HAL::UARTDriver *_port[GPS_MAX_INSTANCES];
+    // Note allowance for an additional instance to contain blended data
+    GPS_timing timing[GPS_MAX_RECEIVERS+1];
+    GPS_State state[GPS_MAX_RECEIVERS+1];
+    AP_GPS_Backend *drivers[GPS_MAX_RECEIVERS];
+    AP_HAL::UARTDriver *_port[GPS_MAX_RECEIVERS];
 
     /// primary GPS instance
     uint8_t primary_instance:2;
@@ -416,12 +432,12 @@ private:
         struct NMEA_detect_state nmea_detect_state;
         struct SBP_detect_state sbp_detect_state;
         struct ERB_detect_state erb_detect_state;
-    } detect_state[GPS_MAX_INSTANCES];
+    } detect_state[GPS_MAX_RECEIVERS];
 
     struct {
         const char *blob;
         uint16_t remaining;
-    } initblob_state[GPS_MAX_INSTANCES];
+    } initblob_state[GPS_MAX_RECEIVERS];
 
     static const uint32_t  _baudrates[];
     static const char _initialisation_blob[];
@@ -456,6 +472,21 @@ private:
 
     // inject data into all backends
     void inject_data_all(const uint8_t *data, uint16_t len);
-};
 
-#define GPS_BAUD_TIME_MS 1200
+    // GPS blending and switching
+    Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS]; // Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)
+    float _hgt_offset_cm[GPS_MAX_RECEIVERS]; // Filtered height offset from GPS instance relative to blended solution in _output_state.location (cm)
+    Vector3f _blended_antenna_offset; // blended antenna offset
+    float _blended_lag_sec = 0.001f * GPS_MAX_RATE_MS; // 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.
+    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
+
+    // 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);
+
+};