diff --git a/conf/firmwares/subsystems/rotorcraft/telemetry_wifi.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_wifi.makefile new file mode 100644 index 0000000000..3cfa8950a4 --- /dev/null +++ b/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 diff --git a/conf/firmwares/subsystems/fixedwing/radio_control_datalink.makefile b/conf/firmwares/subsystems/shared/radio_control_datalink.makefile similarity index 100% rename from conf/firmwares/subsystems/fixedwing/radio_control_datalink.makefile rename to conf/firmwares/subsystems/shared/radio_control_datalink.makefile diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c index 0a3e15b476..c463b3218d 100644 --- a/sw/airborne/firmwares/rotorcraft/datalink.c +++ b/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; } diff --git a/sw/airborne/fms/fms_network.c b/sw/airborne/fms/fms_network.c index d61910aa21..732a29d49c 100644 --- a/sw/airborne/fms/fms_network.c +++ b/sw/airborne/fms/fms_network.c @@ -3,6 +3,7 @@ #include #include #include +#include #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; +} diff --git a/sw/airborne/fms/fms_network.h b/sw/airborne/fms/fms_network.h index bd7f077acd..347fa99368 100644 --- a/sw/airborne/fms/fms_network.h +++ b/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 */ - diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h index 02eeb5c297..8d9ed92f02 100644 --- a/sw/airborne/subsystems/datalink/datalink.h +++ b/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 diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h index 09fcca15ed..c6a1bac215 100644 --- a/sw/airborne/subsystems/datalink/downlink.h +++ b/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" diff --git a/sw/airborne/subsystems/datalink/wifi.c b/sw/airborne/subsystems/datalink/wifi.c new file mode 100644 index 0000000000..02e8fd9c0e --- /dev/null +++ b/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; + } + + } + +} + diff --git a/sw/airborne/subsystems/datalink/wifi.h b/sw/airborne/subsystems/datalink/wifi.h new file mode 100644 index 0000000000..5b03475086 --- /dev/null +++ b/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 */