Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
udp_arch.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
3  *
4  * This file is part of Paparazzi.
5  *
6  * Paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * Paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with Paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
26 #include "mcu_periph/udp.h"
27 #include "udp_socket.h"
28 #include <stdlib.h>
29 #include <stdio.h>
30 #include <errno.h>
31 
32 #include <pthread.h>
33 #include <sys/select.h>
34 
35 #include "rt_priority.h"
36 
37 #ifndef UDP_THREAD_PRIO
38 #define UDP_THREAD_PRIO 10
39 #endif
40 
41 static void *udp_thread(void *data __attribute__((unused)));
42 static pthread_mutex_t udp_mutex = PTHREAD_MUTEX_INITIALIZER;
43 
44 void udp_arch_init(void)
45 {
46  pthread_mutex_init(&udp_mutex, NULL);
47 
48 #ifdef USE_UDP0
49  UDP0Init();
50 #endif
51 #ifdef USE_UDP1
52  UDP1Init();
53 #endif
54 #ifdef USE_UDP2
55  UDP2Init();
56 #endif
57 
58  pthread_t tid;
59  if (pthread_create(&tid, NULL, udp_thread, NULL) != 0) {
60  fprintf(stderr, "udp_arch_init: Could not create UDP reading thread.\n");
61  return;
62  }
63 }
64 
69 void udp_arch_periph_init(struct udp_periph *p, char *host, int port_out, int port_in, bool broadcast)
70 {
71  struct UdpSocket *sock = malloc(sizeof(struct UdpSocket));
72  udp_socket_create(sock, host, port_out, port_in, broadcast);
73  p->network = (void *)sock;
74 }
75 
82 {
83  pthread_mutex_lock(&udp_mutex);
84  int16_t available = p->rx_insert_idx - p->rx_extract_idx;
85  if (available < 0) {
86  available += UDP_RX_BUFFER_SIZE;
87  }
88  pthread_mutex_unlock(&udp_mutex);
89  return (uint16_t)available;
90 }
91 
98 {
99  pthread_mutex_lock(&udp_mutex);
100  uint8_t ret = p->rx_buf[p->rx_extract_idx];
102  pthread_mutex_unlock(&udp_mutex);
103  return ret;
104 }
105 
109 void udp_receive(struct udp_periph *p)
110 {
111  if (p == NULL) return;
112  if (p->network == NULL) return;
113 
114  int16_t i;
117  struct UdpSocket *sock = (struct UdpSocket *) p->network;
118 
119  if (available <= 0) {
120  return; // No space
121  }
122 
123  socklen_t slen = sizeof(struct sockaddr_in);
124  ssize_t byte_read = recvfrom(sock->sockfd, buf, available, MSG_DONTWAIT,
125  (struct sockaddr *)&sock->addr_in, &slen);
126 
127  pthread_mutex_lock(&udp_mutex);
128 
129  if (byte_read > 0) {
130  for (i = 0; i < byte_read; i++) {
131  p->rx_buf[p->rx_insert_idx] = buf[i];
133  }
134  }
135 
136  pthread_mutex_unlock(&udp_mutex);
137 }
138 
142 void udp_send_message(struct udp_periph *p, long fd __attribute__((unused)))
143 {
144  if (p == NULL) return;
145  if (p->network == NULL) return;
146 
147  struct UdpSocket *sock = (struct UdpSocket *) p->network;
148 
149  if (p->tx_insert_idx > 0) {
150  ssize_t bytes_sent = sendto(sock->sockfd, p->tx_buf, p->tx_insert_idx, MSG_DONTWAIT,
151  (struct sockaddr *)&sock->addr_out, sizeof(sock->addr_out));
152  if (bytes_sent != p->tx_insert_idx) {
153  if (bytes_sent < 0) {
154  perror("udp_send_message failed");
155  }
156  else {
157  fprintf(stderr, "udp_send_message: only sent %d bytes instead of %d\n",
158  (int)bytes_sent, p->tx_insert_idx);
159  }
160  }
161  p->tx_insert_idx = 0;
162  }
163 }
164 
168 void udp_send_raw(struct udp_periph *p, long fd __attribute__((unused)), uint8_t *buffer, uint16_t size)
169 {
170  if (p == NULL) return;
171  if (p->network == NULL) return;
172 
173  struct UdpSocket *sock = (struct UdpSocket *) p->network;
174  ssize_t test __attribute__((unused)) = sendto(sock->sockfd, buffer, size, MSG_DONTWAIT,
175  (struct sockaddr *)&sock->addr_out, sizeof(sock->addr_out));
176 }
177 
181 static void *udp_thread(void *data __attribute__((unused)))
182 {
184 
185  /* file descriptor list */
186  fd_set socks_master;
187  /* maximum file descriptor number */
188  int fdmax = 0;
189 
190  /* clear the fd list */
191  FD_ZERO(&socks_master);
192  /* add used sockets */
193  int fd __attribute__((unused));
194 #if USE_UDP0
195  fd = ((struct UdpSocket *)udp0.network)->sockfd;
196  FD_SET(fd, &socks_master);
197  if (fd > fdmax) {
198  fdmax =fd;
199  }
200 #endif
201 #if USE_UDP1
202  fd = ((struct UdpSocket *)udp1.network)->sockfd;
203  FD_SET(fd, &socks_master);
204  if (fd > fdmax) {
205  fdmax =fd;
206  }
207 #endif
208 #if USE_UDP2
209  fd = ((struct UdpSocket *)udp2.network)->sockfd;
210  FD_SET(fd, &socks_master);
211  if (fd > fdmax) {
212  fdmax =fd;
213  }
214 #endif
215 
216  /* socks to be read, modified after each select */
217  fd_set socks;
218 
219  while (1) {
220  /* reset list of socks to check */
221  socks = socks_master;
222 
223  if (select(fdmax + 1, &socks, NULL, NULL, NULL) < 0) {
224  fprintf(stderr, "udp_thread: select failed!");
225  }
226  else {
227 #if USE_UDP0
228  fd = ((struct UdpSocket *)udp0.network)->sockfd;
229  if (FD_ISSET(fd, &socks)) {
230  udp_receive(&udp0);
231  }
232 #endif
233 #if USE_UDP1
234  fd = ((struct UdpSocket *)udp1.network)->sockfd;
235  if (FD_ISSET(fd, &socks)) {
236  udp_receive(&udp1);
237  }
238 #endif
239 #if USE_UDP2
240  fd = ((struct UdpSocket *)udp2.network)->sockfd;
241  if (FD_ISSET(fd, &socks)) {
242  udp_receive(&udp2);
243  }
244 #endif
245  }
246  }
247  return 0;
248 }
unsigned short uint16_t
Definition: types.h:16
uint8_t tx_buf[UDP_TX_BUFFER_SIZE]
Transmit buffer.
Definition: udp.h:44
Easily create and use UDP sockets.
void udp_send_message(struct udp_periph *p, long fd)
Send a message.
Definition: udp_arch.c:142
uint16_t rx_extract_idx
Definition: udp.h:42
Functions to obtain rt priority or set the nice level.
#define UDP_RX_BUFFER_SIZE
Definition: udp.h:35
int sockfd
Definition: udp_socket.h:35
uint16_t udp_char_available(struct udp_periph *p)
Get number of bytes available in receive buffer.
Definition: udp_arch.c:81
uint16_t tx_insert_idx
Definition: udp.h:45
void udp_arch_init(void)
Definition: udp_arch.c:44
signed short int16_t
Definition: types.h:17
void * network
UDP network.
Definition: udp.h:47
void udp_receive(struct udp_periph *p)
Read bytes from UDP.
Definition: udp_arch.c:109
Definition: udp.h:38
struct sockaddr_in addr_in
Definition: udp_socket.h:36
struct sockaddr_in addr_out
Definition: udp_socket.h:37
unsigned char uint8_t
Definition: types.h:14
static pthread_mutex_t udp_mutex
Definition: udp_arch.c:42
int fd
Definition: serial.c:26
static void * udp_thread(void *data)
check for new udp packets to receive or send.
Definition: udp_arch.c:181
#define UDP_THREAD_PRIO
Definition: udp_arch.c:38
uint16_t rx_insert_idx
Definition: udp.h:41
static float p[2][2]
uint8_t rx_buf[UDP_RX_BUFFER_SIZE]
Receive buffer.
Definition: udp.h:40
uint8_t udp_getch(struct udp_periph *p)
Get the last character from the receive buffer.
Definition: udp_arch.c:97
void udp_arch_periph_init(struct udp_periph *p, char *host, int port_out, int port_in, bool broadcast)
Initialize the UDP peripheral.
Definition: udp_arch.c:69
static int get_rt_prio(int prio)
Definition: rt_priority.h:32
arch independent UDP API
int udp_socket_create(struct UdpSocket *sock, char *host, int port_out, int port_in, bool broadcast)
Create UDP socket and bind it.
Definition: udp_socket.c:49
if(PrimarySpektrumState.SpektrumTimer)
void udp_send_raw(struct udp_periph *p, long fd, uint8_t *buffer, uint16_t size)
Send a packet from another buffer.
Definition: udp_arch.c:168