From 2ed80f9c83007c368a3b1a56c0f202ebe941f6cd Mon Sep 17 00:00:00 2001
From: Brendan Bartels <bbartels@iastate.edu>
Date: Mon, 23 Jan 2017 20:06:26 -0600
Subject: [PATCH] quad: cleanup circular buffer and add regression tests
---
quad/sw/modular_quad_pid/src/commands.c | 8 +-
quad/sw/modular_quad_pid/src/communication.c | 2 +-
quad/sw/modular_quad_pid/src/uart_buff.c | 249 +++++++++---
quad/sw/modular_quad_pid/src/uart_buff.h | 11 +-
quad/sw/modular_quad_pid/test/.gitignore | 3 +
quad/sw/modular_quad_pid/test/Makefile | 7 +
.../sw/modular_quad_pid/test/test_uart_buff.c | 370 ++++++++++++++++++
quad/sw/modular_quad_pid/test/xil_types.h | 10 +
8 files changed, 593 insertions(+), 67 deletions(-)
create mode 100644 quad/sw/modular_quad_pid/test/.gitignore
create mode 100644 quad/sw/modular_quad_pid/test/Makefile
create mode 100644 quad/sw/modular_quad_pid/test/test_uart_buff.c
create mode 100644 quad/sw/modular_quad_pid/test/xil_types.h
diff --git a/quad/sw/modular_quad_pid/src/commands.c b/quad/sw/modular_quad_pid/src/commands.c
index 36ce1e9b..a57f6fef 100644
--- a/quad/sw/modular_quad_pid/src/commands.c
+++ b/quad/sw/modular_quad_pid/src/commands.c
@@ -456,17 +456,17 @@ int beginupdate(modular_structs_t *structs) {
int logdata(modular_structs_t *structs)
{
- char *packet = uart_buff_get_packet();
+ size_t length;
+ unsigned char *packet = uart_buff_get_raw(&length);
printf("Logging: %s\n", packet);
- free(packet);
return 0;
}
int response(modular_structs_t *structs)
{
- char *packet = uart_buff_get_packet();
+ size_t length;
+ char *packet = uart_buff_get_raw(&length);
printf("This is the response: %s\n", packet);
- free(packet);
return 0;
}
diff --git a/quad/sw/modular_quad_pid/src/communication.c b/quad/sw/modular_quad_pid/src/communication.c
index c4e166b2..05e06fd6 100644
--- a/quad/sw/modular_quad_pid/src/communication.c
+++ b/quad/sw/modular_quad_pid/src/communication.c
@@ -146,7 +146,7 @@ void uart_interrupt_handler(XUartPs *InstancePtr) {
while (0 == (CsrRegister & XUARTPS_SR_RXEMPTY) && !uart_buff_full()) {
u8 byte = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
- uart_buff_put_byte(byte);
+ uart_buff_add_u8(byte);
CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_SR_OFFSET);
}
diff --git a/quad/sw/modular_quad_pid/src/uart_buff.c b/quad/sw/modular_quad_pid/src/uart_buff.c
index a7a28e50..0954fc9e 100644
--- a/quad/sw/modular_quad_pid/src/uart_buff.c
+++ b/quad/sw/modular_quad_pid/src/uart_buff.c
@@ -4,26 +4,43 @@
#define UART_MAX_BUFF_SIZE 2048
#define UART_MAX_PACKET_SIZE 256
+#define UART_PACKET_HEADER_SIZE 7
+
+// function prototype declarations specific to the circular buffer
+void uart_buff_scan_packet();
+void uart_buff_scan_packet_start();
+void uart_buff_scan_packet_size();
+size_t uart_buff_packet_size();
+void uart_buff_reset_flags();
+size_t uart_buff_calc_index(int);
static volatile u8 buff[UART_MAX_BUFF_SIZE];
-static size_t start = UART_MAX_BUFF_SIZE;
-static size_t packet_data_length = 0;
+u8 packet_buff[UART_MAX_PACKET_SIZE];
+
+static size_t start = 0;
static volatile size_t end = 0;
+
+static size_t packet_data_length = 0;
static u8 packet_start_found = 0;
static u8 packet_size_found = 0;
static u8 packet_ready = 0;
+
+// TODO: Delete these debugging variables
static u32 packets_processed = 0;
/**
* Put a byte into the buffer.
*/
-void uart_buff_put_byte(u8 c) {
+void uart_buff_add_u8(u8 c) {
if (uart_buff_full()) {
return;
}
buff[end] = c;
- end = uart_buff_calc_index(end + 1);
+ end += 1;
+ if (end >= UART_MAX_BUFF_SIZE) {
+ end -= UART_MAX_BUFF_SIZE;
+ }
}
/**
@@ -62,7 +79,7 @@ void uart_buff_scan_packet_start() {
* Scan for the packet size inside the packet.
*/
void uart_buff_scan_packet_size() {
- if (uart_buff_size() < 7) {
+ if (uart_buff_size() < UART_PACKET_HEADER_SIZE) {
// haven't received the "length" bytes yet
return;
}
@@ -89,32 +106,72 @@ int uart_buff_packet_ready() {
/**
* Retrieve a 8-bit from the buffer according to the given offset with respect
- * to the start index of the buffer.
+ * to the start index of the current packet.
*/
u8 uart_buff_get_u8(size_t offset) {
- size_t target = uart_buff_calc_index(start + offset);
+ size_t index = start + offset;
- return buff[target];
+ if (index >= UART_MAX_BUFF_SIZE) {
+ index -= UART_MAX_BUFF_SIZE;
+ }
+
+ return buff[index];
}
/**
- * Retrieve a 16-bit unsigned integer from the buffer according to the given
- * offset with respect to the start index of the buffer.
+ * Retrieve a 16-bit unsigned integer from the buffer according to the given
+ * offset with respect to the start index of the current packet.
*/
u16 uart_buff_get_u16(size_t offset) {
- return uart_buff_get_u8(offset + 1) << 8 |
- uart_buff_get_u8(offset);
+ size_t index_1 = start + offset + 1;
+ size_t index = start + offset;
+
+ // Check common cases first
+ if (index_1 < UART_MAX_BUFF_SIZE) {
+ // in range, no need to correct indices
+ }
+ else if (index >= UART_MAX_BUFF_SIZE) {
+ index_1 -= UART_MAX_BUFF_SIZE;
+ index -= UART_MAX_BUFF_SIZE;
+ }
+ else {
+ index_1 -= UART_MAX_BUFF_SIZE;
+ }
+
+ return buff[index_1] << 8
+ | buff[index];
}
/**
* Retrieve a 32-bit from the buffer according to the given offset with respect
- * to the start index of the buffer.
+ * to the start index of the current packet.
*/
u32 uart_buff_get_u32(size_t offset) {
- return uart_buff_get_u8(offset + 3) << 24 |
- uart_buff_get_u8(offset + 2) << 16 |
- uart_buff_get_u8(offset + 1) << 8 |
- uart_buff_get_u8(offset);
+ size_t index_3 = start + offset + 3;
+ size_t index_2 = start + offset + 2;
+ size_t index_1 = start + offset + 1;
+ size_t index = start + offset;
+
+ if (index_3 < UART_MAX_BUFF_SIZE) {
+ // in range, no need to correct indices
+ }
+ else if (index >= UART_MAX_BUFF_SIZE) {
+ index_3 -= UART_MAX_BUFF_SIZE;
+ index_2 -= UART_MAX_BUFF_SIZE;
+ index_1 -= UART_MAX_BUFF_SIZE;
+ index -= UART_MAX_BUFF_SIZE;
+ }
+ else {
+ index_3 = uart_buff_calc_index(index_3);
+ index_2 = uart_buff_calc_index(index_2);
+ index_1 = uart_buff_calc_index(index_1);
+ index = uart_buff_calc_index(index);
+ }
+
+ return buff[index_3] << 24
+ | buff[index_2] << 16
+ | buff[index_1] << 8
+ | buff[index];
}
/**
@@ -125,7 +182,13 @@ u32 uart_buff_get_u32(size_t offset) {
* This has undefined behavior if a packet is not ready.
*/
u8 uart_buff_data_get_u8(size_t offset) {
- return uart_buff_get_u8(7 + offset);
+ size_t index = start + UART_PACKET_HEADER_SIZE + offset;
+
+ if (index >= UART_MAX_BUFF_SIZE) {
+ index -= UART_MAX_BUFF_SIZE;
+ }
+
+ return buff[index];
}
/**
@@ -136,7 +199,23 @@ u8 uart_buff_data_get_u8(size_t offset) {
* This has undefined behavior if a packet is not ready.
*/
u16 uart_buff_data_get_u16(size_t offset) {
- return uart_buff_get_u16(7 + offset);
+ size_t index_1 = start + UART_PACKET_HEADER_SIZE + offset + 1;
+ size_t index = start + UART_PACKET_HEADER_SIZE + offset;
+
+ // Check common cases first
+ if (index_1 < UART_MAX_BUFF_SIZE) {
+ // in range, no need to correct indices
+ }
+ else if (index >= UART_MAX_BUFF_SIZE) {
+ index_1 -= UART_MAX_BUFF_SIZE;
+ index -= UART_MAX_BUFF_SIZE;
+ }
+ else {
+ index_1 -= UART_MAX_BUFF_SIZE;
+ }
+
+ return buff[index_1] << 8
+ | buff[index];
}
/**
@@ -147,7 +226,31 @@ u16 uart_buff_data_get_u16(size_t offset) {
* This has undefined behavior if a packet is not ready.
*/
u32 uart_buff_data_get_u32(size_t offset) {
- return uart_buff_get_u32(7 + offset);
+ size_t index_3 = start + UART_PACKET_HEADER_SIZE + offset + 3;
+ size_t index_2 = start + UART_PACKET_HEADER_SIZE + offset + 2;
+ size_t index_1 = start + UART_PACKET_HEADER_SIZE + offset + 1;
+ size_t index = start + UART_PACKET_HEADER_SIZE + offset;
+
+ if (index_3 < UART_MAX_BUFF_SIZE) {
+ // in range, no need to correct indices
+ }
+ else if (index >= UART_MAX_BUFF_SIZE) {
+ index_3 -= UART_MAX_BUFF_SIZE;
+ index_2 -= UART_MAX_BUFF_SIZE;
+ index_1 -= UART_MAX_BUFF_SIZE;
+ index -= UART_MAX_BUFF_SIZE;
+ }
+ else {
+ index_3 = uart_buff_calc_index(index_3);
+ index_2 = uart_buff_calc_index(index_2);
+ index_1 = uart_buff_calc_index(index_1);
+ index = uart_buff_calc_index(index);
+ }
+
+ return buff[index_3] << 24
+ | buff[index_2] << 16
+ | buff[index_1] << 8
+ | buff[index];
}
/**
@@ -158,15 +261,38 @@ u32 uart_buff_data_get_u32(size_t offset) {
* This has undefined behavior if a packet is not ready.
*/
float uart_buff_data_get_float(size_t offset) {
- union {
- float f;
- int i;
- } x;
- x.i = uart_buff_data_get_u8(offset + 3) << 24 |
- uart_buff_data_get_u8(offset + 2) << 16 |
- uart_buff_data_get_u8(offset + 1) << 8 |
- uart_buff_data_get_u8(offset);
- return x.f;
+ size_t index_3 = start + UART_PACKET_HEADER_SIZE + offset + 3;
+ size_t index_2 = start + UART_PACKET_HEADER_SIZE + offset + 2;
+ size_t index_1 = start + UART_PACKET_HEADER_SIZE + offset + 1;
+ size_t index = start + UART_PACKET_HEADER_SIZE + offset;
+
+ if (index_3 < UART_MAX_BUFF_SIZE) {
+ // in range, no need to correct indices
+ }
+ else if (index >= UART_MAX_BUFF_SIZE) {
+ index_3 -= UART_MAX_BUFF_SIZE;
+ index_2 -= UART_MAX_BUFF_SIZE;
+ index_1 -= UART_MAX_BUFF_SIZE;
+ index -= UART_MAX_BUFF_SIZE;
+ }
+ else {
+ index_3 = uart_buff_calc_index(index_3);
+ index_2 = uart_buff_calc_index(index_2);
+ index_1 = uart_buff_calc_index(index_1);
+ index = uart_buff_calc_index(index);
+ }
+
+ union {
+ float f;
+ int i;
+ } x;
+
+ x.i = buff[index_3] << 24
+ | buff[index_2] << 16
+ | buff[index_1] << 8
+ | buff[index];
+
+ return x.f;
}
/**
@@ -178,7 +304,7 @@ void uart_buff_consume_packet() {
return;
}
- start = uart_buff_calc_index(start + uart_buff_packet_size() + 1);
+ start = uart_buff_calc_index(start + uart_buff_packet_size());
uart_buff_reset_flags();
packets_processed += 1;
}
@@ -187,7 +313,11 @@ void uart_buff_consume_packet() {
* Return the current number of bytes held in the buffer.
*/
size_t uart_buff_size() {
- return uart_buff_calc_index(end - start - 1);
+ int size = end - start;
+ if (size < 0) {
+ size += UART_MAX_BUFF_SIZE;
+ }
+ return size;
}
/**
@@ -200,8 +330,9 @@ size_t uart_buff_data_length() {
/**
* Return the size of the current packet.
*/
+inline
size_t uart_buff_packet_size() {
- return 1 + 1 + 1 + 2 + packet_data_length + 1;
+ return UART_PACKET_HEADER_SIZE + packet_data_length + 1;
}
/**
@@ -217,15 +348,18 @@ void uart_buff_reset_flags() {
* Return 1 if the buffer is empty, 0 otherwise.
*/
int uart_buff_empty() {
- int effective_end = uart_buff_calc_index(end - 1);
- return start == effective_end;
+ return start == end;
}
/**
* Return 1 if the buffer is full, 0 otherwise.
*/
int uart_buff_full() {
- return start == end;
+ int effective_end = end + 1;
+ if (effective_end >= UART_MAX_BUFF_SIZE) {
+ effective_end -= UART_MAX_BUFF_SIZE;
+ }
+ return start == effective_end;
}
/**
@@ -234,9 +368,6 @@ int uart_buff_full() {
*/
size_t uart_buff_calc_index(int given_index) {
int actual_index = given_index;
- if (actual_index < 0) {
- actual_index += UART_MAX_BUFF_SIZE;
- }
if (actual_index >= UART_MAX_BUFF_SIZE) {
actual_index -= UART_MAX_BUFF_SIZE;
}
@@ -244,20 +375,16 @@ size_t uart_buff_calc_index(int given_index) {
}
/**
- * Return a string pointer to the start of the data section.
- *
- * ALERT! Consumers of this function must free the pointer they received,
- * or else they will introduce a memory leak!
+ * Return a pointer to the start of the packet.
*/
-char * uart_buff_get_packet() {
- size_t packet_size = uart_buff_packet_size();
- char *str = malloc(sizeof(char) * (packet_size + 1));
- str[packet_size + 1] = '\0'; // null terminate string
- int i;
- for (i = 0; i < packet_size; i += 1) {
- str[i] = uart_buff_get_u8(i);
- }
- return str;
+char * uart_buff_get_raw(size_t *packet_size) {
+ // Copy packet into temp buffer
+ *packet_size = uart_buff_packet_size();
+ int i;
+ for (i = 0; i < *packet_size; i += 1) {
+ packet_buff[i] = uart_buff_get_u8(i);
+ }
+ return packet_buff;
}
/**
@@ -265,6 +392,8 @@ char * uart_buff_get_packet() {
*/
void uart_buff_print() {
size_t packet_size = uart_buff_packet_size();
+ size_t data_start = uart_buff_calc_index(start + UART_PACKET_HEADER_SIZE);
+ size_t data_end = uart_buff_calc_index(start + packet_size);
int i;
printf("buffer size = %zu\n", uart_buff_size());
puts("Buffer:");
@@ -277,10 +406,10 @@ void uart_buff_print() {
printf("<-- start");
}
if (packet_ready) {
- if (i == uart_buff_calc_index(start + 7)) {
+ if (i == data_start) {
printf("<-- data start");
}
- if (i == uart_buff_calc_index(start + packet_size)) {
+ if (i == data_end) {
printf("<-- data end");
}
}
@@ -293,6 +422,18 @@ void uart_buff_print() {
puts("'--'");
}
+/**
+ * Reset the state of the buffer.
+ */
+void uart_buff_reset() {
+ start = 0;
+ end = 0;
+ uart_buff_reset_flags();
+}
+
+/**
+ * Return the total number of packets processed.
+ */
u32 uart_buff_packets_processed() {
- return packets_processed;
+ return packets_processed;
}
diff --git a/quad/sw/modular_quad_pid/src/uart_buff.h b/quad/sw/modular_quad_pid/src/uart_buff.h
index 58dfa004..4cce9f76 100644
--- a/quad/sw/modular_quad_pid/src/uart_buff.h
+++ b/quad/sw/modular_quad_pid/src/uart_buff.h
@@ -3,10 +3,7 @@
#include "xil_types.h"
-void uart_buff_put_byte(u8);
-void uart_buff_scan_packet();
-void uart_buff_scan_packet_start();
-void uart_buff_scan_packet_size();
+void uart_buff_add_u8(u8);
int uart_buff_packet_ready();
u8 uart_buff_get_u8(size_t);
u16 uart_buff_get_u16(size_t);
@@ -18,13 +15,11 @@ float uart_buff_data_get_float(size_t);
void uart_buff_consume_packet();
size_t uart_buff_size();
size_t uart_buff_data_length();
-size_t uart_buff_packet_size();
-void uart_buff_reset_flags();
int uart_buff_empty();
int uart_buff_full();
-size_t uart_buff_calc_index(int);
-char * uart_buff_get_packet();
+char * uart_buff_get_raw(size_t *);
void uart_buff_print();
u32 uart_buff_packets_processed();
+void uart_buff_reset();
#endif
diff --git a/quad/sw/modular_quad_pid/test/.gitignore b/quad/sw/modular_quad_pid/test/.gitignore
new file mode 100644
index 00000000..7f3f7940
--- /dev/null
+++ b/quad/sw/modular_quad_pid/test/.gitignore
@@ -0,0 +1,3 @@
+uart_buff.c
+uart_buff.h
+test
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/test/Makefile b/quad/sw/modular_quad_pid/test/Makefile
new file mode 100644
index 00000000..5b6f1828
--- /dev/null
+++ b/quad/sw/modular_quad_pid/test/Makefile
@@ -0,0 +1,7 @@
+.PHONY: test
+test:
+ cp ../src/uart_buff.c ../src/uart_buff.h ./
+ gcc -g -o test test_uart_buff.c uart_buff.c
+
+clean:
+ rm *.o rm *.gch
diff --git a/quad/sw/modular_quad_pid/test/test_uart_buff.c b/quad/sw/modular_quad_pid/test/test_uart_buff.c
new file mode 100644
index 00000000..cf2a27da
--- /dev/null
+++ b/quad/sw/modular_quad_pid/test/test_uart_buff.c
@@ -0,0 +1,370 @@
+#include <stdio.h>
+#include "uart_buff.h"
+#include <math.h>
+#include <string.h>
+
+int main() {
+ int failed = 0;
+ failed += test_empty_when_empty();
+ failed += test_empty_after_receiving_some_data();
+ failed += test_full_is_false_at_start();
+ failed += test_full_after_receiving_some_data();
+ failed += test_packet_get_u8();
+ failed += test_packet_get_u8_with_offset();
+ failed += test_packet_get_u16();
+ failed += test_packet_get_u16_with_offset();
+ failed += test_packet_get_u16_wrapped();
+ failed += test_packet_get_u32();
+ failed += test_packet_get_u32_with_offset();
+ failed += test_packet_get_u32_wrapped_1_4();
+ failed += test_packet_get_u32_wrapped_2_4();
+ failed += test_packet_get_u32_wrapped_3_4();
+ failed += test_packet_get_float();
+ failed += test_packet_get_float_with_offset();
+ failed += test_buffer_size_after_VRPN_packet();
+ failed += test_buffer_size_empty();
+ failed += test_buffer_size_after_garbage_data();
+ failed += test_buffer_size_after_garbage_data_scanned();
+ failed += test_packet_ready_at_start();
+ failed += test_packet_ready_after_receiving_packet();
+ failed += test_packet_ready_after_consuming_packet();
+ failed += test_get_raw();
+
+ printf("Total tests failed: %d\n", failed);
+
+ return 0;
+}
+
+int float_equals(float x1, float x2) {
+ return fabs(x1 - x2) < 10e-5;
+}
+
+void print_test_result(int success, float exp, float act) {
+ if (success) printf("passed\n");
+ else printf("FAILED: expected %f but got %f\n", exp, act);
+}
+
+int failed(char *msg) {
+ printf("%s\n", msg);
+ return 1;
+}
+
+void add_VRPN_packet() {
+ uart_buff_add_u8(0xBE);
+ uart_buff_add_u8(0x04);
+ uart_buff_add_u8(0x00);
+ uart_buff_add_u8(0x00);
+ uart_buff_add_u8(0x00);
+ uart_buff_add_u8(24);
+ uart_buff_add_u8(0x00);
+ float arr[6] = {1.0, 1.2, 1.4, -1.5, -0.5, -1.1};
+ unsigned char *data = (unsigned char *) &arr;
+ int i;
+ for (i = 0; i < 24; i += 1) {
+ uart_buff_add_u8(data[i]);
+ }
+ uart_buff_add_u8(0x00);
+}
+
+void add_basic_packet() {
+ uart_buff_add_u8(0xBE);
+ uart_buff_add_u8(0x04);
+ uart_buff_add_u8(0x00);
+ uart_buff_add_u8(0x00);
+ uart_buff_add_u8(0x00);
+ uart_buff_add_u8(6);
+ uart_buff_add_u8(0x00);
+ unsigned char data[6] = {0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF};
+ int i;
+ for (i = 0; i < 6; i += 1) {
+ uart_buff_add_u8(data[i]);
+ }
+ uart_buff_add_u8(0x00);
+}
+
+void add_garbage_data() {
+ int i;
+ for (i = 0; i < 32; i += 1) {
+ uart_buff_add_u8(i);
+ }
+}
+
+
+int setup_VRPN_packet() {
+ add_VRPN_packet();
+ if (!uart_buff_packet_ready()) {
+ return 0;
+ }
+}
+
+int setup_basic_packet() {
+ add_basic_packet();
+ if (!uart_buff_packet_ready()) {
+ return 0;
+ }
+}
+
+int test_empty_when_empty() {
+ uart_buff_reset();
+ int exp = 1;
+ int act = uart_buff_empty();
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_empty_after_receiving_some_data() {
+ uart_buff_reset();
+ add_garbage_data();
+ int exp = 0;
+ int act = uart_buff_empty();
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_full_is_false_at_start() {
+ uart_buff_reset();
+ int exp = 0;
+ int act = uart_buff_full();
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_full_after_receiving_some_data() {
+ uart_buff_reset();
+ add_garbage_data();
+ int exp = 0;
+ int act = uart_buff_full();
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_buffer_size_empty() {
+ uart_buff_reset();
+ int exp = 0;
+ int act = uart_buff_size();
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_buffer_size_after_garbage_data() {
+ uart_buff_reset();
+ add_garbage_data();
+ int exp = 32;
+ int act = uart_buff_size();
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_buffer_size_after_garbage_data_scanned() {
+ uart_buff_reset();
+ add_garbage_data();
+ uart_buff_packet_ready();
+ int exp = 0;
+ int act = uart_buff_size();
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_buffer_size_after_VRPN_packet() {
+ uart_buff_reset();
+ if(!setup_VRPN_packet()) return failed("FAILED: setup failed");
+ int exp = 32;
+ int act = uart_buff_size();
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_packet_get_u8() {
+ uart_buff_reset();
+ if(!setup_basic_packet()) return failed("FAILED: setup failed");
+ int exp = 0xAA;
+ int act = uart_buff_data_get_u8(0);
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_packet_get_u8_with_offset() {
+ uart_buff_reset();
+ if(!setup_basic_packet()) return failed("FAILED: setup failed");
+ int exp = 0xBB;
+ int act = uart_buff_data_get_u8(1);
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_packet_get_u16() {
+ uart_buff_reset();
+ if(!setup_basic_packet()) return failed("FAILED: setup failed");
+ int exp = 0xBBAA;
+ int act = uart_buff_data_get_u16(0);
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_packet_get_u16_with_offset() {
+ uart_buff_reset();
+ if(!setup_basic_packet()) return failed("FAILED: setup failed");
+ int exp = 0xDDCC;
+ int act = uart_buff_data_get_u16(2);
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_packet_get_u16_wrapped() {
+ uart_buff_reset();
+ int i;
+ for (i = 0; i < 1000; i += 1) uart_buff_add_u8(0);
+ uart_buff_packet_ready();
+ for (i = 0; i < 1040; i += 1) uart_buff_add_u8(0);
+ if(!setup_basic_packet()) return failed("FAILED: setup failed");
+ int exp = 0xBBAA;
+ int act = uart_buff_data_get_u16(0);
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_packet_get_u32() {
+ uart_buff_reset();
+ if(!setup_basic_packet()) return failed("FAILED: setup failed");
+ unsigned int exp = 0xDDCCBBAA;
+ unsigned int act = uart_buff_data_get_u32(0);
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_packet_get_u32_with_offset() {
+ uart_buff_reset();
+ if(!setup_basic_packet()) return failed("FAILED: setup failed");
+ unsigned int exp = 0xFFEEDDCC;
+ unsigned int act = uart_buff_data_get_u32(2);
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_packet_get_u32_wrapped_1_4() {
+ uart_buff_reset();
+ int i;
+ for (i = 0; i < 1000; i += 1) uart_buff_add_u8(0);
+ uart_buff_packet_ready();
+ for (i = 0; i < 1040; i += 1) uart_buff_add_u8(0);
+ if(!setup_basic_packet()) return failed("FAILED: setup failed");
+ int exp = 0xDDCCBBAA;
+ int act = uart_buff_data_get_u32(0);
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_packet_get_u32_wrapped_2_4() {
+ uart_buff_reset();
+ int i;
+ for (i = 0; i < 1000; i += 1) uart_buff_add_u8(0);
+ uart_buff_packet_ready();
+ for (i = 0; i < 1039; i += 1) uart_buff_add_u8(0);
+ if(!setup_basic_packet()) return failed("FAILED: setup failed");
+ int exp = 0xDDCCBBAA;
+ int act = uart_buff_data_get_u32(0);
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+int test_packet_get_u32_wrapped_3_4() {
+ uart_buff_reset();
+ int i;
+ for (i = 0; i < 1000; i += 1) uart_buff_add_u8(0);
+ uart_buff_packet_ready();
+ for (i = 0; i < 1038; i += 1) uart_buff_add_u8(0);
+ if(!setup_basic_packet()) return failed("FAILED: setup failed");
+ int exp = 0xDDCCBBAA;
+ int act = uart_buff_data_get_u32(0);
+ int success = exp == act;
+ print_test_result(success, (float) exp, (float) act);
+ return !success;
+}
+
+
+int test_packet_get_float() {
+ uart_buff_reset();
+ if(!setup_VRPN_packet()) return failed("FAILED: setup failed");
+ float exp = 1.0;
+ float act = uart_buff_data_get_float(0);
+ int success = float_equals(exp, act);
+ print_test_result(success, exp, act);
+ return !success;
+}
+
+int test_packet_get_float_with_offset() {
+ uart_buff_reset();
+ if(!setup_VRPN_packet()) return failed("FAILED: setup failed");
+ float exp = 1.2;
+ float act = uart_buff_data_get_float(4);
+ int success = float_equals(exp, act);
+ print_test_result(success, exp, act);
+ return !success;
+}
+
+int test_packet_ready_at_start() {
+ uart_buff_reset();
+ int exp = 0;
+ int act = uart_buff_packet_ready();
+ int success = act == exp;
+ print_test_result(success, exp, act);
+ return !success;
+}
+
+int test_packet_ready_after_receiving_packet() {
+ uart_buff_reset();
+ if(!setup_basic_packet()) return failed("FAILED: setup failed");
+ int exp = 1;
+ int act = uart_buff_packet_ready();
+ int success = act == exp;
+ print_test_result(success, exp, act);
+ return !success;
+}
+
+int test_packet_ready_after_consuming_packet() {
+ uart_buff_reset();
+ if(!setup_basic_packet()) return failed("FAILED: setup failed");
+ uart_buff_consume_packet();
+ int exp = 0;
+ int act = uart_buff_packet_ready();
+ int success = act == exp;
+ print_test_result(success, exp, act);
+ return !success;
+}
+
+int test_get_raw() {
+ uart_buff_reset();
+ if(!setup_basic_packet()) return failed("FAILED: setup failed");
+ unsigned char exp[15] =
+ {0xBE, 0x04, 0x00, 0x00, 0x00, 0x06, 0x00, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF, 0x00};
+ size_t length;
+ unsigned char *act = (unsigned char *) uart_buff_get_raw(&length);
+ int success = 1;
+ int i;
+ for (i = 0; i < length; i += 1) {
+ success &= (exp[i] == act[i]);
+ if (!success) {
+ printf("FAILED");
+ break;
+ }
+ }
+ if (success) printf("passed\n");
+ return !success;
+}
diff --git a/quad/sw/modular_quad_pid/test/xil_types.h b/quad/sw/modular_quad_pid/test/xil_types.h
new file mode 100644
index 00000000..d67aef84
--- /dev/null
+++ b/quad/sw/modular_quad_pid/test/xil_types.h
@@ -0,0 +1,10 @@
+#ifndef XIL_TYPES_H
+#define XIL_TYPES_H
+
+#include <stddef.h>
+
+typedef unsigned char u8;
+typedef unsigned short u16;
+typedef unsigned int u32;
+
+#endif
--
GitLab