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/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 @@ private:
 
 	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 @@ private:
 			  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 @@ private:
 	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)
 

src/modules/ekf2/EKF2.hpp

@@ -60,7 +60,6 @@
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/topics/ekf_gps_drift.h>
-#include <uORB/topics/ekf_gps_position.h>
 #include <uORB/topics/estimator_innovations.h>
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/estimator_states.h>
@@ -85,15 +84,6 @@
 
 #include "Utility/PreFlightChecker.hpp"
 
-// defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm
-#define BLEND_MASK_USE_SPD_ACC      1
-#define BLEND_MASK_USE_HPOS_ACC     2
-#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
-
 class EKF2 final : public ModuleBase<EKF2>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
@@ -135,38 +125,6 @@ private:
 	void publish_wind_estimate(const hrt_abstime &timestamp);
 	void publish_yaw_estimator_status(const hrt_abstime &timestamp);
 
-	/*
-	 * 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();
-
 	/*
 	 * Calculate filtered WGS84 height from estimated AMSL height
 	 */
@@ -197,34 +155,11 @@ private:
 	// Used to control saving of mag declination to be used on next startup
 	bool _mag_decl_saved = false;	///< true when the magnetic declination has been saved
 
-	// set pose/velocity as invalid if standard deviation is bigger than max_std_dev
-	// TODO: the user should be allowed to set these values by a parameter
-	static constexpr float ep_max_std_dev = 100.0f;	///< Maximum permissible standard deviation for estimated position
-	static constexpr float eo_max_std_dev = 100.0f;	///< Maximum permissible standard deviation for estimated orientation
-	//static constexpr float ev_max_std_dev = 100.0f;	///< Maximum permissible standard deviation for estimated velocity
+	bool _had_valid_terrain{false};			///< true if at any time there was a valid terrain estimate
 
-	// GPS blending and switching
-	gps_message _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS
-	gps_message _gps_blended_state{};		///< internal state data for the blended GPS
-	gps_message _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS
-	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)
-	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
-	bool _had_valid_terrain = false;		///< true if at any time there was a valid terrain estimate
-
-	int32_t _gps_alttitude_ellipsoid[GPS_MAX_RECEIVERS] {};	///< altitude in 1E-3 meters (millimeters) above ellipsoid
-	uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}; ///< storage for previous timestamp to compute dt
+	uint64_t _gps_time_usec{0};
+	int32_t _gps_alttitude_ellipsoid{0};			///< altitude in 1E-3 meters (millimeters) above ellipsoid
+	uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt
 	float   _wgs84_hgt_offset = 0;  ///< height offset between AMSL and WGS84
 
 	bool _imu_bias_reset_request{false};
@@ -242,6 +177,7 @@ private:
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
@@ -255,16 +191,12 @@ private:
 	uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
 	int _range_finder_sub_index = -1; // index for downward-facing range finder subscription
 
-	// because we can have multiple GPS instances
-	uORB::Subscription _gps_subs[GPS_MAX_RECEIVERS] {{ORB_ID(vehicle_gps_position), 0}, {ORB_ID(vehicle_gps_position), 1}};
-
 	sensor_selection_s		_sensor_selection{};
 	vehicle_land_detected_s		_vehicle_land_detected{};
 	vehicle_status_s		_vehicle_status{};
 
 	uORB::Publication<ekf2_timestamps_s>			_ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
 	uORB::Publication<ekf_gps_drift_s>			_ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
-	uORB::Publication<ekf_gps_position_s>			_blended_gps_pub{ORB_ID(ekf_gps_position)};
 	uORB::Publication<estimator_innovations_s>		_estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
 	uORB::Publication<estimator_innovations_s>		_estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
 	uORB::Publication<estimator_innovations_s>		_estimator_innovations_pub{ORB_ID(estimator_innovations)};
@@ -516,12 +448,6 @@ private:
 		(ParamExtFloat<px4::params::EKF2_PCOEF_Z>)
 		_param_ekf2_pcoef_z,	///< static pressure position error coefficient along the Z body axis
 
-		// GPS blending
-		(ParamInt<px4::params::EKF2_GPS_MASK>)
-		_param_ekf2_gps_mask,	///< mask defining when GPS accuracy metrics are used to calculate the blend ratio
-		(ParamFloat<px4::params::EKF2_GPS_TAU>)
-		_param_ekf2_gps_tau,		///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec)
-
 		// Test used to determine if the vehicle is static or moving
 		(ParamExtFloat<px4::params::EKF2_MOVE_TEST>)
 		_param_ekf2_move_test,	///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.

