mirror of https://github.com/ArduPilot/ardupilot
77 lines
1.3 KiB
C++
77 lines
1.3 KiB
C++
#include "UDPDevice.h"
|
|
|
|
#include <fcntl.h>
|
|
#include <stdio.h>
|
|
#include <sys/ioctl.h>
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
UDPDevice::UDPDevice(const char *ip, uint16_t port, bool bcast, bool input):
|
|
_ip(ip),
|
|
_port(port),
|
|
_bcast(bcast),
|
|
_input(input)
|
|
{
|
|
}
|
|
|
|
UDPDevice::~UDPDevice()
|
|
{
|
|
}
|
|
|
|
ssize_t UDPDevice::write(const uint8_t *buf, uint16_t n)
|
|
{
|
|
if (!socket.pollout(0)) {
|
|
return -1;
|
|
}
|
|
if (_connected) {
|
|
return socket.send(buf, n);
|
|
}
|
|
if (_input) {
|
|
// can't send yet
|
|
return -1;
|
|
}
|
|
return socket.sendto(buf, n, _ip, _port);
|
|
}
|
|
|
|
ssize_t UDPDevice::read(uint8_t *buf, uint16_t n)
|
|
{
|
|
ssize_t ret = socket.recv(buf, n, 0);
|
|
if (!_connected && ret > 0) {
|
|
const char *ip;
|
|
uint16_t port;
|
|
socket.last_recv_address(ip, port);
|
|
_connected = socket.connect(ip, port);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
bool UDPDevice::open()
|
|
{
|
|
if (_input) {
|
|
socket.bind(_ip, _port);
|
|
return true;
|
|
}
|
|
if (_bcast) {
|
|
// open now, then connect on first received packet
|
|
socket.set_broadcast();
|
|
return true;
|
|
}
|
|
_connected = socket.connect(_ip, _port);
|
|
return _connected;
|
|
}
|
|
|
|
bool UDPDevice::close()
|
|
{
|
|
return true;
|
|
}
|
|
|
|
void UDPDevice::set_blocking(bool blocking)
|
|
{
|
|
socket.set_blocking(blocking);
|
|
}
|
|
|
|
void UDPDevice::set_speed(uint32_t speed)
|
|
{
|
|
|
|
}
|