quad: fix data retrieve methods

quad/scripts/test_uart_comm.py

-#!/usr/bin/python
+#!/usr/local/bin/python3.6
 
 import sys
 import time
 
-print(sys.version_info)
 import serial
 
 def create_msg(main_type, subtype, msg_id, data):
@@ -46,9 +45,9 @@ def query_received(ser):
 if __name__ == '__main__':
     with serial.Serial('/dev/ttyUSB0', 921600, timeout=5) as ser:
         ser.reset_input_buffer()
-        ser.write(create_test_packet(40))
-        time.sleep(0.05)
-        #while ser.in_waiting != 0:
+        ser.write(create_test_packet(8))
+        # time.sleep(0.05)
+        # while ser.in_waiting != 0:
         #    resp = read_packet(ser)
         #    elapsed = int.from_bytes(resp, byteorder='little')
         #    print(f"Took {elapsed} cycles, {elapsed / 100} us")

quad/scripts/uart_comm_listen.py

+#!/usr/local/bin/python3.6
+
+import sys
+import time
+
+import serial
+
+def read_packet(ser):
+    header = ser.read(7)
+    length = int.from_bytes(header[5:7], byteorder='little')
+    data = ser.read(length)
+    checksum = ser.read()
+    return data
+
+def query_received(ser):
+    # Send request
+    query_msg = create_msg(0, 3, 0, b'')
+    ser.write(query_msg)
+    ser.flush()
+    
+    resp = read_packet(ser)
+    received_str = resp[:-1].decode()
+
+    return tuple(map(int, received_str.split(',')))
+
+if __name__ == '__main__':
+    with serial.Serial('/dev/ttyUSB0', 921600, timeout=5) as ser:
+        i = 0
+        while True:
+            ser.reset_input_buffer()
+            time.sleep(0.05)
+            while ser.in_waiting != 0:
+                resp = read_packet(ser)
+                elapsed = int.from_bytes(resp, byteorder='little')
+                print("{} {}".format(i, elapsed))
+                i += 1
+            ser.flush()
+

quad/scripts/uart_comm_send.py

+#!/usr/local/bin/python3.6
+
+import sys
+import time
+
+import serial
+
+def create_msg(main_type, subtype, msg_id, data):
+    msg = bytes()
+    msg += b'\xBE'
+    msg += main_type.to_bytes(1, 'little')
+    msg += subtype.to_bytes(1, 'little')
+    msg += msg_id.to_bytes(2, 'little')
+    msg += len(data).to_bytes(2, 'little')
+    msg += data
+    
+    checksum = 0
+    for b in msg:
+        checksum ^= b
+    msg += checksum.to_bytes(1, 'little')
+    return msg
+
+def create_test_packet(size=8):
+    data = bytes((i % 256 for i in range(size)))
+    return create_msg(0, 2, 0, data)
+
+if __name__ == '__main__':
+    with serial.Serial('/dev/ttyUSB0', 921600, timeout=5) as ser:
+        for i in range(0, 1):
+            time.sleep(0.5)
+            ser.reset_input_buffer()
+            ser.write(create_test_packet(8))
+            ser.flush()

quad/sw/modular_quad_pid/.gitignore

 Debug/
+Release/
 bootimage/

quad/sw/modular_quad_pid/src/communication.c

@@ -119,7 +119,11 @@ void restore_interrupts(u32 intr_state) {
 void process_received(modular_structs_t *structs) {
 	// Parse as many packets as possible
 	while (uart_buff_packet_ready()) {
+		u32 start = timer_get_count();
 		process_packet(structs);
+		u32 end = timer_get_count();
+		u32 duration = end - start;
+		send_data(0, 0, 0, (char *) &duration, 4);
 	}
 }
 

quad/sw/modular_quad_pid/src/main.c

@@ -52,6 +52,7 @@ int main()
 		// Process all received data
 		process_received(&structs);
 
+
 #ifndef BENCH_TEST
 		// Get the user input and put it into user_input_struct
 		get_user_input(&(structs.log_struct), &(structs.user_input_struct));

quad/sw/modular_quad_pid/src/uart_buff.c

@@ -2,10 +2,10 @@
 #include "uart_buff.h"
 #include <stdlib.h>
 
-#define UART_MAX_BUFF_SIZE 2048
-#define UART_MAX_PACKET_SIZE 256
+#define UART_MAX_BUFF_SIZE 20
+#define UART_MAX_PACKET_SIZE 10
 
-static volatile unsigned char buff[UART_MAX_BUFF_SIZE];
+static volatile u8 buff[UART_MAX_BUFF_SIZE];
 static size_t start = UART_MAX_BUFF_SIZE;
 static size_t packet_data_length = 0;
 static volatile size_t end = 0;
@@ -97,12 +97,12 @@ u8 uart_buff_get_u8(size_t offset) {
 }
 
 /**
- * Retrieve a 16-bit 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 buffer.
  */
 u16 uart_buff_get_u16(size_t offset) {
-  return uart_buff_data_get_u8(offset + 1) << 8 |
-    uart_buff_data_get_u8(offset);
+  return uart_buff_get_u8(offset + 1) << 8 |
+    uart_buff_get_u8(offset);
 }
 
 /**
@@ -110,10 +110,10 @@ u16 uart_buff_get_u16(size_t offset) {
  * to the start index of the buffer.
  */
 u32 uart_buff_get_u32(size_t offset) {
-  return 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 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);
 }
 
 /**
@@ -151,7 +151,7 @@ u32 uart_buff_data_get_u32(size_t offset) {
 
 /**
  * Retrieve a 32-bit floating point number from the buffer according to the
- * given offset with respect to the start of the data portion of the current 
+ * given offset with respect to the start of the data portion of the current
  * packet.
  *
  * This has undefined behavior if a packet is not ready.

quad/sw/modular_quad_pid/src/uart_buff.h

@@ -26,5 +26,4 @@ size_t uart_buff_calc_index(int);
 char * uart_buff_get_packet();
 void uart_buff_print();
 
-
 #endif