src/modules/ekf2/ekf2_params.c

@@ -1375,37 +1375,6 @@ PARAM_DEFINE_FLOAT(EKF2_ABL_GYRLIM, 3.0f);
  */
 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);
-
 /**
  * Vehicle movement test threshold
  *

src/modules/logger/logged_topics.cpp

@@ -95,6 +95,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_command");
 	add_topic("vehicle_control_mode");
 	add_topic("vehicle_global_position", 200);
+	add_topic("vehicle_gps_position", 500);
 	add_topic("vehicle_land_detected");
 	add_topic("vehicle_local_position", 100);
 	add_topic("vehicle_local_position_setpoint", 100);
@@ -121,9 +122,9 @@ void LoggedTopics::add_default_topics()
 	add_topic_multi("optical_flow", 1000, 2);
 	add_topic_multi("sensor_accel", 1000, 3);
 	add_topic_multi("sensor_baro", 1000, 3);
+	add_topic_multi("sensor_gps", 1000, 3);
 	add_topic_multi("sensor_gyro", 1000, 3);
 	add_topic_multi("sensor_mag", 1000, 4);
-	add_topic_multi("vehicle_gps_position", 1000, 2);
 	add_topic_multi("vehicle_imu", 500, 3);
 	add_topic_multi("vehicle_imu_status", 1000, 3);
 
@@ -168,7 +169,6 @@ void LoggedTopics::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");
@@ -176,13 +176,13 @@ void LoggedTopics::add_estimator_replay_topics()
 	add_topic("sensor_combined");
 	add_topic("sensor_selection");
 	add_topic("vehicle_air_data");
+	add_topic("vehicle_gps_position");
 	add_topic("vehicle_land_detected");
 	add_topic("vehicle_magnetometer");
 	add_topic("vehicle_status");
 	add_topic("vehicle_visual_odometry");
 	add_topic("vehicle_visual_odometry_aligned");
 	add_topic_multi("distance_sensor");
-	add_topic_multi("vehicle_gps_position", 0, 2);
 }
 
 void LoggedTopics::add_thermal_calibration_topics()

src/modules/mavlink/mavlink_messages.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -93,6 +93,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/rpm.h>
 #include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/tecs_status.h>
@@ -105,7 +106,6 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -1792,7 +1792,7 @@ public:
 	}
 
 private:
-	uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)};
+	uORB::Subscription _gps_sub{ORB_ID(sensor_gps), 0};
 
 	/* do not allow top copying this class */
 	MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &) = delete;
@@ -1804,7 +1804,7 @@ protected:
 
 	bool send(const hrt_abstime t) override
 	{
-		vehicle_gps_position_s gps;
+		sensor_gps_s gps;
 
 		if (_gps_sub.update(&gps)) {
 			mavlink_gps_raw_int_t msg{};
@@ -1868,7 +1868,7 @@ public:
 	}
 
 private:
-	uORB::Subscription _gps2_sub{ORB_ID(vehicle_gps_position), 1};
+	uORB::Subscription _gps2_sub{ORB_ID(sensor_gps), 1};
 
 	/* do not allow top copying this class */
 	MavlinkStreamGPS2Raw(MavlinkStreamGPS2Raw &) = delete;
@@ -1880,7 +1880,7 @@ protected:
 
 	bool send(const hrt_abstime t) override
 	{
-		vehicle_gps_position_s gps;
+		sensor_gps_s gps;
 
 		if (_gps2_sub.update(&gps)) {
 			mavlink_gps2_raw_t msg = {};

src/modules/mavlink/mavlink_receiver.cpp

@@ -2287,7 +2287,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 
 	const uint64_t timestamp = hrt_absolute_time();
 
-	struct vehicle_gps_position_s hil_gps = {};
+	sensor_gps_s hil_gps{};
 
 	hil_gps.timestamp_time_relative = 0;
 	hil_gps.time_utc_usec = gps.time_usec;

src/modules/mavlink/mavlink_receiver.h

@@ -85,6 +85,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/radio_status.h>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/transponder_report.h>
 #include <uORB/topics/tune_control.h>
@@ -94,7 +95,6 @@
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -250,12 +250,12 @@ private:
 	uORB::Publication<generator_status_s>			_generator_status_pub{ORB_ID(generator_status)};
 	uORB::Publication<optical_flow_s>			_flow_pub{ORB_ID(optical_flow)};
 	uORB::Publication<position_setpoint_triplet_s>		_pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
+	uORB::Publication<sensor_gps_s>				_gps_pub{ORB_ID(sensor_gps)};
 	uORB::Publication<vehicle_attitude_s>			_attitude_pub{ORB_ID(vehicle_attitude)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)};
 	uORB::Publication<vehicle_attitude_setpoint_s>		_fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)};
 	uORB::Publication<vehicle_global_position_s>		_global_pos_pub{ORB_ID(vehicle_global_position)};
