int testBuffer() {
printf("Ring buffer test:\n\n");
RingBuffer_t rb;
rbInit(&rb, 5);
printf("Is empty: %d\n", rbIsEmpty(&rb));
int i;
for(i = 1; i <= rb.size; i++) {
rbWrite(&rb, i);
}
printf("Is empty: %d\n", rbIsEmpty(&rb));
for(i = 0; i < rb.size; i++) {
printf("read value %d:%d\n", i, rbRead(&rb, i));
}
printf("\n");
rbWrite(&rb, rb.size+1);
for(i = 0; i < rb.size; i++) {
printf("read value %d:%d\n", i, rbRead(&rb, i));
}
printf("\nfree\n");
rbFree(&rb);
printf("Is empty: %d\n", rbIsEmpty(&rb));
return 0;
}
int uart0RecvChar (void) {
int result = -1;
LPC_USART0->INTENCLR = UART_INTEN_RXRDY;
if (!rbIsEmpty(&rxBuf))
result = rbPop(&rxBuf);
LPC_USART0->INTENSET = UART_INTEN_RXRDY;
return result;
}
void UART0_IRQHandler () {
uint32_t stat = LPC_USART0->STAT;
if (stat & UART_STATUS_RXRDY)
rbAdd(&rxBuf, LPC_USART0->RXDATA);
if (stat & UART_STATUS_TXRDY) {
if (rbIsEmpty(&txBuf))
LPC_USART0->INTENCLR = UART_INTEN_TXRDY;
else
LPC_USART0->TXDATA = rbPop(&txBuf);
}
}
void ahrs_Update(void)
{
/*从RingBuff_A提取数据*/
{
if(m_ahrs_RX_RingBuff_A.flagOverflow==1)
{
rbClear(&m_ahrs_RX_RingBuff_A);
}
while( !rbIsEmpty(&m_ahrs_RX_RingBuff_A) )
{
uint8_t flagClear = ERROR;
uint8_t cur = rbPop(&m_ahrs_RX_RingBuff_A);
if( m_ahrs_RX_WorkingBuff_A_DataCount == 0 )
{
m_ahrs_RX_WorkingBuff_A[0] = m_ahrs_RX_WorkingBuff_A[1];
m_ahrs_RX_WorkingBuff_A[1] = cur;
if((m_ahrs_RX_WorkingBuff_A[0]==START_BYTE_AG_A) && (m_ahrs_RX_WorkingBuff_A[1]==START_BYTE_AG_B))
m_ahrs_RX_WorkingBuff_A_DataCount = 2;
}
else {
m_ahrs_RX_WorkingBuff_A[m_ahrs_RX_WorkingBuff_A_DataCount++] = cur;
}
if( m_ahrs_RX_WorkingBuff_A_DataCount == PACKETSIZE )
{
//if( m_ahrs_RX_WorkingBuff[PACKETSIZE-1] == END_BYTE )
//{
ahrs_Decode_A();
flagClear = SUCCESS;
//}
//else {
// flagClear = SUCCESS;
//}
}
m_ahrs_RX_PreviousValue_A = cur;
// buffer clear & buffer over-run prevention
if( flagClear || (m_ahrs_RX_WorkingBuff_A_DataCount >= sizeof(m_ahrs_RX_WorkingBuff_A)) )
{
m_ahrs_RX_WorkingBuff_A_DataCount = 0;
memset(m_ahrs_RX_WorkingBuff_A,0,PACKETSIZE*sizeof(uint8_t)); //WorkingBuff清零
}
}
}
/*从RingBuff_B提取数据*/
{
if(m_ahrs_RX_RingBuff_B.flagOverflow==1)
{
rbClear(&m_ahrs_RX_RingBuff_B);
}
while( !rbIsEmpty(&m_ahrs_RX_RingBuff_B) )
{
uint8_t flagClear = ERROR;
uint8_t cur = rbPop(&m_ahrs_RX_RingBuff_B);
if( m_ahrs_RX_WorkingBuff_B_DataCount == 0 )
{
m_ahrs_RX_WorkingBuff_B[0] = m_ahrs_RX_WorkingBuff_B[1];
m_ahrs_RX_WorkingBuff_B[1] = cur;
if((m_ahrs_RX_WorkingBuff_B[0]==START_BYTE_AG_A) && (m_ahrs_RX_WorkingBuff_B[1]==START_BYTE_AG_B))
m_ahrs_RX_WorkingBuff_B_DataCount = 2;
}
else {
m_ahrs_RX_WorkingBuff_B[m_ahrs_RX_WorkingBuff_B_DataCount++] = cur;
}
if( m_ahrs_RX_WorkingBuff_B_DataCount == PACKETSIZE )
{
//if( m_ahrs_RX_WorkingBuff[PACKETSIZE-1] == END_BYTE )
//{
ahrs_Decode_B();
flagClear = SUCCESS;
//}
//else {
// flagClear = SUCCESS;
//}
}
m_ahrs_RX_PreviousValue_B = cur;
// buffer clear & buffer over-run prevention
if( flagClear || (m_ahrs_RX_WorkingBuff_B_DataCount >= sizeof(m_ahrs_RX_WorkingBuff_B)) )
{
m_ahrs_RX_WorkingBuff_B_DataCount = 0;
memset(m_ahrs_RX_WorkingBuff_B,0,PACKETSIZE*sizeof(uint8_t)); //WorkingBuff清零
}
}
}
/*从RingBuff_C提取数据*/
{
if(m_ahrs_RX_RingBuff_C.flagOverflow==1)
{
rbClear(&m_ahrs_RX_RingBuff_C);
}
while( !rbIsEmpty(&m_ahrs_RX_RingBuff_C) )
{
uint8_t flagClear = ERROR;
uint8_t cur = rbPop(&m_ahrs_RX_RingBuff_C);
if( m_ahrs_RX_WorkingBuff_C_DataCount == 0 )
{
m_ahrs_RX_WorkingBuff_C[0] = m_ahrs_RX_WorkingBuff_C[1];
m_ahrs_RX_WorkingBuff_C[1] = cur;
if((m_ahrs_RX_WorkingBuff_C[0]==START_BYTE_AG_A) && (m_ahrs_RX_WorkingBuff_C[1]==START_BYTE_AG_B))
m_ahrs_RX_WorkingBuff_C_DataCount = 2;
}
else {
m_ahrs_RX_WorkingBuff_C[m_ahrs_RX_WorkingBuff_C_DataCount++] = cur;
}
if( m_ahrs_RX_WorkingBuff_C_DataCount == PACKETSIZE )
{
//if( m_ahrs_RX_WorkingBuff[PACKETSIZE-1] == END_BYTE )
//{
ahrs_Decode_C();
flagClear = SUCCESS;
//}
//else {
// flagClear = SUCCESS;
//}
}
m_ahrs_RX_PreviousValue_C = cur;
// buffer clear & buffer over-run prevention
if( flagClear || (m_ahrs_RX_WorkingBuff_C_DataCount >= sizeof(m_ahrs_RX_WorkingBuff_C)) )
{
m_ahrs_RX_WorkingBuff_C_DataCount = 0;
memset(m_ahrs_RX_WorkingBuff_C,0,PACKETSIZE*sizeof(uint8_t)); //WorkingBuff清零
}
}
}
}
uint8_t ahrs_IsReadyToRead(void)
{
u8 temp = 0;
temp = (!rbIsEmpty(&m_ahrs_RX_RingBuff_A))|(!rbIsEmpty(&m_ahrs_RX_RingBuff_B))|(!rbIsEmpty(&m_ahrs_RX_RingBuff_C));
return temp;
}
