quad/sw/modular_quad_pid/src/new_log_data.c

+/*
+ * log_data.c
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+ 
+/*
+ #include "log_data.h"
+ 
+// Current index of the log array
+int arrayIndex = 0;
+// Size of the array
+int arraySize = LOG_STARTING_SIZE;
+int resized = 0;
+
+// The number of times we resized the array
+int resizeCount = 0;
+
+// Pointer to point to the array with all the logging information
+// for now its not dynamic
+log_t logArray[LOG_STARTING_SIZE * 3];// up to 60 seconds of log
+
+int log_data(log_t* log_struct)
+{
+	updateLog(*log_struct);
+    return 0;
+}
+
+*
+ * Fills up an xbox hueg amount of memory with log data
+
+void updateLog(log_t log_struct){
+	// If the first iteration, allocate enough memory for "arraySize" elements of logging
+//	if(logArray == NULL){
+//		// size in memory is 1,720,320 bytes (1.64 megabytes) because logging struct is 420 bytes each
+//		// up to 20 seconds of log before resizing
+//		logArray = malloc(LOG_STARTING_SIZE * sizeof(log_t));
+//		uart0_sendStr("initialized log array.\n");
+//		sleep(1);
+//	}
+
+	// semi dynamic log
+//	if((arrayIndex >= arraySize - 1) && (!resized)){
+//		realloc(logArray, LOG_STARTING_SIZE * 3 * sizeof(log_t)); // up to 60 seconds of log
+//		resized = 1;
+//		arraySize = LOG_STARTING_SIZE * 3;
+//		uart0_sendStr("resized log array.\n");
+//		sleep(1);
+//	}
+
+	if(arrayIndex >= arraySize - 1)
+	{
+		return;
+	}
+
+	// Add log to the array
+	logArray[arrayIndex++] = log_struct;
+
+	// If the index is too big, reallocate memory to double the size as before
+//	if(arrayIndex == arraySize){
+//		arraySize *= 2;
+//		logArray = (log_t *) realloc(logArray, arraySize * sizeof(log_t));
+//		++resizeCount;
+//	}
+//	else if(arrayIndex > arraySize){
+//		// Something fishy has occured
+//		xil_printf("Array index is out of bounds. This shouldn't happen but somehow you did the impossible\n\r");
+//	}
+}
+
+
+*
+ * Prints all the log information.
+ *
+ * TODO: This should probably be transmitting in binary instead of ascii
+
+
+void printLogging(){
+	int i, numBytes;
+	char buf[2304] = {};
+	char comments[256] = {};
+	char header[1024] = {};
+	char units [1024] = {};
+
+	char tempLog[4096*2] = {};
+
+	sprintf(comments, "# MicroCART On-board Quad Log\r\n# Sample size: %d\r\n", arrayIndex);
+	sprintf(header, "%%Time\t" "LoopPeriod\t"
+
+			//current points (measurements)
+			"X_Current_Position\t" "Y_Current_Position\t" "Z_Current_Position\t"
+			"Cam_Meas_Roll\tCam_Meas_Pitch\tCam_Meas_Yaw\t"
+			"Quad_Meas_Roll\tQuad_Meas_Pitch\t"
+			"roll_velocity\tpitch_velocity\tyaw_velocity\t"
+
+			//setpoints
+			"X_setpoint\t" "Y_setpoint\t" "Z_setpoint\t"
+			"Roll_setpoint\tPitch_setpoint\tYaw_setpoint\t"
+			"Roll_vel_setpoint\tPitch_vel_setpoint\tYaw_vel_setpoint\t"
+
+			//corrections
+			"PID_x\t"
+			"PID_y\t"
+			"PID_z\t"
+			"PID_roll\t"
+			"PID_pitch\t"
+			"PID_yaw\t"
+			"PID_roll_vel\t"
+			"PID_pitch_vel\t"
+			"PID_yaw_vel\t"
+
+			//trims
+			"Roll_trim\tPitch_trim\tYaw_trim\tThrottle_trim\t"
+
+			//motor commands
+			"Motor_0\tMotor_1\tMotor_2\tMotor_3\n"
+
+					);
+
+
+	sprintf(units,	"&sec\tsec\t"
+
+			//current points
+			"meters\tmeters\tmeters\t"
+			"radians\tradians\tradians\t"
+			"radians\tradians\t"
+			"radians//sec\tradians//sec\tradians//sec\t"
+
+			//setpoints
+			"meters\tmeters\tmeters\t"
+			"radians\tradians\tradians\t"
+			"radians//sec\tradians//sec\tradians//sec\t"
+
+			//corrections
+			"radians\tradians\tradians\t"
+			"radians//sec\tradians//sec\tradians//sec\t"
+			"none\tnone\tnone\t"
+
+			//trims
+			"none\tnone\tnone\tnone\t"
+
+			//motors
+			"none\tnone\tnone\tnone\n"
+
+					);
+
+	strcat(buf,comments);
+	strcat(buf,header);
+	strcat(buf,units);
+
+
+	numBytes = logData(buf, tempLog);
+	uart0_sendBytes(tempLog, strlen(tempLog));
+	usleep(100000);
+
+	***********************
+	 print & send log data
+	for(i = 0; i < arrayIndex; i++){
+			char* logLine = format(logArray[i]);
+			numBytes = logData(logLine, tempLog);
+			uart0_sendBytes(tempLog, numBytes);
+			usleep(10000);
+			//free(logLine);
+			//break;
+	}
+}
+
+
+char* format(log_t log){
+	char *retString = malloc(4096*2);
+
+			sprintf(retString, 	"%.3f\t%.4f\t" //Time and TimeSlice
+
+								// current points
+								"%.3f\t%.3f\t%.3f\t"
+								"%.3f\t%.3f\t%.3f\t"
+								"%.3f\t%.3f\t"
+								"%.3f\t%.3f\t%.3f\t"
+
+								//setpoints
+								"%.3f\t%.3f\t%.3f\t"
+								"%.3f\t%.3f\t%.3f\t"
+								"%.3f\t%.3f\t%.3f\t"
+
+								//corrections
+								"%.3f\t%.3f\t%.3f\t"
+								"%.3f\t%.3f\t%.3f\t"
+								"%.3f\t%.3f\t%.3f\t"
+
+								//trims
+								"%d\t%d\t%d\t%d\t"
+
+								//motors
+								"%d\t%d\t%d\t%d\n"
+
+								,log.time_stamp, log.time_slice,
+
+								// current points
+								log.local_x_PID.current_point,log.local_y_PID.current_point, log.altitude_PID.current_point,
+								log.currentQuadPosition.roll, log.currentQuadPosition.pitch, log.currentQuadPosition.yaw,
+								log.roll_angle_filtered, log.pitch_angle_filtered,
+								log.phi_dot, log.theta_dot, log.psi_dot,
+
+								//setpoints
+								log.local_x_PID.setpoint, log.local_y_PID.setpoint, log.altitude_PID.setpoint,
+								log.angle_roll_PID.setpoint, log.angle_pitch_PID.setpoint, log.angle_yaw_PID.setpoint,
+								log.ang_vel_roll_PID.setpoint, log.ang_vel_pitch_PID.setpoint, log.ang_vel_pitch_PID.setpoint,
+
+								//corrections
+								log.local_x_PID_values.pid_correction, log.local_y_PID_values.pid_correction, log.altitude_PID_values.pid_correction,
+								log.angle_roll_PID_values.pid_correction, log.angle_pitch_PID_values.pid_correction, log.angle_yaw_PID_values.pid_correction,
+								log.ang_vel_roll_PID_values.pid_correction, log.ang_vel_pitch_PID_values.pid_correction, log.ang_vel_yaw_PID_values.pid_correction,
+
+								//trims
+								log.trims.roll, log.trims.pitch, log.trims.yaw, log.trims.throttle,
+
+								//motors
+								log.motors[0], log.motors[1], log.motors[2], log.motors[3]
+			);
+
+
+		return retString;
+}
+*/

