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

Fixed a bad bug in the fifo_getData functions.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2044 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
pip 2010-10-31 04:58:34 +00:00 committed by pip
parent d5752dff34
commit 9b22bb07e6
1 changed files with 20 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
flight/Libraries/fifo_buffer.c
View File

@ -131,8 +131,8 @@ 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;
uint8_t *buff = buf->buffer;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
@ -144,35 +144,30 @@ uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len)
return 0; // return number of bytes copied
uint8_t *p = (uint8_t *)data;
uint16_t i = num_bytes;
uint16_t i = 0;
if (wr < rd)
{ // we're going to do a rap-around
while (num_bytes > 0)
{
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;
memcpy(p + i, buff + rd, j);
i += j;
num_bytes -= j;
rd += j;
if (rd >= buf_size)
rd = 0;
}
return num_bytes; // return number of bytes copied
return i; // 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;
uint8_t *buff = buf->buffer;
// get number of bytes available
uint16_t num_bytes = fifoBuf_getUsed(buf);
@ -184,29 +179,24 @@ uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len)
return 0; // return number of bytes copied
uint8_t *p = (uint8_t *)data;
uint16_t i = num_bytes;
uint16_t i = 0;
if (wr < rd)
{ // we're going to do a rap-around
while (num_bytes > 0)
{
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;
memcpy(p + i, buff + rd, j);
i += j;
num_bytes -= j;
rd += j;
if (rd >= buf_size)
rd = 0;
}
buf->rd = rd;
return num_bytes; // return number of bytes copied
return i; // return number of bytes copied
}
uint16_t fifoBuf_putByte(t_fifo_buffer *buf, const uint8_t b)
@ -233,6 +223,7 @@ uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len)
uint16_t wr = buf->wr;
uint16_t buf_size = buf->buf_size;
uint8_t *buff = buf->buffer;
uint16_t num_bytes = fifoBuf_getFree(buf);
if (num_bytes > len)
@ -249,7 +240,7 @@ uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len)
uint16_t j = buf_size - wr;
if (j > num_bytes)
j = num_bytes;
memcpy((void *)&buf->buffer[wr], p + i, j);
memcpy(buff + wr, p + i, j);
i += j;
num_bytes -= j;
wr += j;