static void
circular_buffer_test(circular_buffer_t cb)
{
int i;
test_t t;
for (i = 0; i < 7; ++i) {
memset(t.buf, 0, sizeof(t.buf));
t.index = i + 1;
sprintf(t.buf, "the test buffer with id=>%d\n", t.index);
t.len = strlen(t.buf);
circular_buffer_put(cb, (const char*)&t, sizeof(t));
}
fprintf(stdout, "size : %d\n", circular_buffer_size(cb));
fprintf(stdout, "circular buffer get=>%d\n",
circular_buffer_get(cb, sizeof(t), (char*)&t));
fprintf(stdout, "size : %d\n", circular_buffer_size(cb));
test_show(t);
for (i = 0; i < 1; ++i) {
memset(t.buf, 0, sizeof(t.buf));
t.index = (i + 7) * (i + 7);
sprintf(t.buf, "the test buffer with id=>%d\n", t.index);
t.len = strlen(t.buf);
circular_buffer_put(cb, (const char*)&t, sizeof(t));
}
fprintf(stdout, "size : %d\n", circular_buffer_size(cb));
while (!circular_buffer_empty(cb)) {
circular_buffer_get(cb, sizeof(t), (char*)&t);
test_show(t);
}
fprintf(stdout, "size : %d\n", circular_buffer_size(cb));
}
void mesg_queue_destroy(mesg_queue * q) {
pthread_mutex_destroy(&q->lock);
uint32_t queued_entries = circular_buffer_size(q->buffer);
//TODO: Need better handling of the still queued entries
if(queued_entries>0) {
printf("%d entries still queued - unqueueing\n", (int)queued_entries);
for(int i=0; i<queued_entries; ++i) {
void * dummy=NULL;
circular_buffer_get(q->buffer, &dummy);
printf("unqueued %p\n", dummy);
}
}
circular_buffer_free(q->buffer);
free(q);
}
float ma_filter_add(ma_filter* filter, float input) {
uint16_t size = 0;
circular_buffer_size(filter->cb, &size);
//if size is less than depth, then only size of the circular buffer will be used to calculate the average
if (size < MA_FILTER_DEPTH) {
float sum = (filter->average) * size + input;
circular_buffer_append(filter->cb, &input);
filter->average = sum / (size + 1); //computing the average of the by taking the sum of elements in the filter depth and dividing it by depth continuously
return filter->average;
} else { //if size is greater than depth, then depth will be used to calculate the moving average
float sum = (filter->average) * MA_FILTER_DEPTH;
float removing = 1;
circular_buffer_remove_first(filter->cb, &removing);
circular_buffer_append(filter->cb, &input);
sum -= removing;
sum += input;
filter->average = sum / (float)MA_FILTER_DEPTH;
return filter->average;
}
}
void handle_spi_to_bbb(){
//Loop while we have data in the RX buffer to process
while(circular_buffer_size(&rx_buff)){
rx_byte = circular_buffer_pop(&rx_buff);
if(rx_byte == SPI_TX_START){
cmd_idx = CMD_DATA_SIZE;
//Reset all the send variables/tmp storage
cmd_finished = 0;
send_idx = 0;
send_crc_length = 0;
send_crc = 0;
send_crc_idx = 0;
spi_transfer = 1;
memset(&tx_buff, 0, sizeof(circular_buffer_t));
}
//If we are receiving command, store it appropriately
if(cmd_idx > 0){
cmd_data[CMD_DATA_SIZE-cmd_idx] = rx_byte;
cmd_idx--;
//Finished last storage of incoming data
if(cmd_idx == 0){
//Check recieved_crc against calculated CRC
received_crc = (cmd_data[CMD_DATA_SIZE-1]<<8) | cmd_data[CMD_DATA_SIZE-2];
calculated_crc = crc_io_checksum(cmd_data, CMD_DATA_SIZE-2, CRC_16BIT);
//Send appropriate signal if passed/failed
if(calculated_crc == received_crc){
SPIC.DATA = SPI_CRC_PASS;
circular_buffer_push(&tx_buff,SPI_CRC_PASS);
cmd_finished = 1;
}
else{
//SPIC.DATA = SPI_CRC_FAIL;
circular_buffer_push(&tx_buff,SPI_CRC_FAIL);
}
}
}
else if(cmd_finished){
recv_cmd = cmd_data[1];
memcpy(send_data,sensor_data,SENSOR_DATA_SIZE);
send_data[SENSOR_DATA_SIZE] = sensor_status;
send_data[SENSOR_DATA_SIZE+1] = state;
send_idx = SENSOR_DATA_SIZE+2;
send_crc_length = send_idx;
cmd_finished = 0;
while(send_idx){
//SPIC.DATA = send_data[send_crc_length-send_idx];
circular_buffer_push(&tx_buff, send_data[send_crc_length-send_idx]);
send_idx--;
//Calculate CRC
if(send_idx == 0){
send_crc = crc_io_checksum(send_data, send_crc_length, CRC_16BIT);
circular_buffer_push(&tx_buff, send_crc);
circular_buffer_push(&tx_buff, send_crc>> 8);
}
}
spi_transfer = 0;
}
spi_isr = 0;
}
