diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index 55fb1f1e4..7c1416e81 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -415,9 +415,9 @@ 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... - int32_t ret = -2; + int32_t ret = -2; uint8_t count = 5; - while(count-- > 0 && ret < -1){ + while (count-- > 0 && ret < -1) { ret = PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process); } } @@ -515,9 +515,9 @@ 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... - int32_t ret = -2; + int32_t ret = -2; uint8_t count = 5; - while(count-- > 0 && ret < -1){ + while (count-- > 0 && ret < -1) { PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process); } } @@ -551,7 +551,7 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) // it is the caller responsibility to retry in such cases... ret = -2; uint8_t count = 5; - while(count-- > 0 && ret < -1){ + while (count-- > 0 && ret < -1) { ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); } } else { @@ -579,9 +579,9 @@ 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... - int32_t ret = -2; + int32_t ret = -2; uint8_t count = 5; - while(count-- > 0 && ret < -1){ + while (count-- > 0 && ret < -1) { ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); } return ret; diff --git a/flight/pios/common/pios_com.c b/flight/pios/common/pios_com.c index 31f67176a..e5adc5b7c 100644 --- a/flight/pios/common/pios_com.c +++ b/flight/pios/common/pios_com.c @@ -274,35 +274,35 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) 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)); + 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; } -} -return bytes_into_fifo; + + 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; } /**