[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 @@
-
+$(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.)
-# 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

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 */

sw/airborne/mcu_periph/udp.c

@@ -0,0 +1,97 @@
+/*
+ * 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 mcu_periph/udp.c
+ *  \brief arch independent UDP API
+ *
+ */
+
+#include "mcu_periph/udp.h"
+
+void udp_periph_init(struct udp_periph* p, char* host, int port_out, int port_in, bool_t broadcast)
+{
+  p->rx_insert_idx = 0;
+  p->rx_extract_idx = 0;
+  p->tx_insert_idx = 0;
+
+// Arch dependent initialization
+  udp_arch_periph_init(p, host, port_out, port_in, broadcast);
+}
+
+bool_t udp_check_free_space(struct udp_periph* p, uint8_t len)
+{
+  return (UDP_TX_BUFFER_SIZE - p->tx_insert_idx) >= len;
+}
+
+void udp_transmit(struct udp_periph* p, uint8_t data)
+{
+  if (p->tx_insert_idx >= UDP_TX_BUFFER_SIZE) {
+    return;  // no room
+  }
+
+  p->tx_buf[p->tx_insert_idx] = data;
+  p->tx_insert_idx++;
+}
+
+uint16_t udp_char_available(struct udp_periph* p)
+{
+  int16_t available = p->rx_insert_idx - p->rx_extract_idx;
+  if (available < 0) {
+    available += UDP_RX_BUFFER_SIZE;
+  }
+  return (uint16_t)available;
+}
+
+uint8_t udp_getch(struct udp_periph* p)
+{
+  uint8_t ret = p->rx_buf[p->rx_extract_idx];
+  p->rx_extract_idx = (p->rx_extract_idx + 1) % UDP_RX_BUFFER_SIZE;
+  return ret;
+}
+
+void udp_event(void)
+{
+#if USE_UDP0
+  udp_read(&udp0);
+#endif // USE_UDP0
+
+#if USE_UDP1
+  udp_read(&udp1);
+#endif // USE_UDP1
+
+#if USE_UDP2
+  udp_read(&udp2);
+#endif // USE_UDP2
+}
+
+
+#if USE_UDP0
+struct udp_periph udp0;
+#endif // USE_UDP0
+
+#if USE_UDP1
+struct udp_periph udp1;
+#endif // USE_UDP1
+
+#if USE_UDP2
+struct udp_periph udp2;
+#endif // USE_UDP2

sw/airborne/mcu_periph/udp.h

@@ -0,0 +1,104 @@
+/*
+ * 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 mcu_periph/udp.h
+ *  \brief arch independent UDP API
+ *
+ */
+
+#ifndef MCU_PERIPH_UDP_H
+#define MCU_PERIPH_UDP_H
+
+#include "std.h"
+#include "mcu_periph/udp_arch.h"
+
+#define UDP_RX_BUFFER_SIZE 256
+#define UDP_TX_BUFFER_SIZE 256
+
+struct udp_periph {
+  /** Receive buffer */
+  uint8_t rx_buf[UDP_RX_BUFFER_SIZE];
+  uint16_t rx_insert_idx;
+  uint16_t rx_extract_idx;
+  /** Transmit buffer */
+  uint8_t tx_buf[UDP_TX_BUFFER_SIZE];
+  uint16_t tx_insert_idx;
+  /** UDP network */
+  void* network;
+};
+
+extern void     udp_periph_init(struct udp_periph* p, char* host, int port_out, int port_in, bool_t broadcast);
+extern bool_t   udp_check_free_space(struct udp_periph* p, uint8_t len);
+extern void     udp_transmit(struct udp_periph* p, uint8_t data);
+extern uint16_t udp_char_available(struct udp_periph* p);
+extern uint8_t  udp_getch(struct udp_periph* p);
+extern void     udp_event(void);
+extern void     udp_arch_periph_init(struct udp_periph* p, char* host, int port_out, int port_in, bool_t broadcast);
+extern void     udp_send_message(struct udp_periph* p);
+extern void     udp_read(struct udp_periph* p);
+
+#if USE_UDP0
+extern struct udp_periph udp0;
+
+#ifndef UDP0_BROADCAST
+#define UDP0_BROADCAST FALSE
+#endif
+
+#define UDP0Init() udp_periph_init(&udp0, UDP0_HOST, UDP0_PORT_OUT, UDP0_PORT_IN, UDP0_BROADCAST)
+#define UDP0CheckFreeSpace(_x) udp_check_free_space(&udp0, _x)
+#define UDP0Transmit(_x) udp_transmit(&udp0, _x)
+#define UDP0SendMessage() udp_send_message(&udp0)
+#define UDP0ChAvailable() udp_char_available(&udp0)
+#define UDP0Getch() udp_getch(&udp0)
+#endif // USE_UDP0
+
+#if USE_UDP1
+extern struct udp_periph udp1;
+
+#ifndef UDP1_BROADCAST
+#define UDP1_BROADCAST FALSE
+#endif
+
+#define UDP1Init() udp_periph_init(&udp1, UDP1_HOST, UDP1_PORT_OUT, UDP1_PORT_IN, UDP1_BROADCAST)
+#define UDP1CheckFreeSpace(_x) udp_check_free_space(&udp1, _x)
+#define UDP1Transmit(_x) udp_transmit(&udp1, _x)
+#define UDP1SendMessage() udp_send_message(&udp1)
+#define UDP1ChAvailable() udp_char_available(&udp1)
+#define UDP1Getch() udp_getch(&udp1)
+#endif // USE_UDP1
+
+#if USE_UDP2
+extern struct udp_periph udp2;
+
+#ifndef UDP2_BROADCAST
+#define UDP2_BROADCAST FALSE
+#endif
+
+#define UDP2Init() udp_periph_init(&udp2, UDP2_HOST, UDP2_PORT_OUT, UDP2_PORT_IN, UDP2_BROADCAST)
+#define UDP2CheckFreeSpace(_x) udp_check_free_space(&udp2, _x)
+#define UDP2Transmit(_x) udp_transmit(&udp2, _x)
+#define UDP2SendMessage() udp_send_message(&udp2)
+#define UDP2ChAvailable() udp_char_available(&udp2)
+#define UDP2Getch() udp_getch(&udp2)
+#endif // USE_UDP2
+
+#endif /* MCU_PERIPH_UDP_H */

sw/airborne/subsystems/datalink/datalink.h

@@ -45,8 +45,7 @@
 /** Datalink kinds */
 #define PPRZ 1
 #define XBEE 2
-#define UDP 3
+#define SUPERBITRF 3
-#define SUPERBITRF 4
 
 EXTERN bool_t dl_msg_available;
 /** Flag provided to control calls to ::dl_parse_msg. NOT used in this module*/
@@ -88,13 +87,6 @@ EXTERN void dl_parse_msg(void);
     DlCheckAndParse();                          \
   }
 
