diff --git a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml index 74378b04c91..61ca12c6041 100644 --- a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml +++ b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml @@ -20,7 +20,7 @@ - + diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 1d74c8e9c1e..caa8ac1befc 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -5,7 +5,7 @@ - + diff --git a/conf/airframes/ardrone2_raw_optitrack.xml b/conf/airframes/ardrone2_raw_optitrack.xml index 03c5c829ad6..fb95f4f5a31 100644 --- a/conf/airframes/ardrone2_raw_optitrack.xml +++ b/conf/airframes/ardrone2_raw_optitrack.xml @@ -8,7 +8,7 @@ - + diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml index 2974e70926b..efa7da67060 100644 --- a/conf/airframes/ardrone2_sdk.xml +++ b/conf/airframes/ardrone2_sdk.xml @@ -14,7 +14,7 @@ - + diff --git a/conf/boards/ardrone2_raw.makefile b/conf/boards/ardrone2_raw.makefile index 688bec5fe5e..d3768412d03 100644 --- a/conf/boards/ardrone2_raw.makefile +++ b/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\" diff --git a/conf/boards/ardrone2_sdk.makefile b/conf/boards/ardrone2_sdk.makefile index 595c09125a5..d490651ff51 100644 --- a/conf/boards/ardrone2_sdk.makefile +++ b/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\" diff --git a/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_udp.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_udp.makefile new file mode 100644 index 00000000000..bd64e2c4184 --- /dev/null +++ b/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 diff --git a/conf/firmwares/subsystems/rotorcraft/telemetry_udp.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_udp.makefile index 366170373f6..1e813fc4429 100644 --- a/conf/firmwares/subsystems/rotorcraft/telemetry_udp.makefile +++ b/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 with in your airframe file.) diff --git a/conf/firmwares/subsystems/shared/udp.makefile b/conf/firmwares/subsystems/shared/udp.makefile new file mode 100644 index 00000000000..c93e2f74321 --- /dev/null +++ b/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 diff --git a/sw/airborne/arch/omap/mcu_periph/udp_arch.c b/sw/airborne/arch/omap/mcu_periph/udp_arch.c new file mode 100644 index 00000000000..97704f37fff --- /dev/null +++ b/sw/airborne/arch/omap/mcu_periph/udp_arch.c @@ -0,0 +1,125 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * 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 +#include +#include +#include + +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)); + } +} diff --git a/sw/airborne/arch/omap/mcu_periph/udp_arch.h b/sw/airborne/arch/omap/mcu_periph/udp_arch.h new file mode 100644 index 00000000000..78e57751ef0 --- /dev/null +++ b/sw/airborne/arch/omap/mcu_periph/udp_arch.h @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * 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 +#include + +struct UdpNetwork { + int socket_in; + int socket_out; + struct sockaddr_in addr_in; + struct sockaddr_in addr_out; +}; + +#endif /* UDP_ARCH_H */ diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index e94937c4018..ae94534f49a 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/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) { diff --git a/sw/airborne/mcu.c b/sw/airborne/mcu.c index 9290dc4d83d..c1a04c5607a 100644 --- a/sw/airborne/mcu.c +++ b/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 */ diff --git a/sw/airborne/mcu_periph/udp.c b/sw/airborne/mcu_periph/udp.c new file mode 100644 index 00000000000..d6f2d1c56ee --- /dev/null +++ b/sw/airborne/mcu_periph/udp.c @@ -0,0 +1,97 @@ +/* + * Copyright (C) 2014 Freek van tienen + * + * 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 diff --git a/sw/airborne/mcu_periph/udp.h b/sw/airborne/mcu_periph/udp.h new file mode 100644 index 00000000000..efd06776cc1 --- /dev/null +++ b/sw/airborne/mcu_periph/udp.h @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2014 Freek van tienen + * + * 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 */ diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h index ad1f8bb08c5..cdef69a225f 100644 --- a/sw/airborne/subsystems/datalink/datalink.h +++ b/sw/airborne/subsystems/datalink/datalink.h @@ -45,8 +45,7 @@ /** Datalink kinds */ #define PPRZ 1 #define XBEE 2 -#define UDP 3 -#define SUPERBITRF 4 +#define SUPERBITRF 3 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() { \ diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h index aa2ce6d2a32..a888c9f3539 100644 --- a/sw/airborne/subsystems/datalink/downlink.h +++ b/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 */ diff --git a/sw/airborne/subsystems/datalink/udp.c b/sw/airborne/subsystems/datalink/udp.c deleted file mode 100644 index 264a5082941..00000000000 --- a/sw/airborne/subsystems/datalink/udp.c +++ /dev/null @@ -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 - -//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)); - } -} - diff --git a/sw/airborne/subsystems/datalink/udp.h b/sw/airborne/subsystems/datalink/udp.h deleted file mode 100644 index 0d869ba79a8..00000000000 --- a/sw/airborne/subsystems/datalink/udp.h +++ /dev/null @@ -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 */