[datalink] Fix specific UDP datalink to transparent

with error message when using an old version of telemetry udp

conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml

@@ -20,7 +20,7 @@
     </target>
 
 	<!-- Subsystem section -->
-	<subsystem name="telemetry" type="udp"/>
+	<subsystem name="telemetry" type="transparent_udp"/>
     <subsystem name="radio_control" type="datalink"/>
     <subsystem name="motor_mixing"/>
     <subsystem name="actuators" type="ardrone2"/>

conf/airframes/ardrone2_raw.xml

@@ -5,7 +5,7 @@
   <firmware name="rotorcraft">
     <target name="ap" board="ardrone2_raw">
       <define name="AUTOPILOT_DISABLE_AHRS_KILL"/>
-      <subsystem name="telemetry" type="udp"/>
+      <subsystem name="telemetry" type="transparent_udp"/>
       <subsystem name="radio_control" type="datalink"/>
     </target>
 

conf/airframes/ardrone2_raw_optitrack.xml

@@ -8,7 +8,7 @@
       <define name="FAILSAFE_DESCENT_SPEED" value="0.5"/>
       <configure name="USE_MAGNETOMETER" value="0"/>
 
-      <subsystem name="telemetry" type="udp"/>
+      <subsystem name="telemetry" type="transparent_udp"/>
       <subsystem name="radio_control" type="datalink"/>
     </target>
 

conf/airframes/ardrone2_sdk.xml

@@ -14,7 +14,7 @@
     </target>
 
     <subsystem name="radio_control" type="datalink" />
-    <subsystem name="telemetry" type="udp" />
+    <subsystem name="telemetry" type="transparent_udp" />
     <subsystem name="gps" type="ardrone2" />
     <subsystem name="ahrs" type="ardrone2" />
     <subsystem name="ins" type="ardrone2" />

conf/boards/ardrone2_raw.makefile

@@ -33,6 +33,9 @@ ARDRONE2_IP_ADDRESS_PROBE ?= 1
 GPS_PORT           ?= UART1
 GPS_BAUD           ?= B57600
 
+# The datalink default uses UDP
+MODEM_HOST         ?= \"192.168.1.255\"
+
 # Here we define what the UART1_DEV device mapping
 $(TARGET).CFLAGS   += -DUART1_DEV=\"/dev/ttyUSB0\"
 #$(TARGET).CFLAGS  += -DUART0_DEV=\"/dev/ttyO3\"

conf/boards/ardrone2_sdk.makefile

@@ -33,6 +33,9 @@ ARDRONE2_IP_ADDRESS_PROBE ?= 1
 GPS_PORT         ?= UART1
 GPS_BAUD         ?= B57600
 
+# The datalink default uses UDP
+MODEM_HOST       ?= \"192.168.1.255\"
+
 # Here we define what the UART1_DEV device mapping
 $(TARGET).CFLAGS += -DUART1_DEV=\"/dev/ttyUSB0\"
 

conf/firmwares/subsystems/rotorcraft/telemetry_transparent_udp.makefile

@@ -0,0 +1,20 @@
+
+#serial UDP
+
+include $(CFG_SHARED)/udp.makefile
+
+MODEM_DEV         ?= UDP0
+MODEM_PORT_OUT    ?= 4242
+MODEM_PORT_IN     ?= 4243
+MODEM_BROADCAST   ?= TRUE
+
+MODEM_CFLAGS  = -DUSE_$(MODEM_DEV) -D$(MODEM_DEV)_PORT_OUT=$(MODEM_PORT_OUT) -D$(MODEM_DEV)_PORT_IN=$(MODEM_PORT_IN)
+MODEM_CFLAGS += -D$(MODEM_DEV)_BROADCAST=$(MODEM_BROADCAST) -D$(MODEM_DEV)_HOST=$(MODEM_HOST)
+
+TELEM_CFLAGS  = -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(MODEM_DEV) -DPPRZ_UART=$(MODEM_DEV)
+TELEM_CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DDefaultPeriodic='&telemetry_Main'
+
+
+ap.CFLAGS += $(MODEM_CFLAGS) $(TELEM_CFLAGS)
+ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c
+ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/rotorcraft_telemetry.c

