quad: Refactor regression tests

currently one test is failing

quad/sw/modular_quad_pid/test/test_uart_buff.c

@@ -49,37 +49,31 @@ int failed(char *msg) {
   return 1;
 }
 
-void add_VRPN_packet() {
+void add_packet(unsigned char type, unsigned char subtype, unsigned short id, unsigned short length, unsigned char *data) {
   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;
+  uart_buff_add_u8(type);
+  uart_buff_add_u8(subtype);
+  uart_buff_add_u8(id);
+  uart_buff_add_u8(id >> 8);
+  uart_buff_add_u8(length);
+  uart_buff_add_u8(length >> 8);
   int i;
   for (i = 0; i < 24; i += 1) {
     uart_buff_add_u8(data[i]);
   }
-  uart_buff_add_u8(0x00);
+  // fake checksum
+  uart_buff_add_u8(1);
+}
+
+void add_VRPN_packet() {
+  float arr[6] = {1.0, 1.2, 1.4, -1.5, -0.5, -1.1};
+  unsigned char *data = (unsigned char *) &arr;
+  add_packet(4, 0, 0, 24, data);
 }
 
 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);
+  add_packet(4, 0, 0, 6, data);
 }
 
 void add_garbage_data() {
@@ -353,13 +347,14 @@ 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};
+    {0xBE, 0x04, 0x00, 0x00, 0x00, 0x06, 0x00, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF, 0x01};
   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]);
+    printf("%02X == %02X\n", exp[i], act[i]);
+    success = success && (exp[i] == act[i]);
     if (!success) {
       printf("FAILED");
       break;