quad/sw/modular_quad_pid/src/new_log_data.h

+/*
+ * log_data.h
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+
+#ifndef LOG_DATA_H_
+#define LOG_DATA_H_
+ 
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "PID.h"
+#include "type_def.h"
+#include "uart.h"
+#include "sleep.h"
+#include "communication.h"
+
+#define LOG_STARTING_SIZE 4096 //262144 // 2^18      32768  2^15
+
+
+/**
+ * @brief 
+ *      Logs the data obtained throughout the controller loop.
+ *
+ * @param log_struct
+ *      structure of the data to be logged
+ *
+ * @return 
+ *      error message
+ *
+ */
+ int log_data(log_t* log_struct);
+
+ /**
+  * Fills up an xbox hueg amount of memory with log data
+  */
+ void updateLog(log_t log_struct);
+
+ /**
+  * Prints all the log information.
+  */
+ void printLogging();
+
+ char* format(log_t log);
+
+#endif /* LOG_DATA_H_ */

quad/sw/modular_quad_pid/src/old_log_data.h

+/*
+ * log_data.h
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+
+#ifndef LOG_DATA_H_
+#define LOG_DATA_H_
+ 
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "PID.h"
+#include "type_def.h"
+#include "uart.h"
+#include "sleep.h"
+#include "communication.h"
+
+#define LOG_STARTING_SIZE 4096 //262144 // 2^18      32768  2^15
+
+
+/**
+ * @brief 
+ *      Logs the data obtained throughout the controller loop.
+ *
+ * @param log_struct
+ *      structure of the data to be logged
+ *
+ * @return 
+ *      error message
+ *
+ */
+ int log_data(log_t* log_struct);
+
+ /**
+  * Fills up an xbox hueg amount of memory with log data
+  */
+ void updateLog(log_t log_struct);
+
+ /**
+  * Prints all the log information.
+  */
+ void printLogging();
+
+ char* format(log_t log);
+
+#endif /* LOG_DATA_H_ */

quad/sw/modular_quad_pid/src/packet_processing.c

+/*
+ * process_packet.c
+ *
+ *  Created on: Mar 2, 2016
+ *      Author: ucart
+ */
+#include "packet_processing.h"
+#include "uart.h"
+#include "type_def.h"
+#include "sleep.h"
+#include "util.h"
+#include "communication.h"
+
+#define DEBUG 0
+
+tokenList_t tokenize(char* cmd) {
+	int maxTokens = 16;
+	tokenList_t ret;
+	ret.numTokens = 0;
+	ret.tokens = malloc(sizeof(char *)* 20 * maxTokens);
+
+	int i = 0;
+	ret.tokens[0] = NULL;
+	char* token = strtok(cmd, " ");
+	while (token != NULL && i < maxTokens - 1) {
+		ret.tokens[i] = malloc(strlen(token) + 10);
+		strcpy(ret.tokens[i], token);
+		ret.tokens[++i] = NULL;
+		ret.numTokens++;
+		token = strtok(NULL, " ");
+	}
+
+	return ret;
+}
+
+int processUpdate(unsigned char* update, quadPosition_t* currentQuadPosition) {
+	//static char buf[16384];
+	//sprintf(buf, "update..(%d)..[%s]\r\n", strlen(update), update);
+	//uart0_sendStr(buf);
+
+	unsigned char * data;
+	metadata_t md;
+	parse_packet(update, &data, &md);
+
+
+	// Packet must come as [NEARPY], 4 bytes each
+	int packetId = getInt(data, 0);
+//	printf("Packet ID: %d\n", packetId);
+	float y_pos = getFloat(data, 4);
+//	printf("y_pos: %f\n", y_pos);
+	float x_pos = getFloat(data, 8);
+//	printf("x_pos: %f\n", x_pos);
+	float alt_pos = getFloat(data, 12);
+//	printf("alt_pos: %f\n", alt_pos);
+	float roll = getFloat(data, 16);
+//	printf("roll: %f\n", roll);
+	float pitch = getFloat(data, 20);
+//	printf("pitch: %f\n", pitch);
+	float yaw = getFloat(data, 24);
+//	printf("yaw: %f\n", yaw);
+
+	currentQuadPosition->packetId = packetId;
+	currentQuadPosition->y_pos = y_pos;
+	currentQuadPosition->x_pos = x_pos;
+	currentQuadPosition->alt_pos = alt_pos;
+	currentQuadPosition->roll = roll;
+	currentQuadPosition->pitch = pitch;
+	currentQuadPosition->yaw = yaw;
+
+	return 0;
+}
+
+float getFloat(unsigned char* str, int pos) {
+	union {
+		float f;
+		int i;
+	} x;
+	x.i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
+	return x.f;
+}
+
+int getInt(unsigned char* str, int pos) {
+	int i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
+	return i;
+}

quad/sw/modular_quad_pid/src/packet_processing.h

+/*
+ * process_packet.h
+ *
+ *  Created on: Mar 2, 2016
+ *      Author: ucart
+ */
+
+#ifndef PROCESS_PACKET_H_
+#define PROCESS_PACKET_H_
+
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include "type_def.h"
+
+tokenList_t tokenize(char* cmd);
+int processUpdate(unsigned char* update, quadPosition_t* currentQuadPosition);
+//int processCommand(stringBuilder_t * sb, setpoint_t * setpoint_struct, parameter_t * parameter_struct);
+int doProcessing(char* cmd, tokenList_t * tokens, setpoint_t * setpoint_struct, parameter_t * parameter_struct);
+float getFloat(unsigned char* str, int pos);
+int getInt(unsigned char* str, int pos);
+
+#endif /* PROCESS_PACKET_H_ */

quad/sw/modular_quad_pid/src/platform.c