conf/firmwares/subsystems/rotorcraft/telemetry_udp.makefile

@@ -1,8 +1 @@
-
-# Udp telemetry
-
-ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=Udp
-ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=UDP -DDefaultPeriodic='&telemetry_Main'
-ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/udp.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c
-ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/rotorcraft_telemetry.c
-ap.srcs += fms/fms_network.c
+$(error The telemetry_udp subsystem has been renamed, please replace <subsystem name="telemetry" type="udp"/> with <subsystem name="telemetry" type="transparent_udp"/> in your airframe file.)

conf/firmwares/subsystems/shared/udp.makefile

@@ -0,0 +1,14 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+ifndef UDP_INCLUDED
+
+UDP_INCLUDED = 1
+
+#generic spi master driver
+UDP_CFLAGS = -DUSE_UDP
+UDP_SRCS = mcu_periph/udp.c $(SRC_ARCH)/mcu_periph/udp_arch.c fms/fms_network.c
+
+$(TARGET).CFLAGS += $(UDP_CFLAGS)
+$(TARGET).srcs += $(UDP_SRCS)
+
+endif

sw/airborne/arch/omap/mcu_periph/udp_arch.c

@@ -0,0 +1,125 @@
+/*
+ * Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file arch/omap/mcu_periph/udp_arch.c
+ * omap UDP handling
+ */
+
+#include "mcu_periph/udp.h"
+#include <netdb.h>
+#include <netinet/in.h>
+#include <stdlib.h>
+#include <errno.h>
+
+static inline void udp_create_socket(int* sock, const int protocol, const bool_t reuse_addr, const bool_t broadcast);
+
+
+/**
+ * Initialize the UDP stream
+ */
+void udp_arch_periph_init(struct udp_periph* p, char* host, int port_out, int port_in, bool_t broadcast)
+{
+  struct UdpNetwork* network = malloc(sizeof(struct UdpNetwork));
+
+  if (port_out >= 0) {
+    // Create the output socket (enable reuse of the address, and broadcast if necessary)
+    udp_create_socket(&network->socket_out, 0, TRUE, broadcast);
+
+    // Setup the output address
+    network->addr_out.sin_family = PF_INET;
+    network->addr_out.sin_port = htons(port_out);
+    network->addr_out.sin_addr.s_addr = inet_addr(host);
+  }
+
+  if (port_in >= 0) {
+    // Creat the input socket (enable reuse of the address, and disable broadcast)
+    udp_create_socket(&network->socket_in, 0, TRUE, FALSE);
+
+    // Create the input address
+    network->addr_in.sin_family = PF_INET;
+    network->addr_in.sin_port = htons(port_in);
+    network->addr_in.sin_addr.s_addr = htonl(INADDR_ANY);
+
+    bind(network->socket_in, (struct sockaddr*)&network->addr_in, sizeof(network->addr_in));
+  }
+
+  p->network = (void*)network;
+}
+
+/**
+ * Read bytes from UDP
+ */
+void udp_read(struct udp_periph* p)
+{
+  int16_t i;
+  int16_t available = UDP_RX_BUFFER_SIZE - udp_char_available(p);
+  uint8_t buf[UDP_RX_BUFFER_SIZE];
+  struct UdpNetwork* network = (struct UdpNetwork*) p->network;
+
+  if (available <= 0) {
+    return;  // No space
+  }
+
+  socklen_t slen = sizeof(struct sockaddr_in);
+  ssize_t byte_read = recvfrom(network->socket_in, buf, UDP_RX_BUFFER_SIZE, MSG_DONTWAIT,
+                               (struct sockaddr*)&network->addr_in, &slen);
+
+  if (byte_read > 0) {
+    for (i = 0; i < byte_read; i++) {
+      p->rx_buf[p->rx_insert_idx] = buf[i];
+      p->rx_insert_idx = (p->rx_insert_idx + 1) % UDP_RX_BUFFER_SIZE;
+    }
+  }
+}
+
+/**
+ * Send a message
+ */
+void udp_send_message(struct udp_periph* p)
+{
+  struct UdpNetwork* network = (struct UdpNetwork*) p->network;
+
+  if (p->tx_insert_idx > 0) {
+    ssize_t test __attribute__((unused)) = sendto(network->socket_out, p->tx_buf, p->tx_insert_idx, MSG_DONTWAIT,
+                          (struct sockaddr*)&network->addr_out, sizeof(network->addr_out));
+    p->tx_insert_idx = 0;
+  }
+}
+
+/**
+ * Create a new udp socket
+ */
+static inline void udp_create_socket(int* sock, const int protocol, const bool_t reuse_addr, const bool_t broadcast)
+{
+  // Create the socket with the correct protocl
+  *sock = socket(PF_INET, SOCK_DGRAM, protocol);
+  int one = 1;
+
+  // Enable reusing of addres
+  if (reuse_addr) {
+    setsockopt(*sock, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
+  }
+
+  // Enable broadcasting
+  if (broadcast) {
+    setsockopt(*sock, SOL_SOCKET, SO_BROADCAST, &one, sizeof(one));
+  }
+}

sw/airborne/arch/omap/mcu_periph/udp_arch.h

@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file arch/omap/mcu_periph/udp_arch.h
+ * omap UDP handling
+ */
+
+#ifndef UDP_ARCH_H
+#define UDP_ARCH_H
+
+#include "mcu_periph/udp.h"
+#include <sys/socket.h>
+#include <arpa/inet.h>
+
+struct UdpNetwork {
+  int socket_in;
+  int socket_out;
+  struct sockaddr_in addr_in;
+  struct sockaddr_in addr_out;
+};
+
+#endif /* UDP_ARCH_H */

sw/airborne/firmwares/rotorcraft/main.c

@@ -34,15 +34,15 @@
 #include "mcu.h"
 #include "mcu_periph/sys_time.h"
 #include "mcu_periph/i2c.h"
+#if USE_UDP
+#include "mcu_periph/udp.h"
+#endif
 #include "led.h"
 
 #include "subsystems/datalink/telemetry.h"
 #include "subsystems/datalink/datalink.h"
 #include "subsystems/settings.h"
 #include "subsystems/datalink/xbee.h"
-#if DATALINK == UDP
-#include "subsystems/datalink/udp.h"
-#endif
 
 #include "subsystems/commands.h"
 #include "subsystems/actuators.h"
@@ -177,10 +177,6 @@ STATIC_INLINE void main_init( void ) {
   xbee_init();
 #endif
 
-#if DATALINK == UDP
-  udp_init();
-#endif
-
   // register the timers for the periodic functions
   main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
   modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
@@ -282,6 +278,10 @@ STATIC_INLINE void main_event( void ) {
 
   i2c_event();
 
+#if USE_UDP
+  udp_event();
+#endif
+
   DatalinkEvent();
 
   if (autopilot_rc) {

sw/airborne/mcu.c

@@ -50,6 +50,9 @@
 #ifdef USE_USB_SERIAL
 #include "mcu_periph/usb_serial.h"
 #endif
+#ifdef USE_UDP
+#include "mcu_periph/udp.h"
+#endif
 #if USE_SPI
 #include "mcu_periph/spi.h"
 #endif
@@ -152,6 +155,17 @@ void mcu_init(void) {
 #ifdef USE_DAC
   dac_init();
 #endif
+
+#ifdef USE_UDP0
+  UDP0Init();
+#endif
+#ifdef USE_UDP1
+  UDP1Init();
+#endif
+#ifdef USE_UDP2
+  UDP2Init();
+#endif
+
 #else
 INFO("PERIPHERALS_AUTO_INIT not enabled! Peripherals (including sys_time) need explicit initialization.")
 #endif /* PERIPHERALS_AUTO_INIT */