From b75653a22679141116f0353c3ce1f313920b4b49 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 14 Aug 2011 14:19:15 +0200 Subject: [PATCH] PiOS.posix: small corrections and bugfixes to make pios_com_udp work correctly. Telemetry works now. --- .../OpenPilot/System/inc/pios_board_posix.h | 16 ++++--- flight/PiOS.posix/posix/pios_com.c | 9 ++-- flight/PiOS.posix/posix/pios_udp.c | 45 ++++++++++++------- 3 files changed, 43 insertions(+), 27 deletions(-) diff --git a/flight/OpenPilot/System/inc/pios_board_posix.h b/flight/OpenPilot/System/inc/pios_board_posix.h index e43db8bcb..fe6f6ba51 100644 --- a/flight/OpenPilot/System/inc/pios_board_posix.h +++ b/flight/OpenPilot/System/inc/pios_board_posix.h @@ -55,13 +55,19 @@ #define PIOS_COM_MAX_DEVS 256 #define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_COM_TELEM_RF 0 -#define PIOS_COM_GPS 1 -#define PIOS_COM_TELEM_USB 2 +extern uint32_t pios_com_telem_rf_id; +extern uint32_t pios_com_telem_usb_id; +extern uint32_t pios_com_gps_id; +extern uint32_t pios_com_aux_id; +extern uint32_t pios_com_spectrum_id; + +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_GPS (pios_com_gps_id) #ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX 3 -#define PIOS_COM_DEBUG PIOS_COM_AUX +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_DEBUG (PIOS_COM_AUX #endif /** diff --git a/flight/PiOS.posix/posix/pios_com.c b/flight/PiOS.posix/posix/pios_com.c index 76597e247..77c2e2595 100644 --- a/flight/PiOS.posix/posix/pios_com.c +++ b/flight/PiOS.posix/posix/pios_com.c @@ -92,14 +92,13 @@ static uint32_t PIOS_COM_create(void) com_dev = &pios_com_devs[pios_com_num_devs++]; com_dev->magic = PIOS_COM_DEV_MAGIC; - return (pios_com_num_devs-1); + return (pios_com_num_devs); } static struct pios_com_dev * PIOS_COM_find_dev(uint32_t com_dev_id) { - if (com_dev_id>pios_com_num_devs) { - return NULL; - } - return &pios_com_devs[com_dev_id]; + if (!com_dev_id) return NULL; + if (com_dev_id>pios_com_num_devs+1) return NULL; + return &pios_com_devs[com_dev_id-1]; } #endif diff --git a/flight/PiOS.posix/posix/pios_udp.c b/flight/PiOS.posix/posix/pios_udp.c index 7ec6dc309..39077b455 100644 --- a/flight/PiOS.posix/posix/pios_udp.c +++ b/flight/PiOS.posix/posix/pios_udp.c @@ -32,6 +32,7 @@ #if defined(PIOS_INCLUDE_UDP) +#include #include /* We need a list of UDP devices */ @@ -75,6 +76,12 @@ static pios_udp_dev * find_udp_dev_by_id (uint8_t udp) */ void * PIOS_UDP_RxThread(void * udp_dev_n) { + + /* needed because of FreeRTOS.posix scheduling */ + sigset_t set; + sigfillset(&set); + sigprocmask(SIG_BLOCK, &set, NULL); + pios_udp_dev * udp_dev = (pios_udp_dev*) udp_dev_n; /** @@ -93,25 +100,26 @@ void * PIOS_UDP_RxThread(void * udp_dev_n) PIOS_UDP_RX_BUFFER_SIZE, 0, (struct sockaddr *) &udp_dev->client, - (socklen_t*)&udp_dev->clientLength)) < 0) { + (socklen_t*)&udp_dev->clientLength)) >= 0) + { - pthread_exit(NULL); - } - - /* copy received data to buffer if possible */ - /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */ - /* (thats what the USART driver does too!) */ - bool rx_need_yield = false; - if (udp_dev->rx_in_cb) { - (void) (udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield); - } + /* copy received data to buffer if possible */ + /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */ + /* (thats what the USART driver does too!) */ + bool rx_need_yield = false; + if (udp_dev->rx_in_cb) { + (void) (udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (rx_need_yield) { - vPortYieldFromISR(); - } + if (rx_need_yield) { + vPortYieldFromISR(); + } #endif /* PIOS_INCLUDE_FREERTOS */ + } + + } } @@ -172,7 +180,7 @@ static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail) PIOS_Assert(udp_dev); - uint16_t length,len,rem; + int32_t length,len,rem; /** * we send everything directly whenever notified of data to send (lazy!) @@ -186,8 +194,11 @@ static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail) len = sendto(udp_dev->socket, udp_dev->tx_buffer, length, 0, (struct sockaddr *) &udp_dev->client, sizeof(udp_dev->client)); - if (!len) return; - rem -= len; + if (len<=0) { + rem=0; + } else { + rem -= len; + } } tx_bytes_avail -= length; }