From 4b53be944452a1ad6df1f415fd53b2f8d3e332ab Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Wed, 27 Dec 2023 15:39:33 -0800 Subject: [PATCH] Updated GPS driver to use the new platform independent UART driver --- boards/modalai/voxl2/default.px4board | 1 + src/drivers/gps/CMakeLists.txt | 1 + src/drivers/gps/gps.cpp | 327 ++++++++++++-------------- 3 files changed, 155 insertions(+), 174 deletions(-) diff --git a/boards/modalai/voxl2/default.px4board b/boards/modalai/voxl2/default.px4board index ed1e0ed682..b8c02ed9ce 100644 --- a/boards/modalai/voxl2/default.px4board +++ b/boards/modalai/voxl2/default.px4board @@ -3,6 +3,7 @@ CONFIG_BOARD_LINUX_TARGET=y CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu" CONFIG_BOARD_ROOTFSDIR="/data/px4" CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y +CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_QSHELL_POSIX=y CONFIG_MODULES_COMMANDER=y diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt index 9796d24fb3..ce0bc17ea5 100644 --- a/src/drivers/gps/CMakeLists.txt +++ b/src/drivers/gps/CMakeLists.txt @@ -56,4 +56,5 @@ px4_add_module( module.yaml DEPENDS git_gps_devices + drivers__device ) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 4ed29a8c6e..59db88d7c5 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -45,11 +45,11 @@ #include #endif -#include #include #include #include +#include #include #include #include @@ -81,6 +81,7 @@ #include #endif /* __PX4_LINUX */ +using namespace device; using namespace time_literals; #define TIMEOUT_1HZ 1300 //!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error @@ -169,7 +170,10 @@ public: void reset_if_scheduled(); private: - int _serial_fd{-1}; ///< serial interface to GPS +#ifdef __PX4_LINUX + int _spi_fd {-1}; ///< SPI interface to GPS +#endif + Serial *_uart = nullptr; unsigned _baudrate{0}; ///< current baudrate const unsigned _configured_baudrate{0}; ///< configured baudrate (0=auto-detect) char _port[20] {}; ///< device / serial port path @@ -329,8 +333,10 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2) set_device_bus(c - 48); // sub 48 to convert char to integer +#ifdef __PX4_LINUX } else if (_interface == GPSHelper::Interface::SPI) { set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI); +#endif } if (_mode == gps_driver_mode_t::None) { @@ -403,10 +409,22 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) return num_read; } - case GPSCallbackType::writeDeviceData: - gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true); + case GPSCallbackType::writeDeviceData: { + gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true); - return ::write(gps->_serial_fd, data1, (size_t)data2); + int ret = 0; + + if (gps->_uart) { + ret = gps->_uart->write((void *) data1, (size_t) data2); + +#ifdef __PX4_LINUX + } else if (gps->_spi_fd >= 0) { + ret = ::write(gps->_spi_fd, data1, (size_t)data2); +#endif + } + + return ret; + } case GPSCallbackType::setBaudrate: return gps->setBaudrate(data2); @@ -437,10 +455,11 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) // as of 2021 setting the time on Nuttx temporarily pauses interrupts // so only set the time if it is very wrong. // TODO: clock slewing of the RTC for small time differences +#ifndef __PX4_QURT px4_clock_settime(CLOCK_REALTIME, &rtc_gps_time); +#endif } - break; } @@ -449,72 +468,64 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) { + int ret = 0; + const unsigned character_count = 32; // minimum bytes that we want to read + const int max_timeout = 50; + int timeout_adjusted = math::min(max_timeout, timeout); + handleInjectDataTopic(); -#if !defined(__PX4_QURT) + if ((_interface == GPSHelper::Interface::UART) && (_uart)) { + ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted); - /* For non QURT, use the usual polling. */ +// SPI is only supported on Linux +#if defined(__PX4_LINUX) - //Poll only for the serial data. In the same thread we also need to handle orb messages, - //so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the - //two pollings use different underlying mechanisms (at least under posix), which makes this - //impossible. Instead we limit the maximum polling interval and regularly check for new orb - //messages. - //FIXME: add a unified poll() API - const int max_timeout = 50; + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { - pollfd fds[1]; - fds[0].fd = _serial_fd; - fds[0].events = POLLIN; + //Poll only for the SPI data. In the same thread we also need to handle orb messages, + //so ideally we would poll on both, the SPI fd and orb subscription. Unfortunately the + //two pollings use different underlying mechanisms (at least under posix), which makes this + //impossible. Instead we limit the maximum polling interval and regularly check for new orb + //messages. + //FIXME: add a unified poll() API - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout)); + pollfd fds[1]; + fds[0].fd = _spi_fd; + fds[0].events = POLLIN; - if (ret > 0) { - /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. But don't read immediately - * by 1-2 bytes, wait for some more data to save expensive read() calls. - * If we have all requested data available, read it without waiting. - * If more bytes are available, we'll go back to poll() again. - */ - const unsigned character_count = 32; // minimum bytes that we want to read - unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate; - const unsigned sleeptime = character_count * 1000000 / (baudrate / 10); + ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted); -#ifdef __PX4_NUTTX - int err = 0; - int bytes_available = 0; - err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available); + if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. But don't read immediately + * by 1-2 bytes, wait for some more data to save expensive read() calls. + * If we have all requested data available, read it without waiting. + * If more bytes are available, we'll go back to poll() again. + */ + unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate; + const unsigned sleeptime = character_count * 1000000 / (baudrate / 10); - if (err != 0 || bytes_available < (int)character_count) { px4_usleep(sleeptime); + + ret = ::read(_spi_fd, buf, buf_length); + + if (ret > 0) { + _num_bytes_read += ret; + } + + } else { + ret = -1; } - -#else - px4_usleep(sleeptime); -#endif - - ret = ::read(_serial_fd, buf, buf_length); - - if (ret > 0) { - _num_bytes_read += ret; - } - - } else { - ret = -1; } + +#endif } return ret; - -#else - /* For QURT, just use read for now, since this doesn't block, we need to slow it down - * just a bit. */ - px4_usleep(10000); - return ::read(_serial_fd, buf, buf_length); -#endif } void GPS::handleInjectDataTopic() @@ -583,105 +594,38 @@ bool GPS::injectData(uint8_t *data, size_t len) { dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true); - size_t written = ::write(_serial_fd, data, len); - ::fsync(_serial_fd); + size_t written = 0; + + if ((_interface == GPSHelper::Interface::UART) && (_uart)) { + written = _uart->write((const void *) data, len); + +#ifdef __PX4_LINUX + + } else if (_interface == GPSHelper::Interface::SPI) { + written = ::write(_spi_fd, data, len); + ::fsync(_spi_fd); +#endif + } + return written == len; } int GPS::setBaudrate(unsigned baud) { - /* process baud rate */ - int speed; + if (_interface == GPSHelper::Interface::UART) { + if ((_uart) && (_uart->setBaudrate(baud))) { + return 0; + } - switch (baud) { - case 9600: speed = B9600; break; +#ifdef __PX4_LINUX - case 19200: speed = B19200; break; - - case 38400: speed = B38400; break; - - case 57600: speed = B57600; break; - - case 115200: speed = B115200; break; - - case 230400: speed = B230400; break; - -#ifndef B460800 -#define B460800 460800 + } else if (_interface == GPSHelper::Interface::SPI) { + // Can't set the baudrate on a SPI port but just return a success + return 0; #endif - - case 460800: speed = B460800; break; - -#ifndef B921600 -#define B921600 921600 -#endif - - case 921600: speed = B921600; break; - - default: - PX4_ERR("ERR: unknown baudrate: %d", baud); - return -EINVAL; } - struct termios uart_config; - - int termios_state; - - /* fill the struct for the new configuration */ - tcgetattr(_serial_fd, &uart_config); - - /* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */ - - // - // Input flags - Turn off input processing - // - // convert break to null byte, no CR to NL translation, - // no NL to CR translation, don't mark parity errors or breaks - // no input parity check, don't strip high bit off, - // no XON/XOFF software flow control - // - uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | - INLCR | PARMRK | INPCK | ISTRIP | IXON); - // - // Output flags - Turn off output processing - // - // no CR to NL translation, no NL to CR-NL translation, - // no NL to CR translation, no column 0 CR suppression, - // no Ctrl-D suppression, no fill characters, no case mapping, - // no local output processing - // - // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | - // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); - uart_config.c_oflag = 0; - - // - // No line processing - // - // echo off, echo newline off, canonical mode off, - // extended input processing off, signal chars off - // - uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - - /* no parity, one stop bit, disable flow control */ - uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); - - /* set baud rate */ - if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { - GPS_ERR("ERR: %d (cfsetispeed)", termios_state); - return -1; - } - - if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - GPS_ERR("ERR: %d (cfsetospeed)", termios_state); - return -1; - } - - if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { - GPS_ERR("ERR: %d (tcsetattr)", termios_state); - return -1; - } - - return 0; + return -1; } void GPS::initializeCommunicationDump() @@ -840,31 +784,58 @@ GPS::run() _helper = nullptr; } - if (_serial_fd < 0) { - /* open the serial port */ - _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) { - if (_serial_fd < 0) { - PX4_ERR("failed to open %s err: %d", _port, errno); + // Create the UART port instance + _uart = new Serial(_port); + + if (_uart == nullptr) { + PX4_ERR("Error creating serial device %s", _port); + px4_sleep(1); + continue; + } + } + + if ((_interface == GPSHelper::Interface::UART) && (! _uart->isOpen())) { + // Configure the desired baudrate if one was specified by the user. + // Otherwise the default baudrate will be used. + if (_configured_baudrate) { + if (! _uart->setBaudrate(_configured_baudrate)) { + PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port); + px4_sleep(1); + continue; + } + } + + // Open the UART. If this is successful then the UART is ready to use. + if (! _uart->open()) { + PX4_ERR("Error opening serial device %s", _port); px4_sleep(1); continue; } #ifdef __PX4_LINUX - 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); + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) { + _spi_fd = ::open(_port, O_RDWR | O_NOCTTY); - if (status_value < 0) { - PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno); - } + if (_spi_fd < 0) { + PX4_ERR("failed to open SPI port %s err: %d", _port, errno); + px4_sleep(1); + continue; + } - status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed); + int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi) + int status_value = ::ioctl(_spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); - if (status_value < 0) { - PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno); - } + if (status_value < 0) { + PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno); + } + + status_value = ::ioctl(_spi_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); } #endif /* __PX4_LINUX */ @@ -1056,9 +1027,17 @@ GPS::run() } } - if (_serial_fd >= 0) { - ::close(_serial_fd); - _serial_fd = -1; + if ((_interface == GPSHelper::Interface::UART) && (_uart)) { + (void) _uart->close(); + delete _uart; + _uart = nullptr; + +#ifdef __PX4_LINUX + + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { + ::close(_spi_fd); + _spi_fd = -1; +#endif } if (_mode_auto) { @@ -1477,12 +1456,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) break; case 'i': - if (!strcmp(myoptarg, "spi")) { - interface = GPSHelper::Interface::SPI; - - } else if (!strcmp(myoptarg, "uart")) { + if (!strcmp(myoptarg, "uart")) { interface = GPSHelper::Interface::UART; - +#ifdef __PX4_LINUX + } else if (!strcmp(myoptarg, "spi")) { + interface = GPSHelper::Interface::SPI; +#endif } else { PX4_ERR("unknown interface: %s", myoptarg); error_flag = true; @@ -1490,12 +1469,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) break; case 'j': - if (!strcmp(myoptarg, "spi")) { - interface_secondary = GPSHelper::Interface::SPI; - - } else if (!strcmp(myoptarg, "uart")) { + if (!strcmp(myoptarg, "uart")) { interface_secondary = GPSHelper::Interface::UART; - +#ifdef __PX4_LINUX + } else if (!strcmp(myoptarg, "spi")) { + interface_secondary = GPSHelper::Interface::SPI; +#endif } else { PX4_ERR("unknown interface for secondary: %s", myoptarg); error_flag = true;