APM:Libraries
UDPDevice.cpp
Go to the documentation of this file.
1 #include "UDPDevice.h"
2 
3 #include <fcntl.h>
4 #include <stdio.h>
5 #include <sys/ioctl.h>
6 
7 #include <AP_HAL/AP_HAL.h>
8 
9 UDPDevice::UDPDevice(const char *ip, uint16_t port, bool bcast, bool input):
10  _ip(ip),
11  _port(port),
12  _bcast(bcast),
13  _input(input)
14 {
15 }
16 
18 {
19 }
20 
21 ssize_t UDPDevice::write(const uint8_t *buf, uint16_t n)
22 {
23  if (!socket.pollout(0)) {
24  return -1;
25  }
26  if (_connected) {
27  return socket.send(buf, n);
28  }
29  if (_input) {
30  // can't send yet
31  return -1;
32  }
33  return socket.sendto(buf, n, _ip, _port);
34 }
35 
36 ssize_t UDPDevice::read(uint8_t *buf, uint16_t n)
37 {
38  ssize_t ret = socket.recv(buf, n, 0);
39  if (!_connected && ret > 0) {
40  const char *ip;
41  uint16_t port;
42  socket.last_recv_address(ip, port);
43  _connected = socket.connect(ip, port);
44  }
45  return ret;
46 }
47 
49 {
50  if (_input) {
51  socket.bind(_ip, _port);
52  return true;
53  }
54  if (_bcast) {
55  // open now, then connect on first received packet
56  socket.set_broadcast();
57  return true;
58  }
59  _connected = socket.connect(_ip, _port);
60  return _connected;
61 }
62 
64 {
65  return true;
66 }
67 
68 void UDPDevice::set_blocking(bool blocking)
69 {
70  socket.set_blocking(blocking);
71 }
72 
73 void UDPDevice::set_speed(uint32_t speed)
74 {
75 
76 }
bool _bcast
Definition: UDPDevice.h:21
virtual bool close() override
Definition: UDPDevice.cpp:63
bool _connected
Definition: UDPDevice.h:23
SocketAPM socket
Definition: UDPDevice.h:18
virtual ssize_t write(const uint8_t *buf, uint16_t n) override
Definition: UDPDevice.cpp:21
UDPDevice(const char *ip, uint16_t port, bool bcast, bool input)
Definition: UDPDevice.cpp:9
virtual void set_blocking(bool blocking) override
Definition: UDPDevice.cpp:68
virtual bool open() override
Definition: UDPDevice.cpp:48
virtual void set_speed(uint32_t speed) override
Definition: UDPDevice.cpp:73
bool _input
Definition: UDPDevice.h:22
virtual ssize_t read(uint8_t *buf, uint16_t n) override
Definition: UDPDevice.cpp:36
uint16_t _port
Definition: UDPDevice.h:20
const char * _ip
Definition: UDPDevice.h:19
virtual ~UDPDevice()
Definition: UDPDevice.cpp:17