Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 #include <pthread.h>
32 #include <sys/select.h>
33 
34 #include "rt_priority.h"
35 
36 #ifndef UDP_THREAD_PRIO
37 #define UDP_THREAD_PRIO 10
38 #endif
39 
40 static void *udp_thread(void *data __attribute__((unused)));
41 static pthread_mutex_t udp_mutex = PTHREAD_MUTEX_INITIALIZER;
42 
43 void udp_arch_init(void)
44 {
45  pthread_mutex_init(&udp_mutex, NULL);
46 
47 #ifdef USE_UDP0
48  UDP0Init();
49 #endif
50 #ifdef USE_UDP1
51  UDP1Init();
52 #endif
53 #ifdef USE_UDP2
54  UDP2Init();
55 #endif
56 
57  pthread_t tid;
58  if (pthread_create(&tid, NULL, udp_thread, NULL) != 0) {
59  fprintf(stderr, "udp_arch_init: Could not create UDP reading thread.\n");
60  return;
61  }
62 #ifndef __APPLE__
63  pthread_setname_np(tid, "udp");
64 #endif
65 }
66 
71 void udp_arch_periph_init(struct udp_periph *p, char *host, int port_out, int port_in, bool broadcast)
72 {
73  struct UdpSocket *sock = malloc(sizeof(struct UdpSocket));
74  udp_socket_create(sock, host, port_out, port_in, broadcast);
75  p->network = (void *)sock;
76 }
77 
84 {
85  pthread_mutex_lock(&udp_mutex);
86  int available = p->rx_insert_idx - p->rx_extract_idx;
87  if (available < 0) {
88  available += UDP_RX_BUFFER_SIZE;
89  }
90  pthread_mutex_unlock(&udp_mutex);
91  return available;
92 }
93 
100 {
101  pthread_mutex_lock(&udp_mutex);
102  uint8_t ret = p->rx_buf[p->rx_extract_idx];
103  p->rx_extract_idx = (p->rx_extract_idx + 1) % UDP_RX_BUFFER_SIZE;
104  pthread_mutex_unlock(&udp_mutex);
105  return ret;
106 }
107 
111 void udp_receive(struct udp_periph *p)
112 {
113  if (p == NULL) { return; }
114  if (p->network == NULL) { return; }
115 
116  int16_t i;
119  struct UdpSocket *sock = (struct UdpSocket *) p->network;
120 
121  if (available <= 0) {
122  return; // No space
123  }
124 
125  socklen_t slen = sizeof(struct sockaddr_in);
126  ssize_t byte_read = recvfrom(sock->sockfd, buf, available, MSG_DONTWAIT,
127  (struct sockaddr *)&sock->addr_in, &slen);
128 
129  pthread_mutex_lock(&udp_mutex);
130 
131  if (byte_read > 0) {
132  for (i = 0; i < byte_read; i++) {
133  p->rx_buf[p->rx_insert_idx] = buf[i];
134  p->rx_insert_idx = (p->rx_insert_idx + 1) % UDP_RX_BUFFER_SIZE;
135  }
136  }
137 
138  pthread_mutex_unlock(&udp_mutex);
139 }
140 
144 void udp_send_message(struct udp_periph *p, long fd __attribute__((unused)))
145 {
146  if (p == NULL) { return; }
147  if (p->network == NULL) { return; }
148 
149  struct UdpSocket *sock = (struct UdpSocket *) p->network;
150 
151  if (p->tx_insert_idx > 0) {
152  ssize_t bytes_sent = sendto(sock->sockfd, p->tx_buf, p->tx_insert_idx, MSG_DONTWAIT,
153  (struct sockaddr *)&sock->addr_out, sizeof(sock->addr_out));
154  if (bytes_sent != p->tx_insert_idx) {
155  if (bytes_sent < 0) {
156  perror("udp_send_message failed");
157  } else {
158  fprintf(stderr, "udp_send_message: only sent %d bytes instead of %d\n",
159  (int)bytes_sent, p->tx_insert_idx);
160  }
161  }
162  p->tx_insert_idx = 0;
163  }
164 }
165 
169 void udp_send_raw(struct udp_periph *p, long fd __attribute__((unused)), uint8_t *buffer, uint16_t size)
170 {
171  if (p == NULL) { return; }
172  if (p->network == NULL) { return; }
173 
174  struct UdpSocket *sock = (struct UdpSocket *) p->network;
175  ssize_t test __attribute__((unused)) = sendto(sock->sockfd, buffer, size, MSG_DONTWAIT,
176  (struct sockaddr *)&sock->addr_out, sizeof(sock->addr_out));
177 }
178 
182 static void *udp_thread(void *data __attribute__((unused)))
183 {
185 
186  /* file descriptor list */
187  fd_set socks_master;
188  /* maximum file descriptor number */
189  int fdmax = 0;
190 
191  /* clear the fd list */
192  FD_ZERO(&socks_master);
193  /* add used sockets */
194  int fd __attribute__((unused));
195 #if USE_UDP0
196  fd = ((struct UdpSocket *)udp0.network)->sockfd;
197  FD_SET(fd, &socks_master);
198  if (fd > fdmax) {
199  fdmax = fd;
200  }
201 #endif
202 #if USE_UDP1
203  fd = ((struct UdpSocket *)udp1.network)->sockfd;
204  FD_SET(fd, &socks_master);
205  if (fd > fdmax) {
206  fdmax = fd;
207  }
208 #endif
209 #if USE_UDP2
210  fd = ((struct UdpSocket *)udp2.network)->sockfd;
211  FD_SET(fd, &socks_master);
212  if (fd > fdmax) {
213  fdmax = fd;
214  }
215 #endif
216 
217  /* socks to be read, modified after each select */
218  fd_set socks;
219 
220  while (1) {
221  /* reset list of socks to check */
222  socks = socks_master;
223 
224  if (select(fdmax + 1, &socks, NULL, NULL, NULL) < 0) {
225  fprintf(stderr, "udp_thread: select failed!");
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 }
if(GpsFixValid() &&e_identification_started)
static float p[2][2]
#define UDP_THREAD_PRIO
Definition: udp_arch.c:37
void udp_receive(struct udp_periph *p)
Read bytes from UDP.
Definition: udp_arch.c:111
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:71
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:169
static void * udp_thread(void *data)
check for new udp packets to receive or send.
Definition: udp_arch.c:182
void udp_arch_init(void)
Definition: udp_arch.c:43
static pthread_mutex_t udp_mutex
Definition: udp_arch.c:41
uint8_t udp_getch(struct udp_periph *p)
Get the last character from the receive buffer.
Definition: udp_arch.c:99
int udp_char_available(struct udp_periph *p)
Get number of bytes available in receive buffer.
Definition: udp_arch.c:83
void udp_send_message(struct udp_periph *p, long fd)
Send a message.
Definition: udp_arch.c:144
Functions to obtain rt priority or set the nice level.
static int get_rt_prio(int prio)
Definition: rt_priority.h:32
int fd
Definition: serial.c:26
arch independent UDP API
#define UDP_RX_BUFFER_SIZE
Definition: udp.h:35
Definition: udp.h:38
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
Easily create and use UDP sockets.
struct sockaddr_in addr_in
Definition: udp_socket.h:36
struct sockaddr_in addr_out
Definition: udp_socket.h:37
int sockfd
Definition: udp_socket.h:35
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98