applying dual gps patch

resolve transfer errors

reformatting

implementing multi-topics

src/drivers/gps/gps.cpp

@@ -100,7 +100,7 @@ public:
 class GPS
 {
 public:
-	GPS(const char *uart_path, bool fake_gps, bool enable_sat_info);
+	GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num);
 	virtual ~GPS();
 
 	virtual int			init();
@@ -124,13 +124,15 @@ private:
 	GPSHelper			*_helper;					///< instance of GPS parser
 	GPS_Sat_Info			*_sat_info;					///< instance of GPS sat info data object
 	struct vehicle_gps_position_s	_report_gps_pos;				///< uORB topic for gps position
-	orb_advert_t			_report_gps_pos_pub;				///< uORB pub for gps position
+	orb_advert_t			_report_gps_pos_pub[2];				///< uORB pub for gps position
+	int					_gps_orb_instance[2];				///< uORB multi-topic instance
 	struct satellite_info_s		*_p_report_sat_info;				///< pointer to uORB topic for satellite info
 	orb_advert_t			_report_sat_info_pub;				///< uORB pub for satellite info
 	float				_rate;						///< position update rate
 	float				_rate_rtcm_injection;				///< RTCM message injection rate
 	unsigned			_last_rate_rtcm_injection_count; 		///< counter for number of RTCM messages
 	bool				_fake_gps;					///< fake gps output
+	int 				_gps_num;					///< number of GPS connected
 
 	static const int _orb_inject_data_fd_count = 4;
 	int _orb_inject_data_fd[_orb_inject_data_fd_count];
@@ -161,6 +163,11 @@ private:
 	 */
 	void cmd_reset();
 
+	/**
+	 * Publish the gps struct
+	 */
+	void 				publish();
+
 	/**
 	 * This is an abstraction for the poll on serial used.
 	 *
@@ -211,20 +218,23 @@ GPS	*g_dev = nullptr;
 
 }
 
-GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) :
+
+GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num) :
 	_task_should_exit(false),
 	_healthy(false),
 	_mode_changed(false),
 	_mode(GPS_DRIVER_MODE_UBX),
 	_helper(nullptr),
 	_sat_info(nullptr),
-	_report_gps_pos_pub(nullptr),
+	_report_gps_pos_pub{ nullptr, nullptr},
+	_gps_orb_instance{ -1, -1},
 	_p_report_sat_info(nullptr),
 	_report_sat_info_pub(nullptr),
 	_rate(0.0f),
 	_rate_rtcm_injection(0.0f),
 	_last_rate_rtcm_injection_count(0),
-	_fake_gps(fake_gps)
+	_fake_gps(fake_gps),
+	_gps_num(gps_num)
 {
 	/* store port name */
 	strncpy(_port, uart_path, sizeof(_port));
@@ -566,12 +576,7 @@ GPS::task_main()
 			/* no time and satellite information simulated */
 
 
-			if (_report_gps_pos_pub != nullptr) {
+			publish();
-				orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
-
-			} else {
-				_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
-			}
 
 			usleep(2e5);
 
@@ -628,12 +633,7 @@ GPS::task_main()
 					_report_gps_pos.epv = 10000.0f;
 					_report_gps_pos.fix_type = 0;
 
-					if (_report_gps_pos_pub != nullptr) {
+					publish();
-						orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
-
-					} else {
-						_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
-					}
 
 					/* GPS is obviously detected successfully, reset statistics */
 					_helper->resetUpdateRates();
@@ -644,23 +644,13 @@ GPS::task_main()
 				while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
 
 					if (helper_ret & 1) {
-						if (_report_gps_pos_pub != nullptr) {
+						publish();
-							orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
-
-						} else {
-							_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
-						}
 
 						last_rate_count++;
 					}
 
 					if (_p_report_sat_info && (helper_ret & 2)) {
-						if (_report_sat_info_pub != nullptr) {
+						publish();
-							orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
-
-						} else {
-							_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
-						}
 					}
 
 					/* measure update rate every 5 seconds */
@@ -810,6 +800,29 @@ GPS::print_info()
 	usleep(100000);
 }
 
