rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

PiOS.posix: port pios_udp and pios_com to "next"

Browse Source
This commit is contained in:
Corvus Corax 2011-08-14 12:49:07 +02:00
parent 8a47ade16b
commit 5b240a2675
13 changed files with 921 additions and 777 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/OpenPilot/Makefile.posix
View File

@ -165,6 +165,7 @@ endif
SRC += $(PIOSPOSIX)/pios_crc.c
SRC += $(PIOSPOSIX)/pios_sys.c
SRC += $(PIOSPOSIX)/pios_led.c
SRC += $(PIOSPOSIX)/pios_irq.c
SRC += $(PIOSPOSIX)/pios_delay.c
SRC += $(PIOSPOSIX)/pios_sdcard.c
SRC += $(PIOSPOSIX)/pios_udp.c
@ -176,7 +177,7 @@ SRC += $(PIOSPOSIX)/pios_debug.c
SRC += $(PIOSPOSIX)/pios_rcvr.c
## Libraries for flight calculations
#SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
## RTOS and RTOS Portable

1
flight/OpenPilot/System/inc/pios_board_posix.h
View File

@ -52,6 +52,7 @@
//-------------------------
//#define PIOS_USART_TX_BUFFER_SIZE 256
#define PIOS_COM_BUFFER_SIZE 1024
#define PIOS_COM_MAX_DEVS 256
#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE
#define PIOS_COM_TELEM_RF 0

2
flight/OpenPilot/System/inc/pios_config_posix.h
View File

@ -36,6 +36,8 @@
#define PIOS_INCLUDE_SDCARD
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_TELEMETRY_RF
#define PIOS_INCLUDE_UDP
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_RCVR

126
flight/OpenPilot/System/pios_board_posix.c
View File

@ -40,42 +40,16 @@ void Stack_Change() {
void Stack_Change_Weak() {
}
/**
* PIOS_Board_Init()
* initializes all the core systems on this specific hardware
* called from System/openpilot.c
*/
void PIOS_Board_Init(void) {
/* Delay system */
PIOS_DELAY_Init();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor library */
TaskMonitorInitialize();
/* Initialize the PiOS library */
PIOS_COM_Init();
}
const struct pios_udp_cfg pios_udp0_cfg = {
const struct pios_udp_cfg pios_udp_telem_cfg = {
.ip = "0.0.0.0",
.port = 9000,
};
const struct pios_udp_cfg pios_udp1_cfg = {
const struct pios_udp_cfg pios_udp_gps_cfg = {
.ip = "0.0.0.0",
.port = 9001,
};
const struct pios_udp_cfg pios_udp2_cfg = {
const struct pios_udp_cfg pios_udp_debug_cfg = {
.ip = "0.0.0.0",
.port = 9002,
};
@ -84,15 +58,19 @@ const struct pios_udp_cfg pios_udp2_cfg = {
/*
* AUX USART
*/
const struct pios_udp_cfg pios_udp3_cfg = {
const struct pios_udp_cfg pios_udp_aux_cfg = {
.ip = "0.0.0.0",
.port = 9003,
};
#endif
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
/*
* Board specific number of devices.
*/
/*
struct pios_udp_dev pios_udp_devs[] = {
#define PIOS_UDP_TELEM 0
{
@ -115,7 +93,7 @@ struct pios_udp_dev pios_udp_devs[] = {
};
uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs);
*/
/*
* COM devices
*/
@ -126,28 +104,72 @@ uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs);
extern const struct pios_com_driver pios_serial_com_driver;
extern const struct pios_com_driver pios_udp_com_driver;
struct pios_com_dev pios_com_devs[] = {
{
.id = PIOS_UDP_TELEM,
.driver = &pios_udp_com_driver,
},
{
.id = PIOS_UDP_GPS,
.driver = &pios_udp_com_driver,
},
{
.id = PIOS_UDP_LOCAL,
.driver = &pios_udp_com_driver,
},
#ifdef PIOS_COM_AUX
{
.id = PIOS_UDP_AUX,
.driver = &pios_udp_com_driver,
},
#endif
};
uint32_t pios_com_telem_rf_id;
uint32_t pios_com_telem_usb_id;
uint32_t pios_com_gps_id;
uint32_t pios_com_aux_id;
uint32_t pios_com_spectrum_id;
const uint8_t pios_com_num_devices = NELEMENTS(pios_com_devs);
/**
* PIOS_Board_Init()
* initializes all the core systems on this specific hardware
* called from System/openpilot.c
*/
void PIOS_Board_Init(void) {
/* Delay system */
PIOS_DELAY_Init();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor library */
TaskMonitorInitialize();
#if defined(PIOS_INCLUDE_COM)
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
{
uint32_t pios_udp_telem_rf_id;
if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id,
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
#if defined(PIOS_INCLUDE_GPS)
{
uint32_t pios_udp_gps_id;
if (PIOS_USART_Init(&pios_udp_gps_id, &pios_udp_gps_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
PIOS_Assert(rx_buffer);
if (PIOS_COM_Init(&pios_com_gps_id, &pios_udp_com_driver, pios_udp_gps_id,
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
NULL, 0)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_GPS */
#endif
}
/**
* @}

120
flight/PiOS.posix/inc/pios_com.h
View File

@ -1,55 +1,65 @@
/**
******************************************************************************
*
* @file pios_com.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief COM layer functions header
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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 3 of the License, or
* (at your option) any later version.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_COM_H
#define PIOS_COM_H
/* Public Functions */
extern int32_t PIOS_COM_Init(void);
extern int32_t PIOS_COM_ChangeBaud(uint8_t port, uint32_t baud);
extern int32_t PIOS_COM_SendCharNonBlocking(uint8_t port, char c);
extern int32_t PIOS_COM_SendChar(uint8_t port, char c);
extern int32_t PIOS_COM_SendBufferNonBlocking(uint8_t port, uint8_t *buffer, uint16_t len);
extern int32_t PIOS_COM_SendBuffer(uint8_t port, uint8_t *buffer, uint16_t len);
extern int32_t PIOS_COM_SendStringNonBlocking(uint8_t port, char *str);
extern int32_t PIOS_COM_SendString(uint8_t port, char *str);
extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint8_t port, char *format, ...);
extern int32_t PIOS_COM_SendFormattedString(uint8_t port, char *format, ...);
extern uint8_t PIOS_COM_ReceiveBuffer(uint8_t port);
extern int32_t PIOS_COM_ReceiveBufferUsed(uint8_t port);
extern int32_t PIOS_COM_ReceiveHandler(void);
struct pios_com_driver {
void (*init)(uint8_t id);
void (*set_baud)(uint8_t id, uint32_t baud);
int32_t (*tx_nb)(uint8_t id, uint8_t *buffer, uint16_t len);
int32_t (*tx)(uint8_t id, uint8_t *buffer, uint16_t len);
int32_t (*rx)(uint8_t id);
int32_t (*rx_avail)(uint8_t id);
};
#endif /* PIOS_COM_H */
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_COM COM layer functions
* @brief Hardware communication layer
* @{
*
* @file pios_com.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief COM layer functions header
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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 3 of the License, or
* (at your option) any later version.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_COM_H
#define PIOS_COM_H
typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * task_woken);
struct pios_com_driver {
void (*init)(uint32_t id);
void (*set_baud)(uint32_t id, uint32_t baud);
void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail);
void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail);
void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context);
void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context);
};
/* Public Functions */
extern int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len);
extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud);
extern int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c);
extern int32_t PIOS_COM_SendChar(uint32_t com_id, char c);
extern int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len);
extern int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len);
extern int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str);
extern int32_t PIOS_COM_SendString(uint32_t com_id, const char *str);
extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...);
extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...);
extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms);
extern int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id);
#endif /* PIOS_COM_H */
/**
* @}
* @}
*/

