Added in proper multi GPS handling from Jacob's PR

Co-authored-by: Jacob Crabill <jacob@flyvoly.com>

msg/vehicle_gps_position.msg

@@ -35,4 +35,4 @@ uint8 satellites_used		# Number of satellites used
 float32 heading			# heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI])
 float32 heading_offset		# heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI])
 
-uint8 selected			# GPS selection: 0: GPS1, 1: GPS2. 2: GPS1+GPS2 blend
+uint8 selected			# GPS selection: 0: GPS1, 1: GPS2. 2: GPS3. 3. Blending multiple receivers

platforms/common/include/px4_platform_common/i2c_spi_buses.h

@@ -235,11 +235,11 @@ class I2CSPIDriver : public I2CSPIDriverBase
 
 	virtual ~I2CSPIDriver() = default;
 
-	void Run() final
-	{
+	void Run() final {
 		static_cast<T *>(this)->RunImpl();
 
-		if (should_exit()) {
+		if (should_exit())
+		{
 			exit_and_cleanup();
 		}
 	}

src/drivers/uavcan/sensors/gnss.cpp

@@ -52,7 +52,7 @@ using namespace time_literals;
 const char *const UavcanGnssBridge::NAME = "gnss";
 
 UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
-	UavcanCDevSensorBridgeBase("uavcan_gnss", "/dev/uavcan/gnss", "/dev/gnss", ORB_ID(vehicle_gps_position)),
+	UavcanCDevSensorBridgeBase("uavcan_gnss", "/dev/uavcan/gnss", "/dev/gnss", ORB_ID(sensor_gps)),
 	_node(node),
 	_sub_auxiliary(node),
 	_sub_fix(node),

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

@@ -60,10 +60,15 @@ VehicleGPSPosition::Start()
 	ParametersUpdate(true);
 
 	// needed to change the active sensor if the primary stops updating
-	bool registered_1 = _sensor_gps_sub[0].registerCallback();
-	bool registered_2 = _sensor_gps_sub[1].registerCallback();
+	bool anyGpsRegistered = false;
+	bool registered[GPS_MAX_RECEIVERS];
 
-	return registered_1 || registered_2;
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		registered[i] = _sensor_gps_sub[i].registerCallback();
+		anyGpsRegistered |= registered[i];
+	}
+
+	return anyGpsRegistered;
 }
 
 void
