From 0d828f36512de5b667a795f712bd611ce3ac4fb5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 1 Apr 2021 21:02:48 -0400 Subject: [PATCH] gps: add device_id --- .ci/Jenkinsfile-hardware | 2 + .vscode/cmake-variants.yaml | 4 +- msg/satellite_info.msg | 4 +- msg/sensor_gps.msg | 7 +- msg/vehicle_gps_position.msg | 10 ++- src/drivers/drv_sensor.h | 14 ++++ src/drivers/gps/devices | 2 +- src/drivers/gps/gps.cpp | 79 ++++++++++++++++--- src/lib/drivers/device/Device.hpp | 1 - src/modules/ekf2/EKF2.cpp | 2 +- .../VehicleGPSPosition.cpp | 6 +- 11 files changed, 109 insertions(+), 22 deletions(-) diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index e37c83d5a3..a678be9e21 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.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"' diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index ebb9850906..703209e049 100644 --- a/.vscode/cmake-variants.yaml +++ b/.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 diff --git a/msg/satellite_info.msg b/msg/satellite_info.msg index 2980ffcbdc..e5ce95b08c 100644 --- a/msg/satellite_info.msg +++ b/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 diff --git a/msg/sensor_gps.msg b/msg/sensor_gps.msg index 2b9519acc5..242abb90ab 100644 --- a/msg/sensor_gps.msg +++ b/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 diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg index e5c87169b5..7f36de73cb 100644 --- a/msg/vehicle_gps_position.msg +++ b/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 diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 1af334cb74..69f4f318d8 100644 --- a/src/drivers/drv_sensor.h +++ b/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 */ diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index f2eb62c2c7..c0dac87150 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit f2eb62c2c78a2ec47bccfe993ff59acc94155e7e +Subproject commit c0dac87150fb22d50fab493afcb42f8c230c4451 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 5c11a3bd92..4976a0ba02 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -47,6 +47,8 @@ #include +#include +#include #include #include #include @@ -91,7 +93,7 @@ struct GPS_Sat_Info { static constexpr int TASK_STACK_SIZE = 1760; -class GPS : public ModuleBase +class GPS : public ModuleBase, 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(_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); } diff --git a/src/lib/drivers/device/Device.hpp b/src/lib/drivers/device/Device.hpp index 689467f2ab..553917cba6 100644 --- a/src/lib/drivers/device/Device.hpp +++ b/src/lib/drivers/device/Device.hpp @@ -269,7 +269,6 @@ protected: _device_id.devid_s.bus = bus; set_device_address(address); } - }; } // namespace device diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 441c3f1b22..bd6227fd38 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/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, diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index dfd703c3a8..a8dbe630ca 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/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 = gps.timestamp; + gps_output.timestamp_sample = (gps.timestamp_sample != 0) ? gps.timestamp_sample : 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); }