+/******************************************************************************
+*
+* (c) Copyright 2010-2012 Xilinx, Inc. All rights reserved.
+*
+* This file contains confidential and proprietary information of Xilinx, Inc.
+* and is protected under U.S. and international copyright and other
+* intellectual property laws.
+*
+* DISCLAIMER
+* This disclaimer is not a license and does not grant any rights to the
+* materials distributed herewith. Except as otherwise provided in a valid
+* license issued to you by Xilinx, and to the maximum extent permitted by
+* applicable law: (1) THESE MATERIALS ARE MADE AVAILABLE "AS IS" AND WITH ALL
+* FAULTS, AND XILINX HEREBY DISCLAIMS ALL WARRANTIES AND CONDITIONS, EXPRESS,
+* IMPLIED, OR STATUTORY, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
+* MERCHANTABILITY, NON-INFRINGEMENT, OR FITNESS FOR ANY PARTICULAR PURPOSE;
+* and (2) Xilinx shall not be liable (whether in contract or tort, including
+* negligence, or under any other theory of liability) for any loss or damage
+* of any kind or nature related to, arising under or in connection with these
+* materials, including for any direct, or any indirect, special, incidental,
+* or consequential loss or damage (including loss of data, profits, goodwill,
+* or any type of loss or damage suffered as a result of any action brought by
+* a third party) even if such damage or loss was reasonably foreseeable or
+* Xilinx had been advised of the possibility of the same.
+*
+* CRITICAL APPLICATIONS
+* Xilinx products are not designed or intended to be fail-safe, or for use in
+* any application requiring fail-safe performance, such as life-support or
+* safety devices or systems, Class III medical devices, nuclear facilities,
+* applications related to the deployment of airbags, or any other applications
+* that could lead to death, personal injury, or severe property or
+* environmental damage (individually and collectively, "Critical
+* Applications"). Customer assumes the sole risk and liability of any use of
+* Xilinx products in Critical Applications, subject only to applicable laws
+* and regulations governing limitations on product liability.
+*
+* THIS COPYRIGHT NOTICE AND DISCLAIMER MUST BE RETAINED AS PART OF THIS FILE
+* AT ALL TIMES.
+*
+******************************************************************************/
+
+#include "xparameters.h"
+#include "xil_cache.h"
+
+#include "platform_config.h"
+
+/*
+ * Uncomment the following line if ps7 init source files are added in the
+ * source directory for compiling example outside of SDK.
+ */
+/*#include "ps7_init.h"*/
+
+#ifdef STDOUT_IS_16550
+ #include "xuartns550_l.h"
+
+ #define UART_BAUD 9600
+#endif
+
+void
+enable_caches()
+{
+#ifdef __PPC__
+    Xil_ICacheEnableRegion(CACHEABLE_REGION_MASK);
+    Xil_DCacheEnableRegion(CACHEABLE_REGION_MASK);
+#elif __MICROBLAZE__
+#ifdef XPAR_MICROBLAZE_USE_ICACHE
+    Xil_ICacheEnable();
+#endif
+#ifdef XPAR_MICROBLAZE_USE_DCACHE
+    Xil_DCacheEnable();
+#endif
+#endif
+}
+
+void
+disable_caches()
+{
+    Xil_DCacheDisable();
+    Xil_ICacheDisable();
+}
+
+void
+init_uart()
+{
+#ifdef STDOUT_IS_16550
+    XUartNs550_SetBaud(STDOUT_BASEADDR, XPAR_XUARTNS550_CLOCK_HZ, UART_BAUD);
+    XUartNs550_SetLineControlReg(STDOUT_BASEADDR, XUN_LCR_8_DATA_BITS);
+#endif
+#ifdef STDOUT_IS_PS7_UART
+    /* Bootrom/BSP configures PS7 UART to 115200 bps */
+#endif
+}
+
+void
+init_platform()
+{
+    /*
+     * If you want to run this example outside of SDK,
+     * uncomment the following line and also #include "ps7_init.h" at the top.
+     * Make sure that the ps7_init.c and ps7_init.h files are included
+     * along with this example source files for compilation.
+     */
+    /* ps7_init();*/
+    enable_caches();
+    init_uart();
+}
+
+void
+cleanup_platform()
+{
+    disable_caches();
+}

quad/sw/modular_quad_pid/src/platform.h

+/*
+ * Copyright (c) 2008 Xilinx, Inc.  All rights reserved.
+ *
+ * Xilinx, Inc.
+ * XILINX IS PROVIDING THIS DESIGN, CODE, OR INFORMATION "AS IS" AS A
+ * COURTESY TO YOU.  BY PROVIDING THIS DESIGN, CODE, OR INFORMATION AS
+ * ONE POSSIBLE   IMPLEMENTATION OF THIS FEATURE, APPLICATION OR
+ * STANDARD, XILINX IS MAKING NO REPRESENTATION THAT THIS IMPLEMENTATION
+ * IS FREE FROM ANY CLAIMS OF INFRINGEMENT, AND YOU ARE RESPONSIBLE
+ * FOR OBTAINING ANY RIGHTS YOU MAY REQUIRE FOR YOUR IMPLEMENTATION.
+ * XILINX EXPRESSLY DISCLAIMS ANY WARRANTY WHATSOEVER WITH RESPECT TO
+ * THE ADEQUACY OF THE IMPLEMENTATION, INCLUDING BUT NOT LIMITED TO
+ * ANY WARRANTIES OR REPRESENTATIONS THAT THIS IMPLEMENTATION IS FREE
+ * FROM CLAIMS OF INFRINGEMENT, IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ */
+
+#ifndef __PLATFORM_H_
+#define __PLATFORM_H_
+
+#include "platform_config.h"
+
+void init_platform();
+void cleanup_platform();
+
+#endif

quad/sw/modular_quad_pid/src/platform_config.h

+#ifndef __PLATFORM_CONFIG_H_
+#define __PLATFORM_CONFIG_H_
+
+#define STDOUT_IS_PS7_UART
+#define UART_DEVICE_ID 0
+#ifdef __PPC__
+#define CACHEABLE_REGION_MASK 0xf0000001
+#endif
+
+#endif

quad/sw/modular_quad_pid/src/quadposition.h

+#ifndef _QUADPOSITION_H
+#define _QUADPOSITION_H
+
+
+
+#endif /* _QUADPOSITION_H */

quad/sw/modular_quad_pid/src/send_actuator_commands.c

+/*
+ * send_actuator_commands.c
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+ 
+#include "send_actuator_commands.h"
+ 
+int send_actuator_commands(log_t* log_struct, actuator_command_t* actuator_command_struct)
+{
+	int i;
+	// write the PWMs to the motors
+	for (i = 0; i < 4; i++)
+		pwm_write_channel(actuator_command_struct->pwms[i], i);
+
+    log_struct->motors[0] = actuator_command_struct->pwms[0];
+    log_struct->motors[1] = actuator_command_struct->pwms[1];
+    log_struct->motors[2] = actuator_command_struct->pwms[2];
+    log_struct->motors[3] = actuator_command_struct->pwms[3];
+
+    return 0;
+}
+ 

quad/sw/modular_quad_pid/src/send_actuator_commands.h

+/*
+ * send_actuator_commands.h
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+
+#ifndef SEND_ACTUATOR_COMMANDS_H_
+#define SEND_ACTUATOR_COMMANDS_H_
+ 
+#include <stdio.h>
+
+#include "log_data.h"
+#include "actuator_command_processing.h"
+
+/**
+ * @brief 
+ *      Sends commands to the actuators.
+ *
+ * @param log_struct
+ *      structure of the data to be logged
+ *
+ * @param actuator_command_struct
+ *      structure of the commmands to go to the actuators
+ *
+ * @return 
+ *      error message
+ *
+ */
+int send_actuator_commands(log_t* log_struct, actuator_command_t* actuator_command_struct);
+
+#endif /* SEND_ACTUATOR_COMMANDS_H_ */

