Merge branch 'master' of git.ece.iastate.edu:danc/MicroCART_17-18

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

    text	   data	    bss	    dec	    hex	filename
- 164976	  68080	119152120	119385176	71dac58	comm_dev.elf
+ 165412	  68080	119152504	119385996	71daf8c	comm_dev.elf

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

@@ -15,7 +15,17 @@ src/commands.d: ../src/commands.c ../src/communication.h \
  ../../system_bsp/ps7_cortexa9_0/include/xparameters.h \
  ../src/stringBuilder.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
+ ../../system_bsp/ps7_cortexa9_0/include/xil_exception.h \
+ ../src/new_comm.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/xgpiops_hw.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 \
+ ../src/packet_processing.h
 
 ../src/communication.h:
 
@@ -58,3 +68,29 @@ src/commands.d: ../src/commands.c ../src/communication.h \
 ../../system_bsp/ps7_cortexa9_0/include/xscugic_hw.h:
 
 ../../system_bsp/ps7_cortexa9_0/include/xil_exception.h:
+
+../src/new_comm.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/xgpiops_hw.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:
+
+../src/packet_processing.h:

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

@@ -31,8 +31,9 @@ src/initialize_components.d: ../src/initialize_components.c \
  ../../system_bsp/ps7_cortexa9_0/include/xiicps_hw.h \
  ../src/packet_processing.h ../src/conversion.h ../src/platform.h \
  ../src/platform_config.h ../src/communication.h ../src/commands.h \
- ../src/new_comm.h ../src/mio7_led.h \
- ../../system_bsp/ps7_cortexa9_0/include/xil_exception.h
+ ../src/new_comm.h \
+ ../../system_bsp/ps7_cortexa9_0/include/xil_exception.h \
+ ../src/mio7_led.h
 
 ../src/initialize_components.h:
 
@@ -128,6 +129,6 @@ src/initialize_components.d: ../src/initialize_components.c \
 
 ../src/new_comm.h:
 
-../src/mio7_led.h:
-
 ../../system_bsp/ps7_cortexa9_0/include/xil_exception.h:
+
+../src/mio7_led.h:

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

@@ -30,7 +30,9 @@ src/main.d: ../src/main.c ../src/timer.h ../src/log_data.h ../src/PID.h \
  ../../system_bsp/ps7_cortexa9_0/include/xiicps_hw.h \
  ../src/packet_processing.h ../src/conversion.h ../src/platform.h \
  ../src/platform_config.h ../src/actuator_command_processing.h \
- ../src/send_actuator_commands.h ../src/update_gui.h
+ ../src/send_actuator_commands.h ../src/update_gui.h ../src/new_comm.h \
+ ../../system_bsp/ps7_cortexa9_0/include/xil_exception.h \
+ ../src/mio7_led.h ../src/commands.h
 
 ../src/timer.h:
 
@@ -125,3 +127,11 @@ src/main.d: ../src/main.c ../src/timer.h ../src/log_data.h ../src/PID.h \
 ../src/send_actuator_commands.h:
 
 ../src/update_gui.h:
+
+../src/new_comm.h:
+
+../../system_bsp/ps7_cortexa9_0/include/xil_exception.h:
+
+../src/mio7_led.h:
+
+../src/commands.h:

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

@@ -23,8 +23,7 @@ src/new_comm.d: ../src/new_comm.c ../src/new_comm.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 ../src/commands.h \
- ../src/circ_buffer.h
+ ../../system_bsp/ps7_cortexa9_0/include/xtmrctr_l.h ../src/commands.h
 
 ../src/new_comm.h:
 
@@ -89,5 +88,3 @@ src/new_comm.d: ../src/new_comm.c ../src/new_comm.h \
 ../../system_bsp/ps7_cortexa9_0/include/xtmrctr_l.h:
 
 ../src/commands.h:
-
-../src/circ_buffer.h:

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

