gps: add device_id

.ci/Jenkinsfile-hardware

@@ -1027,6 +1027,7 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gps"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag"'
@@ -1038,6 +1039,7 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_acceleration"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_gps_position"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position"'

.vscode/cmake-variants.yaml

@@ -96,11 +96,11 @@ CONFIG:
       buildType: MinSizeRel
       settings:
         CONFIG: cuav_x7pro_default
-    cubepilot_cubeorange_default:
+    cubepilot_cubeorange_console:
       short: cubepilot_cubeorange
       buildType: MinSizeRel
       settings:
-        CONFIG: cubepilot_orange_default
+        CONFIG: cubepilot_orange_console
     cubepilot_cubeyellow_default:
       short: cubepilot_cubeyellow
       buildType: MinSizeRel

msg/satellite_info.msg

@@ -1,4 +1,6 @@
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp_sample
+
 uint8 SAT_INFO_MAX_SATELLITES = 20
 
 uint8 count			# Number of satellites visible to the receiver

msg/sensor_gps.msg

@@ -1,6 +1,9 @@
 # GPS position in WGS84 coordinates.
-# the field 'timestamp' is for the position & velocity (microseconds)
+
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp_sample         # the field 'timestamp_sample' is for the position & velocity (microseconds)
+
+uint32 device_id                # unique device ID for the sensor that does not change between power cycles
 
 int32 lat			# Latitude in 1E-7 degrees
 int32 lon			# Longitude in 1E-7 degrees

msg/vehicle_gps_position.msg

@@ -1,6 +1,9 @@
 # GPS position in WGS84 coordinates.
-# the field 'timestamp' is for the position & velocity (microseconds)
+
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp_sample         # the field 'timestamp_sample' is for the position & velocity (microseconds)
+
+uint32 device_id                # unique device ID for the sensor that does not change between power cycles
 
 int32 lat			# Latitude in 1E-7 degrees
 int32 lon			# Longitude in 1E-7 degrees
@@ -36,4 +39,7 @@ 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])
 
+
+float32[3] position_offset
+
 uint8 selected			# GPS selection: 0: GPS1, 1: GPS2. 2: GPS3. 3. Blending multiple receivers

src/drivers/drv_sensor.h

@@ -177,6 +177,20 @@
 #define DRV_DIST_DEVTYPE_SIM 0x9a
 #define DRV_DIST_DEVTYPE_SRF05 0x9b
 
+#define DRV_GPS_DEVTYPE_ASHTECH 0xA0
+#define DRV_GPS_DEVTYPE_EMLID_REACH 0xA1
+#define DRV_GPS_DEVTYPE_FEMTOMES 0xA2
+#define DRV_GPS_DEVTYPE_MTK 0xA3
+#define DRV_GPS_DEVTYPE_SBF 0xA4
+#define DRV_GPS_DEVTYPE_UBX     0xA5
+#define DRV_GPS_DEVTYPE_UBX_5   0xA6
+#define DRV_GPS_DEVTYPE_UBX_6   0xA7
+#define DRV_GPS_DEVTYPE_UBX_7   0xA8
+#define DRV_GPS_DEVTYPE_UBX_8   0xA9
+#define DRV_GPS_DEVTYPE_UBX_9   0xAA
+#define DRV_GPS_DEVTYPE_UBX_F9P 0xAB
+
+
 #define DRV_DEVTYPE_UNUSED		0xff
 
 #endif /* _DRV_SENSOR_H */

src/drivers/gps/gps.cpp

@@ -47,6 +47,8 @@
 
 #include <termios.h>
 
+#include <drivers/drv_sensor.h>
+#include <lib/drivers/device/Device.hpp>
 #include <lib/parameters/param.h>
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
@@ -91,7 +93,7 @@ struct GPS_Sat_Info {
 static constexpr int TASK_STACK_SIZE = 1760;
 
 
-class GPS : public ModuleBase<GPS>
+class GPS : public ModuleBase<GPS>, public device::Device
 {
 public:
 
@@ -260,6 +262,7 @@ extern "C" __EXPORT int gps_main(int argc, char *argv[]);
 
 GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool enable_sat_info,
 	 Instance instance, unsigned configured_baudrate) :
+	Device(MODULE_NAME),
 	_configured_baudrate(configured_baudrate),
 	_mode(mode),
 	_interface(interface),
@@ -280,6 +283,16 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 		memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
 	}
 
+	if (_interface == GPSHelper::Interface::UART) {
+		set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SERIAL);
+
+		char c = _port[strlen(_port) - 1];
+		set_device_bus(atoi(&c));
+
+	} else if (_interface == GPSHelper::Interface::SPI) {
+		set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI);
+	}
+
 	if (_mode == GPS_DRIVER_MODE_NONE) {
 		// use parameter to select mode if not provided via CLI
 		char protocol_param_name[16];
@@ -288,14 +301,18 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 		param_get(param_find(protocol_param_name), &protocol);
 
 		switch (protocol) {
-		case 1: _mode = GPS_DRIVER_MODE_UBX; break;
+		case 1: _mode = GPS_DRIVER_MODE_UBX;
+			break;
 #ifndef CONSTRAINED_FLASH
 
-		case 2: _mode = GPS_DRIVER_MODE_MTK; break;
+		case 2: _mode = GPS_DRIVER_MODE_MTK;
+			break;
 
-		case 3: _mode = GPS_DRIVER_MODE_ASHTECH; break;
+		case 3: _mode = GPS_DRIVER_MODE_ASHTECH;
+			break;
 
-		case 4: _mode = GPS_DRIVER_MODE_EMLIDREACH; break;
+		case 4: _mode = GPS_DRIVER_MODE_EMLIDREACH;
+			break;
 #endif // CONSTRAINED_FLASH
 		}
 	}
