diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index 421aa5d1ac..83c5f354ff 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -24,6 +24,7 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P if (port) { strncpy(_port, port, sizeof(_port) - 1); _port[sizeof(_port) - 1] = '\0'; + } else { _port[0] = 0; } @@ -37,7 +38,8 @@ SerialImpl::~SerialImpl() } } -bool SerialImpl::configure() { +bool SerialImpl::configure() +{ /* process baud rate */ int speed; @@ -72,6 +74,7 @@ bool SerialImpl::configure() { } struct termios uart_config; + int termios_state; /* fill the struct for the new configuration */ @@ -148,7 +151,7 @@ bool SerialImpl::open() // Configure the serial port if a baudrate has been configured if (_baudrate) { - if ( ! configure()) { + if (! configure()) { PX4_ERR("failed to configure %s err: %d", _port, errno); return false; } @@ -220,7 +223,8 @@ ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t char fds[0].events = POLLIN; hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us); - if (remaining_time <= 0) break; + + if (remaining_time <= 0) { break; } int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time); diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index 59b6c9fad6..71aa15b8bc 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -22,6 +22,7 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P if (port) { strncpy(_port, port, sizeof(_port) - 1); _port[sizeof(_port) - 1] = '\0'; + } else { _port[0] = 0; } @@ -35,7 +36,8 @@ SerialImpl::~SerialImpl() } } -bool SerialImpl::configure() { +bool SerialImpl::configure() +{ /* process baud rate */ int speed; @@ -70,6 +72,7 @@ bool SerialImpl::configure() { } struct termios uart_config; + int termios_state; /* fill the struct for the new configuration */ @@ -146,7 +149,7 @@ bool SerialImpl::open() // Configure the serial port if a baudrate has been configured if (_baudrate) { - if ( ! configure()) { + if (! configure()) { PX4_ERR("failed to configure %s err: %d", _port, errno); return false; } @@ -218,7 +221,8 @@ ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t char fds[0].events = POLLIN; hrt_abstime remaining_time = timeout_us - hrt_elapsed_time(&start_time_us); - if (remaining_time <= 0) break; + + if (remaining_time <= 0) { break; } int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), remaining_time); diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index cf8f4af0ae..cfdbddc4ba 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -19,12 +19,13 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P if (port) { strncpy(_port, port, sizeof(_port) - 1); _port[sizeof(_port) - 1] = '\0'; + } else { _port[0] = 0; } // Start off with a valid bitrate to make sure open can succeed - if (_baudrate == 0) _baudrate = 9600; + if (_baudrate == 0) { _baudrate = 9600; } } @@ -69,6 +70,7 @@ bool SerialImpl::open() if (serial_fd < 0) { PX4_ERR("failed to open %s, fd returned: %d", _port, serial_fd); return false; + } else { PX4_INFO("Successfully opened UART %s with baudrate %u", _port, _baudrate); } @@ -104,7 +106,7 @@ ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) return -1; } - int ret_read = qurt_uart_read(_serial_fd, (char*) buffer, buffer_size, 500); + int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 500); if (ret_read < 0) { PX4_DEBUG("%s read error %d", _port, ret_read); @@ -144,7 +146,7 @@ ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t char return total_bytes_read; } } - + int current_bytes_read = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); if (current_bytes_read < 0) { @@ -169,8 +171,9 @@ ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t char const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us); int64_t time_until_timeout = timeout_us - elapsed_us; uint64_t time_to_sleep = 5000; + if ((time_until_timeout >= 0) && - (time_until_timeout < (int64_t) time_to_sleep)) { + (time_until_timeout < (int64_t) time_to_sleep)) { time_to_sleep = time_until_timeout; } @@ -187,7 +190,7 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) return -1; } - int ret_write = qurt_uart_write(_serial_fd, (const char*) buffer, buffer_size); + int ret_write = qurt_uart_write(_serial_fd, (const char *) buffer, buffer_size); if (ret_write < 0) { PX4_ERR("%s write error %d", _port, ret_write); diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 95ef5d06bb..09eb53fdd2 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -170,7 +170,9 @@ public: void reset_if_scheduled(); private: - int _spi_fd{-1}; ///< SPI interface to GPS +#ifdef __PX4_LINUX + int _spi_fd {-1}; ///< SPI interface to GPS +#endif Serial *_uart = nullptr; ///< UART interface to GPS unsigned _baudrate{0}; ///< current baudrate const unsigned _configured_baudrate{0}; ///< configured baudrate (0=auto-detect) @@ -331,8 +333,11 @@ 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) { @@ -409,11 +414,15 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true); int ret = 0; + if (gps->_uart) { - ret = gps->_uart->write( (void*) data1, (size_t) data2); + 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; @@ -470,14 +479,13 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) if ((_interface == GPSHelper::Interface::UART) && (_uart)) { ret = _uart->readAtLeast(buf, buf_length, character_count, _timeout); +// SPI is only supported on LInux +#if defined(__PX4_LINUX) + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { -// Qurt does not support poll for serial devices (nor external SPI devices for that matter) -#if !defined(__PX4_QURT) - /* For non QURT, use the usual polling. */ - //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 serial fd and orb subscription. Unfortunately the + //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. @@ -502,17 +510,7 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate; const unsigned sleeptime = character_count * 1000000 / (baudrate / 10); -#ifdef __PX4_NUTTX - int err = 0; - int bytes_available = 0; - err = ::ioctl(_spi_fd, FIONREAD, (unsigned long)&bytes_available); - - if (err != 0 || bytes_available < (int)character_count) { - px4_usleep(sleeptime); - } -#else px4_usleep(sleeptime); -#endif ret = ::read(_spi_fd, buf, buf_length); @@ -524,6 +522,7 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) ret = -1; } } + #endif } @@ -599,11 +598,14 @@ bool GPS::injectData(uint8_t *data, size_t len) size_t written = 0; if ((_interface == GPSHelper::Interface::UART) && (_uart)) { - written = _uart->write((const void*) data, len); + 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; @@ -615,9 +617,13 @@ int GPS::setBaudrate(unsigned baud) if (_uart) { return _uart->setBaudrate(baud); } + +#ifdef __PX4_LINUX + } else if (_interface == GPSHelper::Interface::SPI) { // Can't set the baudrate on a SPI port but just return a success return 0; +#endif } return -1; @@ -790,7 +796,9 @@ GPS::run() _uart->open(); - } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)){ +#ifdef __PX4_LINUX + + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) { _spi_fd = ::open(_port, O_RDWR | O_NOCTTY); if (_spi_fd < 0) { @@ -799,7 +807,6 @@ GPS::run() continue; } -#ifdef __PX4_LINUX 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); @@ -812,6 +819,7 @@ GPS::run() if (status_value < 0) { PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno); } + #endif /* __PX4_LINUX */ } @@ -1001,14 +1009,17 @@ GPS::run() } } - if ((_interface == GPSHelper::Interface::UART) && (_uart)){ + if ((_interface == GPSHelper::Interface::UART) && (_uart)) { _uart->close(); delete _uart; - } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)){ +#ifdef __PX4_LINUX + + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { ::close(_spi_fd); _spi_fd = -1; - } +#endif + } if (_mode_auto) { switch (_mode) { @@ -1426,12 +1437,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;