fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 18:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

gps add device_id

Browse Source
This commit is contained in:
Daniel Agar
2021-04-08 08:27:07 +02:00
committed by Beat Küng
parent 1f4960d480
commit 39c96a8884
10 changed files with 104 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/gps.cpp
+55 -5
View File
@@ -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]; // 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.