+void
+GPS::publish()
+{
+	if (_gps_num == 1) {
+		if (_report_gps_pos_pub[0] != nullptr) {
+			orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[0], &_report_gps_pos);
+
+		} else {
+			_report_gps_pos_pub[0] = orb_advertise_multi(ORB_ID(vehicle_gps_position), &_report_gps_pos, &_gps_orb_instance[0], ORB_PRIO_DEFAULT);
+			orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[0], &_report_gps_pos);
+		}
+
+	} else {
+		if (_report_gps_pos_pub[1] != nullptr) {
+			orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[1], &_report_gps_pos);
+
+		} else {
+			_report_gps_pos_pub[1] = orb_advertise_multi(ORB_ID(vehicle_gps_position), &_report_gps_pos, &_gps_orb_instance[1], ORB_PRIO_DEFAULT);
+			orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[1], &_report_gps_pos);
+		}
+	}
+}
+
 /**
  * Local functions in support of the shell command.
  */
@@ -817,8 +830,9 @@ namespace gps
 {
 
 GPS	*g_dev = nullptr;
+GPS	*g_dev2 = nullptr;
 
-void	start(const char *uart_path, bool fake_gps, bool enable_sat_info);
+void	start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num);
 void	stop();
 void	test();
 void	reset();
@@ -828,34 +842,66 @@ void	info();
  * Start the driver.
  */
 void
-start(const char *uart_path, bool fake_gps, bool enable_sat_info)
+start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num)
 {
-	if (g_dev != nullptr) {
+	if (gps_num == 1) {
-		PX4_WARN("gps already started");
+		if (g_dev != nullptr) {
+			PX4_WARN("GPS 1 already started");
+		}
+
+		/* create the driver */
+		g_dev = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);
+
+		if (g_dev == nullptr) {
+			goto fail1;
+		}
+
+		if (OK != g_dev->init()) {
+			goto fail1;
+		}
+
 		return;
-	}
 
-	/* create the driver */
+	} else {
-	g_dev = new GPS(uart_path, fake_gps, enable_sat_info);
+		if (gps_num == 2) {
+			if (g_dev2 != nullptr) {
+				PX4_WARN("GPS 2 already started");
+			}
 
-	if (g_dev == nullptr) {
+			/* create the driver */
-		goto fail;
+			g_dev2 = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);
-	}
+
+			if (g_dev2 == nullptr) {
+				goto fail2;
+			}
+
+			if (OK != g_dev2->init()) {
+				goto fail2;
+			}
 
-	if (OK != g_dev->init()) {
+			return;
-		goto fail;
+		}
 	}
 
-	return;
 
-fail:
+fail1:
 
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
 
-	PX4_ERR("start failed");
+	PX4_ERR("start of GPS 1 failed");
+	return;
+
+fail2:
+
+	if (g_dev2 != nullptr) {
+		delete g_dev2;
+		g_dev2 = nullptr;
+	}
+
+	PX4_ERR("start of GPS 2 failed");
 	return;
 }
 
@@ -867,6 +913,13 @@ stop()
 {
 	delete g_dev;
 	g_dev = nullptr;
+
+	if (g_dev2 != nullptr) {
+		delete g_dev2;
+	}
+	g_dev2 = nullptr;
+
+	px4_task_exit(0);
 }
 
 /**
@@ -903,6 +956,12 @@ info()
 	}
 
 	g_dev->print_info();
+
+	if (g_dev2 != nullptr) {
+		g_dev2->print_info();
+	}
+
+	return;
 }
 
 } // namespace
@@ -913,6 +972,7 @@ gps_main(int argc, char *argv[])
 {
 	/* set to default */
 	const char *device_name = GPS_DEFAULT_UART_PORT;
+	const char *device_name2 = nullptr;
 	bool fake_gps = false;
 	bool enable_sat_info = false;
 
@@ -949,7 +1009,24 @@ gps_main(int argc, char *argv[])
 			}
 		}
 
