Implemented copying of UART received data to circular buffer and processing...

Implemented copying of UART received data to circular buffer and processing circular buffer. Has not been tested

quad/sw/comm_dev/Debug/comm_dev.elf.size

    text	   data	    bss	    dec	    hex	filename
- 163072	  68080	119150040	119381192	71d9cc8	comm_dev.elf
+ 164976	  68080	119152120	119385176	71dac58	comm_dev.elf

quad/sw/comm_dev/Debug/src/circ_buffer.d

+src/circ_buffer.d: ../src/circ_buffer.c ../src/circ_buffer.h
+
+../src/circ_buffer.h:

quad/sw/comm_dev/Debug/src/new_comm.d

-src/new_comm.d: ../src/new_comm.c ../src/new_comm.h ../src/uart.h \
- ../../system_bsp/ps7_cortexa9_0/include/xparameters.h \
- ../../system_bsp/ps7_cortexa9_0/include/xparameters_ps.h \
+src/new_comm.d: ../src/new_comm.c ../src/new_comm.h \
  ../../system_bsp/ps7_cortexa9_0/include/xuartps.h \
  ../../system_bsp/ps7_cortexa9_0/include/xil_types.h \
  ../../system_bsp/ps7_cortexa9_0/include/xil_assert.h \
@@ -12,27 +10,24 @@ src/new_comm.d: ../src/new_comm.c ../src/new_comm.h ../src/uart.h \
  ../../system_bsp/ps7_cortexa9_0/include/xpseudo_asm_gcc.h \
  ../../system_bsp/ps7_cortexa9_0/include/xil_printf.h \
  ../../system_bsp/ps7_cortexa9_0/include/xparameters.h \
- ../src/stringBuilder.h ../src/type_def.h \
+ ../../system_bsp/ps7_cortexa9_0/include/xparameters_ps.h \
+ ../../system_bsp/ps7_cortexa9_0/include/xparameters.h \
  ../../system_bsp/ps7_cortexa9_0/include/xscugic.h \
  ../../system_bsp/ps7_cortexa9_0/include/xscugic_hw.h \
  ../../system_bsp/ps7_cortexa9_0/include/xil_exception.h \
- ../src/mio7_led.h ../../system_bsp/ps7_cortexa9_0/include/xgpiops.h \
+ ../../system_bsp/ps7_cortexa9_0/include/xil_exception.h \
+ ../src/type_def.h ../src/uart.h ../src/stringBuilder.h ../src/mio7_led.h \
+ ../../system_bsp/ps7_cortexa9_0/include/xgpiops.h \
  ../../system_bsp/ps7_cortexa9_0/include/xgpiops_hw.h \
- ../../system_bsp/ps7_cortexa9_0/include/sleep.h \
- ../../system_bsp/ps7_cortexa9_0/include/xil_exception.h ../src/timer.h \
+ ../../system_bsp/ps7_cortexa9_0/include/sleep.h ../src/timer.h \
  ../src/log_data.h ../src/PID.h \
  ../../system_bsp/ps7_cortexa9_0/include/xtime_l.h \
  ../../system_bsp/ps7_cortexa9_0/include/xtmrctr.h \
- ../../system_bsp/ps7_cortexa9_0/include/xtmrctr_l.h
+ ../../system_bsp/ps7_cortexa9_0/include/xtmrctr_l.h ../src/commands.h \
+ ../src/circ_buffer.h
 
 ../src/new_comm.h:
 
-../src/uart.h:
-
-../../system_bsp/ps7_cortexa9_0/include/xparameters.h:
-
-../../system_bsp/ps7_cortexa9_0/include/xparameters_ps.h:
-
 ../../system_bsp/ps7_cortexa9_0/include/xuartps.h:
 
 ../../system_bsp/ps7_cortexa9_0/include/xil_types.h:
@@ -55,9 +50,9 @@ src/new_comm.d: ../src/new_comm.c ../src/new_comm.h ../src/uart.h \
 
 ../../system_bsp/ps7_cortexa9_0/include/xparameters.h:
 
-../src/stringBuilder.h:
+../../system_bsp/ps7_cortexa9_0/include/xparameters_ps.h:
 
-../src/type_def.h:
+../../system_bsp/ps7_cortexa9_0/include/xparameters.h:
 
 ../../system_bsp/ps7_cortexa9_0/include/xscugic.h:
 
@@ -65,6 +60,14 @@ src/new_comm.d: ../src/new_comm.c ../src/new_comm.h ../src/uart.h \
 
 ../../system_bsp/ps7_cortexa9_0/include/xil_exception.h:
 
+../../system_bsp/ps7_cortexa9_0/include/xil_exception.h:
+
+../src/type_def.h:
+
+../src/uart.h:
+
+../src/stringBuilder.h:
+
 ../src/mio7_led.h:
 
 ../../system_bsp/ps7_cortexa9_0/include/xgpiops.h:
@@ -73,8 +76,6 @@ src/new_comm.d: ../src/new_comm.c ../src/new_comm.h ../src/uart.h \
 
 ../../system_bsp/ps7_cortexa9_0/include/sleep.h:
 
