[telemetry_wifi] Added telemetry wifi:

- wifi.[ch] for communication over wifi
- telemetry_wifi makefile
- adjustments to fms_network for doing a network_read
- wifi including in downlink
- wifi implementing datalink macro structure
- fixed some trailing empty lines
- radio_control_datalink makefile moved from fixedwing to shared

conf/firmwares/subsystems/rotorcraft/telemetry_wifi.makefile

@@ -0,0 +1,8 @@
+
+# Wifi telemetry
+
+ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=Wifi
+ap.CFLAGS += -DDOWNLINK_TRANSPORT=WifiTransport -DDATALINK=WIFI
+ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/wifi.c
+ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c
+ap.srcs += fms/fms_network.c

sw/airborne/firmwares/rotorcraft/datalink.c

@@ -42,6 +42,10 @@
 #include "booz_fms.h"
 #endif
 
+#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
+#include "subsystems/radio_control.h"
+#endif
+
 #include "firmwares/rotorcraft/navigation.h"
 
 #include "math/pprz_geodetic_int.h"
@@ -109,6 +113,28 @@ void dl_parse_msg(void) {
     }
     break;
 #endif /* USE_NAVIGATION */
+#ifdef RADIO_CONTROL_TYPE_DATALINK
+  case DL_RC_3CH :
+#ifdef RADIO_CONTROL_DATALINK_LED
+    LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
+#endif
+    parse_rc_3ch_datalink(
+        DL_RC_3CH_throttle_mode(dl_buffer),
+        DL_RC_3CH_roll(dl_buffer),
+        DL_RC_3CH_pitch(dl_buffer));
+    break;
+  case DL_RC_4CH :
+#ifdef RADIO_CONTROL_DATALINK_LED
+    LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
+#endif
+    parse_rc_4ch_datalink(
+        DL_RC_4CH_mode(dl_buffer),
+        DL_RC_4CH_throttle(dl_buffer),
+        DL_RC_4CH_roll(dl_buffer),
+        DL_RC_4CH_pitch(dl_buffer),
+        DL_RC_4CH_yaw(dl_buffer));
+    break;
+#endif // RADIO_CONTROL_TYPE_DATALINK
   default:
     break;
   }

sw/airborne/fms/fms_network.c

@@ -3,6 +3,7 @@
 #include <netdb.h>
 #include <netinet/in.h>
 #include <stdlib.h>
+#include <errno.h>
 
 #include "fms_debug.h"
 
@@ -16,7 +17,7 @@ struct FmsNetwork* network_new(const char* str_ip_out, const int port_out, const
   setsockopt(me->socket_out, SOL_SOCKET, SO_REUSEADDR,
              &so_reuseaddr, sizeof(so_reuseaddr));
 
-	/* only set broadcast option if explicitly enabled */
+  /* only set broadcast option if explicitly enabled */
   if (broadcast)
     setsockopt(me->socket_out, SOL_SOCKET, SO_BROADCAST,
                &broadcast, sizeof(broadcast));
@@ -41,9 +42,20 @@ struct FmsNetwork* network_new(const char* str_ip_out, const int port_out, const
 
 int network_write(struct FmsNetwork* me, char* buf, int len) {
   ssize_t byte_written = sendto(me->socket_out, buf, len, MSG_DONTWAIT,
-				(struct sockaddr*)&me->addr_out, sizeof(me->addr_out));
+                                (struct sockaddr*)&me->addr_out, sizeof(me->addr_out));
   if ( byte_written != len) {
-    TRACE(TRACE_ERROR, "error sending to network %d\n", byte_written);
+    TRACE(TRACE_ERROR, "error sending to network %d (%d)\n", byte_written, errno);
   }
   return len;
 }
+
+int network_read(struct FmsNetwork* me, unsigned char* buf, int len) {
+
+  // MSG_DONTWAIT => nonblocking flag
+  ssize_t byte_read = recvfrom(me->socket_in, buf, len, MSG_DONTWAIT,
+                                (struct sockaddr*)&me->addr_in, (socklen_t *) sizeof(me->addr_in));
+
+  // @TODO: maybe fix if byte_read == -1 => check errno == EWOULDBLOCK and do something accordingly
+
+  return byte_read;
+}

sw/airborne/fms/fms_network.h

@@ -17,6 +17,6 @@ struct FmsNetwork {
 
 extern struct FmsNetwork* network_new(const char* str_ip_out, const int port_out, const int port_in, const int broadcast);
 extern int network_write(struct FmsNetwork* me, char* buf, int len);
+extern int network_read(struct FmsNetwork* me, unsigned char* buf, int len);
 
 #endif /* FMS_NETWORK_H */
-

sw/airborne/subsystems/datalink/datalink.h

@@ -45,6 +45,7 @@
 /** Datalink kinds */
 #define PPRZ 1
 #define XBEE 2
+#define WIFI 3
 
 EXTERN bool_t dl_msg_available;
 /** Flag provided to control calls to ::dl_parse_msg. NOT used in this module*/
@@ -59,22 +60,22 @@ EXTERN void dl_parse_msg(void);
 
 /** Check for new message and parse */
 #define DlCheckAndParse() {   \
-  if (dl_msg_available) {			\
-    dl_parse_msg();				    \
-    dl_msg_available = FALSE;	\
-  }						                \
+  if (dl_msg_available) {      \
+    dl_parse_msg();            \
+    dl_msg_available = FALSE;  \
+  }                            \
 }
 
 #if defined DATALINK && DATALINK == PPRZ
 
-#define DatalinkEvent() {			            \
+#define DatalinkEvent() {                       \
     PprzCheckAndParse(PPRZ_UART, pprz_tp);      \
     DlCheckAndParse();                          \
   }
 
 #elif defined DATALINK && DATALINK == XBEE
 
