diff --git a/quad/sw/modular_quad_pid/src/uart_buff.c b/quad/sw/modular_quad_pid/src/uart_buff.c
index 0954fc9e0e9d86ecf854017fce8443bb998ed459..56f5c1d90274bc80e0150fccc2603631b042aedd 100644
--- a/quad/sw/modular_quad_pid/src/uart_buff.c
+++ b/quad/sw/modular_quad_pid/src/uart_buff.c
@@ -2,16 +2,11 @@
#include "uart_buff.h"
#include <stdlib.h>
-#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];
@@ -19,10 +14,8 @@ 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
@@ -47,50 +40,40 @@ void uart_buff_add_u8(u8 c) {
* Scan for a packet, updating any necessary indices and flags.
*/
void uart_buff_scan_packet() {
- if (!packet_start_found) {
- uart_buff_scan_packet_start();
- }
-
- if (!packet_size_found) {
- uart_buff_scan_packet_size();
- }
-
- if (packet_start_found
- && packet_size_found
- && (uart_buff_size() >= uart_buff_packet_size())) {
- packet_ready = 1;
- }
-}
+ size_t scan_limit = uart_buff_size();
+ size_t scan_iteration = 0;
+ while (!packet_ready && scan_iteration < scan_limit) {
+ scan_iteration += 1;
+
+ if (buff[start] != 0xBE) {
+ start += 1;
+ if (start >= UART_MAX_BUFF_SIZE) {
+ start -= UART_MAX_BUFF_SIZE;
+ }
+ continue;
+ }
-/**
- * Scan for the packet start. Advance the start index of the buffer until
- * a packet start is found.
- */
-void uart_buff_scan_packet_start() {
- while (buff[start] != 0xBE && !uart_buff_empty()) {
- start = uart_buff_calc_index(start + 1);
- }
- if (buff[start] == 0xBE) {
- packet_start_found = 1;
- }
-}
+ if (uart_buff_size() < UART_PACKET_HEADER_SIZE) {
+ // Haven't received the "length" bytes yet. Check back later.
+ break;
+ }
-/**
- * Scan for the packet size inside the packet.
- */
-void uart_buff_scan_packet_size() {
- if (uart_buff_size() < UART_PACKET_HEADER_SIZE) {
- // haven't received the "length" bytes yet
- return;
- }
+ packet_data_length = (uart_buff_get_u8(6) << 8) | uart_buff_get_u8(5);
+ if (uart_buff_packet_size() > UART_MAX_PACKET_SIZE) {
+ // Packet size is too big. Abort this packet.
+ start += 1;
+ if (start >= UART_MAX_BUFF_SIZE) {
+ start -= UART_MAX_BUFF_SIZE;
+ }
+ continue;
+ }
- packet_data_length = (uart_buff_get_u8(6) << 8) | uart_buff_get_u8(5);
- packet_size_found = 1;
+ if (uart_buff_size() < uart_buff_packet_size()) {
+ // Haven't received the whole packet. Check back later.
+ break;
+ }
- if (uart_buff_packet_size() > UART_MAX_PACKET_SIZE) {
- // Packet size is too big. Abort this packet.
- uart_buff_reset_flags();
- start += 1;
+ packet_ready = 1;
}
}
@@ -305,7 +288,7 @@ void uart_buff_consume_packet() {
}
start = uart_buff_calc_index(start + uart_buff_packet_size());
- uart_buff_reset_flags();
+ packet_ready = 0;
packets_processed += 1;
}
@@ -335,15 +318,6 @@ size_t uart_buff_packet_size() {
return UART_PACKET_HEADER_SIZE + packet_data_length + 1;
}
-/**
- * Reset all flags indicating the NOTEstatus of scanned packets.
- */
-void uart_buff_reset_flags() {
- packet_start_found = 0;
- packet_size_found = 0;
- packet_ready = 0;
-}
-
/**
* Return 1 if the buffer is empty, 0 otherwise.
*/
@@ -428,7 +402,11 @@ void uart_buff_print() {
void uart_buff_reset() {
start = 0;
end = 0;
- uart_buff_reset_flags();
+ int i;
+ for (i = 0; i < UART_MAX_BUFF_SIZE; i += 1) {
+ buff[i] = 0;
+ }
+ packet_ready = 0;
}
/**
diff --git a/quad/sw/modular_quad_pid/src/uart_buff.h b/quad/sw/modular_quad_pid/src/uart_buff.h
index 4cce9f765ecb61672ef0f4fd47f3082165386994..99aadd06e17f632e1e4132d14bfefe4218b5b4f5 100644
--- a/quad/sw/modular_quad_pid/src/uart_buff.h
+++ b/quad/sw/modular_quad_pid/src/uart_buff.h
@@ -3,6 +3,10 @@
#include "xil_types.h"
+#define UART_MAX_BUFF_SIZE 2048
+#define UART_MAX_PACKET_SIZE 256
+#define UART_PACKET_HEADER_SIZE 7
+
void uart_buff_add_u8(u8);
int uart_buff_packet_ready();
u8 uart_buff_get_u8(size_t);
diff --git a/quad/sw/modular_quad_pid/test/test_uart_buff.c b/quad/sw/modular_quad_pid/test/test_uart_buff.c
index 3bf3a762671a5efef49da9e2f1bcdc59f01e2c2d..29b127bbd254176baa50543d5bb35d5226facc4d 100644
--- a/quad/sw/modular_quad_pid/test/test_uart_buff.c
+++ b/quad/sw/modular_quad_pid/test/test_uart_buff.c
@@ -28,6 +28,7 @@ int main() {
failed += test_packet_ready_at_start();
failed += test_packet_ready_after_receiving_packet();
failed += test_packet_ready_after_consuming_packet();
+ failed += test_size_when_data_lenth_too_large();
failed += test_get_raw();
printf("Total tests failed: %d\n", failed);
@@ -58,7 +59,7 @@ void add_packet(unsigned char type, unsigned char subtype, unsigned short id, un
uart_buff_add_u8(length);
uart_buff_add_u8(length >> 8);
int i;
- for (i = 0; i < 24; i += 1) {
+ for (i = 0; i < length; i += 1) {
uart_buff_add_u8(data[i]);
}
// fake checksum
@@ -343,6 +344,18 @@ int test_packet_ready_after_consuming_packet() {
return !success;
}
+int test_size_when_data_lenth_too_large() {
+ uart_buff_reset();
+ unsigned char data[UART_MAX_PACKET_SIZE + 1];
+ add_packet(4, 0, 0, UART_MAX_PACKET_SIZE + 1, data);
+ uart_buff_packet_ready();
+ int exp = 0;
+ int act = uart_buff_size();
+ 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");
@@ -353,7 +366,6 @@ int test_get_raw() {
int success = 1;
int i;
for (i = 0; i < length; i += 1) {
- printf("%02X == %02X\n", exp[i], act[i]);
success = success && (exp[i] == act[i]);
if (!success) {
printf("FAILED");