-../../system_bsp/ps7_cortexa9_0/include/xil_exception.h:
-
 ../src/timer.h:
 
 ../src/log_data.h:
@@ -86,3 +87,7 @@ src/new_comm.d: ../src/new_comm.c ../src/new_comm.h ../src/uart.h \
 ../../system_bsp/ps7_cortexa9_0/include/xtmrctr.h:
 
 ../../system_bsp/ps7_cortexa9_0/include/xtmrctr_l.h:
+
+../src/commands.h:
+
+../src/circ_buffer.h:

quad/sw/comm_dev/Debug/src/subdir.mk

@@ -6,6 +6,7 @@
 C_SRCS += \
 ../src/PID.c \
 ../src/actuator_command_processing.c \
+../src/circ_buffer.c \
 ../src/commands.c \
 ../src/communication.c \
 ../src/control_algorithm.c \
@@ -37,6 +38,7 @@ LD_SRCS += \
 OBJS += \
 ./src/PID.o \
 ./src/actuator_command_processing.o \
+./src/circ_buffer.o \
 ./src/commands.o \
 ./src/communication.o \
 ./src/control_algorithm.o \
@@ -64,6 +66,7 @@ OBJS += \
 C_DEPS += \
 ./src/PID.d \
 ./src/actuator_command_processing.d \
+./src/circ_buffer.d \
 ./src/commands.d \
 ./src/communication.d \
 ./src/control_algorithm.d \

quad/sw/comm_dev/src/circ_buffer.c

@@ -16,12 +16,12 @@ size_t buf_end = 0;
 size_t size = 0;
 
 