-#define DatalinkEvent() {			            \
+#define DatalinkEvent() {                       \
     XBeeCheckAndParse(XBEE_UART, xbee_tp);      \
     DlCheckAndParse();                          \
   }
@@ -86,6 +87,14 @@ EXTERN void dl_parse_msg(void);
     DlCheckAndParse();                          \
   }
 
+#elif defined DATALINK && DATALINK == WIFI
+
+#define DatalinkEvent() {                       \
+    WifiCheckAndParse();                        \
+    DlCheckAndParse();                          \
+  }
+
+
 #else
 
 // Unknown DATALINK

sw/airborne/subsystems/datalink/downlink.h

@@ -46,6 +46,7 @@
 #endif
 
 #else /** SITL */
+#include "subsystems/datalink/wifi.h"
 #include "subsystems/datalink/pprz_transport.h"
 #include "subsystems/datalink/xbee.h"
 #include "subsystems/datalink/w5100.h"

sw/airborne/subsystems/datalink/wifi.c

@@ -0,0 +1,92 @@
+/*
+* 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/wifi.h"
+#include "fms/fms_network.h"
+
+//Check if variables are set and else define them
+#ifndef LINK_HOST
+#define LINK_HOST     "192.168.1.0"
+#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 wifi_init( void ) {
+  network = network_new(LINK_HOST, LINK_PORT, DATALINK_PORT, FMS_NETWORK_BROADCAST);
+  udp_buffer_id = 0;
+}
+
+void wifi_transmit( uint8_t data ) {
+  udp_buffer[udp_buffer_id] = data;
+  udp_buffer_id++;
+}
+
+void wifi_send( void ) {
+  network_write(network, udp_buffer, udp_buffer_id);
+  udp_buffer_id =0;
+}
+
+void wifi_receive( void ) {
+  //First check if it isn't already having a message
+  if(dl_msg_available == TRUE) {
+    return;
+  }
+
+  //Read from the network
+  network_read(network, udp_read_buffer, TRANSPORT_PAYLOAD_LEN);
+
+  //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;
+    }
+
+  }
+
+}
+

sw/airborne/subsystems/datalink/wifi.h

@@ -0,0 +1,132 @@
+/*
+* 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.
+*
+*/
+
+/* Wifi ethernet connection over UDP */
+
+#ifndef WIFI_TELEM_H
+#define WIFI_TELEM_H
+
+#include "subsystems/datalink/datalink.h"
+#include "generated/airframe.h"
+
+#include "subsystems/datalink/transport.h"
+
+#define STX 0x99
+
+void wifi_init( void );
+void wifi_transmit( uint8_t data );
+void wifi_send( void );
+void wifi_receive( void );
+
+#define WifiInit() wifi_init()
+#define WifiCheckFreeSpace(_x) (TRUE)
+#define WifiTransmit(_x) wifi_transmit(_x)
+#define WifiSendMessage() wifi_send()
+
+#define WifiTransportSizeOf(_dev, _x) (_x+4)
+#define WifiTransportCheckFreeSpace(_dev, x) TransportLink(_dev, CheckFreeSpace(x))
+#define WifiTransportPut1Byte(_dev, x) TransportLink(_dev, Transmit(x))
+#define WifiTransportSendMessage(_dev) TransportLink(_dev, SendMessage())
+
+#define WifiTransportPutUint8(_dev, _byte) {    \
+    ck_a += _byte;                              \
+    ck_b += ck_a;                               \
+    WifiTransportPut1Byte(_dev, _byte);         \
+  }
+
+#define WifiTransportPut1ByteByAddr(_dev, _byte) {  \
+    uint8_t _x = *(_byte);                          \
+    WifiTransportPutUint8(_dev, _x);                \
+  }
+
+#define WifiTransportPut2Bytes(_dev, _x) {      \
+    uint16_t x16 = _x;                          \
+    WifiTransportPut1Byte(_dev, x16>>8);        \
+    WifiTransportPut1Byte(_dev, x16 & 0xff);    \
+  }
+
+#define WifiTransportPut2ByteByAddr(_dev, _byte) {                 \
+    WifiTransportPut1ByteByAddr(_dev, _byte);                      \
+    WifiTransportPut1ByteByAddr(_dev, (const uint8_t*)_byte+1);    \
+  }
+
+#define WifiTransportPut4ByteByAddr(_dev, _byte) {                 \
+    WifiTransportPut2ByteByAddr(_dev, _byte);                      \
+    WifiTransportPut2ByteByAddr(_dev, (const uint8_t*)_byte+2);    \
+  }
+
+#ifdef __IEEE_BIG_ENDIAN // From machine/ieeefp.h
+#define WifiTransportPutDoubleByAddr(_dev, _byte) {                \
+    WifiTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4);    \
+    WifiTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte);      \
+  }
+#else
+#define WifiTransportPutDoubleByAddr(_dev, _byte) {                \
+    WifiTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte);      \
+    WifiTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4);    \
+  }
+#endif
+
+
+#define WifiTransportPutInt8ByAddr(_dev, _x) WifiTransportPut1ByteByAddr(_dev, _x)
+#define WifiTransportPutUint8ByAddr(_dev, _x) WifiTransportPut1ByteByAddr(_dev, (const uint8_t*)_x)
+#define WifiTransportPutInt16ByAddr(_dev, _x) WifiTransportPut2ByteByAddr(_dev, (const uint8_t*)_x)
+#define WifiTransportPutUint16ByAddr(_dev, _x) WifiTransportPut2ByteByAddr(_dev, (const uint8_t*)_x)
+#define WifiTransportPutInt32ByAddr(_dev, _x) WifiTransportPut4ByteByAddr(_dev, (const uint8_t*)_x)
+#define WifiTransportPutUint32ByAddr(_dev, _x) WifiTransportPut4ByteByAddr(_dev, (const uint8_t*)_x)
+#define WifiTransportPutFloatByAddr(_dev, _x) WifiTransportPut4ByteByAddr(_dev, (const uint8_t*)_x)
+#define WifiTransportPutNamedUint8(_dev, _name, _byte) WifiTransportPutUint8(_dev, _byte)
+
+#define WifiTransportPutArray(_dev, _put, _n, _x) { \
+    uint8_t _i; \
+    WifiTransportPutUint8(_dev, _n); \
+    for(_i = 0; _i < _n; _i++) { \
+      _put(_dev, &_x[_i]); \
+    } \
+  }
+
+#define WifiTransportPutInt16Array(_dev, _n, _x) WifiTransportPutArray(_dev, WifiTransportPutInt16ByAddr, _n, _x)
+
+#define WifiTransportPutUint16Array(_dev, _n, _x) WifiTransportPutArray(_dev, WifiTransportPutUint16ByAddr, _n, _x)
+#define WifiTransportPutUint8Array(_dev, _n, _x) WifiTransportPutArray(_dev, WifiTransportPutUint8ByAddr, _n, _x)
+#define WifiTransportPutFloatArray(_dev, _n, _x) WifiTransportPutArray(_dev, WifiTransportPutFloatByAddr, _n, _x)
+#define WifiTransportPutDoubleArray(_dev, _n, _x) WifiTransportPutArray(_dev, WifiTransportPutDoubleByAddr, _n, _x)
+
+
+#define WifiTransportHeader(_dev, payload_len) { \
+    WifiTransportPut1Byte(_dev, STX); \
+    uint8_t msg_len = WifiTransportSizeOf(_dev, payload_len); \
+    WifiTransportPut1Byte(_dev, msg_len); \
+    ck_a = msg_len; ck_b = msg_len; \
+  }
+
+#define WifiTransportTrailer(_dev) { \
+    WifiTransportPut1Byte(_dev, ck_a); \
+    WifiTransportPut1Byte(_dev, ck_b); \
+    WifiTransportSendMessage(_dev); \
+  }
+
+#define WifiCheckAndParse() {       \
+    wifi_receive(); \
+  }
+
+#endif /* WIFI_TELEM_H */