quad/sw/modular_quad_pid/src/sensor.c

+/*
+ * sensor.c
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+ 
+#include "sensor.h"
+#include "communication.h"
+#include "commands.h"
+
+int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
+{
+	if(startMPU9150() == -1)
+		return -1;
+
+	// read sensor board and fill in gryo/accelerometer/magnetometer struct
+	get_gam_reading(&(raw_sensor_struct->gam));
+
+	// Sets the first iteration to be at the accelerometer value since gyro initializes to {0,0,0} regardless of orientation
+	sensor_struct->pitch_angle_filtered = raw_sensor_struct->gam.accel_roll;
+	sensor_struct->roll_angle_filtered = raw_sensor_struct->gam.accel_pitch;
+
+    return 0;
+}
+
+int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t* raw_sensor_struct)
+{
+	// if there was a new update packet this loop then do the required processing
+	if (user_input_struct->hasPacket == 0x04) {
+		processUpdate((unsigned char *) user_input_struct->sb->buf, &(raw_sensor_struct->currentQuadPosition));
+	}
+
+//	///////// for testing update period of vrpn data from the ground station to the quad
+//	static int update_counter = 0;
+//	if(user_input_struct->hasPacket == 0x04)
+//	{
+//		char buf[200] = {};
+//		// Send a reply to the ground station
+//		snprintf(buf, sizeof(buf), "Received an update packet %dms\r\n", 5 * update_counter);
+//		unsigned char *responsePacket;
+//
+//		metadata_t metadata =
+//		{
+//			BEGIN_CHAR,
+//			MessageTypes[5].ID,
+//			MessageTypes[5].subtypes[1].ID,
+//			0,
+//			(strlen(buf) + 1)
+//		};
+//		formatPacket(&metadata, buf, &responsePacket);
+//
+//		// Send each byte of the packet individually
+//		int i;
+//		for(i = 0; i < 8 + metadata.data_len; i++) {
+//			// Debug print statement for all of the bytes being sent
+////			printf("%d: 0x%x\n", i, responsePacket[i]);
+//
+//			uart0_sendByte(responsePacket[i]);
+//		}
+//		update_counter = 0;
+//	}
+//	else
+//	{
+//		update_counter++;
+//	}
+//
+//	/////////// end testing
+
+
+	// the the sensor board and fill in the readings into the GAM struct
+	get_gam_reading(&(raw_sensor_struct->gam));
+
+	log_struct->gam = raw_sensor_struct->gam;
+
+    return 0;
+}
+ 

quad/sw/modular_quad_pid/src/sensor.h

+/*
+ * sensor.h
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+
+#ifndef SENSOR_H_
+#define SENSOR_H_
+ 
+#include <stdio.h>
+
+#include "log_data.h"
+#include "user_input.h"
+#include "iic_mpu9150_utils.h"
+#include "packet_processing.h"
+#include "uart.h"
+
+/**
+ * @brief
+ *      Initializes the sensors.
+ *
+ * @return
+ *      error message
+ *
+ */
+int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct);
+
+/**
+ * @brief
+ *      Recieves data from the sensors.
+ *
+ * @param log_struct
+ *      structure of the data to be logged
+ *
+ * @param user_input_struct
+ *      structure of the input from the user
+ *
+ * @param raw_sensor_struct
+ *      structure of the raw values from the sensors
+ *
+ * @return 
+ *      error message
+ *
+ */
+int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t* raw_sensor_struct);
+
+#endif /* SENSOR_TEMPLATE_H_ */

quad/sw/modular_quad_pid/src/sensor_processing.c