-		gps::start(device_name, fake_gps, enable_sat_info);
+		/* Allow to use a second gps device */
+		for (int i = 2; i < argc; i++) {
+			if (!strcmp(argv[i], "-dualgps")) {
+				if (argc > i + 1) {
+					device_name2 = argv[i + 1];
+
+				} else {
+					PX4_ERR("Did not get second device address");
+				}
+			}
+		}
+
+		gps::start(device_name, fake_gps, enable_sat_info, 1);
+
+		if (!(device_name2 == nullptr)) {
+			gps::start(device_name2, fake_gps, enable_sat_info, 2);
+		}
+
 	}
 
 	if (!strcmp(argv[1], "stop")) {

src/modules/sdlog2/sdlog2.c

@@ -1188,6 +1188,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct ekf2_replay_s replay;
 		struct vehicle_land_detected_s land_detected;
 		struct cpuload_s cpuload;
+		struct vehicle_gps_position_s dual_gps_pos;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -1368,7 +1369,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	if (log_on_start) {
 		/* check GPS topic to get GPS time */
 		if (log_name_timestamp) {
-			if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
+			if (!copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos)) {
 				gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
 			}
 		}
@@ -1482,7 +1483,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GPS POSITION - LOG MANAGEMENT --- */
-		bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos);
+		bool gps_pos_updated = copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos);
 
 		if (gps_pos_updated && log_name_timestamp) {
 			gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
@@ -1718,6 +1719,27 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(GPS);
 			}
 
+			/* --- GPS POSITION - UNIT #2 --- */
+			if (copy_if_updated_multi(ORB_ID(vehicle_gps_position), 1, &subs.gps_pos_sub, &buf.dual_gps_pos)) {
+				log_msg.msg_type = LOG_GPS_MSG;
+				log_msg.body.log_GPS.gps_time = buf.dual_gps_pos.time_utc_usec;
+				log_msg.body.log_GPS.fix_type = buf.dual_gps_pos.fix_type;
+				log_msg.body.log_GPS.eph = buf.dual_gps_pos.eph;
+				log_msg.body.log_GPS.epv = buf.dual_gps_pos.epv;
+				log_msg.body.log_GPS.lat = buf.dual_gps_pos.lat;
+				log_msg.body.log_GPS.lon = buf.dual_gps_pos.lon;
+				log_msg.body.log_GPS.alt = buf.dual_gps_pos.alt * 0.001f;
+				log_msg.body.log_GPS.vel_n = buf.dual_gps_pos.vel_n_m_s;
+				log_msg.body.log_GPS.vel_e = buf.dual_gps_pos.vel_e_m_s;
+				log_msg.body.log_GPS.vel_d = buf.dual_gps_pos.vel_d_m_s;
+				log_msg.body.log_GPS.cog = buf.dual_gps_pos.cog_rad;
+				log_msg.body.log_GPS.sats = buf.dual_gps_pos.satellites_used;
+				log_msg.body.log_GPS.snr_mean = snr_mean;
+				log_msg.body.log_GPS.noise_per_ms = buf.dual_gps_pos.noise_per_ms;
+				log_msg.body.log_GPS.jamming_indicator = buf.dual_gps_pos.jamming_indicator;
+				LOGBUFFER_WRITE_AND_COUNT(GPS);
+			}
+
 			/* --- SATELLITE INFO - UNIT #1 --- */
 			if (_extended_logging) {
 

src/modules/sdlog2/sdlog2_messages.h

@@ -146,6 +146,7 @@ struct log_LPSP_s {
 
 /* --- GPS - GPS POSITION --- */
 #define LOG_GPS_MSG 8
+#define LOG_DGPS_MSG 58
 struct log_GPS_s {
 	uint64_t gps_time;
 	uint8_t fix_type;
@@ -647,6 +648,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(LPOS, "ffffffffLLfBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"),
 	LOG_FORMAT(LPSP, "ffffffffff",		"X,Y,Z,Yaw,VX,VY,VZ,AX,AY,AZ"),
 	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
+	LOG_FORMAT_S(DGPS, GPS,	 "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
 	LOG_FORMAT_S(ATTC, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT_S(ATC1, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT(STAT, "BBBB",		"MainState,NavState,ArmS,Failsafe"),