diff --git a/flight/Libraries/fifo_buffer.c b/flight/Libraries/fifo_buffer.c new file mode 100644 index 000000000..d0fb42081 --- /dev/null +++ b/flight/Libraries/fifo_buffer.c @@ -0,0 +1,261 @@ +/** + ****************************************************************************** + * + * @file fifo_buffer.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief GPIO input 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 + */ + +#include + +#include "fifo_buffer.h" + +// ***************************************************************************** +// circular buffer functions + +uint16_t fifoBuf_getUsed(t_fifo_buffer *buf) +{ // return the number of bytes available in the rx buffer + + uint16_t rd = buf->rd; + uint16_t wr = buf->wr; + uint16_t buf_size = buf->buf_size; + + uint16_t num_bytes = wr - rd; + if (wr < rd) + num_bytes = (buf_size - rd) + wr; + + return num_bytes; +} + +uint16_t fifoBuf_getFree(t_fifo_buffer *buf) +{ // return the free space size in the buffer + + uint16_t buf_size = buf->buf_size; + + uint16_t num_bytes = fifoBuf_getUsed(buf); + + return ((buf_size - num_bytes) - 1); +} + +void fifoBuf_clearData(t_fifo_buffer *buf) +{ // remove all data from the buffer + buf->rd = buf->wr = 0; +} + +void fifoBuf_removeData(t_fifo_buffer *buf, uint16_t len) +{ // remove a number of bytes from the buffer + + uint16_t rd = buf->rd; + uint16_t buf_size = buf->buf_size; + + // get number of bytes available + uint16_t num_bytes = fifoBuf_getUsed(buf); + + if (num_bytes > len) + num_bytes = len; + + if (num_bytes < 1) + return; + + rd += num_bytes; + if (rd >= buf_size) + rd -= buf_size; + + buf->rd = rd; +} + +int16_t fifoBuf_getBytePeek(t_fifo_buffer *buf) +{ // get a data byte from the buffer without removing it + + uint16_t rd = buf->rd; + + // get number of bytes available + uint16_t num_bytes = fifoBuf_getUsed(buf); + + if (num_bytes < 1) + return -1; + + return buf->buffer[rd]; +} + +int16_t fifoBuf_getByte(t_fifo_buffer *buf) +{ // get a data byte from the buffer + + uint16_t rd = buf->rd; + uint16_t buf_size = buf->buf_size; + + // get number of bytes available + uint16_t num_bytes = fifoBuf_getUsed(buf); + + if (num_bytes < 1) + return -1; + + uint8_t b = buf->buffer[rd]; + if (++rd >= buf_size) + rd = 0; + + buf->rd = rd; + + return b; +} + +uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len) +{ // get data from the buffer without removing it + + uint16_t rd = buf->rd; + uint16_t wr = buf->wr; + uint16_t buf_size = buf->buf_size; + + // get number of bytes available + uint16_t num_bytes = fifoBuf_getUsed(buf); + + if (num_bytes > len) + num_bytes = len; + + if (num_bytes < 1) + return 0; // return number of bytes copied + + uint8_t *p = (uint8_t *)data; + uint16_t i = num_bytes; + + if (wr < rd) + { // we're going to do a rap-around + uint16_t j = buf_size - rd; + if (j > num_bytes) + j = num_bytes; + memcpy(p, (void *)&buf->buffer[rd], j); + p += j; + i -= j; + rd = 0; + } + if (i > 0) + { + memcpy(p, (void *)&buf->buffer[rd], i); + rd += i; + if (rd >= buf_size) + rd = 0; + } + + return num_bytes; // return number of bytes copied +} + +uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len) +{ // get data from our rx buffer + + uint16_t rd = buf->rd; + uint16_t wr = buf->wr; + uint16_t buf_size = buf->buf_size; + + // get number of bytes available + uint16_t num_bytes = fifoBuf_getUsed(buf); + + if (num_bytes > len) + num_bytes = len; + + if (num_bytes < 1) + return 0; // return number of bytes copied + + uint8_t *p = (uint8_t *)data; + uint16_t i = num_bytes; + + if (wr < rd) + { // we're going to do a rap-around + uint16_t j = buf_size - rd; + if (j > num_bytes) + j = num_bytes; + memcpy(p, (void *)&buf->buffer[rd], j); + p += j; + i -= j; + rd = 0; + } + if (i > 0) + { + memcpy(p, (void *)&buf->buffer[rd], i); + rd += i; + if (rd >= buf_size) + rd = 0; + } + + buf->rd = rd; + + return num_bytes; // return number of bytes copied +} + +uint16_t fifoBuf_putByte(t_fifo_buffer *buf, const uint8_t b) +{ // add a data byte to the buffer + + uint16_t wr = buf->wr; + uint16_t buf_size = buf->buf_size; + + uint16_t num_bytes = fifoBuf_getFree(buf); + if (num_bytes < 1) + return 0; + + buf->buffer[wr] = b; + if (++wr >= buf_size) + wr = 0; + + buf->wr = wr; + + return 1; +} + +uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len) +{ // add data to the buffer + + uint16_t wr = buf->wr; + uint16_t buf_size = buf->buf_size; + + uint16_t num_bytes = fifoBuf_getFree(buf); + if (num_bytes > len) + num_bytes = len; + + if (num_bytes < 1) + return 0; // return number of bytes copied + + uint8_t *p = (uint8_t *)data; + uint16_t i = 0; + + while (num_bytes > 0) + { + uint16_t j = buf_size - wr; + if (j > num_bytes) + j = num_bytes; + memcpy((void *)&buf->buffer[wr], p + i, j); + i += j; + num_bytes -= j; + wr += j; + if (wr >= buf_size) + wr = 0; + } + + buf->wr = wr; + + return i; +} + +void fifoBuf_init(t_fifo_buffer *buf) +{ + buf->rd = 0; + buf->wr = 0; + buf->buf_size = FIFO_BUFFER_SIZE; +} + +// ***************************************************************************** diff --git a/flight/Libraries/inc/fifo_buffer.h b/flight/Libraries/inc/fifo_buffer.h new file mode 100644 index 000000000..e03b24c11 --- /dev/null +++ b/flight/Libraries/inc/fifo_buffer.h @@ -0,0 +1,66 @@ +/** + ****************************************************************************** + * + * @file fifo_buffer.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief GPIO 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 _FIFO_BUFFER_H_ +#define _FIFO_BUFFER_H_ + +#include "stm32f10x.h" + +#define FIFO_BUFFER_SIZE 512 + +// ********************* + +typedef struct +{ + uint8_t buffer[FIFO_BUFFER_SIZE] __attribute__ ((aligned(4))); +// uint8_t buffer[FIFO_BUFFER_SIZE]; + volatile uint16_t rd; + volatile uint16_t wr; + uint16_t buf_size; +} t_fifo_buffer; + +// ********************* + +uint16_t fifoBuf_getUsed(t_fifo_buffer *buf); +uint16_t fifoBuf_getFree(t_fifo_buffer *buf); + +void fifoBuf_clearData(t_fifo_buffer *buf); +void fifoBuf_removeData(t_fifo_buffer *buf, uint16_t len); + +int16_t fifoBuf_getBytePeek(t_fifo_buffer *buf); +int16_t fifoBuf_getByte(t_fifo_buffer *buf); + +uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len); +uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len); + +uint16_t fifoBuf_putByte(t_fifo_buffer *buf, const uint8_t b); + +uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len); + +void fifoBuf_init(t_fifo_buffer *buf); + +// ********************* + +#endif diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index ca7fabf70..690d13509 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -223,6 +223,7 @@ SRC += $(FLIGHTLIB)/ahrs_spi_comm.c SRC += $(FLIGHTLIB)/ahrs_comm_objects.c ## Libraries for flight calculations SRC += $(FLIGHTLIB)/buffer.c +SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/CoordinateConversions.c diff --git a/flight/PiOS/STM32F10x/pios_usb_hid.c b/flight/PiOS/STM32F10x/pios_usb_hid.c index c8a1972b5..46fc89e17 100644 --- a/flight/PiOS/STM32F10x/pios_usb_hid.c +++ b/flight/PiOS/STM32F10x/pios_usb_hid.c @@ -36,7 +36,7 @@ #include "usb_lib.h" #include "pios_usb_hid_desc.h" #include "stm32f10x.h" -#include "buffer.h" +#include "fifo_buffer.h" #if defined(PIOS_INCLUDE_USB_HID) @@ -51,20 +51,12 @@ const struct pios_com_driver pios_usb_com_driver = { // so there isn't hte 64 byte cap in place by the USB interrupt packet definition /* Rx/Tx status */ -static volatile uint8_t rx_buffer_new_data_ctr = 0; -static volatile uint8_t rx_buffer_ix; static uint8_t transfer_possible = 0; -static uint8_t rx_buffer[PIOS_USB_HID_DATA_LENGTH + 2] = { 0 }; -static uint8_t tx_buffer[PIOS_USB_HID_DATA_LENGTH + 2] = { 0 }; +static uint8_t rx_packet_buffer[PIOS_USB_HID_DATA_LENGTH + 2] = { 0 }; +static uint8_t tx_packet_buffer[PIOS_USB_HID_DATA_LENGTH + 2] = { 0 }; -#define TX_BUFFER_SIZE 512 -#define RX_BUFFER_SIZE 512 -cBuffer rxBuffer; -cBuffer txBuffer; -static uint8_t rxBufferSpace[TX_BUFFER_SIZE]; -static uint8_t txBufferSpace[RX_BUFFER_SIZE]; - -volatile uint8_t in_irq = 0; +t_fifo_buffer rx_pios_fifo_buffer; +t_fifo_buffer tx_pios_fifo_buffer; /** * Initialises USB COM layer * \param[in] mode currently only mode 0 supported @@ -79,8 +71,8 @@ int32_t PIOS_USB_HID_Init(uint32_t mode) return -1; } - bufferInit(&rxBuffer, &rxBufferSpace[0], RX_BUFFER_SIZE); - bufferInit(&txBuffer, &txBufferSpace[0], TX_BUFFER_SIZE); + fifoBuf_init(&rx_pios_fifo_buffer); + fifoBuf_init(&tx_pios_fifo_buffer); PIOS_USB_HID_Reenumerate(); @@ -191,17 +183,17 @@ int32_t PIOS_USB_HID_CheckAvailable(uint8_t id) void sendChunk() { - uint32_t size = bufferBufferedData(&txBuffer); + uint32_t size = fifoBuf_getUsed(&tx_pios_fifo_buffer); if (size > 0) { if (size > PIOS_USB_HID_DATA_LENGTH) size = PIOS_USB_HID_DATA_LENGTH; #ifdef USB_HID - bufferGetChunkFromFront(&txBuffer, &tx_buffer[1], size + 1); - tx_buffer[0] = 1; /* report ID */ + fifoBuf_getData(&tx_pios_fifo_buffer, &tx_packet_buffer[1], size + 1); + tx_packet_buffer[0] = 1; /* report ID */ #else - bufferGetChunkFromFront(&txBuffer, &tx_buffer[2], size); - tx_buffer[0] = 1; /* report ID */ - tx_buffer[1] = size; /* valid data length */ + fifoBuf_getData(&tx_pios_fifo_buffer, &tx_packet_buffer[2], size); + tx_packet_buffer[0] = 1; /* report ID */ + tx_packet_buffer[1] = size; /* valid data length */ #endif /* Wait for any pending transmissions to complete */ @@ -211,7 +203,7 @@ void sendChunk() #endif } - UserToPMABufferCopy((uint8_t *) tx_buffer, GetEPTxAddr(EP1_IN & 0x7F), size + 2); + UserToPMABufferCopy((uint8_t *) tx_packet_buffer, GetEPTxAddr(EP1_IN & 0x7F), size + 2); SetEPTxCount((EP1_IN & 0x7F), PIOS_USB_HID_DATA_LENGTH + 2); /* Send Buffer */ @@ -236,20 +228,15 @@ int32_t PIOS_USB_HID_TxBufferPutMoreNonBlocking(uint8_t id, const uint8_t * buff if(!transfer_possible) return -1; - uint8_t previous_data = bufferBufferedData(&txBuffer); + uint8_t previous_data = fifoBuf_getUsed(&tx_pios_fifo_buffer); - if (len > bufferRemainingSpace(&txBuffer)) + if (len > fifoBuf_getFree(&tx_pios_fifo_buffer)) return -2; /* Cannot send all requested bytes */ - while(in_irq); - PIOS_IRQ_Disable(); - if(!in_irq) - ret = bufferAddChunkToEnd(&txBuffer, buffer, len); - else - ret = 0; - PIOS_IRQ_Enable(); - if (ret == 0) - return -2; + /* don't check returned bytes because it should always succeed */ + /* after previous thread and no meaningful way to deal with the */ + /* case it only buffers half the bytes */ + ret = fifoBuf_putData(&tx_pios_fifo_buffer, buffer, len); /* If no previous data queued and not sending, then TX complete interrupt not likely so send manually */ if (previous_data == 0 && GetEPTxStatus(ENDP1) != EP_TX_VALID) @@ -269,7 +256,7 @@ int32_t PIOS_USB_HID_TxBufferPutMoreNonBlocking(uint8_t id, const uint8_t * buff */ int32_t PIOS_USB_HID_TxBufferPutMore(uint8_t id, const uint8_t * buffer, uint16_t len) { - if(len > TX_BUFFER_SIZE) + if(len > (fifoBuf_getUsed(&tx_pios_fifo_buffer) + fifoBuf_getFree(&tx_pios_fifo_buffer))) return -1; uint32_t error; @@ -279,7 +266,7 @@ int32_t PIOS_USB_HID_TxBufferPutMore(uint8_t id, const uint8_t * buffer, uint16_ #endif } - return 0; + return error; } /** @@ -292,16 +279,13 @@ int32_t PIOS_USB_HID_RxBufferGet(uint8_t id) { uint8_t read; - while(in_irq); - PIOS_IRQ_Disable(); - if(!in_irq) - read = bufferGetFromFront(&rxBuffer); - else - read = -1; - PIOS_IRQ_Enable(); + if(fifoBuf_getUsed(&rx_pios_fifo_buffer) == 0) + return -1; + + read = fifoBuf_getByte(&rx_pios_fifo_buffer); // If endpoint was stalled and there is now space make it valid - if ((GetEPRxStatus(ENDP1) != EP_RX_VALID) && (bufferRemainingSpace(&rxBuffer) > 62)) { + if ((GetEPRxStatus(ENDP1) != EP_RX_VALID) && (fifoBuf_getFree(&rx_pios_fifo_buffer) > 62)) { SetEPRxStatus(ENDP1, EP_RX_VALID); } return read; @@ -315,7 +299,7 @@ int32_t PIOS_USB_HID_RxBufferGet(uint8_t id) */ int32_t PIOS_USB_HID_RxBufferUsed(uint8_t id) { - return bufferBufferedData(&rxBuffer); + return fifoBuf_getUsed(&rx_pios_fifo_buffer); } /** @@ -324,18 +308,14 @@ int32_t PIOS_USB_HID_RxBufferUsed(uint8_t id) */ void PIOS_USB_HID_EP1_IN_Callback(void) { - in_irq = 1; sendChunk(); - in_irq = 0; } -int hid_rec = 0; /** * EP1 OUT Callback Routine */ void PIOS_USB_HID_EP1_OUT_Callback(void) { - in_irq = 1; uint32_t DataLength = 0; /* Read received data (63 bytes) */ @@ -343,23 +323,21 @@ void PIOS_USB_HID_EP1_OUT_Callback(void) DataLength = GetEPRxCount(ENDP1 & 0x7F); /* Use the memory interface function to write to the selected endpoint */ - PMAToUserBufferCopy((uint8_t *) rx_buffer, GetEPRxAddr(ENDP1 & 0x7F), DataLength); + PMAToUserBufferCopy((uint8_t *) &rx_packet_buffer[0], GetEPRxAddr(ENDP1 & 0x7F), DataLength); /* The first byte is report ID (not checked), the second byte is the valid data length */ #ifdef USB_HID - bufferAddChunkToEnd(&rxBuffer, &rx_buffer[1], PIOS_USB_HID_DATA_LENGTH + 1); + fifoBuf_putData(&rx_pios_fifo_buffer, &rx_packet_buffer[1], PIOS_USB_HID_DATA_LENGTH + 1); #else - hid_rec += rx_buffer[1]; - bufferAddChunkToEnd(&rxBuffer, &rx_buffer[2], rx_buffer[1]); + fifoBuf_putData(&rx_pios_fifo_buffer, &rx_packet_buffer[2], rx_packet_buffer[1]); #endif // Only reactivate endpoint if available space in buffer - if (bufferRemainingSpace(&rxBuffer) > 62) { + if (fifoBuf_getFree(&rx_pios_fifo_buffer) > 62) { SetEPRxStatus(ENDP1, EP_RX_VALID); } else { SetEPRxStatus(ENDP1, EP_RX_NAK); } - in_irq = 0; } #endif diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 7c6cac16b..badad3def 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -38,6 +38,8 @@ 65322D77122897210046CD7C /* MagOrAccelSensorCal.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = MagOrAccelSensorCal.c; path = ../../AHRS/MagOrAccelSensorCal.c; sourceTree = SOURCE_ROOT; }; 654330231218E9780063F913 /* insgps.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps.c; path = ../../AHRS/insgps.c; sourceTree = SOURCE_ROOT; }; 6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = ""; }; + 6549E0D21279B3C800C5476F /* fifo_buffer.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = fifo_buffer.c; sourceTree = ""; }; + 6549E0D31279B3CF00C5476F /* fifo_buffer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = fifo_buffer.h; sourceTree = ""; }; 655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = ""; }; 65632CBF124E09D900469B77 /* guidance.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = guidance.c; sourceTree = ""; }; 65632CC1124E09D900469B77 /* guidance.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = guidance.h; sourceTree = ""; }; @@ -2984,6 +2986,7 @@ 657CEEB6121DBC63007A1FBE /* Libraries */ = { isa = PBXGroup; children = ( + 6549E0D21279B3C800C5476F /* fifo_buffer.c */, 651913371256C5240039C0A3 /* ahrs_comm_objects.c */, 651913381256C5240039C0A3 /* ahrs_spi_comm.c */, 651913391256C5240039C0A3 /* ahrs_spi_program_slave.c */, @@ -2999,6 +3002,7 @@ 657CEEB8121DBC63007A1FBE /* inc */ = { isa = PBXGroup; children = ( + 6549E0D31279B3CF00C5476F /* fifo_buffer.h */, 6519133A1256C52B0039C0A3 /* ahrs_comm_objects.h */, 6519133B1256C52B0039C0A3 /* ahrs_spi_comm.h */, 6519133C1256C52B0039C0A3 /* ahrs_spi_program_master.h */,