+/*
+ * sensor_processing.c
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+ 
+#include "sensor_processing.h"
+#include "timer.h"
+#include <math.h>
+
+int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_sensor_t* raw_sensor_struct, sensor_t* sensor_struct)
+{
+	// copy currentQuadPosition and trimmedRCValues from raw_sensor_struct to sensor_struct
+	deep_copy_Qpos(&(sensor_struct->currentQuadPosition), &(raw_sensor_struct->currentQuadPosition));
+
+	// Psuedo-Nonlinear Extension for determining local x/y position based on yaw angle
+	// provided by Matt Rich
+	//
+	// local x/y/z is the moving frame of reference on the quad that we are transforming so we can assume yaw angle is 0 (well enough)
+	// 		for the autonomous position controllers
+	//
+	// camera given x/y/z is the inertia frame of reference (the global coordinates)
+	//
+	// |local x|	|cos(yaw angle)  -sin(yaw angle)  0| |camera given x|
+	// |local y|  = |sin(yaw angle)   cos(yaw angle)  0| |camera given y|
+	// |local z|	|       0               0         1| |camera given z|
+
+	sensor_struct->currentQuadPosition.x_pos =
+			sensor_struct->currentQuadPosition.x_pos * cos(sensor_struct->currentQuadPosition.yaw) +
+			sensor_struct->currentQuadPosition.y_pos * -sin(sensor_struct->currentQuadPosition.yaw);
+
+	sensor_struct->currentQuadPosition.y_pos =
+			sensor_struct->currentQuadPosition.x_pos * sin(sensor_struct->currentQuadPosition.yaw) +
+			sensor_struct->currentQuadPosition.y_pos * cos(sensor_struct->currentQuadPosition.yaw);
+
+
+	// Calculate Euler angles and velocities using Gimbal Equations below
+	/////////////////////////////////////////////////////////////////////////
+	// | Phi_d   |   |  1  sin(Phi)tan(theta)    cos(Phi)tan(theta) |  | p |
+	// | theta_d | = |  0  cos(Phi)              -sin(Phi)		    |  | q |
+	// | Psi_d   |   |  0  sin(Phi)sec(theta)    cos(Phi)sec(theat) |  | r |
+	//
+	// Phi_dot = p + q sin(Phi) tan(theta) + r cos(Phi) tan(theta)
+	// theta_dot = q cos(Phi) - r sin(Phi)
+	// Psi_dot = q sin(Phi) sec(theta) + r cos(Phi) sec(theta)
+	///////////////////////////////////////////////////////////////////////////
+
+	// javey:
+	//
+	// The gimbal equations are defined in the book "Flight Simulation" by Rolfe and Staples.
+	// Find on page 46, equation 3.6
+
+	// these are calculated to be used in the gimbal equations below
+	// the variable roll(pitch)_angle_filtered is phi(theta)
+	double sin_phi = sin(sensor_struct->roll_angle_filtered);
+	double cos_phi = cos(sensor_struct->roll_angle_filtered);
+	double tan_theta = tan(sensor_struct->pitch_angle_filtered);
+	double sec_theta = 1/cos(sensor_struct->pitch_angle_filtered);
+
+//	Gryo "p" is the angular velocity rotation about the x-axis (defined as var gyro_xVel_p in gam struct)
+//	Gyro "q" is the angular velocity rotation about the y-axis (defined as var gyro_xVel_q in gam struct)
+//	Gyro "r" is the angular velocity rotation about the z-axis (defined as var gyro_xVel_r in gam struct)
+
+	// phi is the conventional symbol used for roll angle, so phi_dot is the roll velocity
+	sensor_struct->phi_dot = raw_sensor_struct->gam.gyro_xVel_p + (raw_sensor_struct->gam.gyro_yVel_q*sin_phi*tan_theta)
+			+ (raw_sensor_struct->gam.gyro_zVel_r*cos_phi*tan_theta);
+
+	// theta is the conventional symbol used for pitch angle, so theta_dot is the pitch velocity
+	sensor_struct->theta_dot = (raw_sensor_struct->gam.gyro_yVel_q*cos_phi)
+			- (raw_sensor_struct->gam.gyro_zVel_r*sin_phi);
+
+	// psi is the conventional symbol used for yaw angle, so psi_dot is the yaw velocity
+	sensor_struct->psi_dot = (raw_sensor_struct->gam.gyro_yVel_q*sin_phi*sec_theta)
+			+ (raw_sensor_struct->gam.gyro_zVel_r*cos_phi*sec_theta);
+
+	// Complementary Filter Calculations
+	sensor_struct->pitch_angle_filtered = 0.98 * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * LOOP_TIME)
+			+ 0.02 * raw_sensor_struct->gam.accel_pitch;
+
+	sensor_struct->roll_angle_filtered = 0.98 * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* LOOP_TIME)
+			+ 0.02 * raw_sensor_struct->gam.accel_roll;
+
+//	static int loop_counter = 0;
+//	loop_counter++;
+//
+//	if(loop_counter == 50)
+//	{
+//		char dMsg[100] = {};
+//		sprintf(dMsg, "Loop time: %.4f\n", LOOP_TIME);
+//		uart0_sendStr(dMsg);
+//		loop_counter = 0;
+//	}
+
+	//logging
+	log_struct->currentQuadPosition = sensor_struct->currentQuadPosition;
+	log_struct->roll_angle_filtered = sensor_struct->roll_angle_filtered;
+	log_struct->pitch_angle_filtered = sensor_struct->pitch_angle_filtered;
+	log_struct->phi_dot = sensor_struct->phi_dot;
+	log_struct->theta_dot = sensor_struct->theta_dot;
+	log_struct->psi_dot = sensor_struct->psi_dot;
+	return 0;
+}
+
+void set_pitch_angle_filtered(sensor_t * sensor_struct, float accel_roll)
+{
+	sensor_struct->pitch_angle_filtered = accel_roll;
+}
+void set_roll_angle_filtered(sensor_t * sensor_struct, float accel_pitch)
+{
+	sensor_struct->roll_angle_filtered = accel_pitch;
+}
+
+void deep_copy_Qpos(quadPosition_t * dest, quadPosition_t * src)
+{
+	dest->packetId = src->packetId;
+	dest->y_pos = src->y_pos;
+	dest->x_pos = src->x_pos;
+	dest->alt_pos = src->alt_pos;
+	dest->roll = src->roll;
+	dest->pitch = src->pitch;
+	dest->yaw = src->yaw;
+
+}

quad/sw/modular_quad_pid/src/sensor_processing.h

+/*
+ * sensor_processing.h
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+
+#ifndef SENSOR_PROCESSING_H_
+#define SENSOR_PROCESSING_H_
+ 
+#include <stdio.h>
+#include "log_data.h"
+#include "sensor.h"
+#include "conversion.h"
+#include "quadposition.h"
+/*
+ * Aero channel declaration
+ */
+
+#define THROTTLE 0
+#define ROLL     1
+#define PITCH    2
+#define YAW      3
+#define GEAR 	 4
+#define FLAP 	 5
+
+/**
+ * Signals from the Rx mins, maxes and ranges
+ */
+//#define THROTTLE_MAX  191900
+//#define THROTTLE_MIN  110200
+//#define THROTTLE_RANGE THROTTLE_MAX - THROTTLE_MIN
+//
+//#define ROLL_MAX      170200
+////#define ROLL_MAX      167664
+////#define ROLL_CENTER   148532
+//#define ROLL_MIN      129400
+//#define ROLL_CENTER   149800
+//#define ROLL_RANGE    ROLL_MAX - ROLL_MIN
+//
+////#define PITCH_MAX     190800
+//#define PITCH_MAX     169900
+////#define PITCH_MIN     135760
+//#define PITCH_MIN     129500
+////#define PITCH_CENTER  152830
+//#define PITCH_CENTER  149700
+//#define PITCH_RANGE   PITCH_MAX - PITCH_MIN
+//
+//#define YAW_MAX       169400
+//#define YAW_MIN       129300
+//#define YAW_CENTER   149800
+//#define YAW_RANGE     YAW_MAX - YAW_MIN
+//
+//#define GEAR_1	170800
+//#define GEAR_0	118300
+//
+//#define FLAP_1		192000
+//#define FLAP_0		107600
+//
+//#define GEAR_KILL     GEAR_0 // The kill point for the program
+//#define BASE          150000
+//
+//#define min 100000
+//#define max 200000
+
+/**
+ * @brief 
+ *      Processes the data from the sensors.
+ *
+ * @param log_struct
+ *      structure of the data to be logged
+ *
+ * @param raw_sensor_struct
+ *      structure of the raw data from the sensors
+ *
+ * @param sensor_struct
+ *      structure of the processed data from the sensors
+ *
+ * @return 
+ *      error message
+ *
+ */
+
+int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_sensor_t * raw_sensor_struct, sensor_t* sensor_struct);
+void deep_copy_Qpos(quadPosition_t * dest, quadPosition_t * src);
+void set_pitch_angle_filtered(sensor_t * sensor_struct, float accel_roll);
+void set_roll_angle_filtered(sensor_t * sensor_struct, float accel_roll);
+
+#endif /* SENSOR_PROCESSING_H_ */

quad/sw/modular_quad_pid/src/stringBuilder.c

