Move GPS blending from ekf2 to sensors module

- new sensors work item that subscribes to N x sensor_gps and publishes vehicle_gps_position
 - blending is now configurable with SENS_GPS_MASK and SENS_GPS_TAU

Co-authored-by: Jacob Crabill <jacob@volans-i.com>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>

src/modules/sensors/CMakeLists.txt

@@ -37,6 +37,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)
 add_subdirectory(vehicle_air_data)
+add_subdirectory(vehicle_gps_position)
 add_subdirectory(vehicle_imu)
 add_subdirectory(vehicle_magnetometer)
 
@@ -57,6 +58,7 @@ px4_add_module(
 		vehicle_acceleration
 		vehicle_angular_velocity
 		vehicle_air_data
+		vehicle_gps_position
 		vehicle_imu
 		vehicle_magnetometer
 	)

src/modules/sensors/sensors.cpp

@@ -73,6 +73,7 @@
 #include "vehicle_acceleration/VehicleAcceleration.hpp"
 #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
 #include "vehicle_air_data/VehicleAirData.hpp"
+#include "vehicle_gps_position/VehicleGPSPosition.hpp"
 #include "vehicle_imu/VehicleIMU.hpp"
 #include "vehicle_magnetometer/VehicleMagnetometer.hpp"
 
@@ -172,6 +173,7 @@ private:
 	VehicleAngularVelocity	_vehicle_angular_velocity;
 	VehicleAirData          *_vehicle_air_data{nullptr};
 	VehicleMagnetometer     *_vehicle_magnetometer{nullptr};
+	VehicleGPSPosition	*_vehicle_gps_position{nullptr};
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 	VehicleIMU      *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
@@ -204,6 +206,7 @@ private:
 	void		adc_poll();
 
 	void		InitializeVehicleAirData();
+	void		InitializeVehicleGPSPosition();
 	void		InitializeVehicleIMU();
 	void		InitializeVehicleMagnetometer();
 
@@ -275,6 +278,11 @@ Sensors::~Sensors()
 		delete _vehicle_air_data;
 	}
 
+	if (_vehicle_gps_position) {
+		_vehicle_gps_position->Stop();
+		delete _vehicle_gps_position;
+	}
+
 	if (_vehicle_magnetometer) {
 		_vehicle_magnetometer->Stop();
 		delete _vehicle_magnetometer;
@@ -483,6 +491,19 @@ void Sensors::InitializeVehicleAirData()
 	}
 }
 