-#elif defined DATALINK && DATALINK == UDP
-
-#define DatalinkEvent() {                       \
-    UdpCheckAndParse();                         \
-    DlCheckAndParse();                          \
-  }
-
 #elif defined DATALINK && DATALINK == SUPERBITRF
 
 #define DatalinkEvent() {                       \

sw/airborne/subsystems/datalink/downlink.h

@@ -49,7 +49,6 @@
 
 #else /** SITL */
 
-#include "subsystems/datalink/udp.h"
 #include "subsystems/datalink/pprz_transport.h"
 #include "subsystems/datalink/pprzlog_transport.h"
 #include "subsystems/datalink/xbee.h"
@@ -63,6 +62,9 @@
 #ifdef USE_USB_SERIAL
 #include "mcu_periph/usb_serial.h"
 #endif
+#ifdef USE_UDP
+#include "mcu_periph/udp.h"
+#endif
 #include "mcu_periph/uart.h"
 
 #endif /** !SITL */

sw/airborne/subsystems/datalink/udp.c

@@ -1,95 +0,0 @@
-/*
-* Copyright (C) 2012-2013 Freek van Tienen and Dino Hensen
-*
-* 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.
-*
-*/
-
-#include "subsystems/datalink/udp.h"
-#include "fms/fms_network.h"
-#include <string.h>
-
-//Check if variables are set and else define them
-#ifndef LINK_HOST
-#define LINK_HOST     "192.168.1.255"
-#endif
-#ifndef LINK_PORT
-#define LINK_PORT             4242
-#endif
-#ifndef DATALINK_PORT
-#define DATALINK_PORT         4243
-#endif
-#ifndef FMS_NETWORK_BROADCAST
-#define FMS_NETWORK_BROADCAST TRUE
-#endif
-
-//Define the buffer, check bytes and FmsNetwork
-char udp_buffer[1496];
-unsigned char udp_read_buffer[128];
-uint16_t udp_buffer_id;
-uint8_t ck_a, ck_b;
-struct FmsNetwork* network;
-
-void udp_init( void ) {
-  network = network_new(LINK_HOST, LINK_PORT, DATALINK_PORT, FMS_NETWORK_BROADCAST);
-  udp_buffer_id = 0;
-}
-
-void udp_transmit( uint8_t data ) {
-  udp_buffer[udp_buffer_id] = data;
-  udp_buffer_id++;
-}
-
-void udp_send( void ) {
-  network_write(network, udp_buffer, udp_buffer_id);
-  udp_buffer_id =0;
-}
-
-void udp_receive( void ) {
-  //First check if it isn't already having a message
-  if(dl_msg_available == TRUE) {
-    return;
-  }
-
-  //Read from the network
-  uint16_t read = network_read(network, udp_read_buffer, TRANSPORT_PAYLOAD_LEN);
-  if(read > 0) {
-
-    //Parse the packet
-    if(udp_read_buffer[0] == STX) {
-      uint8_t size = udp_read_buffer[1]-4; // minus STX, LENGTH, CK_A, CK_B
-      uint8_t ck_aa, ck_bb;
-      ck_aa = ck_bb = size+4;
-
-      // index-offset plus 2 for STX and LENGTH
-      for (int i = 2; i < size+2; i++) {
-        dl_buffer[i-2] = udp_read_buffer[i];
-        ck_aa += udp_read_buffer[i];
-        ck_bb += ck_aa;
-      }
-
-      // if both checksums are good, tell datalink that the message is available
-      if (udp_read_buffer[2+size] == ck_aa && udp_read_buffer[2+size+1] == ck_bb) {
-        dl_msg_available = TRUE;
-      }
-    }
-
-    memset(&udp_read_buffer[0], 0, sizeof(udp_read_buffer));
-  }
-}
-

sw/airborne/subsystems/datalink/udp.h

@@ -1,50 +0,0 @@
-/*
-* Copyright (C) 2012-2013 Freek van Tienen and Dino Hensen
-*
-* 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.
-*
-*/
-
-/* Udp ethernet connection over UDP */
-
-#ifndef UDP_TELEM_H
-#define UDP_TELEM_H
-
-#include "subsystems/datalink/datalink.h"
-#include "generated/airframe.h"
-
-//#include "subsystems/datalink/transport.h"
-#include "subsystems/datalink/pprz_transport.h"
-
-#define STX 0x99
-
-void udp_init( void );
-void udp_transmit( uint8_t data );
-void udp_send( void );
-void udp_receive( void );
-
-#define UdpInit() udp_init()
-#define UdpCheckFreeSpace(_x) (TRUE)
-#define UdpTransmit(_x) udp_transmit(_x)
-#define UdpSendMessage() udp_send()
-
-#define UdpCheckAndParse() {       \
-    udp_receive(); \
-  }
-
-#endif /* UDP_TELEM_H */