+/*
+ * stringBuilder.c
+ *
+ *  Created on: Sep 24, 2014
+ *      Author: ucart
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include "stringBuilder.h"
+
+int stringBuilder_maybeExpand(stringBuilder_t* sb) {
+	if (!sb || !sb->buf) {
+		return STRINGBUILDER_ILLEGALARGUMENT;
+	}
+
+	// See if we need to expand
+	if (sb->length + 1 >= sb->capacity) {
+		if (sb->capacity >= sb->maxCapacity) {
+			// Would exceed maxCapacity, can't do anything
+			return STRINGBUILDER_AT_MAX_CAPACITY;
+		}
+
+		// Compute new size (try doubling)
+		int newCapacity = sb->capacity * 2;
+		if (newCapacity <= 2) {
+			newCapacity = 2;
+		}
+		if (newCapacity >= sb->maxCapacity) {
+			newCapacity = sb->maxCapacity;
+		}
+
+		// Get a pointer to the old buf mem
+		char* oldBuf = sb->buf;
+		sb->buf = malloc(newCapacity);
+		if (!sb->buf) {
+			// Agh, no mem for buf. Restore the old one and return error
+			sb->buf = oldBuf;
+			return STRINGBUILDER_NO_MEM_FOR_EXPANSION;
+		} else {
+			// Got mem for new buf, copy from old buf
+			strncpy(sb->buf, oldBuf, sb->length + 1);
+			sb->capacity = newCapacity;
+			free(oldBuf);
+		}
+	}
+
+	return STRINGBUILDER_SUCCESS;
+}
+
+stringBuilder_t* stringBuilder_createWithMaxCapacity(int initialCapacity,
+		int maxCapacity) {
+	// Invalid arguments
+	if (initialCapacity > maxCapacity || initialCapacity < 1
+			|| maxCapacity < 1) {
+		return NULL ;
+	}
+
+	stringBuilder_t* sb = (stringBuilder_t*) malloc(sizeof(stringBuilder_t));
+	if (!sb) {
+		// No mem for buffer
+		return NULL ;
+	}
+
+	// Try to allocate mem for buf
+	sb->buf = malloc(initialCapacity);
+	if (!sb->buf) {
+		// No mem for buf
+		free(sb);
+		return NULL ;
+	}
+
+	// Set up function pointers
+	sb->addStr = stringBuilder_addStr;
+	sb->addStrAt = stringBuilder_addStrAt;
+	sb->addChar = stringBuilder_addChar;
+	sb->addCharAt = stringBuilder_addCharAt;
+	sb->removeCharAt = stringBuilder_removeCharAt;
+	sb->clear = stringBuilder_clear;
+
+
+	// Good to go
+	sb->length = 0;
+	sb->capacity = initialCapacity;
+	sb->maxCapacity = maxCapacity;
+	sb->buf[0] = '\0';
+	return sb;
+}
+
+stringBuilder_t* stringBuilder_createWithInitialCapacity(int initialCapacity) {
+	return stringBuilder_createWithMaxCapacity(initialCapacity,
+			STRINGBUILDER_DEFAULT_MAX_CAPACITY);
+}
+
+stringBuilder_t* stringBuilder_create() {
+	return stringBuilder_createWithMaxCapacity(
+			STRINGBUILDER_DEFAULT_INITIAL_CAPACITY,
+			STRINGBUILDER_DEFAULT_MAX_CAPACITY);
+}
+
+void stringBuilder_free(stringBuilder_t* sb) {
+	if (sb && sb->buf) {
+		free(sb->buf);
+	}
+	if (sb) {
+		free(sb);
+	}
+}
+
+int stringBuilder_addStr(stringBuilder_t* sb, char* str) {
+	if (!sb || !sb->buf) {
+		return STRINGBUILDER_ILLEGALARGUMENT;
+	}
+
+	while (*str) {
+		int status = sb->addChar(sb, *str);
+		if (status != STRINGBUILDER_SUCCESS) {
+			return status;
+		}
+		str++;
+	}
+
+	return STRINGBUILDER_SUCCESS;
+}
+
+int stringBuilder_addStrAt(stringBuilder_t* sb, char* str, int index) {
+	if (!sb || !sb->buf) {
+		return STRINGBUILDER_ILLEGALARGUMENT;
+	}
+
+	while (*str) {
+		int status = sb->addCharAt(sb, *str, index);
+		if (status != STRINGBUILDER_SUCCESS) {
+			return status;
+		}
+
+		str++;
+		index++;
+	}
+
+	return STRINGBUILDER_SUCCESS;
+}
+
+/** Add a character to end of the StringBuilder */
+int stringBuilder_addChar(stringBuilder_t* sb, char c) {
+	if (!sb || !sb->buf) {
+		return STRINGBUILDER_ILLEGALARGUMENT;
+	}
+
+	return stringBuilder_addCharAt(sb, c, sb->length);
+}
+
+/** Add a character to the StringBuilder at the specified index, if possible */
+int stringBuilder_addCharAt(stringBuilder_t* sb, char c, int index) {
+	if (!sb || !sb->buf || index < 0 || index > sb->length) {
+		return STRINGBUILDER_ILLEGALARGUMENT;
+	}
+
+	// Try expanding if necessary
+	int status = stringBuilder_maybeExpand(sb);
+	if (status != STRINGBUILDER_SUCCESS) {
+		return status;
+	}
+
+	// Move everything right of index over by 1
+	int i;
+	for (i = sb->length; i >= index; i--) {
+		sb->buf[i + 1] = sb->buf[i];
+	}
+
+	// Insert the character and add the null terminator
+	sb->buf[index] = c;
+	sb->length += 1;
+
+	return STRINGBUILDER_SUCCESS;
+}
+
+/** Remove a character from the StringBuilder at the specified index */
+int stringBuilder_removeCharAt(stringBuilder_t* sb, int index) {
+	if (!sb || !sb->buf || index < 0 || index >= sb->length) {
+		return STRINGBUILDER_ILLEGALARGUMENT;
+	}
+
+	// Move everything right of index over left
+	int i;
+	for (i = index; i < sb->length; i++) {
+		sb->buf[i] = sb->buf[i + 1];
+	}
+	sb->length -= 1;
+
+	return STRINGBUILDER_SUCCESS;
+}
+
+void stringBuilder_clear(stringBuilder_t* sb) {
+	sb->length = 0;
+	sb->buf[0] = '\0';
+}
+

quad/sw/modular_quad_pid/src/stringBuilder.h

+/*
+ * stringBuffer.h
+ *
+ *  Created on: Sep 24, 2014
+ *      Author: ucart
+ */
+
+#ifndef STRINGBUILDER_H_
+#define STRINGBUILDER_H_
+
+#include "type_def.h"
+
+#define STRINGBUILDER_DEFAULT_INITIAL_CAPACITY 16
+#define STRINGBUILDER_DEFAULT_MAX_CAPACITY 1024
+
+enum {
+	STRINGBUILDER_SUCCESS = 0,
+	STRINGBUILDER_AT_MAX_CAPACITY = 1,
+	STRINGBUILDER_NO_MEM_FOR_EXPANSION = 2,
+	STRINGBUILDER_ILLEGALARGUMENT = 3
+};
+
+/***** Constructors *****/
+
+/** Create a StringBuilder with a max/initial capacity specified */
+stringBuilder_t* stringBuilder_createWithMaxCapacity(int, int);
+
+/** Create a StringBuilder with an initial capacity specified */
+stringBuilder_t* stringBuilder_createWithInitialCapacity(int);
+
+/** Create a StringBuilder with default initial capacity/max capacity */
+stringBuilder_t* stringBuilder_create();
+
+
+/** Free a StringBuilder */
+void stringBuilder_free(stringBuilder_t*);
+
+
+/***** Methods *****/
+
+/** Add a string to the end of the StringBuilder */
+int stringBuilder_addStr(stringBuilder_t*, char*);
+
+/** Add a string to the StringBuilder at the specified index */
+int stringBuilder_addStrAt(stringBuilder_t*, char*, int);
+
+/** Add a character to end of the StringBuilder */
+int stringBuilder_addChar(stringBuilder_t*, char);
+
+/** Add a character to the StringBuilder at the specified index */
+int stringBuilder_addCharAt(stringBuilder_t*, char, int);
+
+/** Remove a character from the StringBuilder at the specified index */
+int stringBuilder_removeCharAt(stringBuilder_t*, int);
+
+/** Clear a stringBuilder */
+void stringBuilder_clear(stringBuilder_t*);
+
+#endif /* STRINGBUILDER_H_ */

