move fake GPS to standalone module

2026-11-01 06:11:35 +08:00 · 2021-02-20 14:13:36 -05:00
parent 0ea8104344
commit 414f9f81d9
68 changed files with 457 additions and 195 deletions
@@ -87,6 +87,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		#hwtest # Hardware test
@@ -110,6 +110,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -114,6 +114,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		#fake_gps
 		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		#hello
 		#hwtest # Hardware test
@@ -112,6 +112,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		#fake_gps
 		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		#hello
 		#hwtest # Hardware test
@@ -106,6 +106,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -83,6 +83,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		dyn_hello # dynamically loading modules example
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -114,6 +114,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -117,6 +117,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -118,6 +118,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -118,6 +118,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -117,6 +117,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -117,6 +117,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -84,6 +84,7 @@ px4_add_board(
 		work_queue
 	EXAMPLES
 		dyn_hello # dynamically loading modules example
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		#hwtest # Hardware test
@@ -115,6 +115,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -122,6 +122,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -92,6 +92,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		#fake_gps
 		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		#hello
 		#hwtest # Hardware test
@@ -91,6 +91,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		#fake_gps
 		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		#hello
 		#hwtest # Hardware test
@@ -109,6 +109,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -114,6 +114,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -114,6 +114,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -115,6 +115,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -116,6 +116,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -112,6 +112,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -111,6 +111,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -110,6 +110,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -111,6 +111,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -111,6 +111,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -110,6 +110,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -98,6 +98,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		#fake_gps
 		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		#hello
 		#hwtest # Hardware test
@@ -128,6 +128,7 @@ px4_add_board(
 		#ver
 		#work_queue
 	EXAMPLES
 		#fake_gps
 		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		#hello
 		#hwtest # Hardware test
@@ -113,6 +113,7 @@ px4_add_board(
 		work_queue
 	EXAMPLES
 		#fake_gps
 		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		#hello
 		#hwtest # Hardware test
@@ -79,5 +79,4 @@ px4_add_board(
 		usb_connected
 		ver
 		work_queue
 	)
@@ -121,6 +121,7 @@ px4_add_board(
 		ver
 		#work_queue
 	EXAMPLES
 		#fake_gps
 		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		#hello
 		#hwtest # Hardware test
@@ -127,6 +127,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -125,6 +125,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -121,6 +121,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -117,6 +117,7 @@ px4_add_board(
 		work_queue
 	EXAMPLES
 		#fake_gps
 		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		#hello
 		#hwtest # Hardware test
@@ -124,6 +124,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fake_gyro
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
@@ -123,6 +123,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fake_gyro
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
@@ -117,6 +117,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -116,6 +116,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		#fake_gps
 		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		#hello
 		#hwtest # Hardware test
@@ -119,6 +119,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -116,6 +116,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -118,6 +118,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -127,6 +127,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fake_gyro
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
@@ -123,6 +123,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		#fake_gps
 		#fake_magnetometer
 		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		#hello
@@ -125,6 +125,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fake_gyro
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
@@ -119,6 +119,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -119,6 +119,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -123,6 +123,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		#fake_gps
 		#fake_magnetometer
 		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		#hello
@@ -126,6 +126,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		#fake_gps
 		#fake_gyro
 		#fake_magnetometer
 		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
@@ -119,6 +119,7 @@ px4_add_board(
 		work_queue
 		serial_test
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -122,6 +122,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -122,6 +122,7 @@ px4_add_board(
 		work_queue
 		serial_test
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -122,6 +122,7 @@ px4_add_board(
 		work_queue
 		serial_test
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -120,6 +120,7 @@ px4_add_board(
 		work_queue
 		serial_test
 	EXAMPLES
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		hwtest # Hardware test
@@ -83,6 +83,7 @@ px4_add_board(
 		ver
 		work_queue
 	EXAMPLES
 		fake_gps
 		dyn_hello # dynamically loading modules example
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -87,6 +87,7 @@ px4_add_board(
 		work_queue
 	EXAMPLES
 		dyn_hello # dynamically loading modules example
 		fake_gps
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -85,6 +85,7 @@ px4_add_board(
 		work_queue
 	EXAMPLES
 		dyn_hello # dynamically loading modules example
 		fake_gps
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -85,6 +85,7 @@ px4_add_board(
 		work_queue
 	EXAMPLES
 		dyn_hello # dynamically loading modules example
 		fake_gps
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -84,6 +84,7 @@ px4_add_board(
 		work_queue
 	EXAMPLES
 		dyn_hello # dynamically loading modules example
 		fake_gps
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -83,6 +83,7 @@ px4_add_board(
 		work_queue
 	EXAMPLES
 		dyn_hello # dynamically loading modules example
 		fake_gps
 		fake_magnetometer
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
@@ -84,6 +84,7 @@ px4_add_board(
 		work_queue
 	EXAMPLES
 		dyn_hello # dynamically loading modules example
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		#hwtest # Hardware test
@@ -84,6 +84,7 @@ px4_add_board(
 		work_queue
 	EXAMPLES
 		dyn_hello # dynamically loading modules example
 		fake_gps
 		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 		hello
 		#hwtest # Hardware test
@@ -103,8 +103,8 @@ public:
 		Count
 	};
-	GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps, bool enable_sat_info,
+	GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool enable_sat_info, Instance instance,
-	    Instance instance, unsigned configured_baudrate);
+	    unsigned configured_baudrate);
 	~GPS() override;
 	/** @see ModuleBase */
@@ -175,8 +175,6 @@ private:
 	unsigned			_num_bytes_read{0}; 				///< counter for number of read bytes from the UART (within update interval)
 	unsigned			_rate_reading{0}; 				///< reading rate in B/s
 	const bool			_fake_gps;					///< fake gps output
 	const Instance 			_instance;
 	uORB::Subscription		_orb_inject_data_sub{ORB_ID(gps_inject_data)};
@@ -258,12 +256,11 @@ px4::atomic<GPS *> GPS::_secondary_instance{nullptr};
 extern "C" __EXPORT int gps_main(int argc, char *argv[]);
-GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps,
+GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool enable_sat_info,
-	 bool enable_sat_info, Instance instance, unsigned configured_baudrate) :
+	 Instance instance, unsigned configured_baudrate) :
 	_configured_baudrate(configured_baudrate),
 	_mode(mode),
 	_interface(interface),
 	_fake_gps(fake_gps),
 	_instance(instance)
 {
 	/* store port name */
@@ -637,37 +634,35 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
 void
 GPS::run()
 {
-	if (!_fake_gps) {
+	/* open the serial port */
-		/* open the serial port */
+	_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
 		_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
-		if (_serial_fd < 0) {
+	if (_serial_fd < 0) {
-			PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);
+		PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);
-			return;
+		return;
-		}
+	}
 #ifdef __PX4_LINUX
-		if (_interface == GPSHelper::Interface::SPI) {
+	if (_interface == GPSHelper::Interface::SPI) {
-			int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
+		int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
-			int status_value = ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
+		int status_value = ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
-			if (status_value < 0) {
+		if (status_value < 0) {
-				PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
+			PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
-				return;
+			return;
 			}
 			status_value = ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
 			if (status_value < 0) {
 				PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
 				return;
 			}
 		}
-#endif /* __PX4_LINUX */
+		status_value = ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
 		if (status_value < 0) {
 			PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
 			return;
 		}
 	}
 #endif /* __PX4_LINUX */
 	param_t handle = param_find("GPS_YAW_OFFSET");
 	float heading_offset = 0.f;
@@ -719,122 +714,89 @@ GPS::run()
 	/* loop handling received serial bytes and also configuring in between */
 	while (!should_exit()) {
 		if (_helper != nullptr) {
 			delete (_helper);
 			_helper = nullptr;
 		}
-		if (_fake_gps) {
+		switch (_mode) {
-			_report_gps_pos.timestamp = hrt_absolute_time();
+		case GPS_DRIVER_MODE_NONE:
-			_report_gps_pos.lat = (int32_t)47.378301e7f;
+			_mode = GPS_DRIVER_MODE_UBX;
 			_report_gps_pos.lon = (int32_t)8.538777e7f;
 			_report_gps_pos.alt = (int32_t)1200e3f;
 			_report_gps_pos.alt_ellipsoid = 10000;
 			_report_gps_pos.s_variance_m_s = 0.5f;
 			_report_gps_pos.c_variance_rad = 0.1f;
 			_report_gps_pos.fix_type = 3;
 			_report_gps_pos.eph = 0.8f;
 			_report_gps_pos.epv = 1.2f;
 			_report_gps_pos.hdop = 0.9f;
 			_report_gps_pos.vdop = 0.9f;
 			_report_gps_pos.vel_n_m_s = 0.0f;
 			_report_gps_pos.vel_e_m_s = 0.0f;
 			_report_gps_pos.vel_d_m_s = 0.0f;
 			_report_gps_pos.vel_m_s = 0.0f;
 			_report_gps_pos.cog_rad = 0.0f;
 			_report_gps_pos.vel_ned_valid = true;
 			_report_gps_pos.satellites_used = 10;
 			_report_gps_pos.heading = NAN;
 			_report_gps_pos.heading_offset = NAN;
-			/* no time and satellite information simulated */
+		/* FALLTHROUGH */
-
+		case GPS_DRIVER_MODE_UBX:
-
+			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
-			publish();
+						   gps_ubx_dynmodel, heading_offset, ubx_mode);
-
+			break;
 			px4_usleep(200000);
 		} else {
 			if (_helper != nullptr) {
 				delete (_helper);
 				_helper = nullptr;
 			}
 			switch (_mode) {
 			case GPS_DRIVER_MODE_NONE:
 				_mode = GPS_DRIVER_MODE_UBX;
 			/* FALLTHROUGH */
 			case GPS_DRIVER_MODE_UBX:
 				_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
 							   gps_ubx_dynmodel, heading_offset, ubx_mode);
 				break;
 #ifndef CONSTRAINED_FLASH
-			case GPS_DRIVER_MODE_MTK:
+		case GPS_DRIVER_MODE_MTK:
-				_helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos);
+			_helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos);
-				break;
+			break;
-			case GPS_DRIVER_MODE_ASHTECH:
+		case GPS_DRIVER_MODE_ASHTECH:
-				_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset);
+			_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset);
-				break;
+			break;
-			case GPS_DRIVER_MODE_EMLIDREACH:
+		case GPS_DRIVER_MODE_EMLIDREACH:
-				_helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
+			_helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
-				break;
+			break;
 #endif // CONSTRAINED_FLASH
-			default:
+		default:
-				break;
+			break;
 		}
 		_baudrate = _configured_baudrate;
 		GPSHelper::GPSConfig gpsConfig{};
 		gpsConfig.output_mode = GPSHelper::OutputMode::GPS;
 		gpsConfig.gnss_systems = static_cast<GPSHelper::GNSSSystemsMask>(gnssSystemsParam);
 		if (_helper && _helper->configure(_baudrate, gpsConfig) == 0) {
 			/* reset report */
 			memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
 			_report_gps_pos.heading = NAN;
 			_report_gps_pos.heading_offset = heading_offset;
 			if (_mode == GPS_DRIVER_MODE_UBX) {
 				/* GPS is obviously detected successfully, reset statistics */
 				_helper->resetUpdateRates();
 			}
-			_baudrate = _configured_baudrate;
+			int helper_ret;
 			GPSHelper::GPSConfig gpsConfig{};
 			gpsConfig.output_mode = GPSHelper::OutputMode::GPS;
 			gpsConfig.gnss_systems = static_cast<GPSHelper::GNSSSystemsMask>(gnssSystemsParam);
-			if (_helper && _helper->configure(_baudrate, gpsConfig) == 0) {
+			while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !should_exit()) {
-				/* reset report */
+				if (helper_ret & 1) {
-				memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
+					publish();
 				_report_gps_pos.heading = NAN;
 				_report_gps_pos.heading_offset = heading_offset;
-				if (_mode == GPS_DRIVER_MODE_UBX) {
+					last_rate_count++;
 				}
-					/* GPS is obviously detected successfully, reset statistics */
+				if (_p_report_sat_info && (helper_ret & 2)) {
 					publishSatelliteInfo();
 				}
 				reset_if_scheduled();
 				/* measure update rate every 5 seconds */
 				if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
 					float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f;
 					_rate = last_rate_count / dt;
 					_rate_rtcm_injection = _last_rate_rtcm_injection_count / dt;
 					_rate_reading = _num_bytes_read / dt;
 					last_rate_measurement = hrt_absolute_time();
 					last_rate_count = 0;
 					_last_rate_rtcm_injection_count = 0;
 					_num_bytes_read = 0;
 					_helper->storeUpdateRates();
 					_helper->resetUpdateRates();
 				}
-				int helper_ret;
+				if (!_healthy) {
-
+					// Helpful for debugging, but too verbose for normal ops
 				while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !should_exit()) {
 					if (helper_ret & 1) {
 						publish();
 						last_rate_count++;
 					}
 					if (_p_report_sat_info && (helper_ret & 2)) {
 						publishSatelliteInfo();
 					}
 					reset_if_scheduled();
 					/* measure update rate every 5 seconds */
 					if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
 						float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f;
 						_rate = last_rate_count / dt;
 						_rate_rtcm_injection = _last_rate_rtcm_injection_count / dt;
 						_rate_reading = _num_bytes_read / dt;
 						last_rate_measurement = hrt_absolute_time();
 						last_rate_count = 0;
 						_last_rate_rtcm_injection_count = 0;
 						_num_bytes_read = 0;
 						_helper->storeUpdateRates();
 						_helper->resetUpdateRates();
 					}
 					if (!_healthy) {
 						// Helpful for debugging, but too verbose for normal ops
 //						const char *mode_str = "unknown";
 //
 //						switch (_mode) {
@@ -859,46 +821,44 @@ GPS::run()
 //						}
 //
 //						PX4_WARN("module found: %s", mode_str);
-						_healthy = true;
+					_healthy = true;
 					}
 				}
 				if (_healthy) {
 					_healthy = false;
 					_rate = 0.0f;
 					_rate_rtcm_injection = 0.0f;
 				}
 			}
-			if (_mode_auto) {
+			if (_healthy) {
-				switch (_mode) {
+				_healthy = false;
-				case GPS_DRIVER_MODE_UBX:
+				_rate = 0.0f;
 				_rate_rtcm_injection = 0.0f;
 			}
 		}
 		if (_mode_auto) {
 			switch (_mode) {
 			case GPS_DRIVER_MODE_UBX:
 #ifndef CONSTRAINED_FLASH
-					_mode = GPS_DRIVER_MODE_MTK;
+				_mode = GPS_DRIVER_MODE_MTK;
-					break;
+				break;
-				case GPS_DRIVER_MODE_MTK:
+			case GPS_DRIVER_MODE_MTK:
-					_mode = GPS_DRIVER_MODE_ASHTECH;
+				_mode = GPS_DRIVER_MODE_ASHTECH;
-					break;
+				break;
-				case GPS_DRIVER_MODE_ASHTECH:
+			case GPS_DRIVER_MODE_ASHTECH:
-					_mode = GPS_DRIVER_MODE_EMLIDREACH;
+				_mode = GPS_DRIVER_MODE_EMLIDREACH;
-					break;
+				break;
-				case GPS_DRIVER_MODE_EMLIDREACH:
+			case GPS_DRIVER_MODE_EMLIDREACH:
 #endif // CONSTRAINED_FLASH
-					_mode = GPS_DRIVER_MODE_UBX;
+				_mode = GPS_DRIVER_MODE_UBX;
-					px4_usleep(500000); // tried all possible drivers. Wait a bit before next round
+				px4_usleep(500000); // tried all possible drivers. Wait a bit before next round
-					break;
+				break;
-				default:
+			default:
-					break;
+				break;
 				}
 			} else {
 				px4_usleep(500000);
 			}
 		} else {
 			px4_usleep(500000);
 		}
 	}
@@ -927,33 +887,28 @@ GPS::print_status()
 		break;
 	}
-	//GPS Mode
+	// GPS Mode
-	if (_fake_gps) {
+	switch (_mode) {
-		PX4_INFO("protocol: SIMULATED");
+	case GPS_DRIVER_MODE_UBX:
-
+		PX4_INFO("protocol: UBX");
-	} else {
+		break;
 		switch (_mode) {
 		case GPS_DRIVER_MODE_UBX:
 			PX4_INFO("protocol: UBX");
 			break;
 #ifndef CONSTRAINED_FLASH
-		case GPS_DRIVER_MODE_MTK:
+	case GPS_DRIVER_MODE_MTK:
-			PX4_INFO("protocol: MTK");
+		PX4_INFO("protocol: MTK");
-			break;
+		break;
-		case GPS_DRIVER_MODE_ASHTECH:
+	case GPS_DRIVER_MODE_ASHTECH:
-			PX4_INFO("protocol: ASHTECH");
+		PX4_INFO("protocol: ASHTECH");
-			break;
+		break;
-		case GPS_DRIVER_MODE_EMLIDREACH:
+	case GPS_DRIVER_MODE_EMLIDREACH:
-			PX4_INFO("protocol: EMLIDREACH");
+		PX4_INFO("protocol: EMLIDREACH");
-			break;
+		break;
 #endif // CONSTRAINED_FLASH
-		default:
+	default:
-			break;
+		break;
 		}
 	}
 	PX4_INFO("status: %s, port: %s, baudrate: %d", _healthy ? "OK" : "NOT OK", _port, _baudrate);
@@ -966,10 +921,8 @@ GPS::print_status()
 			PX4_INFO("rate velocity: \t\t%6.2f Hz", (double)_helper->getVelocityUpdateRate());
 		}
-		if (!_fake_gps) {
+		PX4_INFO("rate publication:\t\t%6.2f Hz", (double)_rate);
-			PX4_INFO("rate publication:\t\t%6.2f Hz", (double)_rate);
+		PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection);
 			PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection);
 		}
 		print_message(_report_gps_pos);
 	}
@@ -1097,9 +1050,6 @@ There is a thread for each device polling for data. The GPS protocol classes are
 so that they can be used in other projects as well (eg. QGroundControl uses them too).
 ### Examples
 For testing it can be useful to fake a GPS signal (it will signal the system that it has a valid position):
 $ gps stop
 $ gps start -f
 Starting 2 GPS devices (the main GPS on /dev/ttyS3 and the secondary on /dev/ttyS4):
 $ gps start -d /dev/ttyS3 -e /dev/ttyS4
@@ -1115,7 +1065,6 @@ $ gps reset warm
 	PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Optional secondary GPS device", true);
 	PRINT_MODULE_USAGE_PARAM_INT('g', 0, 0, 3000000, "Baudrate (secondary GPS, can also be p:<param_name>)", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('f', "Fake a GPS signal (useful for testing)", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('s', "Enable publication of satellite info", true);
 	PRINT_MODULE_USAGE_PARAM_STRING('i', "uart", "spi|uart", "GPS interface", true);
@@ -1186,7 +1135,6 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 	const char *device_name_secondary = nullptr;
 	int baudrate_main = 0;
 	int baudrate_secondary = 0;
 	bool fake_gps = false;
 	bool enable_sat_info = false;
 	GPSHelper::Interface interface = GPSHelper::Interface::UART;
 	GPSHelper::Interface interface_secondary = GPSHelper::Interface::UART;
@@ -1197,7 +1145,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 	int ch;
 	const char *myoptarg = nullptr;
-	while ((ch = px4_getopt(argc, argv, "b:d:e:fg:si:j:p:", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "b:d:e:g:si:j:p:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'b':
 			if (px4_get_parameter_value(myoptarg, baudrate_main) != 0) {
@@ -1220,10 +1168,6 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			device_name_secondary = myoptarg;
 			break;
 		case 'f':
 			fake_gps = true;
 			break;
 		case 's':
 			enable_sat_info = true;
 			break;
@@ -1290,7 +1234,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 	GPS *gps;
 	if (instance == Instance::Main) {
-		gps = new GPS(device_name, mode, interface, fake_gps, enable_sat_info, instance, baudrate_main);
+		gps = new GPS(device_name, mode, interface, enable_sat_info, instance, baudrate_main);
 		if (gps && device_name_secondary) {
 			task_spawn(argc, argv, Instance::Secondary);
@@ -1308,7 +1252,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 			}
 		}
 	} else { // secondary instance
-		gps = new GPS(device_name_secondary, mode, interface_secondary, fake_gps, enable_sat_info, instance, baudrate_secondary);
+		gps = new GPS(device_name_secondary, mode, interface_secondary, enable_sat_info, instance, baudrate_secondary);
 	}
 	return gps;
@@ -0,0 +1,43 @@
 ############################################################################
 #
 #   Copyright (c) 2021 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_module(
 	MODULE modules__fake_gps
 	MAIN fake_gps
 	COMPILE_FLAGS
 	SRCS
 		FakeGps.cpp
 		FakeGps.hpp
 	DEPENDS
 		px4_work_queue
 )
@@ -0,0 +1,140 @@
 /****************************************************************************
 *
 *   Copyright (c) 2021 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.
 *
 ****************************************************************************/
 #include "FakeGps.hpp"
 using namespace time_literals;
 FakeGps::FakeGps(double latitude_deg, double longitude_deg, float altitude_m) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
 	_latitude(latitude_deg * 10e6),
 	_longitude(longitude_deg * 10e6),
 	_altitude(altitude_m * 10e2f)
 {
 }
 bool FakeGps::init()
 {
 	ScheduleOnInterval(SENSOR_INTERVAL_US);
 	return true;
 }
 void FakeGps::Run()
 {
 	if (should_exit()) {
 		ScheduleClear();
 		exit_and_cleanup();
 		return;
 	}
 	sensor_gps_s sensor_gps{};
 	sensor_gps.time_utc_usec = hrt_absolute_time() + 1613692609599954;
 	sensor_gps.lat = _latitude;
 	sensor_gps.lon = _longitude;
 	sensor_gps.alt = _altitude;
 	sensor_gps.alt_ellipsoid = _altitude;
 	sensor_gps.s_variance_m_s = 0.3740f;
 	sensor_gps.c_variance_rad = 0.6737f;
 	sensor_gps.eph = 2.1060f;
 	sensor_gps.epv = 3.8470f;
 	sensor_gps.hdop = 0.8800f;
 	sensor_gps.vdop = 1.3300f;
 	sensor_gps.noise_per_ms = 101;
 	sensor_gps.jamming_indicator = 35;
 	sensor_gps.vel_m_s = 0.0420f;
 	sensor_gps.vel_n_m_s = 0.0370f;
 	sensor_gps.vel_e_m_s = 0.0200f;
 	sensor_gps.vel_d_m_s = -0.0570f;
 	sensor_gps.cog_rad = 0.3988f;
 	sensor_gps.timestamp_time_relative = 0;
 	sensor_gps.heading = NAN;
 	sensor_gps.heading_offset = 0.0000;
 	sensor_gps.fix_type = 4;
 	sensor_gps.jamming_state = 0;
 	sensor_gps.vel_ned_valid = true;
 	sensor_gps.satellites_used = 14;
 	sensor_gps.timestamp = hrt_absolute_time();
 	_sensor_gps_pub.publish(sensor_gps);
 }
 int FakeGps::task_spawn(int argc, char *argv[])
 {
 	FakeGps *instance = new FakeGps();
 	if (instance) {
 		_object.store(instance);
 		_task_id = task_id_is_work_queue;
 		if (instance->init()) {
 			return PX4_OK;
 		}
 	} else {
 		PX4_ERR("alloc failed");
 	}
 	delete instance;
 	_object.store(nullptr);
 	_task_id = -1;
 	return PX4_ERROR;
 }
 int FakeGps::custom_command(int argc, char *argv[])
 {
 	return print_usage("unknown command");
 }
 int FakeGps::print_usage(const char *reason)
 {
 	if (reason) {
 		PX4_WARN("%s\n", reason);
 	}
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
 )DESCR_STR");
 	PRINT_MODULE_USAGE_NAME("fake_gps", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 	return 0;
 }
 extern "C" __EXPORT int fake_gps_main(int argc, char *argv[])
 {
 	return FakeGps::main(argc, argv);
 }
@@ -0,0 +1,73 @@
 /****************************************************************************
 *
 *   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 <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_gps.h>
 class FakeGps : public ModuleBase<FakeGps>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	FakeGps(double latitude_deg = 29.6603018, double longitude_deg = -82.3160500, float altitude_m = 30.1f);
 	~FakeGps() override = default;
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
 	/** @see ModuleBase */
 	static int custom_command(int argc, char *argv[]);
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);
 	bool init();
 private:
 	static constexpr uint32_t SENSOR_INTERVAL_US{1000000 / 5}; // 5 Hz
 	void Run() override;
 	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
 	int32_t _latitude{296603018};   // Latitude in 1e-7 degrees
 	int32_t _longitude{-823160500}; // Longitude in 1e-7 degrees
 	int32_t _altitude{30100};       // Altitude in 1e-3 meters above MSL, (millimetres)
 };