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>

msg/CMakeLists.txt

@@ -57,7 +57,6 @@ set(msg_files
 	distance_sensor.msg
 	ekf2_timestamps.msg
 	ekf_gps_drift.msg
-	ekf_gps_position.msg
 	esc_report.msg
 	esc_status.msg
 	estimator_innovations.msg
@@ -118,6 +117,7 @@ set(msg_files
 	sensor_baro.msg
 	sensor_combined.msg
 	sensor_correction.msg
+	sensor_gps.msg
 	sensor_gyro.msg
 	sensor_gyro_fifo.msg
 	sensor_mag.msg

msg/ekf_gps_position.msg → msg/sensor_gps.msg

@@ -1,19 +1,36 @@
-# EKF blended position in WGS84 coordinates.
+# GPS position in WGS84 coordinates.
+# the field 'timestamp' is for the position & velocity (microseconds)
 uint64 timestamp		# time since system start (microseconds)
+
 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)
+float32 c_variance_rad		# GPS course accuracy estimate, (radians)
 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 hdop			# Horizontal dilution of precision
+float32 vdop			# Vertical dilution of precision
+
+int32 noise_per_ms		# GPS noise per millisecond
+int32 jamming_indicator		# indicates jamming is occurring
+
 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)
+float32 cog_rad			# Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
 bool vel_ned_valid		# True if NED velocity is valid
+
+int32 timestamp_time_relative	# timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds)
+uint64 time_utc_usec		# Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0
+
 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

msg/tools/uorb_rtps_message_ids.yaml

@@ -43,7 +43,7 @@ rtps:
     id: 19
   - msg: ekf_gps_drift
     id: 20
-  - msg: ekf_gps_position
+  - msg: sensor_gps
     id: 21
   - msg: esc_report
     id: 22

msg/vehicle_gps_position.msg

@@ -34,3 +34,5 @@ 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: GPS3. 3. Blending multiple receivers

src/drivers/gps/definitions.h

@@ -41,7 +41,7 @@
 
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/defines.h>
-#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/satellite_info.h>
 
 #define GPS_INFO(...) PX4_INFO(__VA_ARGS__)

src/drivers/gps/gps.cpp

@@ -59,6 +59,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/gps_dump.h>
 #include <uORB/topics/gps_inject_data.h>
+#include <uORB/topics/sensor_gps.h>
 
 #include "devices/src/ashtech.h"
 #include "devices/src/emlid_reach.h"
@@ -160,10 +161,10 @@ class GPS : public ModuleBase<GPS>
 
 	GPS_Sat_Info			*_sat_info{nullptr};				///< instance of GPS sat info data object
 
-	vehicle_gps_position_s		_report_gps_pos{};				///< uORB topic for gps position
+	sensor_gps_s			_report_gps_pos{};				///< uORB topic for gps position
 	satellite_info_s		*_p_report_sat_info{nullptr};			///< pointer to uORB topic for satellite info
 
-	uORB::PublicationMulti<vehicle_gps_position_s>	_report_gps_pos_pub{ORB_ID(vehicle_gps_position)};	///< uORB pub for gps position
+	uORB::PublicationMulti<sensor_gps_s>	_report_gps_pos_pub{ORB_ID(sensor_gps)};	///< uORB pub for gps position
 	uORB::PublicationMulti<satellite_info_s>	_report_sat_info_pub{ORB_ID(satellite_info)};		///< uORB pub for satellite info
 
 	float				_rate{0.0f};					///< position update rate

src/drivers/uavcan/sensors/gnss.cpp

@@ -52,13 +52,11 @@ 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),
 	_sub_fix2(node),
-	_pub_fix2(node),
-	_orb_to_uavcan_pub_timer(node, TimerCbBinder(this, &UavcanGnssBridge::broadcast_from_orb)),
 	_report_pub(nullptr),
 	_channel_using_fix2(new bool[_max_channels])
 {
@@ -81,13 +79,6 @@ UavcanGnssBridge::init()
 		return res;
 	}
 
-	res = _pub_fix2.init(uavcan::TransferPriority::MiddleLower);
-
-	if (res < 0) {
-		PX4_WARN("GNSS fix2 pub failed %i", res);
-		return res;
-	}
-
 	res = _sub_auxiliary.start(AuxiliaryCbBinder(this, &UavcanGnssBridge::gnss_auxiliary_sub_cb));
 
 	if (res < 0) {
@@ -109,8 +100,6 @@ UavcanGnssBridge::init()
 		return res;
 	}
 
-	_orb_to_uavcan_pub_timer.startPeriodic(uavcan::MonotonicDuration::fromUSec(1000000U / ORB_TO_UAVCAN_FREQUENCY_HZ));
-
 	return res;
 }
 