quad/sw/modular_quad_pid/src/timer.c

+/*
+ * timer.c
+ *
+ *  Created on: Feb 24, 2016
+ *      Author: ucart
+ */
+
+
+
+#include "timer.h"
+
+XTime before = 0, after = 0;
+XTmrCtr axi_timer;
+float LOOP_TIME;
+float time_stamp = 0;
+
+int timer_init()
+{
+
+	// using a axi_timer core because we've had problems with the Global Timer
+	XTmrCtr_Initialize(&axi_timer, XPAR_AXI_TIMER_0_DEVICE_ID);
+
+	return 0;
+}
+
+int timer_start_loop()
+{
+	//timing code
+	LOOP_TIME = ((float)(after - before)) / ((float) COUNTS_PER_SECOND);
+	XTime_GetTime(&before);
+	XTmrCtr_Reset(&axi_timer, 0);
+	XTmrCtr_Start(&axi_timer, 0);
+
+	return 0;
+}
+
+int timer_end_loop(log_t *log_struct)
+{
+	// get number of useconds its taken to run the loop thus far
+	int usec_loop = XTmrCtr_GetValue(&axi_timer, 0) / (PL_CLK_CNTS_PER_USEC);
+
+	// attempt to make each loop run for the same amount of time
+	while(usec_loop < DESIRED_USEC_PER_LOOP)
+	{
+		usec_loop = XTmrCtr_GetValue(&axi_timer, 0) / (PL_CLK_CNTS_PER_USEC);
+	}
+
+//	static int counter = 0;
+//	counter++;
+//	if(counter % 50 == 0)
+//		printf("usec_loop: %d\n", usec_loop);
+
+	//timing code
+	XTime_GetTime(&after);
+	time_stamp += LOOP_TIME;
+	XTmrCtr_Stop(&axi_timer, 0);
+
+	// for timing debugging, its a separate hardware PL timer not associated with the PS
+	// used this to compare to the PS clock to make sure the timing matched
+//	float axi_timer_val = ((float)XTmrCtr_GetValue(&axi_timer, 0)) / ((float)100000000);
+
+
+	// Log the timing information
+	log_struct->time_stamp = time_stamp;
+	log_struct->time_slice = LOOP_TIME;
+
+	return 0;
+}

quad/sw/modular_quad_pid/src/timer.h

+/*
+ * timer.h
+ *
+ *  Created on: Feb 24, 2016
+ *      Author: ucart
+ */
+
+#ifndef TIMER_H_
+#define TIMER_H_
+
+#include "log_data.h"
+#include "xtime_l.h"
+#include <xtmrctr.h>
+
+extern XTime before;
+extern XTime after;
+extern XTmrCtr axi_timer;
+extern float LOOP_TIME;
+extern float time_stamp;
+
+// desired loop time is not guaranteed (its possible that the loop may take longer than desired)
+#define DESIRED_USEC_PER_LOOP 5000 // gives 5ms loops
+
+#define PL_CLK_CNTS_PER_USEC 100
+
+/**
+ * @brief
+ *      Initializes the items necessary for loop timing.
+ *
+ * @return
+ *      error message
+ *
+ */
+int timer_init();
+
+/**
+ * @brief
+ *      Does processing of the loop timer at the beginning of the control loop.
+ *
+ * @return
+ *      error message
+ *
+ */
+int timer_start_loop();
+
+/**
+ * @brief
+ *      Does processing of the loop timer at the end of the control loop.
+ *
+ * @return
+ *      error message
+ *
+ */
+int timer_end_loop(log_t *log_struct);
+
+#endif /* TIMER_H_ */

quad/sw/modular_quad_pid/src/type_def.h

