/
UDPDevice.cpp
76 lines (65 loc) · 1.33 KB
/
UDPDevice.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
#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)
{
}