diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index 90c37ba285..392270f35c 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -1030,6 +1030,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"' @@ -1041,6 +1042,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/msg/sensor_gps.msg b/msg/sensor_gps.msg index 2b9519acc5..1130a353fa 100644 --- a/msg/sensor_gps.msg +++ b/msg/sensor_gps.msg @@ -2,6 +2,8 @@ # the field 'timestamp' is for the position & velocity (microseconds) uint64 timestamp # time since system start (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 int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 1af334cb74..eb589d656a 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -177,6 +177,22 @@ #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_6 0xA6 +#define DRV_GPS_DEVTYPE_UBX_7 0xA7 +#define DRV_GPS_DEVTYPE_UBX_8 0xA8 +#define DRV_GPS_DEVTYPE_UBX_9 0xA9 +#define DRV_GPS_DEVTYPE_UBX_F9P 0xAA + +#define DRV_GPS_DEVTYPE_SIM 0xAF + + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */ diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index f2eb62c2c7..ac1b5ce8e0 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit f2eb62c2c78a2ec47bccfe993ff59acc94155e7e +Subproject commit ac1b5ce8e05d104e0f0a0cd5915e95f332066463 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 5c11a3bd92..a024acdcda 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]; // last digit of path (eg /dev/ttyS2) + 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]; @@ -345,7 +358,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 +430,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); @@ -662,14 +675,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 +756,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 @@ -779,6 +796,37 @@ GPS::run() /* GPS is obviously detected successfully, reset statistics */ _helper->resetUpdateRates(); + + // populate specific ublox model + if (get_device_type() == DRV_GPS_DEVTYPE_UBX) { + GPSDriverUBX *driver_ubx = (GPSDriverUBX *)_helper; + + switch (driver_ubx->board()) { + 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; + } + } } int helper_ret; @@ -987,6 +1035,8 @@ void GPS::publish() { if (_instance == Instance::Main || _is_gps_main_advertised.load()) { + _report_gps_pos.device_id = get_device_id(); + _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. diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 9c5df7b299..88e796cfc5 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -64,6 +64,8 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : for (uint8_t i = 0; i < _max_channels; i++) { _channel_using_fix2[i] = false; } + + set_device_type(DRV_GPS_DEVTYPE_UAVCAN); } UavcanGnssBridge::~UavcanGnssBridge() @@ -282,6 +284,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure const bool valid_pos_cov, const bool valid_vel_cov) { sensor_gps_s report{}; + report.device_id = get_device_id(); /* * FIXME HACK diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index 25170ae4a7..4f629aa01c 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -114,6 +114,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report) // update device id as we now know our device node_id _device_id.devid_s.address = static_cast(node_id); + _device_id.devid_s.bus_type = DeviceBusType::DeviceBusType_UAVCAN; // Publish to the appropriate topic, abort on failure channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->instance); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7035a6622e..187c88012f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2232,6 +2232,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) sensor_gps_s gps{}; + device::Device::DeviceId device_id{}; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.address = msg->sysid; + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + gps.device_id = device_id.devid; + gps.lat = hil_gps.lat; gps.lon = hil_gps.lon; gps.alt = hil_gps.alt; diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index d43da83b4b..3cdac7302f 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -46,6 +46,7 @@ #include #include // to get PWM flags +#include using namespace math; using namespace matrix; @@ -364,6 +365,14 @@ void Sih::send_gps() gps_no_fix(); } + // device id + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + device_id.devid_s.bus = 0; + device_id.devid_s.address = 0; + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + _sensor_gps.device_id = device_id.devid; + _sensor_gps_pub.publish(_sensor_gps); } diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index b715040217..1dc01db12a 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -432,6 +433,14 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg) if (_sensor_gps_pubs[i] == nullptr) { _sensor_gps_pubs[i] = new uORB::PublicationMulti {ORB_ID(sensor_gps)}; _gps_ids[i] = hil_gps.id; + + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + device_id.devid_s.bus = 0; + device_id.devid_s.address = i; + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + gps.device_id = device_id.devid; + _sensor_gps_pubs[i]->publish(gps); break; }