17
flight/PiOS.posix/inc/pios_com_priv.h
View File

@ -1,5 +1,10 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_COM COM layer functions
* @brief Hardware communication layer
* @{
*
* @file pios_com_priv.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
@ -29,13 +34,11 @@
#include <pios.h>
struct pios_com_dev {
uint8_t id;
const struct pios_com_driver * const driver;
};
extern struct pios_com_dev pios_com_devs[];
extern const uint8_t pios_com_num_devices;
extern int32_t PIOS_COM_ReceiveHandler(uint32_t com_id);
#endif /* PIOS_COM_PRIV_H */
/**
* @}
* @}
*/

38
flight/PiOS.posix/inc/pios_irq.h Normal file
View File

@ -0,0 +1,38 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_IRQ IRQ Setup Functions
* @{
*
* @file pios_irq.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief IRQ functions header.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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 3 of the License, or
* (at your option) any later version.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_IRQ_H
#define PIOS_IRQ_H
/* Public Functions */
extern int32_t PIOS_IRQ_Disable(void);
extern int32_t PIOS_IRQ_Enable(void);
#endif /* PIOS_IRQ_H */

16
flight/PiOS.posix/inc/pios_udp.h
View File

@ -31,21 +31,5 @@
/* Global Types */
/* Public Functions */
//extern void PIOS_UDP_Init(void);
void PIOS_UDP_Init(void);
extern void PIOS_UDP_ChangeBaud(uint8_t usart, uint32_t baud);
extern int32_t PIOS_UDP_RxBufferFree(uint8_t usart);
extern int32_t PIOS_UDP_RxBufferUsed(uint8_t usart);
extern int32_t PIOS_UDP_RxBufferGet(uint8_t usart);
extern int32_t PIOS_UDP_RxBufferPeek(uint8_t usart);
extern int32_t PIOS_UDP_RxBufferPut(uint8_t usart, uint8_t b);
extern int32_t PIOS_UDP_TxBufferFree(uint8_t usart);
extern int32_t PIOS_UDP_TxBufferGet(uint8_t usart);
extern int32_t PIOS_UDP_TxBufferPutMoreNonBlocking(uint8_t usart, uint8_t *buffer, uint16_t len);
extern int32_t PIOS_UDP_TxBufferPutMore(uint8_t usart, uint8_t *buffer, uint16_t len);
extern int32_t PIOS_UDP_TxBufferPutNonBlocking(uint8_t usart, uint8_t b);
extern int32_t PIOS_UDP_TxBufferPut(uint8_t usart, uint8_t b);
#endif /* PIOS_UDP_H */

33
flight/PiOS.posix/inc/pios_udp_priv.h
View File

@ -29,6 +29,7 @@
#include <pios.h>
#include <stdio.h>
#include <pthread.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <stdlib.h>
@ -37,30 +38,34 @@
#include <fcntl.h>
#include <netinet/in.h>
struct pios_udp_cfg {
const char * ip;
uint16_t port;
};
struct pios_udp_buffer {
uint8_t buf[PIOS_UDP_RX_BUFFER_SIZE];
uint16_t head;
uint16_t tail;
uint16_t size;
};
typedef struct {
const struct pios_udp_cfg * cfg;
pthread_t rxThread;
struct pios_udp_dev {
const struct pios_udp_cfg * const cfg;
struct pios_udp_buffer rx;
int socket;
struct sockaddr_in server;
struct sockaddr_in client;
uint32_t clientLength;
};
extern struct pios_udp_dev pios_udp_devs[];
extern uint8_t pios_udp_num_devices;
pthread_cond_t cond;
pthread_mutex_t mutex;
pios_com_callback tx_out_cb;
uint32_t tx_out_context;
pios_com_callback rx_in_cb;
uint32_t rx_in_context;
uint8_t rx_buffer[PIOS_UDP_RX_BUFFER_SIZE];
uint8_t tx_buffer[PIOS_UDP_RX_BUFFER_SIZE];
} pios_udp_dev;
extern int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg);
#endif /* PIOS_UDP_PRIV_H */

1
flight/PiOS.posix/pios.h
View File

@ -58,6 +58,7 @@
#include <pios_sys.h>
#include <pios_delay.h>
#include <pios_led.h>
#include <pios_irq.h>
#include <pios_sdcard.h>
#include <pios_udp.h>
#include <pios_com.h>

744
flight/PiOS.posix/posix/pios_com.c
View File

@ -1,285 +1,523 @@
/**
******************************************************************************
*
* @file pios_com.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief COM layer functions
* @see The GNU Public License (GPL) Version 3
* @defgroup PIOS_COM COM layer functions
* @{
*
*****************************************************************************/
/*
* This program 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 3 of the License, or
* (at your option) any later version.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* Project Includes */
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_COM COM layer functions
* @brief Hardware communication layer
* @{
*
* @file pios_com.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief COM layer functions
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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 3 of the License, or
* (at your option) any later version.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* Project Includes */
#include "pios.h"
#if defined(PIOS_INCLUDE_COM)
#include "fifo_buffer.h"
#include <pios_com_priv.h>
static struct pios_com_dev * find_com_dev_by_id (uint8_t port)
{
if (port >= pios_com_num_devices) {
/* Undefined COM port for this board (see pios_board.c) */
return NULL;
}
#if !defined(PIOS_INCLUDE_FREERTOS)
#include "pios_delay.h" /* PIOS_DELAY_WaitmS */
#endif
/* Get a handle for the device configuration */
return &(pios_com_devs[port]);
enum pios_com_dev_magic {
PIOS_COM_DEV_MAGIC = 0xaa55aa55,
};
struct pios_com_dev {
enum pios_com_dev_magic magic;
uint32_t lower_id;
const struct pios_com_driver * driver;
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreHandle tx_sem;
xSemaphoreHandle rx_sem;
#endif
bool has_rx;
bool has_tx;
t_fifo_buffer rx;
t_fifo_buffer tx;
};
static bool PIOS_COM_validate(struct pios_com_dev * com_dev)
{
return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC));
}
/**
* Initialises COM layer
* \param[in] mode currently only mode 0 supported
* \return < 0 if initialisation failed
*/
int32_t PIOS_COM_Init(void)
{
int32_t ret = 0;
/* If any COM assignment: */
#if defined(PIOS_INCLUDE_SERIAL)
PIOS_SERIAL_Init();
#endif
#if defined(PIOS_INCLUDE_UDP)
PIOS_UDP_Init();
#endif
return ret;
}
/**
* Change the port speed without re-initializing
* \param[in] port COM port
* \param[in] baud Requested baud rate
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_ChangeBaud(uint8_t port, uint32_t baud)
#if defined(PIOS_INCLUDE_FREERTOS) && 0
// static struct pios_com_dev * PIOS_COM_alloc(void)
//{
// struct pios_com_dev * com_dev;
//
// com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev));
// if (!com_dev) return (NULL);
//
// com_dev->magic = PIOS_COM_DEV_MAGIC;
// return(com_dev);
//}
#else
static struct pios_com_dev pios_com_devs[PIOS_COM_MAX_DEVS];
static uint8_t pios_com_num_devs;
static uint32_t PIOS_COM_create(void)
{
struct pios_com_dev * com_dev;
struct pios_com_dev * com_dev;
com_dev = find_com_dev_by_id (port);
if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) {
return (PIOS_COM_MAX_DEVS+1);
}
if (!com_dev) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
com_dev = &pios_com_devs[pios_com_num_devs++];
com_dev->magic = PIOS_COM_DEV_MAGIC;
/* Invoke the driver function if it exists */
if (com_dev->driver->set_baud) {
com_dev->driver->set_baud(com_dev->id, baud);
}
return 0;
return (pios_com_num_devs-1);
}
/**
* Sends a package over given port
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendBufferNonBlocking(uint8_t port, uint8_t *buffer, uint16_t len)
static struct pios_com_dev * PIOS_COM_find_dev(uint32_t com_dev_id)
{
struct pios_com_dev * com_dev;
if (com_dev_id>pios_com_num_devs) {
return NULL;
}
return &pios_com_devs[com_dev_id];
}
#endif
com_dev = find_com_dev_by_id (port);
static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield);
static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield);
static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield);
static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield);
if (!com_dev) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
/**
* Initialises COM layer
* \param[out] handle
* \param[in] driver
* \param[in] id
* \return < 0 if initialisation failed
*/
int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len)
{
PIOS_Assert(com_id);
PIOS_Assert(driver);
/* Invoke the driver function if it exists */
if (com_dev->driver->tx_nb) {
return com_dev->driver->tx_nb(com_dev->id, buffer, len);
}
bool has_rx = (rx_buffer && rx_buffer_len > 0);
bool has_tx = (tx_buffer && tx_buffer_len > 0);
PIOS_Assert(has_rx || has_tx);
PIOS_Assert(driver->bind_tx_cb || !has_tx);
PIOS_Assert(driver->bind_rx_cb || !has_rx);
return 0;
uint32_t com_dev_id;
struct pios_com_dev * com_dev;
com_dev_id = PIOS_COM_create();
com_dev = PIOS_COM_find_dev(com_dev_id);
if (!com_dev) goto out_fail;
com_dev->driver = driver;
com_dev->lower_id = lower_id;
com_dev->has_rx = has_rx;
com_dev->has_tx = has_tx;
if (has_rx) {
fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len);
#if defined(PIOS_INCLUDE_FREERTOS)
vSemaphoreCreateBinary(com_dev->rx_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
(com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, com_dev_id);
if (com_dev->driver->rx_start) {
/* Start the receiver */
(com_dev->driver->rx_start)(com_dev->lower_id,
fifoBuf_getFree(&com_dev->rx));
}
}
if (has_tx) {
fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len);
#if defined(PIOS_INCLUDE_FREERTOS)
vSemaphoreCreateBinary(com_dev->tx_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
(com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, com_dev_id);
}
*com_id = com_dev_id;
return(0);
out_fail:
return(-1);
}
/**
* Sends a package over given port
* (blocking function)
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendBuffer(uint8_t port, uint8_t *buffer, uint16_t len)
static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield)
{
struct pios_com_dev * com_dev;
#if defined(PIOS_INCLUDE_FREERTOS)
static signed portBASE_TYPE xHigherPriorityTaskWoken;
xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken);
com_dev = find_com_dev_by_id (port);
if (!com_dev) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
/* Invoke the driver function if it exists */
if (com_dev->driver->tx) {
return com_dev->driver->tx(com_dev->id, buffer, len);
}
return 0;
if (xHigherPriorityTaskWoken != pdFALSE) {
*need_yield = true;
} else {
*need_yield = false;
}
#else
*need_yield = false;
#endif
}
/**
* Sends a single character over given port
* \param[in] port COM port
* \param[in] c character
* \return -1 if port not available
* \return -2 buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendCharNonBlocking(uint8_t port, char c)
{
return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)&c, 1);
}
/**
* Sends a single character over given port
* (blocking function)
* \param[in] port COM port
* \param[in] c character
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendChar(uint8_t port, char c)
{
return PIOS_COM_SendBuffer(port, (uint8_t *)&c, 1);
}
/**
* Sends a string over given port
* \param[in] port COM port
* \param[in] str zero-terminated string
* \return -1 if port not available
* \return -2 buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendStringNonBlocking(uint8_t port, char *str)
{
return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)str, (uint16_t)strlen(str));
}
/**
* Sends a string over given port
* (blocking function)
* \param[in] port COM port
* \param[in] str zero-terminated string
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendString(uint8_t port, char *str)
{
return PIOS_COM_SendBuffer(port, (uint8_t *)str, strlen(str));
}
/**
* Sends a formatted string (-> printf) over given port
* \param[in] port COM port
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
* \param[in] ... optional arguments,
* 128 characters supported maximum!
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendFormattedStringNonBlocking(uint8_t port, char *format, ...)
{
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
va_list args;
va_start(args, format);
vsprintf((char *)buffer, format, args);
return PIOS_COM_SendBufferNonBlocking(port, buffer, (uint16_t)strlen((char *)buffer));
}
/**
* Sends a formatted string (-> printf) over given port
* (blocking function)
* \param[in] port COM port
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
* \param[in] ... optional arguments,
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendFormattedString(uint8_t port, char *format, ...)
{
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
va_list args;
va_start(args, format);
vsprintf((char *)buffer, format, args);
return PIOS_COM_SendBuffer(port, buffer, (uint16_t)strlen((char *)buffer));
}
/**
* Transfer bytes from port buffers into another buffer
* \param[in] port COM port
static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield)
{
#if defined(PIOS_INCLUDE_FREERTOS)
static signed portBASE_TYPE xHigherPriorityTaskWoken;
xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken);
if (xHigherPriorityTaskWoken != pdFALSE) {
*need_yield = true;
} else {
*need_yield = false;
}
#else
*need_yield = false;
#endif
}
static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(context);
bool valid = PIOS_COM_validate(com_dev);
PIOS_Assert(valid);
PIOS_Assert(com_dev->has_rx);
PIOS_IRQ_Disable();
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len);
PIOS_IRQ_Enable();
if (bytes_into_fifo > 0) {
/* Data has been added to the buffer */
PIOS_COM_UnblockRx(com_dev, need_yield);
}
if (headroom) {
*headroom = fifoBuf_getFree(&com_dev->rx);
}
return (bytes_into_fifo);
}
static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(context);
bool valid = PIOS_COM_validate(com_dev);
PIOS_Assert(valid);
PIOS_Assert(buf);
PIOS_Assert(buf_len);
PIOS_Assert(com_dev->has_tx);
PIOS_IRQ_Disable();
uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len);
PIOS_IRQ_Enable();
if (bytes_from_fifo > 0) {
/* More space has been made in the buffer */
PIOS_COM_UnblockTx(com_dev, need_yield);
}
if (headroom) {
*headroom = fifoBuf_getUsed(&com_dev->tx);
}
return (bytes_from_fifo);
}
/**
* Change the port speed without re-initializing
* \param[in] port COM port
* \param[in] baud Requested baud rate
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
/* Invoke the driver function if it exists */
if (com_dev->driver->set_baud) {
com_dev->driver->set_baud(com_dev->lower_id, baud);
}
return 0;
}
/**
* Sends a package over given port
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
PIOS_Assert(com_dev->has_tx);
if (len >= fifoBuf_getFree(&com_dev->tx)) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
PIOS_IRQ_Disable();
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len);
PIOS_IRQ_Enable();
if (bytes_into_fifo > 0) {
/* More data has been put in the tx buffer, make sure the tx is started */
if (com_dev->driver->tx_start) {
com_dev->driver->tx_start(com_dev->lower_id,
fifoBuf_getUsed(&com_dev->tx));
}
}
return (0);
}
/**
* Sends a package over given port
* (blocking function)
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
PIOS_Assert(com_dev->has_tx);
int32_t rc;
do {
rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len);
#if defined(PIOS_INCLUDE_FREERTOS)
if (rc == -2) {
/* Make sure the transmitter is running while we wait */
if (com_dev->driver->tx_start) {
(com_dev->driver->tx_start)(com_dev->lower_id,
fifoBuf_getUsed(&com_dev->tx));
}
if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) {
return -3;
}
}
#endif
} while (rc == -2);
return rc;
}
/**
* Sends a single character over given port
* \param[in] port COM port
* \param[in] c character
* \return -1 if port not available
* \return -2 buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c)
{
return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1);
}
/**
* Sends a single character over given port
* (blocking function)
* \param[in] port COM port
* \param[in] c character
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendChar(uint32_t com_id, char c)
{
return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1);
}
/**
* Sends a string over given port
* \param[in] port COM port
* \param[in] str zero-terminated string
* \return -1 if port not available
* \return -2 buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str)
{
return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str));
}
/**
* Sends a string over given port
* (blocking function)
* \param[in] port COM port
* \param[in] str zero-terminated string
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendString(uint32_t com_id, const char *str)
{
return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str));
}
/**
* Sends a formatted string (-> printf) over given port
* \param[in] port COM port
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
* \param[in] ... optional arguments,
* 128 characters supported maximum!
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...)
{
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
va_list args;
va_start(args, format);
vsprintf((char *)buffer, format, args);
return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer));
}
/**
* Sends a formatted string (-> printf) over given port
* (blocking function)
* \param[in] port COM port
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
* \param[in] ... optional arguments,
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...)
{
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
va_list args;
va_start(args, format);
vsprintf((char *)buffer, format, args);
return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer));
}
/**
* Transfer bytes from port buffers into another buffer
* \param[in] port COM port
* \returns Byte from buffer
*/
uint8_t PIOS_COM_ReceiveBuffer(uint8_t port)
{
struct pios_com_dev * com_dev;
com_dev = find_com_dev_by_id (port);
//PIOS_DEBUG_Assert(com_dev);
//PIOS_DEBUG_Assert(com_dev->driver->rx);
return com_dev->driver->rx(com_dev->id);
}
/**
* Get the number of bytes waiting in the buffer
* \param[in] port COM port
* \return Number of bytes used in buffer
*/
int32_t PIOS_COM_ReceiveBufferUsed(uint8_t port)
*/
uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms)
{
struct pios_com_dev * com_dev;
PIOS_Assert(buf);
PIOS_Assert(buf_len);
com_dev = find_com_dev_by_id (port);
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!com_dev) {
/* Undefined COM port for this board (see pios_board.c) */
return 0;
}
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
PIOS_Assert(0);
}
PIOS_Assert(com_dev->has_rx);
if (!com_dev->driver->rx_avail) {
return 0;
}
check_again:
PIOS_IRQ_Disable();
uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len);
PIOS_IRQ_Enable();
return com_dev->driver->rx_avail(com_dev->id);
if (bytes_from_fifo == 0 && timeout_ms > 0) {
/* No more bytes in receive buffer */
/* Make sure the receiver is running while we wait */
if (com_dev->driver->rx_start) {
/* Notify the lower layer that there is now room in the rx buffer */
(com_dev->driver->rx_start)(com_dev->lower_id,
fifoBuf_getFree(&com_dev->rx));
}
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) {
/* Make sure we don't come back here again */
timeout_ms = 0;
goto check_again;
}
#else
PIOS_DELAY_WaitmS(1);
timeout_ms--;
goto check_again;
#endif
}
/* Return received byte */
return (bytes_from_fifo);
}
#endif
/**
* Get the number of bytes waiting in the buffer
* \param[in] port COM port
* \return Number of bytes used in buffer
*/
int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
PIOS_Assert(0);
}
PIOS_Assert(com_dev->has_rx);
return (fifoBuf_getUsed(&com_dev->rx));
}
#endif
/**
* @}
* @}
*/

