AP_HAL_Linux: make flow_control a property of SerialDevice rather than UARTDriver's

Make it possible to change flow control settings for individual devices.
This commit is contained in:
Staroselskii Georgii 2016-07-07 18:46:12 +03:00 committed by Lucas De Marchi
parent 897d2fb1fb
commit e7a5945056
5 changed files with 44 additions and 10 deletions

View File

@ -3,6 +3,8 @@
#include <stdint.h> #include <stdint.h>
#include <stdlib.h> #include <stdlib.h>
#include "AP_HAL_Linux.h"
class SerialDevice { class SerialDevice {
public: public:
virtual ~SerialDevice() {} virtual ~SerialDevice() {}
@ -13,4 +15,9 @@ public:
virtual ssize_t read(uint8_t *buf, uint16_t n) = 0; virtual ssize_t read(uint8_t *buf, uint16_t n) = 0;
virtual void set_blocking(bool blocking) = 0; virtual void set_blocking(bool blocking) = 0;
virtual void set_speed(uint32_t speed) = 0; virtual void set_speed(uint32_t speed) = 0;
virtual AP_HAL::UARTDriver::flow_control get_flow_control(void) { return AP_HAL::UARTDriver::FLOW_CONTROL_ENABLE; }
virtual void set_flow_control(AP_HAL::UARTDriver::flow_control flow_control_setting)
{
/* most devices simply igmore this setting */
};
}; };

View File

@ -109,3 +109,24 @@ void UARTDevice::set_speed(uint32_t baudrate)
cfsetspeed(&t, baudrate); cfsetspeed(&t, baudrate);
tcsetattr(_fd, TCSANOW, &t); tcsetattr(_fd, TCSANOW, &t);
} }
void UARTDevice::set_flow_control(AP_HAL::UARTDriver::flow_control flow_control_setting)
{
struct termios t;
if (_flow_control == flow_control_setting) {
return;
}
tcgetattr(_fd, &t);
if (flow_control_setting != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE) {
t.c_cflag |= CRTSCTS;
} else {
t.c_cflag &= ~CRTSCTS;
}
tcsetattr(_fd, TCSANOW, &t);
_flow_control = flow_control_setting;
}

View File

@ -14,9 +14,15 @@ public:
virtual ssize_t read(uint8_t *buf, uint16_t n) override; virtual ssize_t read(uint8_t *buf, uint16_t n) override;
virtual void set_blocking(bool blocking) override; virtual void set_blocking(bool blocking) override;
virtual void set_speed(uint32_t speed) override; virtual void set_speed(uint32_t speed) override;
virtual void set_flow_control(enum AP_HAL::UARTDriver::flow_control flow_control_setting) override;
virtual AP_HAL::UARTDriver::flow_control get_flow_control(void) override
{
return _flow_control;
}
private: private:
void _disable_crlf(); void _disable_crlf();
AP_HAL::UARTDriver::flow_control _flow_control = AP_HAL::UARTDriver::flow_control::FLOW_CONTROL_DISABLE;
int _fd = -1; int _fd = -1;
const char *_device_path; const char *_device_path;

View File

@ -35,8 +35,7 @@ using namespace Linux;
UARTDriver::UARTDriver(bool default_console) : UARTDriver::UARTDriver(bool default_console) :
device_path(NULL), device_path(NULL),
_packetise(false), _packetise(false)
_flow_control(FLOW_CONTROL_DISABLE)
{ {
if (default_console) { if (default_console) {
_console = true; _console = true;
@ -74,21 +73,18 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
case DEVICE_TCP: case DEVICE_TCP:
{ {
_tcp_start_connection(); _tcp_start_connection();
_flow_control = FLOW_CONTROL_ENABLE;
break; break;
} }
case DEVICE_UDP: case DEVICE_UDP:
{ {
_udp_start_connection(); _udp_start_connection();
_flow_control = FLOW_CONTROL_ENABLE;
break; break;
} }
case DEVICE_UDPIN: case DEVICE_UDPIN:
{ {
_udpin_start_connection(); _udpin_start_connection();
_flow_control = FLOW_CONTROL_ENABLE;
break; break;
} }
@ -96,7 +92,6 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
case DEVICE_QFLIGHT: case DEVICE_QFLIGHT:
{ {
_qflight_start_connection(); _qflight_start_connection();
_flow_control = FLOW_CONTROL_DISABLE;
break; break;
} }
#endif #endif
@ -275,7 +270,6 @@ bool UARTDriver::_serial_start_connection()
_device = new UARTDevice(device_path); _device = new UARTDevice(device_path);
_connected = _device->open(); _connected = _device->open();
_device->set_blocking(false); _device->set_blocking(false);
_flow_control = FLOW_CONTROL_DISABLE;
return true; return true;
} }
@ -285,7 +279,6 @@ bool UARTDriver::_qflight_start_connection()
{ {
_device = new QFLIGHTDevice(device_path); _device = new QFLIGHTDevice(device_path);
_connected = _device->open(); _connected = _device->open();
_flow_control = FLOW_CONTROL_DISABLE;
return true; return true;
} }

View File

@ -36,7 +36,15 @@ public:
bool _write_pending_bytes(void); bool _write_pending_bytes(void);
virtual void _timer_tick(void); virtual void _timer_tick(void);
enum flow_control get_flow_control(void) { return _flow_control; } virtual enum flow_control get_flow_control(void) override
{
return _device->get_flow_control();
}
virtual void set_flow_control(enum flow_control flow_control_setting) override
{
_device->set_flow_control(flow_control_setting);
}
private: private:
AP_HAL::OwnPtr<SerialDevice> _device; AP_HAL::OwnPtr<SerialDevice> _device;
@ -48,7 +56,6 @@ private:
char *_flag; char *_flag;
bool _connected; // true if a client has connected bool _connected; // true if a client has connected
bool _packetise; // true if writes should try to be on mavlink boundaries bool _packetise; // true if writes should try to be on mavlink boundaries
enum flow_control _flow_control;
void _allocate_buffers(uint16_t rxS, uint16_t txS); void _allocate_buffers(uint16_t rxS, uint16_t txS);
void _deallocate_buffers(); void _deallocate_buffers();