@@ -16,6 +16,15 @@ src/uart.d: ../src/uart.c ../src/uart.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/new_comm.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/xgpiops_hw.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 ../src/commands.h \
  ../../system_bsp/ps7_cortexa9_0/include/xstatus.h
 
 ../src/uart.h:
@@ -56,4 +65,30 @@ src/uart.d: ../src/uart.c ../src/uart.h \
 
 ../../system_bsp/ps7_cortexa9_0/include/xil_exception.h:
 
+../src/new_comm.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/xgpiops_hw.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:
+
+../src/commands.h:
+
 ../../system_bsp/ps7_cortexa9_0/include/xstatus.h:

quad/sw/comm_dev/src/circ_buffer.c

@@ -10,10 +10,10 @@
      _a < _b ? _a : _b; })
 
 
-unsigned char buffer[CIRC_BUFFER_SIZE];
-size_t buf_start = 0;
-size_t buf_end = 0;
-size_t size = 0;
+static unsigned char buffer[CIRC_BUFFER_SIZE];
+static size_t buf_start = 0;
+static size_t buf_end = 0;
+static size_t size = 0;
 
 
 struct data_chunk getChunk(size_t amount) {

quad/sw/comm_dev/src/commands.c

@@ -2,6 +2,7 @@
 #include "commands.h"
 #include "type_def.h"
 #include "uart.h"
+#include "packet_processing.h"
 
 struct MessageType MessageTypes[MAX_TYPE] = 
 {
@@ -473,7 +474,8 @@ int debug(unsigned char *packet, int dataLen, modular_structs_t *structs)
 	control algorithm in order to get this to work. */
 int update(unsigned char *packet, int dataLen,	modular_structs_t *structs)
 {
-	printf("function for debug\n");
+	//MIO7_led_on();
+	processUpdate(packet, &(structs->raw_sensor_struct.currentQuadPosition));
 	return 0;
 }
 

quad/sw/comm_dev/src/initialize_components.c

@@ -100,6 +100,8 @@ int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct
 	if (initUartComms()) {
 		return -1;
 	}
+	// Initialize loop timers
+	timer_init();
 	return 0;
 
 	// Initialize I2C controller and start the sensor board

quad/sw/comm_dev/src/main.c

@@ -15,6 +15,7 @@
 #include "actuator_command_processing.h"
 #include "send_actuator_commands.h"
 #include "update_gui.h"
+#include "new_comm.h"
 
 int main()
 {
@@ -34,7 +35,11 @@ int main()
 		return -1;
 	}
 
-	while(1) {}
+	while(1) {
+		timer_start_loop();
+		process_received(&structs);
+		timer_end_loop(&(structs.log_struct));
+	}
 
 	// Loops to make sure the quad is responding correctly before starting the control loop
 	protection_loops();

quad/sw/comm_dev/src/new_comm.c

@@ -15,15 +15,20 @@
 // Maximum number of bytes to be received within our loop time, plus 64 for a little buffer
 // (bit/s) * (seconds) / (10 bits / byte for UART)
 #define UART_BUF_SIZE (((BAUD_RATE * (DESIRED_USEC_PER_LOOP / 1000) / 1000) / 10) + 64)
+#define MAX_PACKET_SIZE 256
 
 // Declaration of interrupt handler
 void Handler(void *CallBackRef, u32 Event, unsigned int EventData);
 
 // Pointer to the UART driver instance
-XUartPs* uartInstPtr;
+static XUartPs* uartInstPtr;
 
 static u8 recvBuf[UART_BUF_SIZE];
-static size_t received_in_buf = 0;
+static volatile size_t received_in_buf = 0;
+
+static u8 secondaryBuf[UART_BUF_SIZE + MAX_PACKET_SIZE];
+static volatile size_t secondaryEnd = 0; // Index of start of valid data in the secondary buffer
+static size_t secondaryBegin = 0; // Index of one past end of valid data in the secondary buffer
 
 int initUartComms() {
 	uartInstPtr = uart0_init(COMM_UART_DEVICE_ID, BAUD_RATE);
@@ -34,7 +39,7 @@ int initUartComms() {
 
 	uart0_clearFIFOs();
 
-	if (uart0_int_init(COMM_UART_INT_IRQ_ID) != XST_SUCCESS) {
+	if (uart0_int_init(COMM_UART_INT_IRQ_ID, (Xil_ExceptionHandler) uartInterruptHandler) != XST_SUCCESS) {
 		return -1;
 	}
 
@@ -45,7 +50,7 @@ int initUartComms() {
 	 * a pointer to the UART driver instance as the callback reference
 	 * so the handlers are able to access the instance data
 	 */
-	XUartPs_SetHandler(uartInstPtr, (XUartPs_Handler)Handler, uartInstPtr);
+	//XUartPs_SetHandler(uartInstPtr, (XUartPs_Handler)Handler, uartInstPtr);
 
 	u32 IntrMask = XUARTPS_IXR_RXFULL | XUARTPS_IXR_RXOVR | XUARTPS_IXR_TOUT;
 
@@ -68,7 +73,7 @@ int initUartComms() {
 	// Second argument is the number of bytes to trigger an interrupt at
 	XUartPs_SetFifoThreshold(uartInstPtr, 12);
 
-	XUartPs_Recv(uartInstPtr, recvBuf, UART_BUF_SIZE);
+	//XUartPs_Recv(uartInstPtr, recvBuf, UART_BUF_SIZE);
 
 	return 0;
 }
@@ -105,47 +110,108 @@ int process_packet(unsigned char* packet, modular_structs_t *structs) {
 
 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++;
+	while (secondaryBegin < secondaryEnd && secondaryBuf[secondaryBegin] != BEGIN_CHAR) {
+		secondaryBegin++;
 	}
-	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;
+	while (secondaryEnd - secondaryBegin >= min_packet_size) {
+		unsigned char* packet_header = secondaryBuf + secondaryBegin;
 		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);
+		if (secondaryEnd - secondaryBegin >= min_packet_size + packet_len) {
+			process_packet(secondaryBuf + secondaryBegin, structs);
+			secondaryBegin += min_packet_size + packet_len;
 		}
 		else {
 			// Not enough data for a packet
 			break;
 		}
-
 	}
 }
 
+u32 disable_interrupts() {
+	u32 ImrRegister;
+	ImrRegister = XUartPs_ReadReg(uartInstPtr->Config.BaseAddress, XUARTPS_IMR_OFFSET);
+	XUartPs_WriteReg(uartInstPtr->Config.BaseAddress, XUARTPS_IDR_OFFSET, XUARTPS_IXR_MASK);
+	return ImrRegister;
+}
+
+void restore_interrupts(u32 intr_state) {
+	XUartPs_WriteReg(uartInstPtr->Config.BaseAddress, XUARTPS_IER_OFFSET, intr_state);
+}
 
 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");
+//	// Move unprocessed bytes to front of secondary buffer
+//	size_t unprocessed_size = secondaryEnd - secondaryBegin;
+//	memmove(secondaryBuf, secondaryBuf + secondaryBegin, unprocessed_size);
+//	secondaryBegin = 0;
+//	secondaryEnd = unprocessed_size;
+//
+//	// Temporarily disable the receive interrupt
+//	XUartPs_Recv(uartInstPtr, recvBuf, 0);
+//	if (received_in_buf) {
+//		volatile int a = 3;
+//	}
+//	// Put received data into secondary buffer
+//	memcpy(secondaryBuf + secondaryEnd, recvBuf, received_in_buf);
+//
+//	secondaryEnd += received_in_buf;
+//	received_in_buf = 0;
+//	// Re-enable interrupts and process copied data
+//	XUartPs_Recv(uartInstPtr, recvBuf, UART_BUF_SIZE);
+	if (secondaryEnd != 0) {
+		volatile int asdasdf = 3;
+		MIO7_led_off();
 	}
-	received_in_buf = 0;
-	// Re-enable interrupts and process copied data
-	XUartPs_Recv(uartInstPtr, recvBuf, UART_BUF_SIZE);
+
 	parse_available(structs);
+
+	u32 intr_state = disable_interrupts();
+
+	// Move unprocessed bytes to front of secondary buffer
+	size_t unprocessed_size = secondaryEnd - secondaryBegin;
+	memmove(secondaryBuf, secondaryBuf + secondaryBegin, unprocessed_size);
+	secondaryBegin = 0;
+	secondaryEnd = unprocessed_size;
+
+	restore_interrupts(intr_state);
+	//unsigned char in_fifo = XUartPs_ReadReg(uartInstPtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
+	return;
+}
+
+void uartInterruptHandler(XUartPs *InstancePtr) {
+	u32 IsrStatus;
+
+	/*
+	 * Read the interrupt ID register to determine which
+	 * interrupt is active
+	 */
+	IsrStatus = XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
+				   XUARTPS_IMR_OFFSET);
+
+	IsrStatus &= XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
+				   XUARTPS_ISR_OFFSET);
+
+	u32 CsrRegister;
+	/*
+	* Read the Channel Status Register to determine if there is any data in
+	 * the RX FIFO
+	 */
+	CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
+				XUARTPS_SR_OFFSET);
+
+	while (0 == (CsrRegister & XUARTPS_SR_RXEMPTY)) {
+		secondaryBuf[secondaryEnd] = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
+		secondaryEnd += 1;
+		CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_SR_OFFSET);
+		MIO7_led_on();
+	}
+
+	// Clear the interrupt status.
+	XUartPs_WriteReg(InstancePtr->Config.BaseAddress, XUARTPS_ISR_OFFSET,
+			IsrStatus);
 }
 
 /**************************************************************************/