+/*
+ * struct_def.h
+ *
+ *  Created on: Mar 2, 2016
+ *      Author: ucart
+ */
+
+#ifndef TYPE_DEF_H_
+#define TYPE_DEF_H_
+
+/**
+ * @brief
+ *      The modes for autonomous and manual flight.
+ *
+ */
+enum flight_mode{
+	AUTO_FLIGHT_MODE,
+	MANUAL_FLIGHT_MODE
+};
+
+//----------------------------------------------------------------------------------------------
+//     index||     0    |     1    |      2      |  3 & 4 |      5 & 6       |  7+  |   end    |
+//---------------------------------------------------------------------------------------------|
+// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
+//-------------------------------------------------------------------------------------------- |
+//     bytes||     1    |     1    |      1      |    2   |        2         | var  |    1     |
+//----------------------------------------------------------------------------------------------
+typedef struct {
+	char begin_char;
+	char msg_type;
+	char msg_subtype;
+	int msg_id;
+	int data_len;
+} metadata_t;
+
+
+// String builder data type
+typedef struct stringBuilder_s {
+	char* buf;
+	int length;
+	int capacity;
+	int maxCapacity;
+
+	// Methods
+	int (*addStr)(struct stringBuilder_s*, char*);
+	int (*addStrAt)(struct stringBuilder_s*, char*, int);
+	int (*addChar)(struct stringBuilder_s*, char);
+	int (*addCharAt)(struct stringBuilder_s*, char, int);
+	int (*removeCharAt)(struct stringBuilder_s*, int);
+	void (*clear)(struct stringBuilder_s*);
+} stringBuilder_t;
+
+typedef struct {
+	char** tokens;
+	int numTokens;
+} tokenList_t;
+
+typedef struct commands{
+	int pitch, roll, yaw, throttle;
+}commands;
+
+typedef struct raw{
+	int x,y,z;
+}raw;
+typedef struct PID_Consts{
+	float P, I, D;
+}PID_Consts;
+
+//Camera system info
+typedef struct {
+	int packetId;
+
+	double y_pos;
+	double x_pos;
+	double alt_pos;
+
+	double yaw;
+	double roll;
+	double pitch;
+} quadPosition_t;
+
+typedef struct {
+	float yaw;
+	float roll;
+	float pitch;
+	float throttle;
+} quadTrims_t;
+
+//Gyro, accelerometer, and magnetometer data structure
+//Used for reading an instance of the sensor data
+typedef struct {
+
+	// GYRO
+	//Xint16 raw_gyro_x, raw_gyro_y, raw_gyro_z;
+
+	float gyro_xVel_p; // In degrees per second
+	float gyro_yVel_q;
+	float gyro_zVel_r;
+
+	// ACCELEROMETER
+	//Xint16 raw_accel_x, raw_accel_y, raw_accel_z;
+
+	float accel_x; //In g
+	float accel_y;
+	float accel_z;
+
+	float accel_roll;
+	float accel_pitch;
+
+
+	// MAG
+	//Xint16 raw_mag_x, raw_mag_y, raw_mag_z;
+
+	float heading; // In degrees
+
+	float mag_x; //Magnetic north: ~50 uT
+	float mag_y;
+	float mag_z;
+
+
+
+}gam_t;
+
+typedef struct PID_t {
+	double current_point;	// Current value of the system
+	double setpoint;		// Desired value of the system
+	float Kp;				// Proportional constant
+	float Ki;				// Integral constant
+	float Kd;				// Derivative constant
+	double prev_error;		// Previous error
+	double acc_error;		// Accumulated error
+	double pid_correction;	// Correction factor computed by the PID
+	float dt; 				// sample period
+} PID_t;
+
+typedef struct PID_values{
+	float P;	// The P component contribution to the correction output
+	float I;	// The I component contribution to the correction output
+	float D;	// The D component contribution to the correction output
+	float error; // the error of this PID calculation
+	float change_in_error; // error change from the previous calc. to this one
+	float pid_correction; // the correction output (P + I + D)
+} PID_values;
+
+///////// MAIN MODULAR STRUCTS
+/**
+ * @brief
+ *      Holds the data inputed by the user
+ *
+ */
+typedef struct user_input_t {
+	int rc_commands[6]; 	// Commands from the RC transmitter
+
+
+//	float cam_x_pos;	// Current x position from the camera system
+//	float cam_y_pos;	// Current y position from the camera system
+//	float cam_z_pos;	// Current z position from the camera system
+//	float cam_roll;		// Current roll angle from the camera system
+//	float cam_pitch;	// Current pitch angle from the camera system
+//	float cam_yaw;		// Current yaw angle from the camera system
+
+	float yaw_manual_setpoint;
+	float roll_angle_manual_setpoint;
+	float pitch_angle_manual_setpoint;
+
+	int hasPacket;
+	stringBuilder_t * sb;
+} user_input_t;
+
+/**
+ * @brief
+ *      Holds the log data to be sent to the ground station. It may hold the
+ * timestamp of when a sensor's data was obtained.
+ *
+ */
+typedef struct log_t {
+	// Time
+	float time_stamp;
+	float time_slice;
+
+	// Id
+	int packetId;
+
+	gam_t gam; 	// Raw and calculated gyro, accel, and mag values are all in gam_t
+	float phi_dot, theta_dot, psi_dot; // gimbal equation values
+
+	quadPosition_t currentQuadPosition;
+
+	float roll_angle_filtered, pitch_angle_filtered;
+	float lidar_altitude;
+
+	float pid_P_component, pid_I_component, pid_D_component; // use these generically for any PID that you are testing
+
+	// PID coefficients and errors
+	PID_t local_x_PID, local_y_PID, altitude_PID;
+	PID_t angle_yaw_PID, angle_roll_PID, angle_pitch_PID;
+	PID_t ang_vel_yaw_PID, ang_vel_roll_PID, ang_vel_pitch_PID;
+
+	PID_values local_x_PID_values, local_y_PID_values, altitude_PID_values;
+	PID_values angle_yaw_PID_values, angle_roll_PID_values, angle_pitch_PID_values;
+	PID_values ang_vel_yaw_PID_values, ang_vel_roll_PID_values, ang_vel_pitch_PID_values;
+
+	// RC commands
+	commands commands;
+
+	//trimmed values
+	quadTrims_t trims;
+
+	int motors[4];
+
+} log_t;
+
+/**
+ * @brief
+ *      Holds the raw data from the sensors and the timestamp if available
+ *
+ */
+typedef struct raw_sensor {
+	int acc_x;		// accelerometer x data
+	int acc_x_t;	// time of accelerometer x data
+
+	int acc_y;		// accelerometer y data
+	int acc_y_t;	// time of accelerometer y data
+
+	int acc_z;		// accelerometer z data
+	int acc_z_t;	// time of accelerometer z data
+
+
+	int gyr_x;		// gyroscope x data
+	int gyr_x_t;	// time of gyroscope x data
+
+	int gyr_y;		// gyroscope y data
+	int gyr_y_t;	// time of gyroscope y data
+
+	int gyr_z;		// gyroscope z data
+	int gyr_z_t;	// time of gyroscope z data
+
+	int ldr_z;		//lidar z data (altitude)
+	int ldr_z_t;	//time of lidar z data
+
+	gam_t gam;
+
+	// Structures to hold the current quad position & orientation
+	quadPosition_t currentQuadPosition;
+
+} raw_sensor_t;
+
+/**
+ * @brief
+ *      Holds the processed data from the sensors and the timestamp if available
+ *
+ */
+typedef struct sensor {
+	int acc_x;		// accelerometer x data
+	int acc_x_t;	// time of accelerometer x data
+
+	int acc_y;		// accelerometer y data
+	int acc_y_t;	// time of accelerometer y data
+
+	int acc_z;		// accelerometer z data
+	int acc_z_t;	// time of accelerometer z data
+
+
+	int gyr_x;		// gyroscope x data
+	int gyr_x_t;	// time of gyroscope x data
+
+	int gyr_y;		// gyroscope y data
+	int gyr_y_t;	// time of gyroscope y data
+
+	int gyr_z;		// gyroscope z data
+	int gyr_z_t;	// time of gyroscope z data
+
+	int ldr_z;		//lidar z data (altitude)
+	int ldr_z_t;	//time of lidar z data
+
+	float pitch_angle_filtered;
+	float roll_angle_filtered;
+	float lidar_altitude;
+
+	float phi_dot, theta_dot, psi_dot;
+
+	// Structures to hold the current quad position & orientation
+	quadPosition_t currentQuadPosition;
+	quadTrims_t trims;
+
+} sensor_t;
+
+/**
+ * @brief
+ *      Holds the setpoints to be used in the controller
+ *
+ */
+typedef struct setpoint_t {
+	quadPosition_t desiredQuadPosition;
+} setpoint_t;
+
+/**
+ * @brief
+ *      Holds the parameters that are specific to whatever type of
+ *      control algorithm is being used
+ *
+ */
+typedef struct parameter_t {
+	PID_t roll_angle_pid, roll_ang_vel_pid;
+	PID_t pitch_angle_pid, pitch_ang_vel_pid;
+	PID_t yaw_ang_vel_pid;
+	PID_t local_x_pid;
+	PID_t local_y_pid;
+	PID_t yaw_angle_pid;
+	PID_t alt_pid;
+} parameter_t;
+
+/**
+ * @brief
+ *      Holds user defined data for the controller
+ *
+ */
+typedef struct user_defined_t {
+	int flight_mode;
+	int engaging_auto;
+} user_defined_t;
+
+/**
+ * @brief
+ *      Holds the raw actuator values before processing
+ *
+ */
+typedef struct raw_actuator_t {
+
+	int controller_corrected_motor_commands[6];
+
+} raw_actuator_t;
+
+/**
+ * @brief
+ *      Holds processed commands to go to the actuators
+ *
+ */
+typedef struct actuator_command_t {
+	int pwms[4];
+} actuator_command_t;
+
+/**
+ * @brief
+ * 		Structures to be used throughout
+ */
+typedef struct {
+	user_input_t user_input_struct;
+	log_t log_struct;
+	raw_sensor_t raw_sensor_struct;
+	sensor_t sensor_struct;
+	setpoint_t setpoint_struct;
+	parameter_t parameter_struct;
+	user_defined_t user_defined_struct;
+	raw_actuator_t raw_actuator_struct;
+	actuator_command_t actuator_command_struct;
+}modular_structs_t;
+
+//////// END MAIN MODULAR STRUCTS
+
+#endif /* TYPE_DEF_H_ */