From 44837a11f27f2ed382b203685305b45c6cacbc32 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jan 2013 19:42:30 +1100 Subject: [PATCH] HAL_PX4: added baudrate support to UART driver --- libraries/AP_HAL_PX4/UARTDriver.cpp | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_PX4/UARTDriver.cpp b/libraries/AP_HAL_PX4/UARTDriver.cpp index a95ec0ca1e..0542fda90f 100644 --- a/libraries/AP_HAL_PX4/UARTDriver.cpp +++ b/libraries/AP_HAL_PX4/UARTDriver.cpp @@ -11,6 +11,7 @@ #include #include #include +#include using namespace PX4; @@ -23,11 +24,11 @@ PX4UARTDriver::PX4UARTDriver(const char *devpath) { extern const AP_HAL::HAL& hal; /* - this UART driver just maps to fd 0/1, which goes to whatever is - setup for the PX4 console. Baud rate control is not available. + this UART driver maps to a serial device in /dev */ -void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) { +void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) +{ if (!_initialised) { _fd = open(_devpath, O_RDWR); if (_fd == -1) { @@ -37,9 +38,18 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) { } _initialised = true; } + + if (b != 0) { + // set the baud rate + struct termios t; + tcgetattr(_fd, &t); + cfsetspeed(&t, b); + tcsetattr(_fd, TCSANOW, &t); + } } -void PX4UARTDriver::begin(uint32_t b) { +void PX4UARTDriver::begin(uint32_t b) +{ begin(b, 0, 0); }