ekf2: Add support for use of multiple GPS receivers (#9765)

msg/CMakeLists.txt

@@ -52,6 +52,7 @@ set(msg_files
 	distance_sensor.msg
 	ekf2_innovations.msg
 	ekf2_timestamps.msg
+	ekf_gps_position.msg
 	esc_report.msg
 	esc_status.msg
 	estimator_status.msg
@@ -127,9 +128,9 @@ set(msg_files
 	vehicle_magnetometer.msg
 	vehicle_rates_setpoint.msg
 	vehicle_roi.msg
-	vehicle_trajectory_waypoint.msg
 	vehicle_status.msg
 	vehicle_status_flags.msg
+	vehicle_trajectory_waypoint.msg
 	vtol_vehicle_status.msg
 	wind_estimate.msg
 	vehicle_constraints.msg

msg/ekf_gps_position.msg

@@ -0,0 +1,16 @@
+# EKF blended position in WGS84 coordinates.
+int32 lat			# Latitude in 1E-7 degrees
+int32 lon			# Longitude in 1E-7 degrees
+int32 alt			# Altitude in 1E-3 meters above MSL, (millimetres)
+int32 alt_ellipsoid 		# Altitude in 1E-3 meters bove Ellipsoid, (millimetres)
+float32 s_variance_m_s		# GPS speed accuracy estimate, (metres/sec)
+uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
+float32 eph			# GPS horizontal position accuracy (metres)
+float32 epv			# GPS vertical position accuracy (metres)
+float32 vel_m_s			# GPS ground speed, (metres/sec)
+float32 vel_n_m_s		# GPS North velocity, (metres/sec)
+float32 vel_e_m_s		# GPS East velocity, (metres/sec)
+float32 vel_d_m_s		# GPS Down velocity, (metres/sec)
+bool vel_ned_valid		# True if NED velocity is valid
+uint8 satellites_used		# Number of satellites used
+uint8 selected			# GPS selection: 0: GPS1, 1: GPS2. 2: GPS1+GPS2 blend

src/modules/ekf2/ekf2_params.c

@@ -1268,3 +1268,34 @@ PARAM_DEFINE_FLOAT(EKF2_ABL_GYRLIM, 3.0f);
  * @decimal 2
  */
 PARAM_DEFINE_FLOAT(EKF2_ABL_TAU, 0.5f);
+
+/**
+ * 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 EKF2
+ * @min 0
+ * @max 7
+ * @bit 0 use speed accuracy
+ * @bit 1 use hpos accuracy
+ * @bit 2 use vpos accuracy
+ */
+PARAM_DEFINE_INT32(EKF2_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 EKF2
+ * @min 1.0
+ * @max 100.0
+ * @unit s
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_GPS_TAU, 10.0f);

src/modules/logger/logger.cpp

@@ -687,6 +687,7 @@ void Logger::add_estimator_replay_topics()
 {
 	// for estimator replay (need to be at full rate)
 	add_topic("ekf2_timestamps");
+	add_topic("ekf_gps_position");
 
 	// current EKF2 subscriptions
 	add_topic("airspeed");