AP_HAL_Linux: made UARTDriver use TCPServerDevice

This commit is contained in:
Staroselskii Georgii 2015-06-19 19:15:30 +03:00 committed by Andrew Tridgell
parent 56f760f022
commit a3f47878a2
2 changed files with 25 additions and 8 deletions

View File

@ -28,6 +28,7 @@
#include "UDPDevice.h" #include "UDPDevice.h"
#include "ConsoleDevice.h" #include "ConsoleDevice.h"
#include "TCPClientDevice.h" #include "TCPClientDevice.h"
#include "TCPServerDevice.h"
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -75,9 +76,7 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
switch (_parseDevicePath(device_path)) { switch (_parseDevicePath(device_path)) {
case DEVICE_TCP: case DEVICE_TCP:
{ {
_device = new TCPClientDevice(_ip, _base_port); _tcp_start_connection();
_device->open();
_connected = false;
break; break;
} }
@ -255,6 +254,23 @@ void LinuxUARTDriver::_udp_start_connection(void)
_packetise = true; _packetise = true;
} }
void LinuxUARTDriver::_tcp_start_connection(void)
{
if (_flag != NULL) {
if (!strcmp(_flag, "wait")) {
_device = new TCPServerDevice(_ip, _base_port);
} else {
_device = new TCPClientDevice(_ip, _base_port);
}
} else {
_device = new TCPClientDevice(_ip, _base_port);
}
if (_device->open()) {
_connected = true;
}
}
/* /*
shutdown a UART shutdown a UART
*/ */

View File

@ -48,6 +48,7 @@ private:
void _allocate_buffers(uint16_t rxS, uint16_t txS); void _allocate_buffers(uint16_t rxS, uint16_t txS);
void _deallocate_buffers(); void _deallocate_buffers();
void _udp_start_connection(void); void _udp_start_connection(void);
void _tcp_start_connection(void);
bool _serial_start_connection(void); bool _serial_start_connection(void);
enum device_type { enum device_type {