-	uORB::Publication<vehicle_gps_position_s>		_gps_pub{ORB_ID(vehicle_gps_position)};
 	uORB::Publication<vehicle_land_detected_s>		_land_detector_pub{ORB_ID(vehicle_land_detected)};
 	uORB::Publication<vehicle_local_position_s>		_local_pos_pub{ORB_ID(vehicle_local_position)};
 	uORB::Publication<vehicle_odometry_s>			_mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};

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.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);

src/modules/sih/sih.cpp

@@ -229,16 +229,16 @@ void Sih::init_variables()
 
 void Sih::init_sensors()
 {
-	_vehicle_gps_pos.fix_type = 3;  // 3D fix
-	_vehicle_gps_pos.satellites_used = 8;
-	_vehicle_gps_pos.heading = NAN;
-	_vehicle_gps_pos.heading_offset = NAN;
-	_vehicle_gps_pos.s_variance_m_s = 0.5f;
-	_vehicle_gps_pos.c_variance_rad = 0.1f;
-	_vehicle_gps_pos.eph = 0.9f;
-	_vehicle_gps_pos.epv = 1.78f;
-	_vehicle_gps_pos.hdop = 0.7f;
-	_vehicle_gps_pos.vdop = 1.1f;
+	_sensor_gps.fix_type = 3;  // 3D fix
+	_sensor_gps.satellites_used = 8;
+	_sensor_gps.heading = NAN;
+	_sensor_gps.heading_offset = NAN;
+	_sensor_gps.s_variance_m_s = 0.5f;
+	_sensor_gps.c_variance_rad = 0.1f;
+	_sensor_gps.eph = 0.9f;
+	_sensor_gps.epv = 1.78f;
+	_sensor_gps.hdop = 0.7f;
+	_sensor_gps.vdop = 1.1f;
 }
 
 // read the motor signals outputted from the mixer