@@ -95,54 +100,62 @@ VehicleGPSPosition::Run()
 {
 	ParametersUpdate();
 
-	// read gps1 data if available
-	bool gps1_updated = _sensor_gps_sub[0].updated();
+	// Check all GPS instance
+	bool any_gps_updated = false;
+	bool gps_updated[GPS_MAX_RECEIVERS];
 
-	if (gps1_updated) {
-		_sensor_gps_sub[0].copy(&_gps_state[0]);
-	}
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
 
-	// check for second GPS receiver data
-	bool gps2_updated = _sensor_gps_sub[1].updated();
+		gps_updated[i] = _sensor_gps_sub[i].updated();
 
-	if (gps2_updated) {
-		_sensor_gps_sub[1].copy(&_gps_state[1]);
+		if (gps_updated[i]) {
+			any_gps_updated = true;
+			_sensor_gps_sub[i].copy(&_gps_state[i]);
+		}
 	}
 
-	if ((_param_sens_gps_mask.get() == 0) && gps1_updated) {
+
+	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) && (gps1_updated || gps2_updated)) {
-		// blend dual receivers if available
+	} else if ((_param_sens_gps_mask.get() > 0) && any_gps_updated) {
+		// blend multiple receivers if available
 
 		// calculate blending weights
 		if (!blend_gps_data()) {
-			// handle case where the blended states cannot be updated
-			if (_gps_state[0].fix_type > _gps_state[1].fix_type) {
-				// GPS 1 has the best fix status so use that
-				_gps_select_index = 0;
-
-			} else if (_gps_state[1].fix_type > _gps_state[0].fix_type) {
-				// GPS 2 has the best fix status so use that
-				_gps_select_index = 1;
-
-			} else if (_gps_select_index == 2) {
-				// use last receiver we received data from
-				if (gps1_updated) {
-					_gps_select_index = 0;
-
-				} else if (gps2_updated) {
-					_gps_select_index = 1;
+			// 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;
 				}
 			}
 
-			// Only use selected receiver data if it has been updated
-			if ((gps1_updated && _gps_select_index == 0) || (gps2_updated && _gps_select_index == 1)) {
-				_gps_new_output_data = true;
+			// Second, compare GPS's with best fix and take the one with most satellites
+			uint8_t max_sats = 0;
 
-			} else {
-				_gps_new_output_data = false;
+			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;
 			}
 		}
 
@@ -151,7 +164,7 @@ VehicleGPSPosition::Run()
 			apply_gps_offsets();
 
 			// calculate a blended output from the offset corrected receiver data
-			if (_gps_select_index == 2) {
+			if (_gps_select_index == GPS_MAX_RECEIVERS) {
 				calc_gps_blend_output();
 			}
 
@@ -180,13 +193,30 @@ VehicleGPSPosition::blend_gps_data()
 	// 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 = 0.3f;
+	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++) {
-		float raw_dt = 0.001f * (float)(_gps_state[i].timestamp - _time_prev_us[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 < 0.3f) {
+		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) {
@@ -197,6 +227,8 @@ VehicleGPSPosition::blend_gps_data()
 		if (_gps_dt[i] < dt_min) {
 			dt_min = _gps_dt[i];
 		}
+
+		gps_count++;
 	}
 
 	// Find the receiver that is last be updated
@@ -216,8 +248,8 @@ VehicleGPSPosition::blend_gps_data()
 		}
 	}
 
-	if ((max_us - min_us) > 300000) {
-		// A receiver has timed out so fall out of blending
+	if (gps_count < 2) {
+		// Less than 2 receivers left, so fall out of blending
 		return false;
 	}
 
@@ -258,11 +290,6 @@ VehicleGPSPosition::blend_gps_data()
 			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;
-
-				} else {
-					// not all receivers support this metric so set it to zero and don't use it
-					speed_accuracy_sum_sq = 0.0f;
-					break;
 				}
 			}
 		}
@@ -274,11 +301,6 @@ VehicleGPSPosition::blend_gps_data()
 			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;
-
-				} else {
-					// not all receivers support this metric so set it to zero and don't use it
-					horizontal_accuracy_sum_sq = 0.0f;
-					break;
 				}
 			}
 		}
@@ -290,11 +312,6 @@ VehicleGPSPosition::blend_gps_data()
 			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;
-
-				} else {
-					// not all receivers support this metric so set it to zero and don't use it
-					vertical_accuracy_sum_sq = 0.0f;
-					break;
 				}
 			}
 		}
@@ -391,7 +408,7 @@ VehicleGPSPosition::blend_gps_data()
 		// offsets for each physical receiver
 		update_gps_blend_states();
 		update_gps_offsets();
-		_gps_select_index = 2;
+		_gps_select_index = GPS_MAX_RECEIVERS;
 	}
 
 	return true;

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp

@@ -52,8 +52,12 @@
 #define BLEND_MASK_USE_VPOS_ACC     4
 
 // define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution
-#define GPS_MAX_RECEIVERS 2
-#define GPS_BLENDED_INSTANCE 2
+#define GPS_MAX_RECEIVERS 3
+#define GPS_BLENDED_INSTANCE GPS_MAX_RECEIVERS
+
+// Set the GPS timeout to 2s, after which a receiver will be ignored
+#define GPS_TIMEOUT_US 2000000
+#define GPS_TIMEOUT_S (GPS_TIMEOUT_US/1e6f)
 
 class VehicleGPSPosition : public ModuleParams, public px4::WorkItem
 {
@@ -130,7 +134,8 @@ class VehicleGPSPosition : public ModuleParams, public px4::WorkItem
 
 	uORB::SubscriptionCallbackWorkItem _sensor_gps_sub[GPS_MAX_RECEIVERS] {	/**< sensor data subscription */
 		{this, ORB_ID(sensor_gps), 0},
-		{this, ORB_ID(sensor_gps), 1}
+		{this, ORB_ID(sensor_gps), 1},
+		{this, ORB_ID(sensor_gps), 2}
 	};
 
 	DEFINE_PARAMETERS(