mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-21 11:54:15 +01:00
PiOS.posix: port pios_udp and pios_com to "next"
This commit is contained in:
Corvus Corax
parent
8a47ade16b
commit
5b240a2675
@ -165,6 +165,7 @@ endif
|
|||||||
SRC += $(PIOSPOSIX)/pios_crc.c
|
SRC += $(PIOSPOSIX)/pios_crc.c
|
||||||
SRC += $(PIOSPOSIX)/pios_sys.c
|
SRC += $(PIOSPOSIX)/pios_sys.c
|
||||||
SRC += $(PIOSPOSIX)/pios_led.c
|
SRC += $(PIOSPOSIX)/pios_led.c
|
||||||
|
SRC += $(PIOSPOSIX)/pios_irq.c
|
||||||
SRC += $(PIOSPOSIX)/pios_delay.c
|
SRC += $(PIOSPOSIX)/pios_delay.c
|
||||||
SRC += $(PIOSPOSIX)/pios_sdcard.c
|
SRC += $(PIOSPOSIX)/pios_sdcard.c
|
||||||
SRC += $(PIOSPOSIX)/pios_udp.c
|
SRC += $(PIOSPOSIX)/pios_udp.c
|
||||||
@ -176,7 +177,7 @@ SRC += $(PIOSPOSIX)/pios_debug.c
|
|||||||
SRC += $(PIOSPOSIX)/pios_rcvr.c
|
SRC += $(PIOSPOSIX)/pios_rcvr.c
|
||||||
|
|
||||||
## Libraries for flight calculations
|
## Libraries for flight calculations
|
||||||
#SRC += $(FLIGHTLIB)/fifo_buffer.c
|
SRC += $(FLIGHTLIB)/fifo_buffer.c
|
||||||
SRC += $(FLIGHTLIB)/WorldMagModel.c
|
SRC += $(FLIGHTLIB)/WorldMagModel.c
|
||||||
SRC += $(FLIGHTLIB)/CoordinateConversions.c
|
SRC += $(FLIGHTLIB)/CoordinateConversions.c
|
||||||
## RTOS and RTOS Portable
|
## RTOS and RTOS Portable
|
||||||
|
|||||||
@ -52,6 +52,7 @@
|
|||||||
//-------------------------
|
//-------------------------
|
||||||
//#define PIOS_USART_TX_BUFFER_SIZE 256
|
//#define PIOS_USART_TX_BUFFER_SIZE 256
|
||||||
#define PIOS_COM_BUFFER_SIZE 1024
|
#define PIOS_COM_BUFFER_SIZE 1024
|
||||||
|
#define PIOS_COM_MAX_DEVS 256
|
||||||
#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE
|
#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE
|
||||||
|
|
||||||
#define PIOS_COM_TELEM_RF 0
|
#define PIOS_COM_TELEM_RF 0
|
||||||
|
|||||||
@ -36,6 +36,8 @@
|
|||||||
#define PIOS_INCLUDE_SDCARD
|
#define PIOS_INCLUDE_SDCARD
|
||||||
#define PIOS_INCLUDE_FREERTOS
|
#define PIOS_INCLUDE_FREERTOS
|
||||||
#define PIOS_INCLUDE_COM
|
#define PIOS_INCLUDE_COM
|
||||||
|
#define PIOS_INCLUDE_IRQ
|
||||||
|
#define PIOS_INCLUDE_TELEMETRY_RF
|
||||||
#define PIOS_INCLUDE_UDP
|
#define PIOS_INCLUDE_UDP
|
||||||
#define PIOS_INCLUDE_SERVO
|
#define PIOS_INCLUDE_SERVO
|
||||||
#define PIOS_INCLUDE_RCVR
|
#define PIOS_INCLUDE_RCVR
|
||||||
|
|||||||
@ -40,42 +40,16 @@ void Stack_Change() {
|
|||||||
void Stack_Change_Weak() {
|
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 */
|
const struct pios_udp_cfg pios_udp_telem_cfg = {
|
||||||
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 = {
|
|
||||||
.ip = "0.0.0.0",
|
.ip = "0.0.0.0",
|
||||||
.port = 9000,
|
.port = 9000,
|
||||||
};
|
};
|
||||||
const struct pios_udp_cfg pios_udp1_cfg = {
|
const struct pios_udp_cfg pios_udp_gps_cfg = {
|
||||||
.ip = "0.0.0.0",
|
.ip = "0.0.0.0",
|
||||||
.port = 9001,
|
.port = 9001,
|
||||||
};
|
};
|
||||||
const struct pios_udp_cfg pios_udp2_cfg = {
|
const struct pios_udp_cfg pios_udp_debug_cfg = {
|
||||||
.ip = "0.0.0.0",
|
.ip = "0.0.0.0",
|
||||||
.port = 9002,
|
.port = 9002,
|
||||||
};
|
};
|
||||||
@ -84,15 +58,19 @@ const struct pios_udp_cfg pios_udp2_cfg = {
|
|||||||
/*
|
/*
|
||||||
* AUX USART
|
* AUX USART
|
||||||
*/
|
*/
|
||||||
const struct pios_udp_cfg pios_udp3_cfg = {
|
const struct pios_udp_cfg pios_udp_aux_cfg = {
|
||||||
.ip = "0.0.0.0",
|
.ip = "0.0.0.0",
|
||||||
.port = 9003,
|
.port = 9003,
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
|
||||||
|
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Board specific number of devices.
|
* Board specific number of devices.
|
||||||
*/
|
*/
|
||||||
|
/*
|
||||||
struct pios_udp_dev pios_udp_devs[] = {
|
struct pios_udp_dev pios_udp_devs[] = {
|
||||||
#define PIOS_UDP_TELEM 0
|
#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);
|
uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs);
|
||||||
|
*/
|
||||||
/*
|
/*
|
||||||
* COM devices
|
* 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_serial_com_driver;
|
||||||
extern const struct pios_com_driver pios_udp_com_driver;
|
extern const struct pios_com_driver pios_udp_com_driver;
|
||||||
|
|
||||||
struct pios_com_dev pios_com_devs[] = {
|
uint32_t pios_com_telem_rf_id;
|
||||||
{
|
uint32_t pios_com_telem_usb_id;
|
||||||
.id = PIOS_UDP_TELEM,
|
uint32_t pios_com_gps_id;
|
||||||
.driver = &pios_udp_com_driver,
|
uint32_t pios_com_aux_id;
|
||||||
},
|
uint32_t pios_com_spectrum_id;
|
||||||
{
|
|
||||||
.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
|
|
||||||
};
|
|
||||||
|
|
||||||
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
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @}
|
* @}
|
||||||
|
|||||||
@ -1,55 +1,65 @@
|
|||||||
/**
|
/**
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
*
|
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||||
* @file pios_com.h
|
* @{
|
||||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
* @addtogroup PIOS_COM COM layer functions
|
||||||
* Parts by Thorsten Klose (tk@midibox.org)
|
* @brief Hardware communication layer
|
||||||
* @brief COM layer functions header
|
* @{
|
||||||
* @see The GNU Public License (GPL) Version 3
|
*
|
||||||
*
|
* @file pios_com.h
|
||||||
*****************************************************************************/
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||||
/*
|
* Parts by Thorsten Klose (tk@midibox.org)
|
||||||
* This program is free software; you can redistribute it and/or modify
|
* @brief COM layer functions header
|
||||||
* it under the terms of the GNU General Public License as published by
|
* @see The GNU Public License (GPL) Version 3
|
||||||
* 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
|
* This program is free software; you can redistribute it and/or modify
|
||||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
* it under the terms of the GNU General Public License as published by
|
||||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
* for more details.
|
* (at your option) any later version.
|
||||||
*
|
*
|
||||||
* You should have received a copy of the GNU General Public License along
|
* This program is distributed in the hope that it will be useful, but
|
||||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
*/
|
* for more details.
|
||||||
|
*
|
||||||
#ifndef PIOS_COM_H
|
* You should have received a copy of the GNU General Public License along
|
||||||
#define PIOS_COM_H
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
/* Public Functions */
|
*/
|
||||||
extern int32_t PIOS_COM_Init(void);
|
|
||||||
extern int32_t PIOS_COM_ChangeBaud(uint8_t port, uint32_t baud);
|
#ifndef PIOS_COM_H
|
||||||
extern int32_t PIOS_COM_SendCharNonBlocking(uint8_t port, char c);
|
#define PIOS_COM_H
|
||||||
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);
|
typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * task_woken);
|
||||||
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);
|
struct pios_com_driver {
|
||||||
extern int32_t PIOS_COM_SendString(uint8_t port, char *str);
|
void (*init)(uint32_t id);
|
||||||
extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint8_t port, char *format, ...);
|
void (*set_baud)(uint32_t id, uint32_t baud);
|
||||||
extern int32_t PIOS_COM_SendFormattedString(uint8_t port, char *format, ...);
|
void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail);
|
||||||
extern uint8_t PIOS_COM_ReceiveBuffer(uint8_t port);
|
void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail);
|
||||||
extern int32_t PIOS_COM_ReceiveBufferUsed(uint8_t port);
|
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);
|
||||||
extern int32_t PIOS_COM_ReceiveHandler(void);
|
};
|
||||||
|
|
||||||
struct pios_com_driver {
|
/* Public Functions */
|
||||||
void (*init)(uint8_t id);
|
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);
|
||||||
void (*set_baud)(uint8_t id, uint32_t baud);
|
extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud);
|
||||||
int32_t (*tx_nb)(uint8_t id, uint8_t *buffer, uint16_t len);
|
extern int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c);
|
||||||
int32_t (*tx)(uint8_t id, uint8_t *buffer, uint16_t len);
|
extern int32_t PIOS_COM_SendChar(uint32_t com_id, char c);
|
||||||
int32_t (*rx)(uint8_t id);
|
extern int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len);
|
||||||
int32_t (*rx_avail)(uint8_t id);
|
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);
|
||||||
#endif /* PIOS_COM_H */
|
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 */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|||||||
@ -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
|
* @file pios_com_priv.h
|
||||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||||
@ -29,13 +34,11 @@
|
|||||||
|
|
||||||
#include <pios.h>
|
#include <pios.h>
|
||||||
|
|
||||||
struct pios_com_dev {
|
extern int32_t PIOS_COM_ReceiveHandler(uint32_t com_id);
|
||||||
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;
|
|
||||||
|
|
||||||
#endif /* PIOS_COM_PRIV_H */
|
#endif /* PIOS_COM_PRIV_H */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|||||||
38
flight/PiOS.posix/inc/pios_irq.h
Normal file
38
flight/PiOS.posix/inc/pios_irq.h
Normal 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 */
|
||||||
@ -31,21 +31,5 @@
|
|||||||
/* Global Types */
|
/* Global Types */
|
||||||
|
|
||||||
/* Public Functions */
|
/* 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 */
|
#endif /* PIOS_UDP_H */
|
||||||
|
|||||||
@ -29,6 +29,7 @@
|
|||||||
|
|
||||||
#include <pios.h>
|
#include <pios.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
#include <pthread.h>
|
||||||
#include <sys/socket.h>
|
#include <sys/socket.h>
|
||||||
#include <arpa/inet.h>
|
#include <arpa/inet.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
@ -37,30 +38,34 @@
|
|||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
#include <netinet/in.h>
|
#include <netinet/in.h>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
struct pios_udp_cfg {
|
struct pios_udp_cfg {
|
||||||
const char * ip;
|
const char * ip;
|
||||||
uint16_t port;
|
uint16_t port;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct pios_udp_buffer {
|
typedef struct {
|
||||||
uint8_t buf[PIOS_UDP_RX_BUFFER_SIZE];
|
const struct pios_udp_cfg * cfg;
|
||||||
uint16_t head;
|
pthread_t rxThread;
|
||||||
uint16_t tail;
|
|
||||||
uint16_t size;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct pios_udp_dev {
|
|
||||||
const struct pios_udp_cfg * const cfg;
|
|
||||||
struct pios_udp_buffer rx;
|
|
||||||
int socket;
|
int socket;
|
||||||
struct sockaddr_in server;
|
struct sockaddr_in server;
|
||||||
struct sockaddr_in client;
|
struct sockaddr_in client;
|
||||||
uint32_t clientLength;
|
uint32_t clientLength;
|
||||||
};
|
|
||||||
|
|
||||||
extern struct pios_udp_dev pios_udp_devs[];
|
pthread_cond_t cond;
|
||||||
extern uint8_t pios_udp_num_devices;
|
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 */
|
#endif /* PIOS_UDP_PRIV_H */
|
||||||
|
|||||||
@ -58,6 +58,7 @@
|
|||||||
#include <pios_sys.h>
|
#include <pios_sys.h>
|
||||||
#include <pios_delay.h>
|
#include <pios_delay.h>
|
||||||
#include <pios_led.h>
|
#include <pios_led.h>
|
||||||
|
#include <pios_irq.h>
|
||||||
#include <pios_sdcard.h>
|
#include <pios_sdcard.h>
|
||||||
#include <pios_udp.h>
|
#include <pios_udp.h>
|
||||||
#include <pios_com.h>
|
#include <pios_com.h>
|
||||||
|
|||||||
@ -1,285 +1,523 @@
|
|||||||
/**
|
/**
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
*
|
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||||
* @file pios_com.c
|
* @{
|
||||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
* @addtogroup PIOS_COM COM layer functions
|
||||||
* Parts by Thorsten Klose (tk@midibox.org)
|
* @brief Hardware communication layer
|
||||||
* @brief COM layer functions
|
* @{
|
||||||
* @see The GNU Public License (GPL) Version 3
|
*
|
||||||
* @defgroup PIOS_COM COM layer functions
|
* @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 free software; you can redistribute it and/or modify
|
||||||
*
|
* it under the terms of the GNU General Public License as published by
|
||||||
* This program is distributed in the hope that it will be useful, but
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
* (at your option) any later version.
|
||||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
*
|
||||||
* for more details.
|
* This program is distributed in the hope that it will be useful, but
|
||||||
*
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
* You should have received a copy of the GNU General Public License along
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
* for more details.
|
||||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
*
|
||||||
*/
|
* 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 */
|
*/
|
||||||
|
|
||||||
|
/* Project Includes */
|
||||||
#include "pios.h"
|
#include "pios.h"
|
||||||
|
|
||||||
#if defined(PIOS_INCLUDE_COM)
|
#if defined(PIOS_INCLUDE_COM)
|
||||||
|
|
||||||
|
#include "fifo_buffer.h"
|
||||||
#include <pios_com_priv.h>
|
#include <pios_com_priv.h>
|
||||||
|
|
||||||
static struct pios_com_dev * find_com_dev_by_id (uint8_t port)
|
#if !defined(PIOS_INCLUDE_FREERTOS)
|
||||||
{
|
#include "pios_delay.h" /* PIOS_DELAY_WaitmS */
|
||||||
if (port >= pios_com_num_devices) {
|
#endif
|
||||||
/* Undefined COM port for this board (see pios_board.c) */
|
|
||||||
return NULL;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Get a handle for the device configuration */
|
enum pios_com_dev_magic {
|
||||||
return &(pios_com_devs[port]);
|
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));
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
#if defined(PIOS_INCLUDE_FREERTOS) && 0
|
||||||
* Initialises COM layer
|
// static struct pios_com_dev * PIOS_COM_alloc(void)
|
||||||
* \param[in] mode currently only mode 0 supported
|
//{
|
||||||
* \return < 0 if initialisation failed
|
// struct pios_com_dev * com_dev;
|
||||||
*/
|
//
|
||||||
int32_t PIOS_COM_Init(void)
|
// com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev));
|
||||||
{
|
// if (!com_dev) return (NULL);
|
||||||
int32_t ret = 0;
|
//
|
||||||
|
// com_dev->magic = PIOS_COM_DEV_MAGIC;
|
||||||
/* If any COM assignment: */
|
// return(com_dev);
|
||||||
#if defined(PIOS_INCLUDE_SERIAL)
|
//}
|
||||||
PIOS_SERIAL_Init();
|
#else
|
||||||
#endif
|
static struct pios_com_dev pios_com_devs[PIOS_COM_MAX_DEVS];
|
||||||
|
static uint8_t pios_com_num_devs;
|
||||||
#if defined(PIOS_INCLUDE_UDP)
|
static uint32_t PIOS_COM_create(void)
|
||||||
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)
|
|
||||||
{
|
{
|
||||||
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) {
|
com_dev = &pios_com_devs[pios_com_num_devs++];
|
||||||
/* Undefined COM port for this board (see pios_board.c) */
|
com_dev->magic = PIOS_COM_DEV_MAGIC;
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Invoke the driver function if it exists */
|
return (pios_com_num_devs-1);
|
||||||
if (com_dev->driver->set_baud) {
|
|
||||||
com_dev->driver->set_baud(com_dev->id, baud);
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
}
|
||||||
|
static struct pios_com_dev * PIOS_COM_find_dev(uint32_t com_dev_id)
|
||||||
/**
|
|
||||||
* 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)
|
|
||||||
{
|
{
|
||||||
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) */
|
* Initialises COM layer
|
||||||
return -1;
|
* \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 */
|
bool has_rx = (rx_buffer && rx_buffer_len > 0);
|
||||||
if (com_dev->driver->tx_nb) {
|
bool has_tx = (tx_buffer && tx_buffer_len > 0);
|
||||||
return com_dev->driver->tx_nb(com_dev->id, buffer, len);
|
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);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield)
|
||||||
* 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)
|
|
||||||
{
|
{
|
||||||
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 (xHigherPriorityTaskWoken != pdFALSE) {
|
||||||
|
*need_yield = true;
|
||||||
if (!com_dev) {
|
} else {
|
||||||
/* Undefined COM port for this board (see pios_board.c) */
|
*need_yield = false;
|
||||||
return -1;
|
}
|
||||||
}
|
#else
|
||||||
|
*need_yield = false;
|
||||||
/* Invoke the driver function if it exists */
|
#endif
|
||||||
if (com_dev->driver->tx) {
|
|
||||||
return com_dev->driver->tx(com_dev->id, buffer, len);
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield)
|
||||||
* Sends a single character over given port
|
{
|
||||||
* \param[in] port COM port
|
#if defined(PIOS_INCLUDE_FREERTOS)
|
||||||
* \param[in] c character
|
static signed portBASE_TYPE xHigherPriorityTaskWoken;
|
||||||
* \return -1 if port not available
|
xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken);
|
||||||
* \return -2 buffer is full
|
|
||||||
* caller should retry until buffer is free again
|
if (xHigherPriorityTaskWoken != pdFALSE) {
|
||||||
* \return 0 on success
|
*need_yield = true;
|
||||||
*/
|
} else {
|
||||||
int32_t PIOS_COM_SendCharNonBlocking(uint8_t port, char c)
|
*need_yield = false;
|
||||||
{
|
}
|
||||||
return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)&c, 1);
|
#else
|
||||||
}
|
*need_yield = false;
|
||||||
|
#endif
|
||||||
/**
|
}
|
||||||
* Sends a single character over given port
|
|
||||||
* (blocking function)
|
static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
|
||||||
* \param[in] port COM port
|
{
|
||||||
* \param[in] c character
|
struct pios_com_dev * com_dev = PIOS_COM_find_dev(context);
|
||||||
* \return -1 if port not available
|
|
||||||
* \return 0 on success
|
bool valid = PIOS_COM_validate(com_dev);
|
||||||
*/
|
PIOS_Assert(valid);
|
||||||
int32_t PIOS_COM_SendChar(uint8_t port, char c)
|
PIOS_Assert(com_dev->has_rx);
|
||||||
{
|
|
||||||
return PIOS_COM_SendBuffer(port, (uint8_t *)&c, 1);
|
PIOS_IRQ_Disable();
|
||||||
}
|
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len);
|
||||||
|
PIOS_IRQ_Enable();
|
||||||
/**
|
|
||||||
* Sends a string over given port
|
if (bytes_into_fifo > 0) {
|
||||||
* \param[in] port COM port
|
/* Data has been added to the buffer */
|
||||||
* \param[in] str zero-terminated string
|
PIOS_COM_UnblockRx(com_dev, need_yield);
|
||||||
* \return -1 if port not available
|
}
|
||||||
* \return -2 buffer is full
|
|
||||||
* caller should retry until buffer is free again
|
if (headroom) {
|
||||||
* \return 0 on success
|
*headroom = fifoBuf_getFree(&com_dev->rx);
|
||||||
*/
|
}
|
||||||
int32_t PIOS_COM_SendStringNonBlocking(uint8_t port, char *str)
|
|
||||||
{
|
return (bytes_into_fifo);
|
||||||
return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)str, (uint16_t)strlen(str));
|
}
|
||||||
}
|
|
||||||
|
static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
|
||||||
/**
|
{
|
||||||
* Sends a string over given port
|
struct pios_com_dev * com_dev = PIOS_COM_find_dev(context);
|
||||||
* (blocking function)
|
|
||||||
* \param[in] port COM port
|
bool valid = PIOS_COM_validate(com_dev);
|
||||||
* \param[in] str zero-terminated string
|
PIOS_Assert(valid);
|
||||||
* \return -1 if port not available
|
PIOS_Assert(buf);
|
||||||
* \return 0 on success
|
PIOS_Assert(buf_len);
|
||||||
*/
|
PIOS_Assert(com_dev->has_tx);
|
||||||
int32_t PIOS_COM_SendString(uint8_t port, char *str)
|
|
||||||
{
|
PIOS_IRQ_Disable();
|
||||||
return PIOS_COM_SendBuffer(port, (uint8_t *)str, strlen(str));
|
uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len);
|
||||||
}
|
PIOS_IRQ_Enable();
|
||||||
|
|
||||||
/**
|
if (bytes_from_fifo > 0) {
|
||||||
* Sends a formatted string (-> printf) over given port
|
/* More space has been made in the buffer */
|
||||||
* \param[in] port COM port
|
PIOS_COM_UnblockTx(com_dev, need_yield);
|
||||||
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
|
}
|
||||||
* \param[in] ... optional arguments,
|
|
||||||
* 128 characters supported maximum!
|
if (headroom) {
|
||||||
* \return -2 if non-blocking mode activated: buffer is full
|
*headroom = fifoBuf_getUsed(&com_dev->tx);
|
||||||
* caller should retry until buffer is free again
|
}
|
||||||
* \return 0 on success
|
|
||||||
*/
|
return (bytes_from_fifo);
|
||||||
int32_t PIOS_COM_SendFormattedStringNonBlocking(uint8_t port, char *format, ...)
|
}
|
||||||
{
|
|
||||||
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
|
/**
|
||||||
|
* Change the port speed without re-initializing
|
||||||
va_list args;
|
* \param[in] port COM port
|
||||||
|
* \param[in] baud Requested baud rate
|
||||||
va_start(args, format);
|
* \return -1 if port not available
|
||||||
vsprintf((char *)buffer, format, args);
|
* \return 0 on success
|
||||||
return PIOS_COM_SendBufferNonBlocking(port, buffer, (uint16_t)strlen((char *)buffer));
|
*/
|
||||||
}
|
int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
|
||||||
|
{
|
||||||
/**
|
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
|
||||||
* Sends a formatted string (-> printf) over given port
|
|
||||||
* (blocking function)
|
if (!PIOS_COM_validate(com_dev)) {
|
||||||
* \param[in] port COM port
|
/* Undefined COM port for this board (see pios_board.c) */
|
||||||
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
|
return -1;
|
||||||
* \param[in] ... optional arguments,
|
}
|
||||||
* \return -1 if port not available
|
|
||||||
* \return 0 on success
|
/* Invoke the driver function if it exists */
|
||||||
*/
|
if (com_dev->driver->set_baud) {
|
||||||
int32_t PIOS_COM_SendFormattedString(uint8_t port, char *format, ...)
|
com_dev->driver->set_baud(com_dev->lower_id, baud);
|
||||||
{
|
}
|
||||||
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
|
|
||||||
va_list args;
|
return 0;
|
||||||
|
}
|
||||||
va_start(args, format);
|
|
||||||
vsprintf((char *)buffer, format, args);
|
/**
|
||||||
return PIOS_COM_SendBuffer(port, buffer, (uint16_t)strlen((char *)buffer));
|
* Sends a package over given port
|
||||||
}
|
* \param[in] port COM port
|
||||||
|
* \param[in] buffer character buffer
|
||||||
/**
|
* \param[in] len buffer length
|
||||||
* Transfer bytes from port buffers into another buffer
|
* \return -1 if port not available
|
||||||
* \param[in] port COM port
|
* \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
|
* \returns Byte from buffer
|
||||||
*/
|
*/
|
||||||
uint8_t PIOS_COM_ReceiveBuffer(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;
|
|
||||||
|
|
||||||
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)
|
|
||||||
{
|
{
|
||||||
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) {
|
if (!PIOS_COM_validate(com_dev)) {
|
||||||
/* Undefined COM port for this board (see pios_board.c) */
|
/* Undefined COM port for this board (see pios_board.c) */
|
||||||
return 0;
|
PIOS_Assert(0);
|
||||||
}
|
}
|
||||||
|
PIOS_Assert(com_dev->has_rx);
|
||||||
|
|
||||||
if (!com_dev->driver->rx_avail) {
|
check_again:
|
||||||
return 0;
|
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
69
flight/PiOS.posix/posix/pios_irq.c
Normal 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
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
@ -34,16 +34,32 @@
|
|||||||
|
|
||||||
#include <pios_udp_priv.h>
|
#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 */
|
/* 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 = {
|
const struct pios_com_driver pios_udp_com_driver = {
|
||||||
.set_baud = PIOS_UDP_ChangeBaud,
|
.set_baud = PIOS_UDP_ChangeBaud,
|
||||||
.tx_nb = PIOS_UDP_TxBufferPutMoreNonBlocking,
|
.tx_start = PIOS_UDP_TxStart,
|
||||||
.tx = PIOS_UDP_TxBufferPutMore,
|
.rx_start = PIOS_UDP_RxStart,
|
||||||
.rx = PIOS_UDP_RxBufferGet,
|
.bind_tx_cb = PIOS_UDP_RegisterTxCallback,
|
||||||
.rx_avail = PIOS_UDP_RxBufferUsed,
|
.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) {
|
if (udp >= pios_udp_num_devices) {
|
||||||
/* Undefined UDP port for this board (see pios_board.c) */
|
/* 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 */
|
/* Get a handle for the device configuration */
|
||||||
return &(pios_udp_devs[udp]);
|
return &(pios_udp_devices[udp]);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Open some UDP sockets
|
* RxThread
|
||||||
*/
|
|
||||||
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
|
|
||||||
*/
|
*/
|
||||||
void PIOS_UDP_RECV(uint8_t udp) {
|
void * PIOS_UDP_RxThread(void * udp_dev_n)
|
||||||
|
|
||||||
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)
|
|
||||||
{
|
{
|
||||||
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) */
|
* receive
|
||||||
return -2;
|
*/
|
||||||
}
|
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 */
|
pthread_exit(NULL);
|
||||||
PIOS_UDP_RECV(udp);
|
}
|
||||||
|
|
||||||
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);
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
#if defined(PIOS_INCLUDE_FREERTOS)
|
||||||
* Returns number of used bytes in receive buffer
|
if (rx_need_yield) {
|
||||||
* \param[in] UDP UDP name
|
vPortYieldFromISR();
|
||||||
* \return > 0: number of used bytes
|
}
|
||||||
* \return 0 if UDP not available
|
#endif /* PIOS_INCLUDE_FREERTOS */
|
||||||
* \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;
|
|
||||||
|
|
||||||
/* 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)
|
* Open UDP socket
|
||||||
* \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
|
|
||||||
*/
|
*/
|
||||||
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 */
|
pios_udp_dev * udp_dev = &pios_udp_devices[pios_udp_num_devices];
|
||||||
udp_dev = find_udp_dev_by_id(udp);
|
|
||||||
|
|
||||||
if (!udp_dev) {
|
pios_udp_num_devices++;
|
||||||
/* 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));
|
|
||||||
|
|
||||||
|
|
||||||
/* No error */
|
/* initialize */
|
||||||
return 0;
|
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>
|
void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud)
|
||||||
* (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)
|
|
||||||
{
|
{
|
||||||
struct pios_udp_dev * udp_dev;
|
/**
|
||||||
|
* doesn't apply!
|
||||||
/* 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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* puts a byte onto the transmit buffer
|
static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail)
|
||||||
* \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)
|
|
||||||
{
|
{
|
||||||
return PIOS_UDP_TxBufferPutMoreNonBlocking(udp, &b, 1);
|
/**
|
||||||
|
* lazy!
|
||||||
|
*/
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* puts a byte onto the transmit buffer<BR>
|
static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail)
|
||||||
* (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)
|
|
||||||
{
|
{
|
||||||
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
|
#endif
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user