@@ -268,7 +257,18 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 				    const float (&pos_cov)[9], const float (&vel_cov)[9],
 				    const bool valid_pos_cov, const bool valid_vel_cov)
 {
-	auto report = ::vehicle_gps_position_s();
+	// This bridge does not support redundant GNSS receivers yet.
+	if (_receiver_node_id < 0) {
+		_receiver_node_id = msg.getSrcNodeID().get();
+		PX4_INFO("GNSS receiver node ID: %d", _receiver_node_id);
+
+	} else {
+		if (_receiver_node_id != msg.getSrcNodeID().get()) {
+			return;  // This GNSS receiver is the redundant one, ignore it.
+		}
+	}
+
+	sensor_gps_s report{};
 
 	/*
 	 * FIXME HACK
@@ -396,44 +396,3 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 
 	publish(msg.getSrcNodeID().get(), &report);
 }
-
-void
-UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &)
-{
-	vehicle_gps_position_s orb_msg{};
-
-	if (!_orb_sub_gnss.update(&orb_msg)) {
-		return;
-	}
-
-	// Convert to UAVCAN
-	using uavcan::equipment::gnss::Fix2;
-	Fix2 msg;
-
-	msg.gnss_timestamp = uavcan::UtcTime::fromUSec(orb_msg.time_utc_usec);
-	msg.gnss_time_standard = Fix2::GNSS_TIME_STANDARD_UTC;
-
-	msg.longitude_deg_1e8   = std::int64_t(orb_msg.lon) * 10LL;
-	msg.latitude_deg_1e8    = std::int64_t(orb_msg.lat) * 10LL;
-	msg.height_ellipsoid_mm = orb_msg.alt_ellipsoid;
-	msg.height_msl_mm       = orb_msg.alt;
-
-	msg.ned_velocity[0] = orb_msg.vel_n_m_s;
-	msg.ned_velocity[1] = orb_msg.vel_e_m_s;
-	msg.ned_velocity[2] = orb_msg.vel_d_m_s;
-
-	msg.sats_used = orb_msg.satellites_used;
-	msg.status    = orb_msg.fix_type;
-	// mode skipped
-	// sub mode skipped
-
-	// diagonal covariance matrix
-	msg.covariance.resize(2, orb_msg.eph * orb_msg.eph);
-	msg.covariance.resize(3, orb_msg.epv * orb_msg.epv);
-	msg.covariance.resize(6, orb_msg.s_variance_m_s * orb_msg.s_variance_m_s);
-
-	msg.pdop = (orb_msg.hdop > orb_msg.vdop) ? orb_msg.hdop : orb_msg.vdop;  // this is a hack :(
-
-	// Publishing now
-	_pub_fix2.broadcast(msg);
-}

src/drivers/uavcan/sensors/gnss.hpp

@@ -46,7 +46,7 @@
 
 #include <uORB/Subscription.hpp>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/sensor_gps.h>
 
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/gnss/Auxiliary.hpp>
@@ -82,8 +82,6 @@ class UavcanGnssBridge : public UavcanCDevSensorBridgeBase
 			  const float (&pos_cov)[9], const float (&vel_cov)[9],
 			  const bool valid_pos_cov, const bool valid_vel_cov);
 
-	void broadcast_from_orb(const uavcan::TimerEvent &);
-
 	typedef uavcan::MethodBinder < UavcanGnssBridge *,
 		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &) >
 		AuxiliaryCbBinder;
@@ -106,16 +104,11 @@ class UavcanGnssBridge : public UavcanCDevSensorBridgeBase
 	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
 	uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2CbBinder> _sub_fix2;
 
-	uavcan::Publisher<uavcan::equipment::gnss::Fix2> _pub_fix2;
-
-	uavcan::TimerEventForwarder<TimerCbBinder> _orb_to_uavcan_pub_timer;
-
 	uint64_t	_last_gnss_auxiliary_timestamp{0};
 	float		_last_gnss_auxiliary_hdop{0.0f};
 	float		_last_gnss_auxiliary_vdop{0.0f};
 
-	uORB::PublicationMulti<vehicle_gps_position_s>	_gps_pub{ORB_ID(vehicle_gps_position)};
-	uORB::Subscription				_orb_sub_gnss{ORB_ID(vehicle_gps_position)};
+	uORB::PublicationMulti<sensor_gps_s>	_gps_pub{ORB_ID(sensor_gps)};
 
 	int	_receiver_node_id{-1};
 	bool	_old_fix_subscriber_active{true};

src/lib/parameters/param_translation.cpp

@@ -98,6 +98,18 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2020-08-23 (v1.12 alpha): translate GPS blending parameters from EKF2 -> SENS
+	{
+		if (strcmp("EKF2_GPS_MASK", node->name) == 0) {
+			strcpy(node->name, "SENS_GPS_MASK");
+			PX4_INFO("copying %s -> %s", "EKF2_GPS_MASK", "SENS_GPS_MASK");
+		}
+
+		if (strcmp("EKF2_GPS_TAU", node->name) == 0) {
+			strcpy(node->name, "SENS_GPS_TAU");
+			PX4_INFO("copying %s -> %s", "EKF2_GPS_TAU", "SENS_GPS_TAU");
+		}
+	}
 
 	// translate (SPI) calibration ID parameters. This can be removed after the next release (current release=1.10)