HAL_Linux: allow startup before network bringup
this makes it possible to bootup ardupilot before the desired network interface is available. This is very useful for when using 3G dongles in aircraft
This commit is contained in:
parent
288c20a58e
commit
81a3d439a3
@ -9,6 +9,8 @@
|
|||||||
|
|
||||||
#include "TCPServerDevice.h"
|
#include "TCPServerDevice.h"
|
||||||
|
|
||||||
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
TCPServerDevice::TCPServerDevice(const char *ip, uint16_t port, bool wait):
|
TCPServerDevice::TCPServerDevice(const char *ip, uint16_t port, bool wait):
|
||||||
_ip(ip),
|
_ip(ip),
|
||||||
_port(port),
|
_port(port),
|
||||||
@ -62,19 +64,25 @@ bool TCPServerDevice::open()
|
|||||||
listener.reuseaddress();
|
listener.reuseaddress();
|
||||||
|
|
||||||
if (!listener.bind(_ip, _port)) {
|
if (!listener.bind(_ip, _port)) {
|
||||||
|
if (hal.scheduler->millis() - _last_bind_warning > 5000) {
|
||||||
::printf("bind failed on %s port %u - %s\n",
|
::printf("bind failed on %s port %u - %s\n",
|
||||||
_ip,
|
_ip,
|
||||||
_port,
|
_port,
|
||||||
strerror(errno));
|
strerror(errno));
|
||||||
::exit(1);
|
_last_bind_warning = hal.scheduler->millis();
|
||||||
|
}
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!listener.listen(1)) {
|
if (!listener.listen(1)) {
|
||||||
|
if (hal.scheduler->millis() - _last_bind_warning > 5000) {
|
||||||
::printf("listen failed on %s port %u - %s\n",
|
::printf("listen failed on %s port %u - %s\n",
|
||||||
_ip,
|
_ip,
|
||||||
_port,
|
_port,
|
||||||
strerror(errno));
|
strerror(errno));
|
||||||
::exit(1);
|
_last_bind_warning = hal.scheduler->millis();
|
||||||
|
}
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
listener.set_blocking(false);
|
listener.set_blocking(false);
|
||||||
|
@ -23,6 +23,7 @@ private:
|
|||||||
uint16_t _port;
|
uint16_t _port;
|
||||||
bool _wait;
|
bool _wait;
|
||||||
bool _blocking = false;
|
bool _blocking = false;
|
||||||
|
uint32_t _last_bind_warning = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
@ -251,7 +251,7 @@ errout:
|
|||||||
bool LinuxUARTDriver::_serial_start_connection()
|
bool LinuxUARTDriver::_serial_start_connection()
|
||||||
{
|
{
|
||||||
_device = new UARTDevice(device_path);
|
_device = new UARTDevice(device_path);
|
||||||
_device->open();
|
_connected = _device->open();
|
||||||
_device->set_blocking(false);
|
_device->set_blocking(false);
|
||||||
_flow_control = FLOW_CONTROL_DISABLE;
|
_flow_control = FLOW_CONTROL_DISABLE;
|
||||||
|
|
||||||
@ -264,7 +264,7 @@ bool LinuxUARTDriver::_serial_start_connection()
|
|||||||
void LinuxUARTDriver::_udp_start_connection(void)
|
void LinuxUARTDriver::_udp_start_connection(void)
|
||||||
{
|
{
|
||||||
_device = new UDPDevice(_ip, _base_port);
|
_device = new UDPDevice(_ip, _base_port);
|
||||||
_device->open();
|
_connected = _device->open();
|
||||||
_device->set_blocking(false);
|
_device->set_blocking(false);
|
||||||
|
|
||||||
/* try to write on MAVLink packet boundaries if possible */
|
/* try to write on MAVLink packet boundaries if possible */
|
||||||
@ -276,9 +276,7 @@ void LinuxUARTDriver::_tcp_start_connection(void)
|
|||||||
bool wait = (_flag && strcmp(_flag, "wait") == 0);
|
bool wait = (_flag && strcmp(_flag, "wait") == 0);
|
||||||
_device = new TCPServerDevice(_ip, _base_port, wait);
|
_device = new TCPServerDevice(_ip, _base_port, wait);
|
||||||
|
|
||||||
if (_device->open()) {
|
_connected = _device->open();
|
||||||
_connected = true;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
@ -446,6 +444,17 @@ int LinuxUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
|
|||||||
{
|
{
|
||||||
int ret = 0;
|
int ret = 0;
|
||||||
|
|
||||||
|
/*
|
||||||
|
allow for delayed connection. This allows ArduPilot to start
|
||||||
|
before a network interface is available.
|
||||||
|
*/
|
||||||
|
if (!_connected) {
|
||||||
|
_connected = _device->open();
|
||||||
|
}
|
||||||
|
if (!_connected) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
ret = _device->write(buf, n);
|
ret = _device->write(buf, n);
|
||||||
|
|
||||||
if (ret > 0) {
|
if (ret > 0) {
|
||||||
|
@ -19,7 +19,7 @@ UDPDevice::~UDPDevice()
|
|||||||
|
|
||||||
ssize_t UDPDevice::write(const uint8_t *buf, uint16_t n)
|
ssize_t UDPDevice::write(const uint8_t *buf, uint16_t n)
|
||||||
{
|
{
|
||||||
return socket.sendto(buf, n, _ip, _port);
|
return socket.send(buf, n);
|
||||||
}
|
}
|
||||||
|
|
||||||
ssize_t UDPDevice::read(uint8_t *buf, uint16_t n)
|
ssize_t UDPDevice::read(uint8_t *buf, uint16_t n)
|
||||||
@ -29,7 +29,7 @@ ssize_t UDPDevice::read(uint8_t *buf, uint16_t n)
|
|||||||
|
|
||||||
bool UDPDevice::open()
|
bool UDPDevice::open()
|
||||||
{
|
{
|
||||||
return true;
|
return socket.connect(_ip, _port);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool UDPDevice::close()
|
bool UDPDevice::close()
|
||||||
|
Loading…
Reference in New Issue
Block a user