diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp index 288bd01df7..19e935c724 100644 --- a/libraries/AP_HAL_PX4/RCOutput.cpp +++ b/libraries/AP_HAL_PX4/RCOutput.cpp @@ -574,7 +574,7 @@ void PX4RCOutput::timer_tick(void) /* enable sbus output */ -bool PX4RCOutput::enable_sbus_out(uint16_t rate_hz) +bool PX4RCOutput::enable_px4io_sbus_out(uint16_t rate_hz) { int fd = open("/dev/px4io", 0); if (fd == -1) { diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h index c5e4792f58..0787669e06 100644 --- a/libraries/AP_HAL_PX4/RCOutput.h +++ b/libraries/AP_HAL_PX4/RCOutput.h @@ -38,7 +38,7 @@ public: void set_output_mode(enum output_mode mode) override; void timer_tick(void) override; - bool enable_sbus_out(uint16_t rate_hz) override; + bool enable_px4io_sbus_out(uint16_t rate_hz) override; // set default output update rate void set_default_rate(uint16_t rate_hz) override; diff --git a/libraries/AP_HAL_PX4/UARTDriver.cpp b/libraries/AP_HAL_PX4/UARTDriver.cpp index 68a8ef4fe7..d86d24860d 100644 --- a/libraries/AP_HAL_PX4/UARTDriver.cpp +++ b/libraries/AP_HAL_PX4/UARTDriver.cpp @@ -28,7 +28,8 @@ PX4UARTDriver::PX4UARTDriver(const char *devpath, const char *perf_name) : _in_timer(false), _perf_uart(perf_alloc(PC_ELAPSED, perf_name)), _os_start_auto_space(-1), - _flow_control(FLOW_CONTROL_DISABLE) + _flow_control(FLOW_CONTROL_DISABLE), + _unbuffered_writes(false) { } @@ -153,6 +154,44 @@ void PX4UARTDriver::set_flow_control(enum flow_control fcontrol) _flow_control = fcontrol; } +void PX4UARTDriver::configure_parity(uint8_t v) { + if (_fd == -1) { + return; + } + struct termios t; + tcgetattr(_fd, &t); + if (v != 0) { + // enable parity + t.c_cflag |= PARENB; + if (v == 1) { + t.c_cflag |= PARODD; + } else { + t.c_cflag &= ~PARODD; + } + } + else { + // disable parity + t.c_cflag &= ~PARENB; + } + tcsetattr(_fd, TCSANOW, &t); +} + +void PX4UARTDriver::set_stop_bits(int n) { + if (_fd == -1) { + return; + } + struct termios t; + tcgetattr(_fd, &t); + if (n > 1) t.c_cflag |= CSTOPB; + else t.c_cflag &= ~CSTOPB; + tcsetattr(_fd, TCSANOW, &t); +} + +bool PX4UARTDriver::set_unbuffered_writes(bool on) { + _unbuffered_writes = on; + return _unbuffered_writes; +} + void PX4UARTDriver::begin(uint32_t b) { begin(b, 0, 0); @@ -256,7 +295,7 @@ int16_t PX4UARTDriver::read() } /* - write one byte to the buffer + write one byte */ size_t PX4UARTDriver::write(uint8_t c) { @@ -268,6 +307,11 @@ size_t PX4UARTDriver::write(uint8_t c) return 0; } + if (_unbuffered_writes) { + // write one byte to the file descriptor + return _write_fd(&c, 1); + } + while (_writebuf.space() == 0) { if (_nonblocking_writes) { return 0; @@ -278,14 +322,14 @@ size_t PX4UARTDriver::write(uint8_t c) } /* - write size bytes to the write buffer + * write size bytes */ size_t PX4UARTDriver::write(const uint8_t *buffer, size_t size) { if (_uart_owner_pid != getpid()){ return 0; } - if (!_initialised) { + if (!_initialised) { try_initialise(); return 0; } @@ -302,6 +346,11 @@ size_t PX4UARTDriver::write(const uint8_t *buffer, size_t size) return ret; } + if (_unbuffered_writes) { + // write buffer straight to the file descriptor + return _write_fd(buffer, size); + } + return _writebuf.write(buffer, size); } diff --git a/libraries/AP_HAL_PX4/UARTDriver.h b/libraries/AP_HAL_PX4/UARTDriver.h index 18aa4a2c62..847b2aaf30 100644 --- a/libraries/AP_HAL_PX4/UARTDriver.h +++ b/libraries/AP_HAL_PX4/UARTDriver.h @@ -39,6 +39,10 @@ public: void set_flow_control(enum flow_control flow_control); enum flow_control get_flow_control(void) { return _flow_control; } + void configure_parity(uint8_t v); + void set_stop_bits(int n); + bool set_unbuffered_writes(bool on); + private: const char *_devpath; int _fd; @@ -47,6 +51,7 @@ private: volatile bool _in_timer; bool _nonblocking_writes; + bool _unbuffered_writes; // we use in-task ring buffers to reduce the system call cost // of ::read() and ::write() in the main loop