+void Sensors::InitializeVehicleGPSPosition()
+{
+	if (_vehicle_gps_position == nullptr) {
+		if (orb_exists(ORB_ID(sensor_gps), 0) == PX4_OK) {
+			_vehicle_gps_position = new VehicleGPSPosition();
+
+			if (_vehicle_gps_position) {
+				_vehicle_gps_position->Start();
+			}
+		}
+	}
+}
+
 void Sensors::InitializeVehicleIMU()
 {
 	// create a VehicleIMU instance for each accel/gyro pair
@@ -553,6 +574,7 @@ void Sensors::Run()
 	if (_last_config_update == 0) {
 		InitializeVehicleAirData();
 		InitializeVehicleIMU();
+		InitializeVehicleGPSPosition();
 		InitializeVehicleMagnetometer();
 		_voted_sensors_update.init(_sensor_combined);
 		parameter_update_poll(true);
@@ -592,6 +614,7 @@ void Sensors::Run()
 		_voted_sensors_update.initializeSensors();
 		InitializeVehicleAirData();
 		InitializeVehicleIMU();
+		InitializeVehicleGPSPosition();
 		InitializeVehicleMagnetometer();
 		_last_config_update = hrt_absolute_time();
 
@@ -678,6 +701,11 @@ int Sensors::print_status()
 	PX4_INFO_RAW("\n");
 	_vehicle_angular_velocity.PrintStatus();
 
+	if (_vehicle_gps_position) {
+		PX4_INFO_RAW("\n");
+		_vehicle_gps_position->PrintStatus();
+	}
+
 	PX4_INFO_RAW("\n");
 
 	for (auto &i : _vehicle_imu_list) {

src/modules/sensors/vehicle_gps_position/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(vehicle_gps_position
+	VehicleGPSPosition.cpp
+	VehicleGPSPosition.hpp
+)
+target_link_libraries(vehicle_gps_position PRIVATE ecl_geo px4_work_queue)

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

@@ -0,0 +1,742 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "VehicleGPSPosition.hpp"
+
+#include <px4_platform_common/log.h>
+#include <lib/ecl/geo/geo.h>
+#include <lib/mathlib/mathlib.h>
+
+namespace sensors
+{
+using namespace matrix;
+using math::constrain;
+
+VehicleGPSPosition::VehicleGPSPosition() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
+{
+}
+
+VehicleGPSPosition::~VehicleGPSPosition()
+{
+	Stop();
+	perf_free(_cycle_perf);
+}
+
+bool VehicleGPSPosition::Start()
+{
+	// force initial updates
+	ParametersUpdate(true);
+
+	// needed to change the active sensor if the primary stops updating
+	bool anyGpsRegistered = false;
+	bool registered[GPS_MAX_RECEIVERS];
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		registered[i] = _sensor_gps_sub[i].registerCallback();
+		anyGpsRegistered |= registered[i];
+	}
+
+	return anyGpsRegistered;
+}
+
+void VehicleGPSPosition::Stop()
+{
+	Deinit();
+
+	// clear all registered callbacks
+	for (auto &sub : _sensor_gps_sub) {
+		sub.unregisterCallback();
+	}
+}
+
+void VehicleGPSPosition::ParametersUpdate(bool force)
+{
+	// Check if parameters have changed
+	if (_params_sub.updated() || force) {
+		// clear update
+		parameter_update_s param_update;
+		_params_sub.copy(&param_update);
+
+		updateParams();
+	}
+}
+
+void VehicleGPSPosition::Run()
+{
+	perf_begin(_cycle_perf);
+	ParametersUpdate();
+
+	// Check all GPS instance
+	bool any_gps_updated = false;
+	bool gps_updated[GPS_MAX_RECEIVERS];
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+
+		gps_updated[i] = _sensor_gps_sub[i].updated();
+
+		if (gps_updated[i]) {
+			any_gps_updated = true;
+			_sensor_gps_sub[i].copy(&_gps_state[i]);
+		}
+	}
+
+
+	if ((_param_sens_gps_mask.get() == 0) && gps_updated[0]) {
+		// When GPS blending is disabled we always use the first receiver instance
+		Publish(_gps_state[0]);
+
+	} else if ((_param_sens_gps_mask.get() > 0) && any_gps_updated) {
+		// blend multiple receivers if available
+
+		// calculate blending weights
+		if (!blend_gps_data()) {
+			// Only use selected receiver data if it has been updated
+			_gps_new_output_data = false;
+			_gps_select_index = 0;
+
+			// Find the single "best" GPS from the data we have
+			// First, find the GPS(s) with the best fix
+			uint8_t best_fix = 0;
+
+			for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+				if (_gps_state[i].fix_type > best_fix) {
+					best_fix = _gps_state[i].fix_type;
+				}
+			}
+
+			// Second, compare GPS's with best fix and take the one with most satellites
+			uint8_t max_sats = 0;
+
+			for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+				if (_gps_state[i].fix_type == best_fix && _gps_state[i].satellites_used > max_sats) {
+					max_sats = _gps_state[i].satellites_used;
+					_gps_select_index = i;
+				}
+			}
+
+			// Check for new data on selected GPS, and clear blend offsets
+			for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+				if (gps_updated[i] && _gps_select_index == i) {
+					_gps_new_output_data = true;
+				}
+
+				_NE_pos_offset_m[i].zero();
+				_hgt_offset_mm[i] = 0.0f;
+			}
+		}
+
+		if (_gps_new_output_data) {
+			// correct the _gps_state data for steady state offsets and write to _gps_output
+			apply_gps_offsets();
+
+			// calculate a blended output from the offset corrected receiver data
+			if (_gps_select_index == GPS_MAX_RECEIVERS) {
+				calc_gps_blend_output();
+			}
+
+			// write selected GPS to EKF
+			Publish(_gps_output[_gps_select_index]);
+
+			// clear flag to avoid re-use of the same data
+			_gps_new_output_data = false;
+		}
+	}
+
+	perf_end(_cycle_perf);
+}
+
+bool VehicleGPSPosition::blend_gps_data()
+{
+	// zero the blend weights
+	memset(&_blend_weights, 0, sizeof(_blend_weights));
+
+	/*
+	 * If both receivers have the same update rate, use the oldest non-zero time.
+	 * If two receivers with different update rates are used, use the slowest.
+	 * If time difference is excessive, use newest to prevent a disconnected receiver
+	 * from blocking updates.
+	 */
+
+	// Calculate the time step for each receiver with some filtering to reduce the effects of jitter
+	// Find the largest and smallest time step.
+	float dt_max = 0.0f;
+	float dt_min = GPS_TIMEOUT_S;
+	uint8_t gps_count = 0; // Count of receivers which have an active >=2D fix
+	const hrt_abstime hrt_now = hrt_absolute_time();
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		const float raw_dt = 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]);
+		const float present_dt = 1e-6f * (float)(hrt_now - _gps_state[i].timestamp);
+
+		if (raw_dt > 0.0f && raw_dt < GPS_TIMEOUT_S) {
+			_gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i];
+
+		} else if (present_dt >= GPS_TIMEOUT_S) {
+			// Timed out - kill the stored fix for this receiver and don't track its (stale) gps_dt
+			_gps_state[i].timestamp = 0;
+			_gps_state[i].fix_type = 0;
+			_gps_state[i].satellites_used = 0;
+			_gps_state[i].vel_ned_valid = 0;
+
+			continue;
+		}
+
+		// Only count GPSs with at least a 2D fix for blending purposes
+		if (_gps_state[i].fix_type < 2) {
+			continue;
+		}
+
+		if (_gps_dt[i] > dt_max) {
+			dt_max = _gps_dt[i];
+			_gps_slowest_index = i;
+		}
+
+		if (_gps_dt[i] < dt_min) {
+			dt_min = _gps_dt[i];
+		}
+
+		gps_count++;
+	}
+
+	// Find the receiver that is last be updated
+	uint64_t max_us = 0; // newest non-zero system time of arrival of a GPS message
+	uint64_t min_us = -1; // oldest non-zero system time of arrival of a GPS message
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		// Find largest and smallest times
+		if (_gps_state[i].timestamp > max_us) {
+			max_us = _gps_state[i].timestamp;
+			_gps_newest_index = i;
+		}
+
+		if ((_gps_state[i].timestamp < min_us) && (_gps_state[i].timestamp > 0)) {
+			min_us = _gps_state[i].timestamp;
+			_gps_oldest_index = i;
+		}
+	}
+
+	if (gps_count < 2) {
+		// Less than 2 receivers left, so fall out of blending
+		return false;
+	}
+
+	/*
+	 * If the largest dt is less than 20% greater than the smallest, then we have  receivers
+	 * running at the same rate then we wait until we have two messages with an arrival time
+	 * difference that is less than 50% of the smallest time step and use the time stamp from
+	 * the newest data.
+	 * Else we have two receivers at different update rates and use the slowest receiver
+	 * as the timing reference.
+	 */
+
+	if ((dt_max - dt_min) < 0.2f * dt_min) {
+		// both receivers assumed to be running at the same rate
+		if ((max_us - min_us) < (uint64_t)(5e5f * dt_min)) {
+			// data arrival within a short time window enables the two measurements to be blended
+			_gps_time_ref_index = _gps_newest_index;
+			_gps_new_output_data = true;
+		}
+
+	} else {
+		// both receivers running at different rates
+		_gps_time_ref_index = _gps_slowest_index;
+
+		if (_gps_state[_gps_time_ref_index].timestamp > _time_prev_us[_gps_time_ref_index]) {
+			// blend data at the rate of the slower receiver
+			_gps_new_output_data = true;
+		}
+	}
+
+	if (_gps_new_output_data) {
+		_gps_blended_state.timestamp = _gps_state[_gps_time_ref_index].timestamp;
+
+		// calculate the sum squared speed accuracy across all GPS sensors
+		float speed_accuracy_sum_sq = 0.0f;
+
+		if (_param_sens_gps_mask.get() & BLEND_MASK_USE_SPD_ACC) {
+			for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+				if (_gps_state[i].fix_type >= 3 && _gps_state[i].s_variance_m_s > 0.0f) {
+					speed_accuracy_sum_sq += _gps_state[i].s_variance_m_s * _gps_state[i].s_variance_m_s;
+				}
+			}
+		}
+
+		// calculate the sum squared horizontal position accuracy across all GPS sensors
+		float horizontal_accuracy_sum_sq = 0.0f;
+
+		if (_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC) {
+			for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+				if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph > 0.0f) {
+					horizontal_accuracy_sum_sq += _gps_state[i].eph * _gps_state[i].eph;
+				}
+			}
+		}
+
+		// calculate the sum squared vertical position accuracy across all GPS sensors
+		float vertical_accuracy_sum_sq = 0.0f;
+
+		if (_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC) {
+			for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+				if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv > 0.0f) {
+					vertical_accuracy_sum_sq += _gps_state[i].epv * _gps_state[i].epv;
+				}
+			}
+		}
+
+		// 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 can't 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 speed accuracy
+		float spd_blend_weights[GPS_MAX_RECEIVERS] = {};
+
+		if (speed_accuracy_sum_sq > 0.0f && (_param_sens_gps_mask.get() & 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 (_gps_state[i].fix_type >= 3 && _gps_state[i].s_variance_m_s >= 0.001f) {
+					spd_blend_weights[i] = 1.0f / (_gps_state[i].s_variance_m_s * _gps_state[i].s_variance_m_s);
+					sum_of_spd_weights += spd_blend_weights[i];
+				}
+			}
+
+			// 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;
+			}
+		}
+
+		// calculate a weighting using the reported horizontal position
+		float hpos_blend_weights[GPS_MAX_RECEIVERS] = {};
+
+		if (horizontal_accuracy_sum_sq > 0.0f && (_param_sens_gps_mask.get() & 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 (_gps_state[i].fix_type >= 2 && _gps_state[i].eph >= 0.001f) {
+					hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (_gps_state[i].eph * _gps_state[i].eph);
+					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 && (_param_sens_gps_mask.get() & 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 (_gps_state[i].fix_type >= 3 && _gps_state[i].epv >= 0.001f) {
+					vpos_blend_weights[i] = vertical_accuracy_sum_sq / (_gps_state[i].epv * _gps_state[i].epv);
+					sum_of_vpos_weights += vpos_blend_weights[i];
+				}
+			}
+
+			// 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;
+			};
+		}
+
+		// 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;
+		}
+
+		// With updated weights we can calculate a blended GPS solution and
+		// offsets for each physical receiver
+		update_gps_blend_states();
+		update_gps_offsets();
+		_gps_select_index = GPS_MAX_RECEIVERS;
+	}
+
+	return true;
+}
+
+void VehicleGPSPosition::update_gps_blend_states()
+{
+	// initialise the blended states so we can accumulate the results using the weightings for each GPS receiver.
+	_gps_blended_state = sensor_gps_s{};
+	_gps_blended_state.eph = FLT_MAX;
+	_gps_blended_state.epv = FLT_MAX;
+	_gps_blended_state.s_variance_m_s = FLT_MAX;
+	_gps_blended_state.vel_ned_valid = true;
+	//_gps_blended_state.gdop = FLT_MAX; // TODO: add gdop
+
+	_blended_antenna_offset.zero();
+
+	// combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights()
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		// blend the timing data
+		_gps_blended_state.timestamp += (uint64_t)((double)_gps_state[i].timestamp * (double)_blend_weights[i]);
+
+		// use the highest status
+		if (_gps_state[i].fix_type > _gps_blended_state.fix_type) {
+			_gps_blended_state.fix_type = _gps_state[i].fix_type;
+		}
+
+		// calculate a blended average speed and velocity vector
+		_gps_blended_state.vel_m_s += _gps_state[i].vel_m_s * _blend_weights[i];
+		_gps_blended_state.vel_n_m_s += _gps_state[i].vel_n_m_s * _blend_weights[i];
+		_gps_blended_state.vel_e_m_s += _gps_state[i].vel_e_m_s * _blend_weights[i];
+		_gps_blended_state.vel_d_m_s += _gps_state[i].vel_d_m_s * _blend_weights[i];
+
+		// Assume blended error magnitude, DOP and sat count is equal to the best value from contributing receivers
+		// If any receiver contributing has an invalid velocity, then report blended velocity as invalid
+		if (_blend_weights[i] > 0.0f) {
+
+			if (_gps_state[i].eph > 0.0f
+			    && _gps_state[i].eph < _gps_blended_state.eph) {
+				_gps_blended_state.eph = _gps_state[i].eph;
+			}
+
+			if (_gps_state[i].epv > 0.0f
+			    && _gps_state[i].epv < _gps_blended_state.epv) {
+				_gps_blended_state.epv = _gps_state[i].epv;
+			}
+
+			if (_gps_state[i].s_variance_m_s > 0.0f
+			    && _gps_state[i].s_variance_m_s < _gps_blended_state.s_variance_m_s) {
+				_gps_blended_state.s_variance_m_s = _gps_state[i].s_variance_m_s;
+			}
+
+			// TODO: add gdop
+			// if (_gps_state[i].gdop > 0
+			//     && _gps_state[i].gdop < _gps_blended_state.gdop) {
+			// 	_gps_blended_state.gdop = _gps_state[i].gdop;
+			// }
+
+			if (_gps_state[i].satellites_used > 0
+			    && _gps_state[i].satellites_used > _gps_blended_state.satellites_used) {
+				_gps_blended_state.satellites_used = _gps_state[i].satellites_used;
+			}
+
+			if (!_gps_state[i].vel_ned_valid) {
+				_gps_blended_state.vel_ned_valid = false;
+			}
+		}
+
+		// TODO read parameters for individual GPS antenna positions and blend
+		// Vector3f temp_antenna_offset = _antenna_offset[i];
+		// temp_antenna_offset *= _blend_weights[i];
+		// _blended_antenna_offset += temp_antenna_offset;
+	}
+
+	/*
+	 * Calculate the instantaneous weighted average location using  available GPS instances and store in  _gps_state.
+	 * This is statistically the most likely location, but may not be stable enough for direct use by the EKF.
+	*/
+
+	// Use the GPS with the highest weighting as the reference position
+	float best_weight = 0.0f;
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		if (_blend_weights[i] > best_weight) {
+			best_weight = _blend_weights[i];
+			_gps_best_index = i;
+			_gps_blended_state.lat = _gps_state[i].lat;
+			_gps_blended_state.lon = _gps_state[i].lon;
+			_gps_blended_state.alt = _gps_state[i].alt;
+		}
+	}
+
+	// Convert each GPS position to a local NEU offset relative to the reference position
+	Vector2f blended_NE_offset_m;
+	blended_NE_offset_m.zero();
+	float blended_alt_offset_mm = 0.0f;
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		if ((_blend_weights[i] > 0.0f) && (i != _gps_best_index)) {
+			// calculate the horizontal offset
+			Vector2f horiz_offset{};
+			get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7),
+						    (_gps_blended_state.lon / 1.0e7), (_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
+						    &horiz_offset(0), &horiz_offset(1));
+
+			// sum weighted offsets
+			blended_NE_offset_m += horiz_offset * _blend_weights[i];
+
+			// calculate vertical offset
+			float vert_offset = (float)(_gps_state[i].alt - _gps_blended_state.alt);
+
+			// sum weighted offsets
+			blended_alt_offset_mm += vert_offset * _blend_weights[i];
+		}
+	}
+
+	// Add the sum of weighted offsets to the reference position to obtain the blended position
+	double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7;
+	double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7;
+	double lat_deg_res, lon_deg_res;
+	add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res,
+				      &lon_deg_res);
+	_gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res);
+	_gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res);
+	_gps_blended_state.alt += (int32_t)blended_alt_offset_mm;
+
+	// Take GPS heading from the highest weighted receiver that is publishing a valid .heading value
+	uint8_t gps_best_yaw_index = 0;
+	best_weight = 0.0f;
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		if (PX4_ISFINITE(_gps_state[i].heading) && (_blend_weights[i] > best_weight)) {
+			best_weight = _blend_weights[i];
+			gps_best_yaw_index = i;
+		}
+	}
+
+	_gps_blended_state.heading = _gps_state[gps_best_yaw_index].heading;
+	_gps_blended_state.heading_offset = _gps_state[gps_best_yaw_index].heading_offset;
+}
+
+void VehicleGPSPosition::update_gps_offsets()
+{
+	// Calculate filter coefficients to be applied to the offsets for each GPS position and height offset
+	// Increase the filter time constant proportional to the inverse of the weighting
+	// 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] = {};
+	float omega_lpf = 1.0f / fmaxf(_param_sens_gps_tau.get(), 1.0f);
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		if (_gps_state[i].timestamp - _time_prev_us[i] > 0) {
+			// calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter
+			float min_alpha = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]),
+						    0.0f, 1.0f);
+
+			// scale the filter coefficient so that time constant is inversely proprtional to weighting
+			if (_blend_weights[i] > min_alpha) {
+				alpha[i] = min_alpha / _blend_weights[i];
+
+			} else {
+				alpha[i] = 1.0f;
+			}
+
+			_time_prev_us[i] = _gps_state[i].timestamp;
+		}
+	}
+
+	// Calculate a filtered position delta for each GPS relative to the blended solution state
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		Vector2f offset;
+		get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
+					    (_gps_blended_state.lat / 1.0e7), (_gps_blended_state.lon / 1.0e7), &offset(0), &offset(1));
+		_NE_pos_offset_m[i] = offset * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]);
+		_hgt_offset_mm[i] = (float)(_gps_blended_state.alt - _gps_state[i].alt) *  alpha[i] +
+				    _hgt_offset_mm[i] * (1.0f - alpha[i]);
+	}
+
+	// calculate offset limits from the largest difference between receivers
+	Vector2f max_ne_offset{};
+	float max_alt_offset = 0;
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		for (uint8_t j = i; j < GPS_MAX_RECEIVERS; j++) {
+			if (i != j) {
+				Vector2f offset;
+				get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
+							    (_gps_state[j].lat / 1.0e7), (_gps_state[j].lon / 1.0e7), &offset(0), &offset(1));
+				max_ne_offset(0) = fmaxf(max_ne_offset(0), fabsf(offset(0)));
+				max_ne_offset(1) = fmaxf(max_ne_offset(1), fabsf(offset(1)));
+				max_alt_offset = fmaxf(max_alt_offset, fabsf((float)(_gps_state[i].alt - _gps_state[j].alt)));
+			}
+		}
+	}
+
+	// apply offset limits
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		_NE_pos_offset_m[i](0) = constrain(_NE_pos_offset_m[i](0), -max_ne_offset(0), max_ne_offset(0));
+		_NE_pos_offset_m[i](1) = constrain(_NE_pos_offset_m[i](1), -max_ne_offset(1), max_ne_offset(1));
+		_hgt_offset_mm[i] = constrain(_hgt_offset_mm[i], -max_alt_offset, max_alt_offset);
+	}
+}
+
+void VehicleGPSPosition::apply_gps_offsets()
+{
+	// calculate offset corrected output for each physical GPS.
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		// Add the sum of weighted offsets to the reference position to obtain the blended position
+		double lat_deg_now = (double)_gps_state[i].lat * 1.0e-7;
+		double lon_deg_now = (double)_gps_state[i].lon * 1.0e-7;
+		double lat_deg_res, lon_deg_res;
+		add_vector_to_global_position(lat_deg_now, lon_deg_now, _NE_pos_offset_m[i](0), _NE_pos_offset_m[i](1), &lat_deg_res,
+					      &lon_deg_res);
+		_gps_output[i].lat = (int32_t)(1.0E7 * lat_deg_res);
+		_gps_output[i].lon = (int32_t)(1.0E7 * lon_deg_res);
+		_gps_output[i].alt = _gps_state[i].alt + (int32_t)_hgt_offset_mm[i];
+
+		// other receiver data is used uncorrected
+		_gps_output[i].timestamp	= _gps_state[i].timestamp;
+		_gps_output[i].fix_type		= _gps_state[i].fix_type;
+		_gps_output[i].vel_m_s		= _gps_state[i].vel_m_s;
+		_gps_output[i].vel_n_m_s	= _gps_state[i].vel_n_m_s;
+		_gps_output[i].vel_e_m_s	= _gps_state[i].vel_e_m_s;
+		_gps_output[i].vel_d_m_s	= _gps_state[i].vel_d_m_s;
+		_gps_output[i].eph		= _gps_state[i].eph;
+		_gps_output[i].epv		= _gps_state[i].epv;
+		_gps_output[i].s_variance_m_s		= _gps_state[i].s_variance_m_s;
+		//_gps_output[i].gdop		= _gps_state[i].gdop; // TODO: add gdop
+		_gps_output[i].satellites_used		= _gps_state[i].satellites_used;
+		_gps_output[i].vel_ned_valid	= _gps_state[i].vel_ned_valid;
+		_gps_output[i].heading		= _gps_state[i].heading;
+		_gps_output[i].heading_offset	= _gps_state[i].heading_offset;
+	}
+}
+
+void VehicleGPSPosition::calc_gps_blend_output()
+{
+	// Convert each GPS position to a local NEU offset relative to the reference position
+	// which is defined as the positon of the blended solution calculated from non offset corrected data
+	Vector2f blended_NE_offset_m;
+	blended_NE_offset_m.zero();
+	float blended_alt_offset_mm = 0.0f;
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		if (_blend_weights[i] > 0.0f) {
+			// calculate the horizontal offset
+			Vector2f horiz_offset{};
+			get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7),
+						    (_gps_blended_state.lon / 1.0e7),
+						    (_gps_output[i].lat / 1.0e7),
+						    (_gps_output[i].lon / 1.0e7),
+						    &horiz_offset(0),
+						    &horiz_offset(1));
+
+			// sum weighted offsets
+			blended_NE_offset_m += horiz_offset * _blend_weights[i];
+
+			// calculate vertical offset
+			float vert_offset = (float)(_gps_output[i].alt - _gps_blended_state.alt);
+
+			// sum weighted offsets
+			blended_alt_offset_mm += vert_offset * _blend_weights[i];
+		}
+	}
+
+	// Add the sum of weighted offsets to the reference position to obtain the blended position
+	double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7;
+	double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7;
+	double lat_deg_res, lon_deg_res;
+	add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res,
+				      &lon_deg_res);
+	_gps_output[GPS_BLENDED_INSTANCE].lat = (int32_t)(1.0E7 * lat_deg_res);
+	_gps_output[GPS_BLENDED_INSTANCE].lon = (int32_t)(1.0E7 * lon_deg_res);
+	_gps_output[GPS_BLENDED_INSTANCE].alt = _gps_blended_state.alt + (int32_t)blended_alt_offset_mm;
+
+	// Copy remaining data from internal states to output
+	_gps_output[GPS_BLENDED_INSTANCE].timestamp	= _gps_blended_state.timestamp;
+	_gps_output[GPS_BLENDED_INSTANCE].fix_type	= _gps_blended_state.fix_type;
+	_gps_output[GPS_BLENDED_INSTANCE].vel_m_s	= _gps_blended_state.vel_m_s;
+	_gps_output[GPS_BLENDED_INSTANCE].vel_n_m_s	= _gps_blended_state.vel_n_m_s;
+	_gps_output[GPS_BLENDED_INSTANCE].vel_e_m_s	= _gps_blended_state.vel_e_m_s;
+	_gps_output[GPS_BLENDED_INSTANCE].vel_d_m_s	= _gps_blended_state.vel_d_m_s;
+	_gps_output[GPS_BLENDED_INSTANCE].eph		= _gps_blended_state.eph;
+	_gps_output[GPS_BLENDED_INSTANCE].epv		= _gps_blended_state.epv;
+	_gps_output[GPS_BLENDED_INSTANCE].s_variance_m_s		= _gps_blended_state.s_variance_m_s;
+	//_gps_output[GPS_BLENDED_INSTANCE].gdop		= _gps_blended_state.gdop; // TODO: add gdop
+	_gps_output[GPS_BLENDED_INSTANCE].satellites_used		= _gps_blended_state.satellites_used;
+	_gps_output[GPS_BLENDED_INSTANCE].vel_ned_valid	= _gps_blended_state.vel_ned_valid;
+	_gps_output[GPS_BLENDED_INSTANCE].heading		= _gps_blended_state.heading;
+	_gps_output[GPS_BLENDED_INSTANCE].heading_offset	= _gps_blended_state.heading_offset;
+}
+
+void VehicleGPSPosition::Publish(const sensor_gps_s &gps)
+{
+	vehicle_gps_position_s gps_output{};
+
+	gps_output.timestamp = gps.timestamp;
+	gps_output.time_utc_usec = gps.time_utc_usec;
+	gps_output.lat = gps.lat;
+	gps_output.lon = gps.lon;
+	gps_output.alt = gps.alt;
+	gps_output.alt_ellipsoid = gps.alt_ellipsoid;
+	gps_output.s_variance_m_s = gps.s_variance_m_s;
+	gps_output.c_variance_rad = gps.c_variance_rad;
+	gps_output.eph = gps.eph;
+	gps_output.epv = gps.epv;
+	gps_output.hdop = gps.hdop;
+	gps_output.vdop = gps.vdop;
+	gps_output.noise_per_ms = gps.noise_per_ms;
+	gps_output.jamming_indicator = gps.jamming_indicator;
+	gps_output.vel_m_s = gps.vel_m_s;
+	gps_output.vel_n_m_s = gps.vel_n_m_s;
+	gps_output.vel_e_m_s = gps.vel_e_m_s;
+	gps_output.vel_d_m_s = gps.vel_d_m_s;
+	gps_output.cog_rad = gps.cog_rad;
+	gps_output.timestamp_time_relative = gps.timestamp_time_relative;
+	gps_output.heading = gps.heading;
+	gps_output.heading_offset = gps.heading_offset;
+	gps_output.fix_type = gps.fix_type;
+	gps_output.vel_ned_valid = gps.vel_ned_valid;
+	gps_output.satellites_used = gps.satellites_used;
+
+	gps_output.selected = _gps_select_index;
+
+	_vehicle_gps_position_pub.publish(gps_output);
+}
+
+void VehicleGPSPosition::PrintStatus()
+{
+	PX4_INFO("selected GPS: %d", _gps_select_index);
+}
+
+}; // namespace sensors

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp

@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <lib/mathlib/math/Limits.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+using namespace time_literals;
+
+namespace sensors
+{
+class VehicleGPSPosition : public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+
+	VehicleGPSPosition();
+	~VehicleGPSPosition() override;
+
+	bool Start();
+	void Stop();
+
+	void PrintStatus();
+
+private:
+	void Run() override;
+
+	void ParametersUpdate(bool force = false);
+	void Publish(const sensor_gps_s &gps);
+
+	/*
+	 * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical
+	 * receiver solutions. This internal state cannot be used directly by estimators because if physical receivers
+	 * have significant position differences, variation in receiver estimated accuracy will cause undesirable
+	 * variation in the position solution.
+	*/
+	bool blend_gps_data();
+
+	/*
+	 * Calculate internal states used to blend GPS data from multiple receivers using weightings calculated
+	 * by calc_blend_weights()
+	 * States are written to _gps_state and _gps_blended_state class variables
+	 */
+	void update_gps_blend_states();
+
+	/*
+	 * The location in _gps_blended_state 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.
+	*/
+	void update_gps_offsets();
+
+	/*
+	 * Apply the steady state physical receiver offsets calculated by update_gps_offsets().
+	*/
+	void apply_gps_offsets();
+
+	/*
+	 Calculate GPS output that is a blend of the offset corrected physical receiver data
+	*/
+	void calc_gps_blend_output();
+
+	// defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm
+	static constexpr uint8_t BLEND_MASK_USE_SPD_ACC  = 1;
+	static constexpr uint8_t BLEND_MASK_USE_HPOS_ACC = 2;
+	static constexpr uint8_t BLEND_MASK_USE_VPOS_ACC = 4;
+
+	// define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution
+	static constexpr int GPS_MAX_RECEIVERS = 2;
+	static constexpr int GPS_BLENDED_INSTANCE = GPS_MAX_RECEIVERS;
+
+	// Set the GPS timeout to 2s, after which a receiver will be ignored
+	static constexpr hrt_abstime GPS_TIMEOUT_US = 2_s;
+	static constexpr float GPS_TIMEOUT_S = (GPS_TIMEOUT_US / 1e6f);
+
+	uORB::Publication<vehicle_gps_position_s> _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)};
+
+	uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
+
+	uORB::SubscriptionCallbackWorkItem _sensor_gps_sub[GPS_MAX_RECEIVERS] {	/**< sensor data subscription */
+		{this, ORB_ID(sensor_gps), 0},
+		{this, ORB_ID(sensor_gps), 1},
+	};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+
+	// GPS blending and switching
+	sensor_gps_s _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS
+	sensor_gps_s _gps_blended_state{};		///< internal state data for the blended GPS
+	sensor_gps_s _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS
+	matrix::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_mm[GPS_MAX_RECEIVERS] {};	///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm)
+	matrix::Vector3f _blended_antenna_offset{};		///< blended antenna offset
+	float _blend_weights[GPS_MAX_RECEIVERS] {};	///< blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
+	uint64_t _time_prev_us[GPS_MAX_RECEIVERS] {};	///< the previous value of time_us for that GPS instance - used to detect new data.
+	uint8_t _gps_best_index{0};			///< index of the physical receiver with the lowest reported error
+	uint8_t _gps_select_index{0};			///< 0 = GPS1, 1 = GPS2, 2 = blended
+	uint8_t _gps_time_ref_index{0};			///< index of the receiver that is used as the timing reference for the blending update
+	uint8_t _gps_oldest_index{0};			///< index of the physical receiver with the oldest data
+	uint8_t _gps_newest_index{0};			///< index of the physical receiver with the newest data
+	uint8_t _gps_slowest_index{0};			///< index of the physical receiver with the slowest update rate
+	float _gps_dt[GPS_MAX_RECEIVERS] {};		///< average time step in seconds.
+	bool  _gps_new_output_data{false};		///< true if there is new output data for the EKF
+
+	DEFINE_PARAMETERS(
+		// GPS blending
+		(ParamInt<px4::params::SENS_GPS_MASK>)
+		_param_sens_gps_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio
+		(ParamFloat<px4::params::SENS_GPS_TAU>)
+		_param_sens_gps_tau ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec)
+	)
+};
+}; // namespace sensors

src/modules/sensors/vehicle_gps_position/params.c

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Multi GPS Blending Control Mask.
+ *
+ * Set bits in the following positions to set which GPS accuracy metrics will be used to calculate the blending weight. Set to zero to disable and always used first GPS instance.
+ * 0 : Set to true to use speed accuracy
+ * 1 : Set to true to use horizontal position accuracy
+ * 2 : Set to true to use vertical position accuracy
+ *
+ * @group Sensors
+ * @min 0
+ * @max 7
+ * @bit 0 use speed accuracy
+ * @bit 1 use hpos accuracy
+ * @bit 2 use vpos accuracy
+ */
+PARAM_DEFINE_INT32(SENS_GPS_MASK, 0);
+
+/**
+ * Multi GPS Blending Time Constant
+ *
+ * Sets the longest time constant that will be applied to the calculation of GPS position and height offsets used to correct data from multiple GPS data for steady state position differences.
+ *
+ *
+ * @group Sensors
+ * @min 1.0
+ * @max 100.0
+ * @unit s
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f);