rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/amorale/OP-1218_pios_com_thread_safe' into rel-14.01

Browse Source
This commit is contained in:
Alessio Morale 2014-02-10 20:52:15 +01:00
commit fd92e2a497
2 changed files with 88 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
flight/modules/RadioComBridge/RadioComBridge.c
View File

@ -415,7 +415,11 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
// Send the data straight to the telemetry port.
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process);
int32_t ret = -2;
uint8_t count = 5;
while(count-- > 0 && ret < -1){
ret = PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process);
}
}
}
} else {
@ -511,7 +515,11 @@ static void serialRxTask(__attribute__((unused)) void *parameters)
// Send the data over the radio link.
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process);
int32_t ret = -2;
uint8_t count = 5;
while(count-- > 0 && ret < -1){
PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process);
}
}
} else {
vTaskDelay(5);
@ -541,7 +549,11 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
if (outputPort) {
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
ret = -2;
uint8_t count = 5;
while(count-- > 0 && ret < -1){
ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
}
} else {
ret = -1;
}
@ -567,7 +579,12 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
if (outputPort && PIOS_COM_Available(outputPort)) {
// FIXME following call can fail (with -2 error code) if buffer is full
// it is the caller responsibility to retry in such cases...
return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
int32_t ret = -2;
uint8_t count = 5;
while(count-- > 0 && ret < -1){
ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
}
return ret;
} else {
return -1;
}

101
flight/pios/common/pios_com.c
View File

@ -51,6 +51,7 @@ struct pios_com_dev {
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreHandle tx_sem;
xSemaphoreHandle rx_sem;
xSemaphoreHandle sendbuffer_sem;
#endif
bool has_rx;
@ -155,6 +156,9 @@ int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, ui
#endif /* PIOS_INCLUDE_FREERTOS */
(com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, (uint32_t)com_dev);
}
#if defined(PIOS_INCLUDE_FREERTOS)
com_dev->sendbuffer_sem = xSemaphoreCreateMutex();
#endif /* PIOS_INCLUDE_FREERTOS */
*com_id = (uint32_t)com_dev;
return 0;
@ -267,6 +271,40 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
return 0;
}
static int32_t PIOS_COM_SendBufferNonBlockingInternal(struct pios_com_dev *com_dev, const uint8_t *buffer, uint16_t len)
{
PIOS_Assert(com_dev);
PIOS_Assert(com_dev->has_tx);
if (com_dev->driver->available && !com_dev->driver->available(com_dev->lower_id)) {
/*
* Underlying device is down/unconnected.
* Dump our fifo contents and act like an infinite data sink.
* Failure to do this results in stale data in the fifo as well as
* possibly having the caller block trying to send to a device that's
* no longer accepting data.
*/
fifoBuf_clearData(&com_dev->tx);
return len;
}
if (len > fifoBuf_getFree(&com_dev->tx)) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len);
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 bytes_into_fifo;
}
/**
* Sends a package over given port
* \param[in] port COM port
@ -275,6 +313,8 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
* \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 -3 another thread is already sending, caller should
* retry until com is available again
* \return number of bytes transmitted on success
*/
int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len)
@ -285,39 +325,19 @@ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, u
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
PIOS_Assert(com_dev->has_tx);
if (com_dev->driver->available && !com_dev->driver->available(com_dev->lower_id)) {
/*
* Underlying device is down/unconnected.
* Dump our fifo contents and act like an infinite data sink.
* Failure to do this results in stale data in the fifo as well as
* possibly having the caller block trying to send to a device that's
* no longer accepting data.
*/
fifoBuf_clearData(&com_dev->tx);
return len;
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->sendbuffer_sem, 0) != pdTRUE) {
return -3;
}
if (len > fifoBuf_getFree(&com_dev->tx)) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len);
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 bytes_into_fifo;
#endif /* PIOS_INCLUDE_FREERTOS */
int32_t ret = PIOS_COM_SendBufferNonBlockingInternal(com_dev, buffer, len);
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreGive(com_dev->sendbuffer_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
return ret;
}
/**
* Sends a package over given port
* (blocking function)
@ -325,6 +345,7 @@ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, u
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return -2 if mutex can't be taken;
* \return number of bytes transmitted on success
*/
int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len)
@ -335,9 +356,12 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
PIOS_Assert(com_dev->has_tx);
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->sendbuffer_sem, 0) != pdTRUE) {
return -2;
}
#endif /* PIOS_INCLUDE_FREERTOS */
uint32_t max_frag_len = fifoBuf_getSize(&com_dev->tx);
uint32_t bytes_to_send = len;
while (bytes_to_send) {
@ -348,13 +372,16 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
} else {
frag_size = bytes_to_send;
}
int32_t rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, frag_size);
int32_t rc = PIOS_COM_SendBufferNonBlockingInternal(com_dev, buffer, frag_size);
if (rc >= 0) {
bytes_to_send -= rc;
buffer += rc;
} else {
switch (rc) {
case -1:
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreGive(com_dev->sendbuffer_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
/* Device is invalid, this will never work */
return -1;
@ -367,17 +394,23 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
}
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->tx_sem, 5000) != pdTRUE) {
xSemaphoreGive(com_dev->sendbuffer_sem);
return -3;
}
#endif
continue;
default:
/* Unhandled return code */
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreGive(com_dev->sendbuffer_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
return rc;
}
}
}
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreGive(com_dev->sendbuffer_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
return len;
}