-struct data_chunk getChunk() {
+struct data_chunk getChunk(size_t amount) {
     size_t chunk_end = buf_end;
     if (buf_end < buf_start) {
         chunk_end = CIRC_BUFFER_SIZE;
     }
-    size_t ret_size = chunk_end - buf_start;
+    size_t ret_size = circ_buf_min(amount, chunk_end - buf_start);
     struct data_chunk c = {
             ret_size,
              &buffer[buf_start]
@@ -45,7 +45,8 @@ size_t putChunk(struct data_chunk c) {
     size_t will_place = circ_buf_min(CIRC_BUFFER_SIZE - size, c.length);
     // Actual amount placed so far
     size_t placed = 0;
-    while (placed < will_place) {
+    int loops = 0; // Limit on number of iterations, just in case
+    while (placed < will_place && loops < 2) {
         // Available room without wrapping
         size_t to_copy;
         if (buf_end  >= buf_start) {
@@ -58,19 +59,21 @@ size_t putChunk(struct data_chunk c) {
         memcpy(buffer + buf_end, c.data + placed, to_copy);
         // Update buffer endpoint
         buf_end += to_copy;
-        if (buf_end > CIRC_BUFFER_SIZE) {
-            printf("Error: buf_end: %d\n", buf_end);
-        }
         // If we copied to the end
-        if (buf_end == CIRC_BUFFER_SIZE) {
+        if (buf_end >= CIRC_BUFFER_SIZE) {
             buf_end = 0;
         }
         placed += to_copy;
+        loops += 1;
     }
     size += placed;
     return placed;
 }
 
-size_t get_buffer_size() {
-    return size;
+size_t circ_buf_size(){
+	return size;
+}
+
+size_t circ_buf_available() {
+    return CIRC_BUFFER_SIZE - size;
 }

quad/sw/comm_dev/src/circ_buffer.h

@@ -6,9 +6,7 @@
 #define CIRC_BUFFER_H
 
 #define CIRC_BUFFER_SIZE 2048
-#include <unistd.h>
-#include <memory.h>
-#include <stdio.h>
+#include <string.h>
 
 struct data_chunk {
     size_t length;
@@ -20,7 +18,7 @@ struct data_chunk {
  * Does not move the buffer forward. You must call markConsumed to
  * mark the area as free.
  */
-struct data_chunk getChunk();
+struct data_chunk getChunk(size_t amount);
 
 /*
  * Marks the n_consumed bytes as used, and that
@@ -37,6 +35,12 @@ size_t putChunk(struct data_chunk);
 /*
  * Returns the remaining size in the buffer
  */
-size_t get_buffer_size();
+size_t circ_buf_available();
+
+/*
+ * Returns the amount of data in the circular buffer
+ */
+size_t circ_buf_size();
+
 
 #endif //CIRC_BUFFER_H

quad/sw/comm_dev/src/new_comm.c

@@ -5,7 +5,6 @@
  *      Author: dawehr
  */
 #include "new_comm.h"
-#include "timer.h"
 
 #define INTC		XScuGic
 #define COMM_UART_DEVICE_ID		XPAR_PS7_UART_0_DEVICE_ID
@@ -20,11 +19,14 @@
 // Declaration of interrupt handler
 void Handler(void *CallBackRef, u32 Event, unsigned int EventData);
 
+// Pointer to the UART driver instance
+XUartPs* uartInstPtr;
 
 static u8 recvBuf[UART_BUF_SIZE];
+static size_t received_in_buf = 0;
 
 int initUartComms() {
-	XUartPs* uartInstPtr = uart0_init(COMM_UART_DEVICE_ID, 115200);
+	uartInstPtr = uart0_init(COMM_UART_DEVICE_ID, BAUD_RATE);
 	// Initialize UART0 (Bluetooth/WiFi)
 	if(!uartInstPtr) {
 		return -1;
@@ -51,7 +53,7 @@ int initUartComms() {
 
 
 	/*
-	 * Set the receiver timeout. If it is not set, and the last few bytes
+	 * Set the receiver timeout. If it is noDidn'tt set, and the last few bytes
 	 * of data do not trigger the over-water or full interrupt, the bytes
 	 * will not be received. By default it is disabled.
 	 * Timeout duration = RecvTimeout x 4 x Bit Period. 0 disables the
@@ -70,6 +72,81 @@ int initUartComms() {
 
 	return 0;
 }
+// TODO: Be consistent with underscore_names or camelCase
+
+int process_packet(unsigned char* packet, modular_structs_t *structs) {
+	metadata_t meta_data;
+	// parse metadata
+	meta_data.begin_char = packet[0];
+	meta_data.msg_type = packet[1];
+	meta_data.msg_subtype = packet[2];
+	meta_data.msg_id = (packet[4] << 8) | (packet[3]);
+	meta_data.data_len = (packet[6] << 8) | (packet[5]);
+	unsigned char packet_checksum = packet[7+meta_data.data_len];
+	unsigned char* packet_data = packet + sizeof(metadata_t);
+
+	// Compute checksum
+	int i;
+	unsigned char calculated_checksum = 0;
+	for(i = 0; i < meta_data.data_len + 7; i++){
+		calculated_checksum ^= packet[i];
+	}
+	// Discard if checksum didn't match
+	if(packet_checksum != calculated_checksum) {
+		printf("Checksums did not match: 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
+		return -1;
+	}
+
+	// Call appropriate function for packet
+	(* (MessageTypes[meta_data.msg_type].subtypes[meta_data.msg_subtype].functionPtr))(packet_data, meta_data.data_len, structs);
+
+	return 0;
+}
+
+void parse_available(modular_structs_t *structs) {
+	// Discard all data before packet begin character
+	struct data_chunk all_data = getChunk(circ_buf_size());
+	size_t discard_data = 0;
+	while (all_data.length - discard_data > 0 && all_data.data[discard_data] != BEGIN_CHAR) {
+		discard_data++;
+	}
+	markConsumed(discard_data);
+
+	// Minimum size of a packet (header + checksum)
+	int min_packet_size = sizeof(metadata_t) + 1;
+	// TODO: Loop limit?
+	while (circ_buf_available() >= min_packet_size) {
+		struct data_chunk header = getChunk(sizeof(metadata_t));
+		unsigned char* packet_header = header.data;
+		size_t packet_len = (packet_header[6] << 8) | (packet_header[5]);
+		if (circ_buf_available() >= min_packet_size + packet_len) {
+			struct data_chunk packet = getChunk(min_packet_size + packet_len);
+			process_packet(packet.data, structs);
+			markConsumed(packet.length);
+		}
+		else {
+			// Not enough data for a packet
+			break;
+		}
+
+	}
+}
+
+
+void process_received(modular_structs_t *structs) {
+	// Temporarily disable the receive interrupt
+	XUartPs_Recv(uartInstPtr, NULL, 0);
+	// Put received data into circular buffer
+	struct data_chunk to_place = {received_in_buf, recvBuf};
+	size_t actually_placed = putChunk(to_place);
+	if (actually_placed != to_place.length) {
+		printf("Error: Couldn't place all data into circular buffer");
+	}
+	received_in_buf = 0;
+	// Re-enable interrupts and process copied data
+	XUartPs_Recv(uartInstPtr, recvBuf, UART_BUF_SIZE);
+	parse_available(structs);
+}
 
 /**************************************************************************/
 /**
@@ -96,6 +173,7 @@ void Handler(void *CallBackRef, u32 Event, unsigned int EventData)
 {
 	/* data has been received */
 	if (Event == XUARTPS_EVENT_RECV_DATA || Event == XUARTPS_EVENT_RECV_TOUT || Event == XUARTPS_IXR_RXOVR) {
+		received_in_buf = EventData;
 		if (EventData >= UART_BUF_SIZE) {
 			MIO7_led_on();
 		}

quad/sw/comm_dev/src/new_comm.h

@@ -8,13 +8,18 @@
 #ifndef NEW_COMM_H_
 #define NEW_COMM_H_
 
-#include "uart.h"
 #include <xuartps.h>
-#include "mio7_led.h"
 #include "xparameters.h"
 #include "xscugic.h"
 #include "xil_exception.h"
 
+#include "type_def.h"
+#include "uart.h"
+#include "mio7_led.h"
+#include "timer.h"
+#include "commands.h"
+#include "circ_buffer.h"
+
 int initUartComms();
 
 #endif /* NEW_COMM_H_ */