@@ -351,21 +351,21 @@ void Sih::send_IMU()
 
 void Sih::send_gps()
 {
-	_vehicle_gps_pos.timestamp = _now;
-	_vehicle_gps_pos.lat = (int32_t)(_gps_lat * 1e7);       // Latitude in 1E-7 degrees
-	_vehicle_gps_pos.lon = (int32_t)(_gps_lon * 1e7); // Longitude in 1E-7 degrees
-	_vehicle_gps_pos.alt = (int32_t)(_gps_alt * 1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres)
-	_vehicle_gps_pos.alt_ellipsoid = (int32_t)(_gps_alt * 1000); // Altitude in 1E-3 meters bove Ellipsoid, (millimetres)
-	_vehicle_gps_pos.vel_ned_valid = true;              // True if NED velocity is valid
-	_vehicle_gps_pos.vel_m_s = sqrtf(_gps_vel(0) * _gps_vel(0) + _gps_vel(1) * _gps_vel(
-			1)); // GPS ground speed, (metres/sec)
-	_vehicle_gps_pos.vel_n_m_s = _gps_vel(0);           // GPS North velocity, (metres/sec)
-	_vehicle_gps_pos.vel_e_m_s = _gps_vel(1);           // GPS East velocity, (metres/sec)
-	_vehicle_gps_pos.vel_d_m_s = _gps_vel(2);           // GPS Down velocity, (metres/sec)
-	_vehicle_gps_pos.cog_rad = atan2(_gps_vel(1),
-					 _gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
+	_sensor_gps.timestamp = _now;
+	_sensor_gps.lat = (int32_t)(_gps_lat * 1e7);       // Latitude in 1E-7 degrees
+	_sensor_gps.lon = (int32_t)(_gps_lon * 1e7); // Longitude in 1E-7 degrees
+	_sensor_gps.alt = (int32_t)(_gps_alt * 1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres)
+	_sensor_gps.alt_ellipsoid = (int32_t)(_gps_alt * 1000); // Altitude in 1E-3 meters bove Ellipsoid, (millimetres)
+	_sensor_gps.vel_ned_valid = true;              // True if NED velocity is valid
+	_sensor_gps.vel_m_s = sqrtf(_gps_vel(0) * _gps_vel(0) + _gps_vel(1) * _gps_vel(
+					    1)); // GPS ground speed, (metres/sec)
+	_sensor_gps.vel_n_m_s = _gps_vel(0);           // GPS North velocity, (metres/sec)
+	_sensor_gps.vel_e_m_s = _gps_vel(1);           // GPS East velocity, (metres/sec)
+	_sensor_gps.vel_d_m_s = _gps_vel(2);           // GPS Down velocity, (metres/sec)
+	_sensor_gps.cog_rad = atan2(_gps_vel(1),
+				    _gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
 
-	_vehicle_gps_pos_pub.publish(_vehicle_gps_pos);
+	_sensor_gps_pub.publish(_sensor_gps);
 }
 
 void Sih::publish_sih()

src/modules/sih/sih.hpp

@@ -50,10 +50,10 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/vehicle_angular_velocity.h>   // to publish groundtruth
 #include <uORB/topics/vehicle_attitude.h>           // to publish groundtruth
 #include <uORB/topics/vehicle_global_position.h>    // to publish groundtruth
-#include <uORB/topics/vehicle_gps_position.h>
 
 extern "C" __EXPORT int sih_main(int argc, char *argv[]);
 
@@ -104,8 +104,8 @@ private:
 	PX4Barometer     _px4_baro{6620172};  // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
 
 	// to publish the gps position
-	vehicle_gps_position_s				_vehicle_gps_pos{};
-	uORB::Publication<vehicle_gps_position_s>	_vehicle_gps_pos_pub{ORB_ID(vehicle_gps_position)};
+	sensor_gps_s			_sensor_gps{};
+	uORB::Publication<sensor_gps_s>	_sensor_gps_pub{ORB_ID(sensor_gps)};
 
 	// angular velocity groundtruth
 	vehicle_angular_velocity_s			_vehicle_angular_velocity_gt{};

src/modules/simulator/simulator.h

@@ -65,10 +65,10 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/vehicle_status.h>
@@ -231,7 +231,7 @@ private:
 
 	// HIL GPS
 	static constexpr int MAX_GPS = 3;
-	uORB::PublicationMulti<vehicle_gps_position_s>	*_vehicle_gps_position_pubs[MAX_GPS] {};
+	uORB::PublicationMulti<sensor_gps_s>	*_sensor_gps_pubs[MAX_GPS] {};
 	uint8_t _gps_ids[MAX_GPS] {};
 	std::default_random_engine _gen{};
 

src/modules/simulator/simulator_mavlink.cpp

@@ -336,7 +336,7 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
 	mavlink_msg_hil_gps_decode(msg, &hil_gps);
 
 	if (!_gps_blocked) {
-		vehicle_gps_position_s gps{};
+		sensor_gps_s gps{};
 
 		gps.timestamp = hrt_absolute_time();
 		gps.time_utc_usec = hil_gps.time_usec;
@@ -356,16 +356,16 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
 
 		// New publishers will be created based on the HIL_GPS ID's being different or not
 		for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) {
-			if (_vehicle_gps_position_pubs[i] && _gps_ids[i] == hil_gps.id) {
-				_vehicle_gps_position_pubs[i]->publish(gps);
+			if (_sensor_gps_pubs[i] && _gps_ids[i] == hil_gps.id) {
+				_sensor_gps_pubs[i]->publish(gps);
 				break;
 
 			}
 
-			if (_vehicle_gps_position_pubs[i] == nullptr) {
-				_vehicle_gps_position_pubs[i] = new uORB::PublicationMulti<vehicle_gps_position_s> {ORB_ID(vehicle_gps_position)};
+			if (_sensor_gps_pubs[i] == nullptr) {
+				_sensor_gps_pubs[i] = new uORB::PublicationMulti<sensor_gps_s> {ORB_ID(sensor_gps)};
 				_gps_ids[i] = hil_gps.id;
-				_vehicle_gps_position_pubs[i]->publish(gps);
+				_sensor_gps_pubs[i]->publish(gps);
 				break;
 			}
 		}