mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
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"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
TCPServerDevice::TCPServerDevice(const char *ip, uint16_t port, bool wait):
|
||||
_ip(ip),
|
||||
_port(port),
|
||||
@ -62,19 +64,25 @@ bool TCPServerDevice::open()
|
||||
listener.reuseaddress();
|
||||
|
||||
if (!listener.bind(_ip, _port)) {
|
||||
::printf("bind failed on %s port %u - %s\n",
|
||||
_ip,
|
||||
_port,
|
||||
strerror(errno));
|
||||
::exit(1);
|
||||
if (hal.scheduler->millis() - _last_bind_warning > 5000) {
|
||||
::printf("bind failed on %s port %u - %s\n",
|
||||
_ip,
|
||||
_port,
|
||||
strerror(errno));
|
||||
_last_bind_warning = hal.scheduler->millis();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!listener.listen(1)) {
|
||||
::printf("listen failed on %s port %u - %s\n",
|
||||
_ip,
|
||||
_port,
|
||||
strerror(errno));
|
||||
::exit(1);
|
||||
if (hal.scheduler->millis() - _last_bind_warning > 5000) {
|
||||
::printf("listen failed on %s port %u - %s\n",
|
||||
_ip,
|
||||
_port,
|
||||
strerror(errno));
|
||||
_last_bind_warning = hal.scheduler->millis();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
listener.set_blocking(false);
|
||||
|
@ -23,6 +23,7 @@ private:
|
||||
uint16_t _port;
|
||||
bool _wait;
|
||||
bool _blocking = false;
|
||||
uint32_t _last_bind_warning = 0;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
@ -251,7 +251,7 @@ errout:
|
||||
bool LinuxUARTDriver::_serial_start_connection()
|
||||
{
|
||||
_device = new UARTDevice(device_path);
|
||||
_device->open();
|
||||
_connected = _device->open();
|
||||
_device->set_blocking(false);
|
||||
_flow_control = FLOW_CONTROL_DISABLE;
|
||||
|
||||
@ -264,7 +264,7 @@ bool LinuxUARTDriver::_serial_start_connection()
|
||||
void LinuxUARTDriver::_udp_start_connection(void)
|
||||
{
|
||||
_device = new UDPDevice(_ip, _base_port);
|
||||
_device->open();
|
||||
_connected = _device->open();
|
||||
_device->set_blocking(false);
|
||||
|
||||
/* 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);
|
||||
_device = new TCPServerDevice(_ip, _base_port, wait);
|
||||
|
||||
if (_device->open()) {
|
||||
_connected = true;
|
||||
}
|
||||
_connected = _device->open();
|
||||
}
|
||||
|
||||
/*
|
||||
@ -445,6 +443,17 @@ size_t LinuxUARTDriver::write(const uint8_t *buffer, size_t size)
|
||||
int LinuxUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
|
||||
{
|
||||
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);
|
||||
|
||||
|
@ -19,7 +19,7 @@ UDPDevice::~UDPDevice()
|
||||
|
||||
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)
|
||||
@ -29,7 +29,7 @@ ssize_t UDPDevice::read(uint8_t *buf, uint16_t n)
|
||||
|
||||
bool UDPDevice::open()
|
||||
{
|
||||
return true;
|
||||
return socket.connect(_ip, _port);
|
||||
}
|
||||
|
||||
bool UDPDevice::close()
|
||||
|
Loading…
Reference in New Issue
Block a user