quad/sw/modular_quad_pid/src/gam.h

+#ifndef _GAM_H
+#define _GAM_H
+
+#include "xbasic_types.h"
+
+//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;
+#endif /* _GAM_H */

quad/sw/modular_quad_pid/src/iic_mpu9150_utils.c

+/**
+ * IIC_MPU9150_UTILS.c
+ *
+ * Utility functions for using I2C on a Diligent Zybo board and
+ * focused on the SparkFun MPU9150
+ *
+ * For function descriptions please see iic_mpu9150_utils.h
+ *
+ * Author: 	ucart
+ * Created: 01/20/2015
+ */
+
+#include <stdio.h>
+#include <sleep.h>
+#include <math.h>
+
+#include "xparameters.h"
+#include "iic_mpu9150_utils.h"
+#include "xbasic_types.h"
+#include "xiicps.h"
+
+XIicPs_Config* i2c_config;
+XIicPs I2C0;
+double magX_correction = -1, magY_correction, magZ_correction;
+
+int initI2C0(){
+
+	//Make sure CPU_1x clk is enabled for I2C controller
+	Xuint16* aper_ctrl = (Xuint16*) IO_CLK_CONTROL_REG_ADDR;
+
+	if(*aper_ctrl & 0x00040000){
+		xil_printf("CPU_1x is set to I2C0\r\n");
+	}
+
+	else{
+		xil_printf("CPU_1x is not set to I2C0..Setting now\r\n");
+		*aper_ctrl |= 0x00040000;
+	}
+
+
+	// Look up
+	i2c_config = XIicPs_LookupConfig(XPAR_PS7_I2C_0_DEVICE_ID);
+
+	XStatus status = XIicPs_CfgInitialize(&I2C0, i2c_config, i2c_config->BaseAddress);
+
+	// Check if initialization was successful
+	if(status != XST_SUCCESS){
+		printf("ERROR (initI2C0): Initializing I2C0\r\n");
+		return -1;
+	}
+
+	// Reset the controller and set the clock to 400kHz
+	XIicPs_Reset(&I2C0);
+	XIicPs_SetSClk(&I2C0, 400000);
+
+
+	return 0;
+}
+
+int startMPU9150(){
+
+	// Device Reset & Wake up
+	iic0Write(0x6B, 0x80);
+	usleep(5000);
+
+	// Set clock reference to Z Gyro
+	iic0Write(0x6B, 0x03);
+	// Configure Digital Low/High Pass filter
+	iic0Write(0x1A,0x06); // Level 4 low pass on gyroscope
+
+	// Configure Gyro to 2000dps, Accel. to +/-8G
+	iic0Write(0x1B, 0x18);
+	iic0Write(0x1C, 0x10);
+
+	// Enable I2C bypass for AUX I2C (Magnetometer)
+	iic0Write(0x37, 0x02);
+
+	// Setup Mag
+	iic0Write(0x37, 0x02);             //INT_PIN_CFG   -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=0 ; CLKOUT_EN=0
+
+	usleep(100000);
+
+	int i;
+	gam_t temp_gam;
+
+	// Do about 20 reads to warm up the device
+	for(i=0; i < 20; ++i){
+		if(get_gam_reading(&temp_gam) == -1){
+			printf("ERROR (startMPU9150): error occured while getting GAM data\r\n");
+			return -1;
+		}
+		usleep(1000);
+	}
+
+	return 0;
+}
+
+void stopMPU9150(){
+
+	//Put MPU to sleep
+	iic0Write(0x6B, 0b01000000);
+}
+
+void iic0Write(u8 register_addr, u8 data){
+
+	u16 device_addr = MPU9150_DEVICE_ADDR;
+	u8 buf[] = {register_addr, data};
+
+	// Check if within register range
+	if(register_addr < 0 || register_addr > 0x75){
+		printf("ERROR (iic0Write) : Cannot write to register address, 0x%x: out of bounds\r\n", register_addr);
+		return;
+	}
+
+	if(register_addr <= 0x12){
+		device_addr = MPU9150_COMPASS_ADDR;
+	}
+
+	XIicPs_MasterSendPolled(&I2C0, buf, 2, device_addr);
+
+}
+
+void iic0Read(u8* recv_buffer, u8 register_addr, int size){
+
+	u16 device_addr = MPU9150_DEVICE_ADDR;
+	u8 buf[] = {register_addr};
+
+	// Check if within register range
+	if(register_addr < 0 || register_addr > 0x75){
+		printf("ERROR (iic0Read): Cannot read register address, 0x%x: out of bounds\r\n", register_addr);
+	}
+
+	// Set device address to the if 0x00 <= register address <= 0x12
+	if(register_addr <= 0x12){
+		device_addr = MPU9150_COMPASS_ADDR;
+	}
+
+
+	XIicPs_MasterSendPolled(&I2C0, buf, 1, device_addr);
+	XIicPs_MasterRecvPolled(&I2C0, recv_buffer,size,device_addr);
+}
+
+void CalcMagSensitivity(){
+
+	u8 buf[3];
+	u8 ASAX, ASAY, ASAZ;
+
+	// Quickly read from the factory ROM to get correction coefficents
+	iic0Write(0x0A, 0x0F);
+	usleep(10000);
+
+	// Read raw adjustment values
+	iic0Read(buf, 0x10,3);
+	ASAX = buf[0];
+	ASAY = buf[1];
+	ASAZ = buf[2];
+
+	// Set the correction coefficients
+	magX_correction = (ASAX-128)*0.5/128 + 1;
+	magY_correction = (ASAY-128)*0.5/128 + 1;
+	magZ_correction = (ASAZ-128)*0.5/128 + 1;
+}
+
+
+void ReadMag(gam_t* gam){
+
+	u8 mag_data[6];
+	Xint16 raw_magX, raw_magY, raw_magZ;
+
+	// Grab calibrations if not done already
+	if(magX_correction == -1){
+		CalcMagSensitivity();
+	}
+
+	// Set Mag to single read mode
+	iic0Write(0x0A, 0x01);
+	usleep(10000);
+	mag_data[0] = 0;
+
+	// Keep checking if data is ready before reading new mag data
+	while(mag_data[0] == 0x00){
+		iic0Read(mag_data, 0x02, 1);
+	}
+
+	// Get mag data
+	iic0Read(mag_data, 0x03, 6);
+
+	raw_magX = (mag_data[1] << 8) | mag_data[0];
+	raw_magY = (mag_data[3] << 8) | mag_data[2];
+	raw_magZ = (mag_data[5] << 8) | mag_data[4];
+
+	// Set magnetometer data to output
+	gam->mag_x = raw_magX * magX_correction;
+	gam->mag_y = raw_magY * magY_correction;
+	gam->mag_z = raw_magZ * magZ_correction;
+
+}
+
+/**
+ * Get Gyro Accel Mag (GAM) information
+ */
+int get_gam_reading(gam_t* gam) {
+
+	Xint16 raw_accel_x, raw_accel_y, raw_accel_z;
+	Xint16 gyro_x, gyro_y, gyro_z;
+
+	Xuint8 sensor_data[ACCEL_GYRO_READ_SIZE] = {};
+
+	// We should only get mag_data ~10Hz
+	//Xint8 mag_data[6] = {};
+
+	//readHandler = iic0_read_bytes(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
+	iic0Read(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
+
+	//Calculate accelerometer data
+	raw_accel_x = sensor_data[ACC_X_H] << 8 | sensor_data[ACC_X_L];
+	raw_accel_y = sensor_data[ACC_Y_H] << 8 | sensor_data[ACC_Y_L];
+	raw_accel_z = sensor_data[ACC_Z_H] << 8 | sensor_data[ACC_Z_L];
+
+	// put in G's
+	gam->accel_x = (raw_accel_x / 4096.0) + ACCEL_X_BIAS; // 4,096 is the gain per LSB of the measurement reading based on a configuration range of +-8g
+	gam->accel_y = (raw_accel_y / 4096.0) + ACCEL_Y_BIAS;
+	gam->accel_z = (raw_accel_z / 4096.0) + ACCEL_Z_BIAS;
+
+	//Get X and Y angles
+	// javey: this assigns accel_(pitch/roll) in units of radians
+	gam->accel_pitch = atan(gam->accel_x / sqrt(gam->accel_y*gam->accel_y + gam->accel_z*gam->accel_z));
+	gam->accel_roll = -atan(gam->accel_y / sqrt(gam->accel_x*gam->accel_x + gam->accel_z*gam->accel_z)); // negative because sensor board is upside down
+
+	//Convert gyro data to rate (we're only using the most 12 significant bits)
+	gyro_x = (sensor_data[GYR_X_H] << 8) | (sensor_data[GYR_X_L]); //* G_GAIN;
+	gyro_y = (sensor_data[GYR_Y_H] << 8 | sensor_data[GYR_Y_L]);// * G_GAIN;
+	gyro_z = (sensor_data[GYR_Z_H] << 8 | sensor_data[GYR_Z_L]);// * G_GAIN;
+
+	//Get the number of degrees
+	//javey: converted to radians to following SI units
+	gam->gyro_xVel_p = ((gyro_x / GYRO_SENS) * DEG_TO_RAD) + GYRO_X_BIAS;
+	gam->gyro_yVel_q = ((gyro_y / GYRO_SENS) * DEG_TO_RAD) + GYRO_Y_BIAS;
+	gam->gyro_zVel_r = ((gyro_z / GYRO_SENS) * DEG_TO_RAD) + GYRO_Z_BIAS;
+
+	return 0;
+
+}

quad/sw/modular_quad_pid/src/iic_mpu9150_utils.h

+/* iic_mpu9150_utils.h
+ *
+ * A header file for the prototyping constants used for
+ * the I2C Controller 0 (I2C0) on the Zybo Board
+ *
+ * This file is intended SOLELY for the Sparkfun MPU9150
+ * and the Diligent ZyBo Board
+ *
+ * Author: ucart
+ *
+ */
+
+
+#ifndef IIC_MPU9150_UTILS_H
+#define IIC_MPU9150_UTILS_H
+
+
+#include "xbasic_types.h"
+#include "xiicps.h"
+#include "type_def.h"
+
+// System configuration registers
+// (Please see Appendix B: System Level Control Registers in the Zybo TRM)
+#define IIC_SYSTEM_CONTROLLER_RESET_REG_ADDR 	(0xF8000224)
+#define IO_CLK_CONTROL_REG_ADDR 				(0xF800012C)
+
+// IIC0 Registers
+#define IIC0_CONTROL_REG_ADDR 		(0xE0004000)
+#define IIC0_STATUS_REG_ADDR 		(0xE0004004)
+#define IIC0_SLAVE_ADDR_REG 		(0xE0004008)
+#define IIC0_DATA_REG_ADDR 			(0xE000400C)
+#define IIC0_INTR_STATUS_REG_ADDR 	(0xE0004010)
+#define IIC0_TRANFER_SIZE_REG_ADDR	(0xE0004014)
+#define IIC0_INTR_EN			    (0xE0004024)
+#define IIC0_TIMEOUT_REG_ADDR 		(0xE000401C)
+
+// MPU9150 Sensor Defines (Address is defined on the Sparkfun MPU9150 Datasheet)
+#define MPU9150_DEVICE_ADDR 		0b01101000
+#define MPU9150_COMPASS_ADDR 		0x0C
+
+
+#define ACCEL_GYRO_READ_SIZE 		14		//Bytes
+#define ACCEL_GYRO_BASE_ADDR		0x3B	//Starting register address
+
+#define MAG_READ_SIZE 				6
+#define MAG_BASE_ADDR 				0x03
+
+#define RAD_TO_DEG 57.29578
+#define DEG_TO_RAD 0.0174533
+
+// Array indicies when reading from ACCEL_GYRO_BASE_ADDR
+#define ACC_X_H 0
+#define ACC_X_L 1
+#define ACC_Y_H 2
+#define ACC_Y_L 3
+#define ACC_Z_H 4
+#define ACC_Z_L 5
+
+#define GYR_X_H 8
+#define GYR_X_L 9
+#define GYR_Y_H 10
+#define GYR_Y_L 11
+#define GYR_Z_H 12
+#define GYR_Z_L 13
+
+#define MAG_X_L 0
+#define MAG_X_H 1
+#define MAG_Y_L 2
+#define MAG_Y_H 3
+#define MAG_Z_L 4
+#define MAG_Z_H 5
+
+//Interrupt Status Register Masks
+#define ARB_LOST       (0x200)
+#define RX_UNF          (0x80)
+#define TX_OVF          (0x40)
+#define RX_OVF          (0x20)
+#define SLV_RDY         (0x10)
+#define TIME_OUT        (0x08)
+#define NACK      		(0x04)
+#define MORE_DAT 	    (0x02)
+#define TRANS_COMPLETE 	(0x01)
+
+#define WRITE_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | TX_OVF | NACK)
+#define READ_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | RX_UNF | NACK)
+
+
+// Gyro is configured for +/-2000dps
+// Sensitivity gain is based off MPU9150 datasheet (pg. 11)
+#define GYRO_SENS 16.4
+
+#define GYRO_X_BIAS	0.005f
+#define GYRO_Y_BIAS	-0.014f
+#define GYRO_Z_BIAS	0.045f
+
+#define ACCEL_X_BIAS	0.023f
+#define ACCEL_Y_BIAS	0.009f
+#define ACCEL_Z_BIAS	0.087f
+
+// Initialize hardware; Call this FIRST before calling any other functions
+int initI2C0();
+
+void iic0Write(u8 register_addr, u8 data);
+void iic0Read(u8* recv_buffer, u8 register_addr, int size);
+
+
+// Wake up the MPU for data collection
+// Configure Gyro/Accel/Mag
+int startMPU9150();
+
+// Put MPU back to sleep
+void stopMPU9150();
+
+void CalcMagSensitivity();
+void ReadMag(gam_t* gam);
+void ReadGyroAccel(gam_t* gam);
+
+int get_gam_reading(gam_t* gam);
+
+
+/////////////
+// Deprecated functions below
+/////////////
+
+
+// Initialize hardware; Call this FIRST before calling any other functions
+void init_iic0();
+
+// Wake up the MPU for data collection
+void start_mpu9150();
+
+// Put MPU back to sleep
+void stop_mpu9150();
+
+
+// Write a byte of data at the given register address on the MPU
+void iic0_write(Xuint16 reg_addr, Xuint8 data);
+
+// Read a single byte at a given register address on the MPU
+Xuint8 iic0_read(Xuint16 reg_addr);
+
+// Read multiple bytes consecutively at a starting register address
+// places the resulting bytes in rv
+int iic0_read_bytes(Xuint8* rv, Xuint16 reg_addr, int bytes);
+
+// Helper function to initialize I2C0 controller on the Zybo board
+// Called by init_iic0
+void iic0_hw_init();
+
+
+// Clears the interrupt status register
+// Called by configuration functions
+void iic0_clear_intr_status();
+
+
+
+// Configure I2C0 controller on Zybo to receive data
+void iic0_config_ctrl_to_receive();
+
+// Configure I2C0 controller to transmit data
+void iic0_config_ctrl_to_transmit();
+
+
+void wake_mag();
+
+#endif /*IIC_MPU9150_UTILS_H*/

quad/sw/modular_quad_pid/src/initialize_components.c

+/*
+ * initialize_components.c
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+ 
+#include "initialize_components.h"
+#include "communication.h"
+
+extern int Xil_AssertWait;
+
+int protection_loops()
+{
+	int rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap
+
+	read_rec_all(rc_commands);
+
+	// wait for RC receiver to connect to transmitter
+	while(rc_commands[THROTTLE] < 75000)
+		read_rec_all(rc_commands);
+
+	// wait until throttle is low and the gear switch is engaged (so you don't immediately break out of the main loop below)
+	// also wait for the flight mode to be set to manual
+	while(rc_commands[THROTTLE] > 125000 || read_kill(rc_commands[GEAR]) || !read_flap(rc_commands[FLAP]))
+		read_rec_all(rc_commands);
+
+	// wait until the ground station has connected to the quad and acknowledged that its ready to start
+	stringBuilder_t * ack_packet = stringBuilder_create();
+	while(1)
+	{
+		// --------------------------------------
+		// Send request to ground station to start sending VRPN
+		char buf[255] = {};
+		int i;
+
+		// Debug print statement
+		//printf("function for yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
+
+		// Send a reply to the ground station
+		snprintf(buf, sizeof(buf), "The quad is ready to receive VRPN data.\r\n");
+		unsigned char *responsePacket;
+
+		metadata_t metadata =
+		{
+			BEGIN_CHAR,
+			MessageTypes[4].ID,
+			MessageTypes[4].subtypes[1].ID,
+			0,
+			(strlen(buf) + 1)
+		};
+		formatPacket(&metadata, buf, &responsePacket);
+
+		// Send each byte of the packet individually
+		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]);
+		}
+
+		usleep(10000);
+
+		tryReceivePacket(ack_packet, 0);
+
+		unsigned char * data;
+		metadata_t md;
+		parse_packet((unsigned char *) ack_packet->buf, &data, &md);
+
+		if(md.msg_type == 0x04 && md.msg_subtype == 0x01)
+			break;
+
+		// --------------------------------------
+	}
+
+	// let the pilot/observers know that the quad is now active
+	MIO7_led_on();
+
+	return 0;
+}
+
+int initializeAllComponents(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)
+{
+    // Turn off LED 7 to let observers know that the quad is not yet active
+	MIO7_led_off();
+
+	// Use the stringbuilder to keep track of data received
+	if(!(user_input_struct->sb = stringBuilder_create()))
+		return -1;
+
+	// Initialize the controller
+	control_algorithm_init(parameter_struct);
+
+	// Xilinx given initialization
+	init_platform();
+
+	//disable blocking asserts
+	//Xil_AssertWait = FALSE;
+
+	// Initialize UART0 (Bluetooth)
+	if(!uart0_init(XPAR_PS7_UART_0_DEVICE_ID, 921600))
+		return -1;
+
+	uart0_clearFIFOs();
+
+	//Enable blocking asserts
+	//Xil_AssertWait = TRUE;
+
+	// Initialize I2C controller and start the sensor board
+	if (initI2C0() == -1) {
+		return -1;
+	}
+
+	// Initialize PWM Recorders and Motor outputs
+	pwm_init();
+
+	// Initialize loop timers
+	timer_init();
+
+	//manual flight mode
+	user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
+
+	// Get the first loop data from accelerometer for the gyroscope to use
+	if(sensor_init(raw_sensor_struct, sensor_struct) == -1)
+		return -1;
+
+	return 0;
+}

quad/sw/modular_quad_pid/src/initialize_components.h

+/*
+ * initialize_components.h
+ *
+ *  Created on: Nov 12, 2015
+ *      Author: ucart
+ */
+
+#ifndef INITALIZE_COMPONENTS_H_
+#define INITALIZE_COMPONENTS_H_
+
+#include "timer.h"
+#include "control_algorithm.h"
+#include "platform.h"
+#include "uart.h"
+#include "iic_mpu9150_utils.h"
+#include "util.h"
+#include "controllers.h"
+
+/**
+ * @brief 
+ *      Runs loops to make sure the quad is responding and in the correct state before starting.
+ *
+ * @return
+ *      error message
+ *
+ */
+int protection_loops();
+
+/**
+ * @brief
+ *      Initializes the sensors, communication, and anything else that needs
+ * initialization.
+ *
+ * @return 
+ *      error message
+ *
+ */
+int initializeAllComponents(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);
+
+#endif /* INITALIZE_COMPONENTS_H_ */
+

quad/sw/modular_quad_pid/src/log_data.c

+/*
+ * log_data.c
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+ 
+#include "log_data.h"
+#include "communication.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;//, j;
+	char buf[2304] = {};
+	char comments[256] = {};
+	char header[1024] = {};
+	char units [1024] = {};
+
+	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);
+
+	// Send a reply to the ground station
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[0].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+//	printf("Checksum: 0x%02x", responsePacket[metadata.data_len + 7]);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		uart0_sendByte(responsePacket[i]);
+		if(i < 8 || i == metadata.data_len + 8)
+			printf("%d: 0x%02x\n", i, responsePacket[i]);
+	}
+
+	//uart0_sendBytes(buf, strlen(buf));
+	//usleep(100000);
+
+	/*************************/
+	/* print & send log data */
+	for(i = 0; i < arrayIndex; i++){
+		char* logLine = format(logArray[i]);
+
+
+		metadata_t metadata =
+		{
+			BEGIN_CHAR,
+			MessageTypes[5].ID,
+			MessageTypes[5].subtypes[0].ID,
+			0,
+			(strlen(logLine) + 1)
+		};
+		formatPacket(&metadata, (u8 *)logLine, &responsePacket);
+
+		// Send each byte of the packet individually
+//		for(j = 0; j < 8 + metadata.data_len; j++) {
+//			uart0_sendByte(responsePacket[j]);
+//			printf("%d: 0x%02x\n", j, responsePacket[j]);
+//		}
+//		uart0_sendBytes(responsePacket, 8 + metadata.data_len);
+
+		uart0_sendMetaData(metadata);
+		uart0_sendBytes(logLine, metadata.data_len);
+		//uart0_sendByte(0);
+		uart0_sendByte(responsePacket[7 + metadata.data_len]);
+
+//
+//		if((i % 5) == 0)
+		usleep(15000);
+
+		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
+								"%.2f\t%.2f\t%.2f\t%.2f\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/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"
+
+#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/lscript.ld

+/*******************************************************************/
+/*                                                                 */
+/* This file is automatically generated by linker script generator.*/
+/*                                                                 */
+/* Version: Xilinx EDK 14.7 EDK_P.20131013                                */
+/*                                                                 */
+/* Copyright (c) 2010 Xilinx, Inc.  All rights reserved.           */
+/*                                                                 */
+/* Description : Cortex-A9 Linker Script                          */
+/*                                                                 */
+/*******************************************************************/
+
+_STACK_SIZE = DEFINED(_STACK_SIZE) ? _STACK_SIZE : 0x100000;
+_HEAP_SIZE = DEFINED(_HEAP_SIZE) ? _HEAP_SIZE : 0x6400000;
+
+_ABORT_STACK_SIZE = DEFINED(_ABORT_STACK_SIZE) ? _ABORT_STACK_SIZE : 1024;
+_SUPERVISOR_STACK_SIZE = DEFINED(_SUPERVISOR_STACK_SIZE) ? _SUPERVISOR_STACK_SIZE : 2048;
+_FIQ_STACK_SIZE = DEFINED(_FIQ_STACK_SIZE) ? _FIQ_STACK_SIZE : 1024;
+_UNDEF_STACK_SIZE = DEFINED(_UNDEF_STACK_SIZE) ? _UNDEF_STACK_SIZE : 1024;
+
+/* Define Memories in the system */
+
+MEMORY
+{
+   ps7_ddr_0_S_AXI_BASEADDR : ORIGIN = 0x00100000, LENGTH = 0x1FF00000
+   ps7_ram_0_S_AXI_BASEADDR : ORIGIN = 0x00000000, LENGTH = 0x00030000
+   ps7_ram_1_S_AXI_BASEADDR : ORIGIN = 0xFFFF0000, LENGTH = 0x0000FE00
+}
+
+/* Specify the default entry point to the program */
+
+ENTRY(_vector_table)
+
+/* Define the sections, and where they are mapped in memory */
+
+SECTIONS
+{
+.text : {
+   *(.vectors)
+   *(.boot)
+   *(.text)
+   *(.text.*)
+   *(.gnu.linkonce.t.*)
+   *(.plt)
+   *(.gnu_warning)
+   *(.gcc_execpt_table)
+   *(.glue_7)
+   *(.glue_7t)
+   *(.vfp11_veneer)
+   *(.ARM.extab)
+   *(.gnu.linkonce.armextab.*)
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.init : {
+   KEEP (*(.init))
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.fini : {
+   KEEP (*(.fini))
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.rodata : {
+   __rodata_start = .;
+   *(.rodata)
+   *(.rodata.*)
+   *(.gnu.linkonce.r.*)
+   __rodata_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.rodata1 : {
+   __rodata1_start = .;
+   *(.rodata1)
+   *(.rodata1.*)
+   __rodata1_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.sdata2 : {
+   __sdata2_start = .;
+   *(.sdata2)
+   *(.sdata2.*)
+   *(.gnu.linkonce.s2.*)
+   __sdata2_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.sbss2 : {
+   __sbss2_start = .;
+   *(.sbss2)
+   *(.sbss2.*)
+   *(.gnu.linkonce.sb2.*)
+   __sbss2_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.data : {
+   __data_start = .;
+   *(.data)
+   *(.data.*)
+   *(.gnu.linkonce.d.*)
+   *(.jcr)
+   *(.got)
+   *(.got.plt)
+   __data_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.data1 : {
+   __data1_start = .;
+   *(.data1)
+   *(.data1.*)
+   __data1_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.got : {
+   *(.got)
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.ctors : {
+   __CTOR_LIST__ = .;
+   ___CTORS_LIST___ = .;
+   KEEP (*crtbegin.o(.ctors))
+   KEEP (*(EXCLUDE_FILE(*crtend.o) .ctors))
+   KEEP (*(SORT(.ctors.*)))
+   KEEP (*(.ctors))
+   __CTOR_END__ = .;
+   ___CTORS_END___ = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.dtors : {
+   __DTOR_LIST__ = .;
+   ___DTORS_LIST___ = .;
+   KEEP (*crtbegin.o(.dtors))
+   KEEP (*(EXCLUDE_FILE(*crtend.o) .dtors))
+   KEEP (*(SORT(.dtors.*)))
+   KEEP (*(.dtors))
+   __DTOR_END__ = .;
+   ___DTORS_END___ = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.fixup : {
+   __fixup_start = .;
+   *(.fixup)
+   __fixup_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.eh_frame : {
+   *(.eh_frame)
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.eh_framehdr : {
+   __eh_framehdr_start = .;
+   *(.eh_framehdr)
+   __eh_framehdr_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.gcc_except_table : {
+   *(.gcc_except_table)
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.mmu_tbl (ALIGN(16384)) : {
+   __mmu_tbl_start = .;
+   *(.mmu_tbl)
+   __mmu_tbl_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.ARM.exidx : {
+   __exidx_start = .;
+   *(.ARM.exidx*)
+   *(.gnu.linkonce.armexidix.*.*)
+   __exidx_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.preinit_array : {
+   __preinit_array_start = .;
+   KEEP (*(SORT(.preinit_array.*)))
+   KEEP (*(.preinit_array))
+   __preinit_array_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.init_array : {
+   __init_array_start = .;
+   KEEP (*(SORT(.init_array.*)))
+   KEEP (*(.init_array))
+   __init_array_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.fini_array : {
+   __fini_array_start = .;
+   KEEP (*(SORT(.fini_array.*)))
+   KEEP (*(.fini_array))
+   __fini_array_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.ARM.attributes : {
+   __ARM.attributes_start = .;
+   *(.ARM.attributes)
+   __ARM.attributes_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.sdata : {
+   __sdata_start = .;
+   *(.sdata)
+   *(.sdata.*)
+   *(.gnu.linkonce.s.*)
+   __sdata_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.sbss (NOLOAD) : {
+   __sbss_start = .;
+   *(.sbss)
+   *(.sbss.*)
+   *(.gnu.linkonce.sb.*)
+   __sbss_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.tdata : {
+   __tdata_start = .;
+   *(.tdata)
+   *(.tdata.*)
+   *(.gnu.linkonce.td.*)
+   __tdata_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.tbss : {
+   __tbss_start = .;
+   *(.tbss)
+   *(.tbss.*)
+   *(.gnu.linkonce.tb.*)
+   __tbss_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.bss (NOLOAD) : {
+   __bss_start = .;
+   *(.bss)
+   *(.bss.*)
+   *(.gnu.linkonce.b.*)
+   *(COMMON)
+   __bss_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+_SDA_BASE_ = __sdata_start + ((__sbss_end - __sdata_start) / 2 );
+
+_SDA2_BASE_ = __sdata2_start + ((__sbss2_end - __sdata2_start) / 2 );
+
+/* Generate Stack and Heap definitions */
+
+.heap (NOLOAD) : {
+   . = ALIGN(16);
+   _heap = .;
+   HeapBase = .;
+   _heap_start = .;
+   . += _HEAP_SIZE;
+   _heap_end = .;
+   HeapLimit = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.stack (NOLOAD) : {
+   . = ALIGN(16);
+   _stack_end = .;
+   . += _STACK_SIZE;
+   _stack = .;
+   __stack = _stack;
+   . = ALIGN(16);
+   _irq_stack_end = .;
+   . += _STACK_SIZE;
+   __irq_stack = .;
+   _supervisor_stack_end = .;
+   . += _SUPERVISOR_STACK_SIZE;
+   . = ALIGN(16);
+   __supervisor_stack = .;
+   _abort_stack_end = .;
+   . += _ABORT_STACK_SIZE;
+   . = ALIGN(16);
+   __abort_stack = .;
+   _fiq_stack_end = .;
+   . += _FIQ_STACK_SIZE;
+   . = ALIGN(16);
+   __fiq_stack = .;
+   _undef_stack_end = .;
+   . += _UNDEF_STACK_SIZE;
+   . = ALIGN(16);
+   __undef_stack = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+_end = .;
+}
+

quad/sw/modular_quad_pid/src/main.c

+/*
+ * main.c
+ *
+ *  Created on: Nov 11, 2015
+ *      Author: ucart
+ */
+#include <stdio.h>
+#include "timer.h"
+#include "log_data.h"
+#include "initialize_components.h"
+#include "user_input.h"
+#include "sensor.h"
+#include "sensor_processing.h"
+#include "control_algorithm.h"
+#include "actuator_command_processing.h"
+#include "send_actuator_commands.h"
+#include "update_gui.h"
+
+int main()
+{
+	// Structures to be used throughout
+	modular_structs_t structs = { };
+
+
+	// Initialize all required components and structs:
+	// Uart, PWM receiver/generator, I2C, Sensor Board
+	// Xilinx Platform, Loop Timer, Control Algorithm
+	int init_error = initializeAllComponents(&(structs.user_input_struct), &(structs.log_struct),
+			&(structs.raw_sensor_struct), &(structs.sensor_struct), &(structs.setpoint_struct), &(structs.parameter_struct),
+			&(structs.user_defined_struct), &(structs.raw_actuator_struct), &(structs.actuator_command_struct));
+
+	if (init_error != 0) {
+		printf("ERROR (main): Problem initializing...Goodbye\r\n");
+		return -1;
+	}
+
+	// Loops to make sure the quad is responding correctly before starting the control loop
+	protection_loops();
+
+	printf("The quad loop is now beginning.\n");
+
+	// Main control loop
+	do
+	{
+		// Processing of loop timer at the beginning of the control loop
+		timer_start_loop();
+
+		// Get the user input and put it into user_input_struct
+		get_user_input(&(structs.log_struct), &(structs.user_input_struct));
+
+		// Get data from the sensors and put it into raw_sensor_struct
+		get_sensors(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct));
+
+		// Process the sensor data and put it into sensor_struct
+		sensor_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct), &(structs.sensor_struct));
+
+		// Run the control algorithm
+		control_algorithm(&(structs.log_struct), &(structs.user_input_struct), &(structs.sensor_struct), &(structs.setpoint_struct),
+				&(structs.parameter_struct), &(structs.user_defined_struct), &(structs.raw_actuator_struct), &structs);
+
+		// Process the commands going to the actuators
+		actuator_command_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_actuator_struct), &(structs.actuator_command_struct));
+
+		// send the actuator commands
+		send_actuator_commands(&(structs.log_struct), &(structs.actuator_command_struct));
+
+		// update the GUI
+		update_GUI(&(structs.log_struct));
+
+		// Processing of loop timer at the end of the control loop
+		timer_end_loop(&(structs.log_struct));
+
+		if(structs.user_defined_struct.flight_mode == AUTO_FLIGHT_MODE)
+		{
+			// Log the data collected in this loop
+			log_data(&(structs.log_struct));
+
+			static int loop_counter = 0;
+			loop_counter++;
+
+			// toggle the MIO7 on and off to show that the quad is in AUTO_FLIGHT_MODE
+			if(loop_counter == 10)
+			{
+				MIO7_led_off();
+			}
+			else if(loop_counter >= 20)
+			{
+
+				MIO7_led_on();
+				loop_counter = 0;
+			}
+
+		}
+		if(structs.user_defined_struct.flight_mode == MANUAL_FLIGHT_MODE)
+			MIO7_led_on();
+
+	} while(!kill_condition(&(structs.user_input_struct)));
+
+
+	stringBuilder_free((structs.user_input_struct).sb);
+
+	pwm_kill();
+
+	MIO7_led_off();
+
+	printLogging();
+
+	flash_MIO_7_led(10, 100);
+
+	return 0;
+}
+
+

quad/sw/modular_quad_pid/src/mio7_led.c

+/*
+ * mio7_led.c
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: Amy Seibert
+ */
+ 
+ #include "mio7_led.h"
+ 
+void flash_MIO_7_led(int how_many_times, int ms_between_flashes)
+{
+	if(how_many_times <= 0)
+		return;
+
+	while(how_many_times)
+	{
+		MIO7_led_on();
+
+		usleep(ms_between_flashes * 500);
+
+		MIO7_led_off();
+
+		usleep(ms_between_flashes * 500);
+
+		how_many_times--;
+	}
+}
+
+void MIO7_led_off()
+{
+	XGpioPs Gpio;
+
+	XGpioPs_Config * ConfigPtr = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
+	XGpioPs_CfgInitialize(&Gpio, ConfigPtr, ConfigPtr->BaseAddr);
+
+	XGpioPs_SetDirectionPin(&Gpio, 7, 1);
+
+	// Disable LED
+	XGpioPs_WritePin(&Gpio, 7, 0x00);
+}
+
+void MIO7_led_on()
+{
+	XGpioPs Gpio;
+
+	XGpioPs_Config * ConfigPtr = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
+	XGpioPs_CfgInitialize(&Gpio, ConfigPtr, ConfigPtr->BaseAddr);
+
+	XGpioPs_SetDirectionPin(&Gpio, 7, 1);
+
+	// Enable LED
+	XGpioPs_WritePin(&Gpio, 7, 0x01);
+}

quad/sw/modular_quad_pid/src/mio7_led.h

+/*
+ * mio7_led.h
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: Amy Seibert
+ */
+
+#ifndef MIO7_LED_H_
+#define MIO7_LED_H_
+ 
+#include <stdio.h>
+#include <xgpiops.h>
+#include "sleep.h"
+
+/**
+ * @brief 
+ *      Flashes the MIO7 LED how_many_times times and with ms_between_flashes between the flashes. 
+ *
+ * @param how_many_times
+ *      times the LED should be flashed
+ *
+ * @param ms_between_flashes
+ *      time between flashes in milliseconds
+ *
+ */
+void flash_MIO_7_led(int how_many_times, int ms_between_flashes);
+
+/**
+ * @brief 
+ *      Turns off MIO7 LED. 
+ *
+ */
+void MIO7_led_off();
+
+/**
+ * @brief 
+ *      Turns on MIO7 LED. 
+ *
+ */
+void MIO7_led_on();
+
+#endif /* MIO7_LED_H_ */

quad/sw/modular_quad_pid/src/new_PID.h

+/*
+ * PID.h
+ *
+ *  Created on: Nov 10, 2014
+ *      Author: ucart
+ */
+
+#ifndef PID_H_
+#define PID_H_
+
+#include "type_def.h"
+
+// Yaw constants
+
+// when using units of radians
+#define YAW_ANGULAR_VELOCITY_KP 200.0 * 2292.0f
+#define YAW_ANGULAR_VELOCITY_KI 0.0f
+#define YAW_ANGULAR_VELOCITY_KD 0.0f
+#define YAW_ANGLE_KP 2.6f
+#define YAW_ANGLE_KI 0.0f
+#define YAW_ANGLE_KD 0.0f
+
+// when using units of radians
+#define ROLL_ANGULAR_VELOCITY_KP 100.0 * 46.0f
+#define ROLL_ANGULAR_VELOCITY_KI 0.0f
+#define ROLL_ANGULAR_VELOCITY_KD 100.0 * 5.5f
+#define ROLL_ANGLE_KP 15.0f
+#define ROLL_ANGLE_KI 0.0f
+#define ROLL_ANGLE_KD 0.2f
+#define YPOS_KP 0.08f
+#define YPOS_KI 0.01f
+#define YPOS_KD 0.1f
+
+
+//Pitch constants
+
+// when using units of radians
+#define PITCH_ANGULAR_VELOCITY_KP 100.0 * 46.0f
+#define PITCH_ANGULAR_VELOCITY_KI 0.0f
+#define PITCH_ANGULAR_VELOCITY_KD 100.0 * 5.5f
+#define PITCH_ANGLE_KP 15.0f
+#define PITCH_ANGLE_KI 0.0f
+#define PITCH_ANGLE_KD 0.2f
+#define XPOS_KP 0.08f
+#define XPOS_KI 0.01f
+#define XPOS_KD 0.1f
+
+
+//Throttle constants
+#define ALT_ZPOS_KP -9804.0f
+#define ALT_ZPOS_KI -817.0f
+#define ALT_ZPOS_KD -7353.0f
+
+// Computes control error and correction
+PID_values pid_computation(PID_t *pid);
+
+#endif /* PID_H_ */

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