mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 07:13:56 -04:00
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:
parent
897d2fb1fb
commit
e7a5945056
@ -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 */
|
||||||
|
};
|
||||||
};
|
};
|
||||||
|
@ -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;
|
||||||
|
}
|
||||||
|
@ -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;
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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();
|
||||||
|
Loading…
Reference in New Issue
Block a user