diff --git a/quad/sw/comm_dev/Debug/comm_dev.elf b/quad/sw/comm_dev/Debug/comm_dev.elf
index d4592cdccf610f6e7390e0ade5ce2a46ca06b61a..c89ec02fb8eca665daf9ce76ce9781fa6b69ab28 100755
Binary files a/quad/sw/comm_dev/Debug/comm_dev.elf and b/quad/sw/comm_dev/Debug/comm_dev.elf differ
diff --git a/quad/sw/comm_dev/Debug/comm_dev.elf.size b/quad/sw/comm_dev/Debug/comm_dev.elf.size
index 2e91f9243d4717375c3e523484a5b9347fe1814e..1cc576e697fc33d0beb2f08f71f67152bc29d7b2 100644
--- a/quad/sw/comm_dev/Debug/comm_dev.elf.size
+++ b/quad/sw/comm_dev/Debug/comm_dev.elf.size
@@ -1,2 +1,2 @@
text data bss dec hex filename
- 163072 68080 119150040 119381192 71d9cc8 comm_dev.elf
+ 164976 68080 119152120 119385176 71dac58 comm_dev.elf
diff --git a/quad/sw/comm_dev/Debug/src/circ_buffer.d b/quad/sw/comm_dev/Debug/src/circ_buffer.d
new file mode 100644
index 0000000000000000000000000000000000000000..52a6a5bc9e7ee8f0c66e0f0b059ac303b46b7017
--- /dev/null
+++ b/quad/sw/comm_dev/Debug/src/circ_buffer.d
@@ -0,0 +1,3 @@
+src/circ_buffer.d: ../src/circ_buffer.c ../src/circ_buffer.h
+
+../src/circ_buffer.h:
diff --git a/quad/sw/comm_dev/Debug/src/circ_buffer.o b/quad/sw/comm_dev/Debug/src/circ_buffer.o
new file mode 100644
index 0000000000000000000000000000000000000000..6992e3db9620a839c22d41b26406a6ada6fa7c4a
Binary files /dev/null and b/quad/sw/comm_dev/Debug/src/circ_buffer.o differ
diff --git a/quad/sw/comm_dev/Debug/src/new_comm.d b/quad/sw/comm_dev/Debug/src/new_comm.d
index fafc3d57bc29f28f85aea603279a847540b1056d..c17d385502e1da41d043991d223b60b5b295e85c 100644
--- a/quad/sw/comm_dev/Debug/src/new_comm.d
+++ b/quad/sw/comm_dev/Debug/src/new_comm.d
@@ -1,6 +1,4 @@
-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:
diff --git a/quad/sw/comm_dev/Debug/src/new_comm.o b/quad/sw/comm_dev/Debug/src/new_comm.o
index 07d4b93b993d93ce1ab6b1ef3398e8c9f0058c4f..8acf99e4a5580543fbab13f4c496b5bff35db90f 100644
Binary files a/quad/sw/comm_dev/Debug/src/new_comm.o and b/quad/sw/comm_dev/Debug/src/new_comm.o differ
diff --git a/quad/sw/comm_dev/Debug/src/subdir.mk b/quad/sw/comm_dev/Debug/src/subdir.mk
index 0f7b898c7a88dcb76919c1725041ac09bd2c64b4..93787270269585533e1a119fb1bf514745210e86 100644
--- a/quad/sw/comm_dev/Debug/src/subdir.mk
+++ b/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 \
diff --git a/quad/sw/comm_dev/src/circ_buffer.c b/quad/sw/comm_dev/src/circ_buffer.c
index ef90c1acf18b8cda3bbe81f08c0cdd6272ce54a8..a844220d7362f6daf480c5e6225e3ecc214d00dd 100644
--- a/quad/sw/comm_dev/src/circ_buffer.c
+++ b/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;
}
diff --git a/quad/sw/comm_dev/src/circ_buffer.h b/quad/sw/comm_dev/src/circ_buffer.h
index 493543112a723424ce3a81df94e7f8e2987c06d2..cad546a7b0e5f3d809e28719e8d8b4bb1d9f5d8a 100644
--- a/quad/sw/comm_dev/src/circ_buffer.h
+++ b/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
diff --git a/quad/sw/comm_dev/src/new_comm.c b/quad/sw/comm_dev/src/new_comm.c
index d051fefdb88fa708de16a4304cf6acb9336d1e18..b43a793abdf78e6b6881d5279b6072a05b7b26bc 100644
--- a/quad/sw/comm_dev/src/new_comm.c
+++ b/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();
}
diff --git a/quad/sw/comm_dev/src/new_comm.h b/quad/sw/comm_dev/src/new_comm.h
index 72bbce798e39089d4e8c35b83599a5de7d5e71f6..fa6ff4c45f43ef01f7daa420904522c264d19470 100644
--- a/quad/sw/comm_dev/src/new_comm.h
+++ b/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_ */