@@ -172,11 +238,9 @@ void process_received(modular_structs_t *structs) {
 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) {
+	if (Event == XUARTPS_EVENT_RECV_DATA || Event == XUARTPS_EVENT_RECV_TOUT
+			|| Event == XUARTPS_IXR_RXOVR || Event == XUARTPS_IXR_RXFULL) {
 		received_in_buf = EventData;
-		if (EventData >= UART_BUF_SIZE) {
-			MIO7_led_on();
-		}
 	}
 }
 

quad/sw/comm_dev/src/new_comm.h

@@ -18,8 +18,9 @@
 #include "mio7_led.h"
 #include "timer.h"
 #include "commands.h"
-#include "circ_buffer.h"
 
 int initUartComms();
+void process_received(modular_structs_t *structs);
+void uartInterruptHandler(XUartPs *InstancePtr);
 
 #endif /* NEW_COMM_H_ */

quad/sw/comm_dev/src/uart.c

@@ -214,7 +214,7 @@ char uart_recvByte(XUartPs* uartps_ptr) {
 * @note		None.
 *
 ****************************************************************************/
-int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId)
+int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler)
 {
 	int Status;
 
@@ -246,7 +246,7 @@ int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId)
 	 * performs the specific interrupt processing for the device
 	 */
 	Status = XScuGic_Connect(&Intc, UartIntrId,
-				  (Xil_ExceptionHandler) XUartPs_InterruptHandler,
+				  handler,
 				  (void *) UartInstancePtr);
 	if (Status != XST_SUCCESS) {
 		return XST_FAILURE;
@@ -279,8 +279,8 @@ XUartPs* uart0_init(u16 deviceID, int baudRate){
 }
 
 // Initializes the interrupt system for UART 0
-int uart0_int_init(u16 UartIntrId) {
-	return SetupInterruptSystem(_Uart0PS, UartIntrId);
+int uart0_int_init(u16 UartIntrId, Xil_ExceptionHandler handler) {
+	return SetupInterruptSystem(_Uart0PS, UartIntrId, handler);
 }
 
 void uart0_clearFIFOs(){