@@ -345,7 +362,7 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 	case GPSCallbackType::writeDeviceData:
 		gps->dumpGpsData((uint8_t *)data1, (size_t)data2, true);
 
-		return write(gps->_serial_fd, data1, (size_t)data2);
+		return ::write(gps->_serial_fd, data1, (size_t)data2);
 
 	case GPSCallbackType::setBaudrate:
 		return gps->setBaudrate(data2);
@@ -417,7 +434,7 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 #ifdef __PX4_NUTTX
 			int err = 0;
 			int bytes_available = 0;
-			err = ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
+			err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
 
 			if (err != 0 || bytes_available < (int)character_count) {
 				px4_usleep(sleeptime);
@@ -609,6 +626,7 @@ void GPS::initializeCommunicationDump()
 
 	//make sure to use a large enough queue size, so that we don't lose messages. You may also want
 	//to increase the logger rate for that.
+	_dump_from_device->timestamp = hrt_absolute_time();
 	_dump_communication_pub.publish(*_dump_from_device);
 
 	_should_dump_communication = true;
@@ -662,14 +680,14 @@ GPS::run()
 
 	if (_interface == GPSHelper::Interface::SPI) {
 		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) {
 			PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
 			return;
 		}
 
-		status_value = ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
+		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);
@@ -743,19 +761,23 @@ GPS::run()
 		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);
+			set_device_type(DRV_GPS_DEVTYPE_UBX);
 			break;
 #ifndef CONSTRAINED_FLASH
 
 		case GPS_DRIVER_MODE_MTK:
 			_helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos);
+			set_device_type(DRV_GPS_DEVTYPE_MTK);
 			break;
 
 		case GPS_DRIVER_MODE_ASHTECH:
 			_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset);
+			set_device_type(DRV_GPS_DEVTYPE_ASHTECH);
 			break;
 
 		case GPS_DRIVER_MODE_EMLIDREACH:
 			_helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
+			set_device_type(DRV_GPS_DEVTYPE_EMLID_REACH);
 			break;
 #endif // CONSTRAINED_FLASH
 
@@ -987,6 +1009,44 @@ void
 GPS::publish()
 {
 	if (_instance == Instance::Main || _is_gps_main_advertised.load()) {
+
+		if (_mode == GPS_DRIVER_MODE_UBX) {
+			// populate specific ublox model
+			if (_helper && (get_device_type() == DRV_GPS_DEVTYPE_UBX)) {
+				switch (static_cast<GPSDriverUBX *>(_helper)->board()) {
+				case GPSDriverUBX::Board::u_blox5:
+					set_device_type(DRV_GPS_DEVTYPE_UBX_5);
+					break;
+
+				case GPSDriverUBX::Board::u_blox6:
+					set_device_type(DRV_GPS_DEVTYPE_UBX_6);
+					break;
+
+				case GPSDriverUBX::Board::u_blox7:
+					set_device_type(DRV_GPS_DEVTYPE_UBX_7);
+					break;
+
+				case GPSDriverUBX::Board::u_blox8:
+					set_device_type(DRV_GPS_DEVTYPE_UBX_8);
+					break;
+
+				case GPSDriverUBX::Board::u_blox9:
+					set_device_type(DRV_GPS_DEVTYPE_UBX_9);
+					break;
+
+				case GPSDriverUBX::Board::u_blox9_F9P:
+					set_device_type(DRV_GPS_DEVTYPE_UBX_F9P);
+					break;
+
+				default:
+					set_device_type(DRV_GPS_DEVTYPE_UBX);
+					break;
+				};
+			}
+		}
+
+		_report_gps_pos.device_id = get_device_id();
+		_report_gps_pos.timestamp = hrt_absolute_time();
 		_report_gps_pos_pub.publish(_report_gps_pos);
 		// Heading/yaw data can be updated at a lower rate than the other navigation data.
 		// The uORB message definition requires this data to be set to a NAN if no new valid data is available.
@@ -1000,6 +1060,7 @@ GPS::publishSatelliteInfo()
 {
 	if (_instance == Instance::Main) {
 		if (_p_report_sat_info != nullptr) {
+			_p_report_sat_info->timestamp = hrt_absolute_time();
 			_report_sat_info_pub.publish(*_p_report_sat_info);
 		}
 

src/lib/drivers/device/Device.hpp

@@ -269,7 +269,6 @@ protected:
 		_device_id.devid_s.bus = bus;
 		set_device_address(address);
 	}
-
 };
 
 } // namespace device

src/modules/ekf2/EKF2.cpp

@@ -1479,7 +1479,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 
 		if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
 			gps_message gps_msg{
-				.time_usec = vehicle_gps_position.timestamp,
+				.time_usec = vehicle_gps_position.timestamp_sample,
 				.lat = vehicle_gps_position.lat,
 				.lon = vehicle_gps_position.lon,
 				.alt = vehicle_gps_position.alt,

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

@@ -141,9 +141,9 @@ void VehicleGPSPosition::Run()
 void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected)
 {
 	vehicle_gps_position_s gps_output{};
-
+	gps_output.timestamp_sample = (gps.timestamp_sample != 0) ? gps.timestamp_sample : gps.timestamp;
-	gps_output.timestamp = gps.timestamp;
 	gps_output.time_utc_usec = gps.time_utc_usec;
+	gps_output.device_id = gps.device_id;
 	gps_output.lat = gps.lat;
 	gps_output.lon = gps.lon;
 	gps_output.alt = gps.alt;
@@ -170,7 +170,7 @@ void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected)
 	gps_output.satellites_used = gps.satellites_used;
 
 	gps_output.selected = selected;
-
+	gps_output.timestamp = hrt_absolute_time();
 	_vehicle_gps_position_pub.publish(gps_output);
 }