69
flight/PiOS.posix/posix/pios_irq.c Normal file
View File

@ -0,0 +1,69 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_IRQ IRQ Setup Functions
* @brief STM32 Hardware code to enable and disable interrupts
* @{
*
* @file pios_irq.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org)
* @brief IRQ Enable/Disable routines
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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 3 of the License, or
* (at your option) any later version.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* Project Includes */
#include "pios.h"
#if defined(PIOS_INCLUDE_IRQ)
/* Private Function Prototypes */
/**
* Disables all interrupts (nested)
* \return < 0 On errors
*/
int32_t PIOS_IRQ_Disable(void)
{
#if defined(PIOS_INCLUDE_FREERTOS)
taskENTER_CRITICAL();
#endif
return 0;
}
/**
* Enables all interrupts (nested)
* \return < 0 on errors
* \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before)
*/
int32_t PIOS_IRQ_Enable(void)
{
#if defined(PIOS_INCLUDE_FREERTOS)
taskEXIT_CRITICAL();
#endif
return 0;
}
#endif
/**
* @}
* @}
*/

528
flight/PiOS.posix/posix/pios_udp.c
View File

@ -34,16 +34,32 @@
#include <pios_udp_priv.h>
/* We need a list of UDP devices */
#define PIOS_UDP_MAX_DEV 256
static int8_t pios_udp_num_devices = 0;
static pios_udp_dev pios_udp_devices[PIOS_UDP_MAX_DEV];
/* Provide a COM driver */
static void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud);
static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context);
static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context);
static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail);
static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail);
const struct pios_com_driver pios_udp_com_driver = {
.set_baud = PIOS_UDP_ChangeBaud,
.tx_nb = PIOS_UDP_TxBufferPutMoreNonBlocking,
.tx = PIOS_UDP_TxBufferPutMore,
.rx = PIOS_UDP_RxBufferGet,
.rx_avail = PIOS_UDP_RxBufferUsed,
.set_baud = PIOS_UDP_ChangeBaud,
.tx_start = PIOS_UDP_TxStart,
.rx_start = PIOS_UDP_RxStart,
.bind_tx_cb = PIOS_UDP_RegisterTxCallback,
.bind_rx_cb = PIOS_UDP_RegisterRxCallback,
};
static struct pios_udp_dev * find_udp_dev_by_id (uint8_t udp)
static pios_udp_dev * find_udp_dev_by_id (uint8_t udp)
{
if (udp >= pios_udp_num_devices) {
/* Undefined UDP port for this board (see pios_board.c) */
@ -51,410 +67,164 @@ static struct pios_udp_dev * find_udp_dev_by_id (uint8_t udp)
}
/* Get a handle for the device configuration */
return &(pios_udp_devs[udp]);
return &(pios_udp_devices[udp]);
}
/**
* Open some UDP sockets
*/
void PIOS_UDP_Init(void)
{
struct pios_udp_dev * udp_dev;
uint8_t i;
for (i = 0; i < pios_udp_num_devices; i++) {
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(i);
//PIOS_DEBUG_Assert(udp_dev);
/* Clear buffer counters */
udp_dev->rx.head = udp_dev->rx.tail = udp_dev->rx.size = 0;
/* assign socket */
udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
memset(&udp_dev->server,0,sizeof(udp_dev->server));
memset(&udp_dev->client,0,sizeof(udp_dev->client));
udp_dev->server.sin_family = AF_INET;
udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip);
udp_dev->server.sin_port = htons(udp_dev->cfg->port);
int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server));
/* use nonblocking IO */
int flags = fcntl(udp_dev->socket, F_GETFL, 0);
fcntl(udp_dev->socket, F_SETFL, flags | O_NONBLOCK);
printf("udp dev %i - socket %i opened - result %i\n",i,udp_dev->socket,res);
/* TODO do some error handling - wait no, we can't - we are void anyway ;) */
}
}
/**
* Changes the baud rate of the UDP peripheral without re-initialising.
* \param[in] udp UDP name (GPS, TELEM, AUX)
* \param[in] baud Requested baud rate
*/
void PIOS_UDP_ChangeBaud(uint8_t udp, uint32_t baud)
{
}
/**
* puts a byte onto the receive buffer
* \param[in] UDP UDP name
* \param[in] b byte which should be put into Rx buffer
* \return 0 if no error
* \return -1 if UDP not available
* \return -2 if buffer full (retry)
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_RxBufferPut(uint8_t udp, uint8_t b)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -1;
}
if (udp_dev->rx.size >= sizeof(udp_dev->rx.buf)) {
/* Buffer full (retry) */
return -2;
}
/* Copy received byte into receive buffer */
udp_dev->rx.buf[udp_dev->rx.head++] = b;
if (udp_dev->rx.head >= sizeof(udp_dev->rx.buf)) {
udp_dev->rx.head = 0;
}
udp_dev->rx.size++;
/* No error */
return 0;
}
/**
* attempt to receive
* RxThread
*/
void PIOS_UDP_RECV(uint8_t udp) {
struct pios_udp_dev * udp_dev;
unsigned char localbuffer[PIOS_UDP_RX_BUFFER_SIZE];
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return;
}
/* use nonblocking IO */
int flags = fcntl(udp_dev->socket, F_GETFL, 0);
fcntl(udp_dev->socket, F_SETFL, flags | O_NONBLOCK);
/* receive data */
int received;
udp_dev->clientLength=sizeof(udp_dev->client);
if ((received = recvfrom(udp_dev->socket,
localbuffer,
(PIOS_UDP_RX_BUFFER_SIZE - udp_dev->rx.size),
0,
(struct sockaddr *) &udp_dev->client,
(socklen_t*)&udp_dev->clientLength)) < 0) {
return;
}
/* copy received data to buffer */
int t;
for (t=0;t<received;t++) {
PIOS_UDP_RxBufferPut(udp,localbuffer[t]);
}
}
/**
* Returns number of free bytes in receive buffer
* \param[in] UDP UDP name
* \return UDP number of free bytes
* \return 1: UDP available
* \return 0: UDP not available
*/
int32_t PIOS_UDP_RxBufferFree(uint8_t udp)
void * PIOS_UDP_RxThread(void * udp_dev_n)
{
struct pios_udp_dev * udp_dev;
pios_udp_dev * udp_dev = (pios_udp_dev*) udp_dev_n;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
/**
* com devices never get closed except by application "reboot"
* we also never give up our mutex except for waiting
*/
while(1) {
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -2;
}
/**
* receive
*/
int received;
udp_dev->clientLength=sizeof(udp_dev->client);
if ((received = recvfrom(udp_dev->socket,
&udp_dev->rx_buffer,
PIOS_UDP_RX_BUFFER_SIZE,
0,
(struct sockaddr *) &udp_dev->client,
(socklen_t*)&udp_dev->clientLength)) < 0) {
/* fill buffer */
PIOS_UDP_RECV(udp);
pthread_exit(NULL);
}
return (sizeof(udp_dev->rx.buf) - udp_dev->rx.size);
}
/* 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);
}
/**
* Returns number of used bytes in receive buffer
* \param[in] UDP UDP name
* \return > 0: number of used bytes
* \return 0 if UDP not available
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_RxBufferUsed(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
#if defined(PIOS_INCLUDE_FREERTOS)
if (rx_need_yield) {
vPortYieldFromISR();
}
#endif /* PIOS_INCLUDE_FREERTOS */
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -2;
}
/* fill buffer */
PIOS_UDP_RECV(udp);
return (udp_dev->rx.size);
}
/**
* Gets a byte from the receive buffer
* \param[in] UDP UDP name
* \return -1 if UDP not available
* \return -2 if no new byte available
* \return >= 0: number of received bytes
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_RxBufferGet(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -2;
}
/* fill buffer */
PIOS_UDP_RECV(udp);
if (!udp_dev->rx.size) {
/* Nothing new in the buffer */
return -1;
}
/* get byte */
uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail++];
if (udp_dev->rx.tail >= sizeof(udp_dev->rx.buf)) {
udp_dev->rx.tail = 0;
}
udp_dev->rx.size--;
/* Return received byte */
return b;
}
/**
* Returns the next byte of the receive buffer without taking it
* \param[in] UDP UDP name
* \return -1 if UDP not available
* \return -2 if no new byte available
* \return >= 0: number of received bytes
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_RxBufferPeek(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -2;
}
/* fill buffer */
PIOS_UDP_RECV(udp);
if (!udp_dev->rx.size) {
/* Nothing new in the buffer */
return -1;
}
/* get byte */
uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail];
/* Return received byte */
return b;
}
/**
* returns number of free bytes in transmit buffer
* \param[in] UDP UDP name
* \return number of free bytes
* \return 0 if UDP not available
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferFree(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return 0;
}
return PIOS_UDP_RX_BUFFER_SIZE;
}
/**
* returns number of used bytes in transmit buffer
* \param[in] UDP UDP name
* \return number of used bytes
* \return 0 if UDP not available
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferUsed(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return 0;
}
return 0;
}
}
/**
* puts more than one byte onto the transmit buffer (used for atomic sends)
* \param[in] UDP UDP name
* \param[in] *buffer pointer to buffer to be sent
* \param[in] len number of bytes to be sent
* \return 0 if no error
* \return -1 if UDP not available
* \return -2 if buffer full or cannot get all requested bytes (retry)
* \return -3 if UDP not supported by UDPTxBufferPut Routine
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
* Open UDP socket
*/
int32_t PIOS_UDP_TxBufferPutMoreNonBlocking(uint8_t udp, uint8_t *buffer, uint16_t len)
int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
pios_udp_dev * udp_dev = &pios_udp_devices[pios_udp_num_devices];
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -1;
}
if (len >= PIOS_UDP_RX_BUFFER_SIZE) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
/* send data to client - non blocking*/
/* use nonblocking IO */
int flags = fcntl(udp_dev->socket, F_GETFL, 0);
fcntl(udp_dev->socket, F_SETFL, flags | O_NONBLOCK);
sendto(udp_dev->socket, buffer, len, 0,
(struct sockaddr *) &udp_dev->client,
(socklen_t)sizeof(udp_dev->client));
pios_udp_num_devices++;
/* No error */
return 0;
/* initialize */
udp_dev->rx_in_cb = NULL;
udp_dev->tx_out_cb = NULL;
udp_dev->cfg=cfg;
/* assign socket */
udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
memset(&udp_dev->server,0,sizeof(udp_dev->server));
memset(&udp_dev->client,0,sizeof(udp_dev->client));
udp_dev->server.sin_family = AF_INET;
udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip);
udp_dev->server.sin_port = htons(udp_dev->cfg->port);
int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server));
/* Create transmit thread for this connection */
pthread_create(&udp_dev->rxThread, NULL, PIOS_UDP_RxThread, (void*)udp_dev);
printf("udp dev %i - socket %i opened - result %i\n",pios_udp_num_devices-1,udp_dev->socket,res);
return res;
}
/**
* puts more than one byte onto the transmit buffer (used for atomic sends)<BR>
* (blocking function)
* \param[in] UDP UDP name
* \param[in] *buffer pointer to buffer to be sent
* \param[in] len number of bytes to be sent
* \return 0 if no error
* \return -1 if UDP not available
* \return -3 if UDP not supported by UDPTxBufferPut Routine
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferPutMore(uint8_t udp, uint8_t *buffer, uint16_t len)
void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -1;
}
if (len >= PIOS_UDP_RX_BUFFER_SIZE) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
/* send data to client - blocking*/
/* use blocking IO */
int flags = fcntl(udp_dev->socket, F_GETFL, 0);
fcntl(udp_dev->socket, F_SETFL, flags & ~O_NONBLOCK);
sendto(udp_dev->socket, buffer, len, 0,
(struct sockaddr *) &udp_dev->client,
sizeof(udp_dev->client));
/* No error */
return 0;
/**
* doesn't apply!
*/
}
/**
* puts a byte onto the transmit buffer
* \param[in] UDP UDP name
* \param[in] b byte which should be put into Tx buffer
* \return 0 if no error
* \return -1 if UDP not available
* \return -2 if buffer full (retry)
* \return -3 if UDP not supported by UDPTxBufferPut Routine
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferPut_NonBlocking(uint8_t udp, uint8_t b)
static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail)
{
return PIOS_UDP_TxBufferPutMoreNonBlocking(udp, &b, 1);
/**
* lazy!
*/
}
/**
* puts a byte onto the transmit buffer<BR>
* (blocking function)
* \param[in] UDP UDP name
* \param[in] b byte which should be put into Tx buffer
* \return 0 if no error
* \return -1 if UDP not available
* \return -3 if UDP not supported by UDPTxBufferPut Routine
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferPut(uint8_t udp, uint8_t b)
static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail)
{
return PIOS_UDP_TxBufferPutMore(udp, &b, 1);
pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id);
PIOS_Assert(udp_dev);
uint16_t length,len,rem;
/**
* we send everything directly whenever notified of data to send (lazy!)
*/
if (udp_dev->tx_out_cb) {
while (tx_bytes_avail>0) {
bool tx_need_yield = false;
length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield);
rem = length;
while (rem>0) {
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;
}
tx_bytes_avail -= length;
}
}
}
static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context)
{
pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id);
PIOS_Assert(udp_dev);
/*
* Order is important in these assignments since ISR uses _cb
* field to determine if it's ok to dereference _cb and _context
*/
udp_dev->rx_in_context = context;
udp_dev->rx_in_cb = rx_in_cb;
}
static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context)
{
pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id);
PIOS_Assert(udp_dev);
/*
* Order is important in these assignments since ISR uses _cb
* field to determine if it's ok to dereference _cb and _context
*/
udp_dev->tx_out_context = context;
udp_dev->tx_out_cb = tx_out_cb;
}
#endif