quad/sw/modular_quad_pid/src/communication.c

+#include "communication.h"
+
+// QUAD & Ground Station
+// Format the log data from log_message
+//int formatData(unsigned char *log_msg, unsigned char *formattedCommand)
+int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand)
+{
+	*formattedCommand = malloc(sizeof(char) * metadata->data_len + 8);
+	/*if (formattedCommand == NULL)
+	{
+		return -1;
+	}*/
+	
+	//----------------------------------------------------------------------------------------------
+	//	   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	   |
+	//----------------------------------------------------------------------------------------------
+					
+	// Begin Char:
+	(*formattedCommand)[0] = metadata->begin_char;
+
+	// Msg type:
+	(*formattedCommand)[1] = metadata->msg_type;
+	
+	// Msg subtype
+	(*formattedCommand)[2] = metadata->msg_subtype;
+	
+	//Msg id (msgNum is 2 bytes)
+	(*formattedCommand)[3] = metadata->msg_id;
+	
+	// Data length and data - bytes 5&6 for len, 7+ for data
+	(*formattedCommand)[5] = metadata->data_len & 0x000000ff;
+	(*formattedCommand)[6] = (metadata->data_len >> 8) & 0x000000ff;
+	
+//	printf("data length %d\n", metadata->data_len);
+//	printf("data length %x\n", (*formattedCommand)[5]);
+//	printf("data length %x\n", (*formattedCommand)[6]);
+
+	memcpy(&((*formattedCommand)[7]), data, metadata->data_len);
+	
+	// Checksum
+	// receive data and calculate checksum
+	int i;
+	unsigned char packet_checksum = 0;
+	for(i = 0; i < 7 + metadata->data_len; i++)
+	{
+		packet_checksum ^= (*formattedCommand)[i];
+	}
+	
+//	printf("Packet checksum: 0x%02x\n", packet_checksum);
+
+	(*formattedCommand)[7 + metadata->data_len] = packet_checksum;
+	
+	return 0;
+}
+
+// returns the length of the data in bytes (datalen from packet) and fills data
+// and metadata with the packet information
+// use as follows:
+//
+//		packet is the entire packet message (formatted) 
+//		data is an unallocated (char *) (pass it to this function as &data) 
+//		meta_data is a pointer to an instance of metadata_t
+//
+int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data)
+{
+	//----------------------------------------------------------------------------------------------
+	//     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     |
+	//----------------------------------------------------------------------------------------------	
+	
+	// first byte must be the begin char
+	if(packet[0] != 0xBE) {
+		printf("The first packet byte is not the begin char.\n");
+		return -1;
+	}
+
+	// receive metadata
+	meta_data->begin_char = packet[0];
+	meta_data->msg_type = packet[1];
+	meta_data->msg_subtype = packet[2];
+	meta_data->msg_id = (packet[4] << 8) | (packet[3]);
+	meta_data->data_len = (packet[6] << 8) | (packet[5]);
+	unsigned char packet_checksum = packet[7+meta_data->data_len];
+
+//	printf("msg_type: %x\n", meta_data->msg_type);
+//	printf("msg_subtype: %x\n", meta_data->msg_subtype);
+//	printf("msg_type: %d\n", meta_data->data_len);
+	
+	int i;
+	
+	// receive data
+	*data = malloc(meta_data->data_len);
+	for(i = 0; i < meta_data->data_len; i++)
+	{
+		(*data)[i] = packet[7+i];
+	}
+
+	// calculate checksum
+	unsigned char calculated_checksum = 0;
+	for(i = 0; i < meta_data->data_len + 7; i++)
+	{
+		calculated_checksum ^= packet[i];
+	}
+
+	// compare checksum
+	if(packet_checksum != calculated_checksum)
+		printf("Checksums did not match (Quadlog): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
+
+	//////////////////////////////
+	// Send an acknowledgment packet
+
+	// Send a reply to the ground station
+
+	int buf = meta_data->msg_id;
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[0].ID,
+		MessageTypes[0].subtypes[1].ID,
+		0,
+		sizeof(int)
+	};
+	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]);
+	}
+	free(responsePacket);
+
+	return 0;
+}
+
+// QUAD & Ground Station
+// Process the command received
+int processCommand(unsigned char *packet, modular_structs_t *structs) {
+	int validPacket;
+	unsigned char *data;
+	metadata_t metadata;
+	
+	printf("Process Command.\n");
+
+	// Validate the message is correctly formatted
+	validPacket = parse_packet(packet, &data, &metadata);
+	if(validPacket != 0) {
+		printf("Packet is not valid.\n");
+		return -1;
+	}
+	
+	if(metadata.data_len >= 0) {
+		// Call the appropriate subtype function
+		(* (MessageTypes[metadata.msg_type].subtypes[metadata.msg_subtype].functionPtr))(data, metadata.data_len, structs);
+		
+//		printf("%s\n", MessageTypes[metadata.msg_type].subtypes[metadata.msg_subtype].cmdText);
+
+		return 0;
+	} else {
+		printf("Data length is less than 0.\n");
+	}
+	
+	// Only gets here if there is an error
+	return -1;
+}

quad/sw/modular_quad_pid/src/communication.h

+#ifndef _COMMUNICATION_H
+#define _COMMUNICATION_H
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <limits.h>
+#include "commands.h"
+#include "type_def.h"
+#include "uart.h"
+
+int formatCommand(unsigned char *command, unsigned char **formattedCommand);
+int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand);
+int logData(unsigned char *log_msg, unsigned char *formattedCommand);
+int processCommand(unsigned char *command, modular_structs_t *structs);
+int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data);
+
+#endif

quad/sw/modular_quad_pid/src/control_algorithm.c

+/*
+ * control_algorithm.c
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+
+// This implemented modular quadrotor software implements a PID control algorithm
+ 
+#include "control_algorithm.h"
+#include "communication.h"
+
+#define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees
+
+ int control_algorithm_init(parameter_t * parameter_struct)
+ {
+	// HUMAN Piloted (RC) PID DEFINITIONS //////
+	// RC PIDs for roll (2 loops: angle --> angular velocity)
+	parameter_struct->roll_angle_pid.dt = 0.005; parameter_struct->roll_ang_vel_pid.dt = 0.005; // 5 ms calculation period
+
+	// RC PIDs for pitch (2 loops: angle --> angular velocity)
+	parameter_struct->pitch_angle_pid.dt = 0.005; parameter_struct->pitch_ang_vel_pid.dt = 0.005; // 5 ms calculation period
+
+	// initialize Yaw PID_t and PID constants
+	// RC PID for yaw (1 loop angular velocity)
+	parameter_struct->yaw_ang_vel_pid.dt = 0.005; // 5 ms calculation period
+
+	// AUTOMATIC Pilot (Position) PID DEFINITIONS //////
+	// Local X PID using a translation from X camera system data to quad local X position (3 loops: local y position --> angle --> angular velocity)
+	parameter_struct->local_x_pid.dt = 0.100;
+
+	// Local Y PID using a translation from Y camera system data to quad local Y position(3 loops: local x position --> angle --> angular velocity)
+	parameter_struct->local_y_pid.dt = 0.100;
+
+	// CAM PIDs for yaw (2 loops angle --> angular velocity)
+	parameter_struct->yaw_angle_pid.dt = 0.100;
+
+	// CAM PID for altitude (1 loop altitude)
+	parameter_struct->alt_pid.dt = 0.100;
+
+	// PID coeffiecients (Position)
+	setPIDCoeff(&(parameter_struct->local_y_pid), YPOS_KP, YPOS_KI, YPOS_KD);
+	setPIDCoeff(&(parameter_struct->local_x_pid), XPOS_KP, XPOS_KI, XPOS_KD);
+	setPIDCoeff(&(parameter_struct->alt_pid), ALT_ZPOS_KP, ALT_ZPOS_KI, ALT_ZPOS_KD);
+
+	// PID coefficients (Angle)
+	setPIDCoeff(&(parameter_struct->pitch_angle_pid), PITCH_ANGLE_KP, PITCH_ANGLE_KI, PITCH_ANGLE_KD);
+	setPIDCoeff(&(parameter_struct->roll_angle_pid), ROLL_ANGLE_KP, ROLL_ANGLE_KI, ROLL_ANGLE_KD);
+	setPIDCoeff(&(parameter_struct->yaw_angle_pid), YAW_ANGLE_KP, YAW_ANGLE_KI, YAW_ANGLE_KD);
+
+	// PID coefficients (Angular Velocity)
+	setPIDCoeff(&(parameter_struct->pitch_ang_vel_pid), PITCH_ANGULAR_VELOCITY_KP, PITCH_ANGULAR_VELOCITY_KI, PITCH_ANGULAR_VELOCITY_KD);
+	setPIDCoeff(&(parameter_struct->roll_ang_vel_pid), ROLL_ANGULAR_VELOCITY_KP, ROLL_ANGULAR_VELOCITY_KI, ROLL_ANGULAR_VELOCITY_KD);
+	setPIDCoeff(&(parameter_struct->yaw_ang_vel_pid), YAW_ANGULAR_VELOCITY_KP, YAW_ANGULAR_VELOCITY_KI, YAW_ANGULAR_VELOCITY_KD);
+
+	return 0;
+ }
+ 
+ int control_algorithm(log_t* log_struct, user_input_t * user_input_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, modular_structs_t* structs)
+ {
+	// use the 'flap' switch as the flight mode selector
+	int cur_fm_switch = read_flap(user_input_struct->rc_commands[FLAP]);
+	static int last_fm_switch = MANUAL_FLIGHT_MODE;
+
+	// reset flight_mode to MANUAL right away if the flap switch is in manual position
+	// to engage AUTO mode the code waits for a new packet after the flap is switched to auto
+	// before actually engaging AUTO mode
+	if(cur_fm_switch == MANUAL_FLIGHT_MODE)
+		user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
+
+	static float roll_trim = 0.0;
+	static float pitch_trim = 0.0;
+
+	// flap switch was just toggled to auto flight mode
+	if((last_fm_switch != cur_fm_switch) && (cur_fm_switch == AUTO_FLIGHT_MODE))
+	{
+		user_defined_struct->engaging_auto = 1;
+
+		// Read in trimmed values because it should read trim values right when the pilot flips the flight mode switch
+		pitch_trim = user_input_struct->pitch_angle_manual_setpoint; //rc_commands[PITCH] - PITCH_CENTER;
+		roll_trim = user_input_struct->roll_angle_manual_setpoint; //rc_commands[ROLL] - ROLL_CENTER;
+		//sensor_struct->trimmedRCValues.yaw = yaw_manual_setpoint; //rc_commands[YAW] - YAW_CENTER;
+
+//		sensor_struct->trims.roll = raw_actuator_struct->controller_corrected_motor_commands[ROLL];
+//		sensor_struct->trims.pitch = raw_actuator_struct->controller_corrected_motor_commands[PITCH];
+//		sensor_struct->trims.yaw = raw_actuator_struct->controller_corrected_motor_commands[YAW];
+		sensor_struct->trims.throttle = user_input_struct->rc_commands[THROTTLE];
+
+		log_struct->trims.roll = sensor_struct->trims.roll;
+		log_struct->trims.pitch = sensor_struct->trims.pitch;
+		log_struct->trims.yaw = sensor_struct->trims.yaw;
+		log_struct->trims.throttle = sensor_struct->trims.throttle;
+	}
+
+	if(user_input_struct->hasPacket == 0x04 && user_defined_struct->engaging_auto == 1)
+			user_defined_struct->engaging_auto = 2;
+
+	// If the quad has received a packet and it's not an update packet
+	if(user_input_struct->hasPacket != -1 && user_input_struct->hasPacket != 0x04)
+	{
+		processCommand((unsigned char *)user_input_struct->sb->buf, structs);
+	}
+
+
+	// if the flap switch was toggled to AUTO_FLIGHT_MODE and we've received a new packet
+	// then record the current position as the desired position
+	// also reset the previous error and accumulated error from the position PIDs
+	if((cur_fm_switch == AUTO_FLIGHT_MODE) && (user_defined_struct->engaging_auto == 2))
+	{
+		// zero out the accumulated error so the I terms don't cause wild things to happen
+		parameter_struct->alt_pid.acc_error = 0.0;
+		parameter_struct->local_x_pid.acc_error = 0.0;
+		parameter_struct->local_y_pid.acc_error = 0.0;
+
+		// make previous error equal to the current so the D term doesn't spike
+		parameter_struct->alt_pid.prev_error = 0.0;
+		parameter_struct->local_x_pid.prev_error = 0.0;
+		parameter_struct->local_y_pid.prev_error = 0.0;
+
+		setpoint_struct->desiredQuadPosition.alt_pos = sensor_struct->currentQuadPosition.alt_pos;
+		setpoint_struct->desiredQuadPosition.x_pos = sensor_struct->currentQuadPosition.x_pos;
+		setpoint_struct->desiredQuadPosition.y_pos = sensor_struct->currentQuadPosition.y_pos;
+		setpoint_struct->desiredQuadPosition.yaw = 0.0;//currentQuadPosition.yaw;
+
+		// reset the flag that engages auto mode
+		user_defined_struct->engaging_auto = 0;
+
+		// finally engage the AUTO_FLIGHT_MODE
+		// this ensures that we've gotten a new update packet right after the switch was set to auto mode
+		user_defined_struct->flight_mode = AUTO_FLIGHT_MODE;
+	}
+
+	//PIDS///////////////////////////////////////////////////////////////////////
+
+	/* 					Position loop
+	 * Reads current position, and outputs
+	 * a pitch or roll for the angle loop PIDs
+	 */
+
+//		static int counter_between_packets = 0;
+
+	if(user_input_struct->hasPacket == 0x04)
+	{
+		parameter_struct->local_y_pid.current_point = sensor_struct->currentQuadPosition.y_pos;
+		parameter_struct->local_y_pid.setpoint = setpoint_struct->desiredQuadPosition.y_pos;
+
+		parameter_struct->local_x_pid.current_point = sensor_struct->currentQuadPosition.x_pos;
+		parameter_struct->local_x_pid.setpoint = setpoint_struct->desiredQuadPosition.x_pos;
+
+		parameter_struct->alt_pid.current_point = sensor_struct->currentQuadPosition.alt_pos;
+		parameter_struct->alt_pid.setpoint = setpoint_struct->desiredQuadPosition.alt_pos;
+
+		//logging and PID computation
+		log_struct->local_y_PID_values = pid_computation(&(parameter_struct->local_y_pid));
+		log_struct->local_x_PID_values = pid_computation(&(parameter_struct->local_x_pid));
+		log_struct->altitude_PID_values = pid_computation(&(parameter_struct->alt_pid));
+
+		// yaw angular position PID calculation
+		parameter_struct->yaw_angle_pid.current_point = sensor_struct->currentQuadPosition.yaw;// in radians
+		parameter_struct->yaw_angle_pid.setpoint = setpoint_struct->desiredQuadPosition.yaw; // constant setpoint
+
+		//logging and PID computation
+		log_struct->angle_yaw_PID_values = pid_computation(&(parameter_struct->yaw_angle_pid));
+
+	}
+
+
+	/* 					Angle loop
+	 * Calculates current orientation, and outputs
+	 * a pitch, roll, or yaw velocity for the angular velocity loop PIDs
+	 */
+
+	//angle boundaries
+	if(parameter_struct->local_x_pid.pid_correction > ROLL_PITCH_MAX_ANGLE)
+	{
+		parameter_struct->local_x_pid.pid_correction = ROLL_PITCH_MAX_ANGLE;
+	}
+	if(parameter_struct->local_x_pid.pid_correction < -ROLL_PITCH_MAX_ANGLE)
+	{
+		parameter_struct->local_x_pid.pid_correction = -ROLL_PITCH_MAX_ANGLE;
+	}
+	if(parameter_struct->local_y_pid.pid_correction > ROLL_PITCH_MAX_ANGLE)
+	{
+		parameter_struct->local_y_pid.pid_correction = ROLL_PITCH_MAX_ANGLE;
+	}
+	if(parameter_struct->local_y_pid.pid_correction < -ROLL_PITCH_MAX_ANGLE)
+	{
+		parameter_struct->local_y_pid.pid_correction = -ROLL_PITCH_MAX_ANGLE;
+	}
+
+	parameter_struct->pitch_angle_pid.current_point = sensor_struct->pitch_angle_filtered;
+	parameter_struct->pitch_angle_pid.setpoint =
+			(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
+			(parameter_struct->local_x_pid.pid_correction) + pitch_trim : user_input_struct->pitch_angle_manual_setpoint;
+
+	parameter_struct->roll_angle_pid.current_point = sensor_struct->roll_angle_filtered;
+	parameter_struct->roll_angle_pid.setpoint =
+			(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
+			(parameter_struct->local_y_pid.pid_correction) + roll_trim : user_input_struct->roll_angle_manual_setpoint;
+
+
+	//logging and PID computation
+	log_struct->angle_pitch_PID_values = pid_computation(&(parameter_struct->pitch_angle_pid));
+	log_struct->angle_roll_PID_values = pid_computation(&(parameter_struct->roll_angle_pid));
+
+
+	/* 				Angular Velocity Loop
+	 * Takes the desired angular velocity from the angle loop,
+	 * and calculates a PID correction with the current angular velocity
+	 */
+
+	// theta_dot is the angular velocity about the y-axis
+	// it is calculated from using the gimbal equations
+	parameter_struct->pitch_ang_vel_pid.current_point = sensor_struct->theta_dot;
+	parameter_struct->pitch_ang_vel_pid.setpoint = parameter_struct->pitch_angle_pid.pid_correction;
+
+	// phi_dot is the angular velocity about the x-axis
+	// it is calculated from using the gimbal equations
+	parameter_struct->roll_ang_vel_pid.current_point = sensor_struct->phi_dot;
+	parameter_struct->roll_ang_vel_pid.setpoint = parameter_struct->roll_angle_pid.pid_correction;
+
+	// Yaw angular velocity PID
+	// psi_dot is the angular velocity about the z-axis
+	// it is calculated from using the gimbal equations
+	parameter_struct->yaw_ang_vel_pid.current_point = sensor_struct->psi_dot;
+	parameter_struct->yaw_ang_vel_pid.setpoint = (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
+			parameter_struct->yaw_angle_pid.pid_correction : user_input_struct->yaw_manual_setpoint; // no trim added because the controller already works well
+
+	//logging and PID computation
+	log_struct->ang_vel_pitch_PID_values = pid_computation(&(parameter_struct->pitch_ang_vel_pid));
+	log_struct->ang_vel_roll_PID_values = pid_computation(&(parameter_struct->roll_ang_vel_pid));
+	log_struct->ang_vel_yaw_PID_values = pid_computation(&(parameter_struct->yaw_ang_vel_pid));
+
+	//END PIDs///////////////////////////////////////////////////////////////////////
+
+
+	 // here for now so in case any flight command is not PID controlled, it will default to rc_command value:
+	memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);
+
+	// don't use the PID corrections if the throttle is less than about 10% of its range
+	if((user_input_struct->rc_commands[THROTTLE] >
+	118000) || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE))
+	{
+
+		if(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)
+		{
+			//THROTTLE
+			raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] =
+				((int)(parameter_struct->alt_pid.pid_correction)) + sensor_struct->trims.throttle;
+
+			//ROLL
+			raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
+					parameter_struct->roll_ang_vel_pid.pid_correction; // + sensor_struct->trims.roll;
+
+			//PITCH
+			raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
+					parameter_struct->pitch_ang_vel_pid.pid_correction; // + sensor_struct->trims.pitch;
+
+			//YAW
+			raw_actuator_struct->controller_corrected_motor_commands[YAW] =
+					parameter_struct->yaw_ang_vel_pid.pid_correction;// + sensor_struct->trims.yaw;
+
+//			static int slow_down = 0;
+//			slow_down++;
+//			if(slow_down % 50 == 0)
+//			printf("X: %.3f\tY: %.3f\tZ: %.3f\tX_s: %.3f\tX_c: %.3f\tY_s: %.3f\tY_c: %.3f\tZ_s: %.3f\tZ_c: %.3f\t\n",
+//					parameter_struct->local_x_pid.pid_correction,
+//					parameter_struct->local_y_pid.pid_correction,
+//					parameter_struct->alt_pid.pid_correction,
+//					parameter_struct->local_x_pid.setpoint, parameter_struct->local_x_pid.current_point,
+//					parameter_struct->local_y_pid.setpoint, parameter_struct->local_y_pid.current_point,
+//					parameter_struct->alt_pid.setpoint, parameter_struct->alt_pid.current_point);
+		}
+		else{
+			//ROLL
+			raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
+					parameter_struct->roll_ang_vel_pid.pid_correction;
+
+			//PITCH
+			raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
+					parameter_struct->pitch_ang_vel_pid.pid_correction;
+
+			//YAW
+			raw_actuator_struct->controller_corrected_motor_commands[YAW] =
+					parameter_struct->yaw_ang_vel_pid.pid_correction;
+		}
+
+		//BOUNDS CHECKING
+		if(raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] < 0)
+			raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] = 0;
+
+		//BOUNDS CHECKING
+		if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] > 20000)
+			raw_actuator_struct->controller_corrected_motor_commands[ROLL] = 20000;
+
+		if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] < -20000)
+			raw_actuator_struct->controller_corrected_motor_commands[ROLL] = -20000;
+
+		if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] > 20000)
+			raw_actuator_struct->controller_corrected_motor_commands[PITCH] = 20000;
+
+		if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] < -20000)
+			raw_actuator_struct->controller_corrected_motor_commands[PITCH] = -20000;
+
+		if(raw_actuator_struct->controller_corrected_motor_commands[YAW] > 20000)
+			raw_actuator_struct->controller_corrected_motor_commands[YAW] = 20000;
+
+		if(raw_actuator_struct->controller_corrected_motor_commands[YAW] < -20000)
+			raw_actuator_struct->controller_corrected_motor_commands[YAW] = -20000;
+
+	}
+	else
+	{
+		raw_actuator_struct->controller_corrected_motor_commands[ROLL] = 0;
+		raw_actuator_struct->controller_corrected_motor_commands[PITCH] = 0;
+		raw_actuator_struct->controller_corrected_motor_commands[YAW] = 0;
+	}
+
+	//logging
+	// here we are not actually duplicating the logging from the PID computation
+	// the PID computation logs PID_values struct where this logs the PID struct
+	// they contain different sets of data
+	log_struct->local_y_PID = parameter_struct->local_y_pid;
+	log_struct->local_x_PID = parameter_struct->local_x_pid;
+	log_struct->altitude_PID = parameter_struct->alt_pid;
+
+	log_struct->angle_roll_PID = parameter_struct->roll_angle_pid;
+	log_struct->angle_pitch_PID = parameter_struct->pitch_angle_pid;
+	log_struct->angle_yaw_PID = parameter_struct->yaw_angle_pid;
+
+	log_struct->ang_vel_roll_PID = parameter_struct->roll_ang_vel_pid;
+	log_struct->ang_vel_pitch_PID = parameter_struct->pitch_ang_vel_pid;
+	log_struct->ang_vel_yaw_PID = parameter_struct->yaw_ang_vel_pid;
+
+	last_fm_switch = cur_fm_switch;
+
+	if(user_input_struct->hasPacket != -1)
+	{
+		user_input_struct->sb->clear(user_input_struct->sb);
+		user_input_struct->hasPacket = -1;
+	}
+
+    return 0;
+ }
+ 
+ void setPIDCoeff(PID_t* p, float pValue, float iValue, float dValue) {
+
+ 	p->Kp = pValue;
+ 	p->Ki = iValue;
+ 	p->Kd = dValue;
+
+ }
+

quad/sw/modular_quad_pid/src/control_algorithm.h

+/*
+ * control_algorithm.h
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+
+#ifndef CONTROL_ALGORITHM_H_
+#define CONTROL_ALGORITHM_H_
+ 
+#include <stdio.h>
+
+#include "log_data.h"
+#include "sensor_processing.h"
+#include "quadposition.h"
+#include "type_def.h"
+
+/**
+ * @brief 
+ *      Initializes everything used in the control algorithm.
+ *
+ * @return
+ *      error message
+ *
+ */
+int control_algorithm_init(parameter_t * parameter_struct);
+
+/**
+ * @brief 
+ *      Runs the control algorithm on the data and outputs a command for actuators.
+ *
+ * @param log_struct
+ *      structure of the data to be logged
+ *
+ * @param sensor_struct
+ *      structure of the processed data from the sensors
+ *
+ * @param setpoint_struct
+ *      structure of the setpoints used in the controller
+ *
+ * @param parameter_struct
+ *      structure of the parameters used in the controller
+ *
+ * @param user_defined_struct
+ *      structure of the user defined variables
+ *
+ * @param raw_actuator_struct
+ *      structure of the commmands outputted to go to the actuators
+ *
+ * @return 
+ *      error message
+ *
+ */
+int control_algorithm(log_t* log_struct,
+					  user_input_t * user_input_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,
+                      modular_structs_t* structs);
+
+/**
+ * @brief
+ *      Internally used functions
+ *
+ */
+void setPIDCoeff(PID_t* p, float pValue, float iValue, float dValue);
+
+#endif /* CONTROL_ALGORITHM_H_ */

quad/sw/modular_quad_pid/src/controllers.c

+/*
+ * controllers.c
+ *
+ *  Created on: Oct 11, 2014
+ *      Author: ucart
+ */
+
+/**
+ * Lots of useful information in controllers.h, look in there first
+ */
+#include "controllers.h"
+#include "iic_mpu9150_utils.h"
+#include "quadposition.h"
+#include "util.h"
+#include "uart.h"
+#include "sleep.h"
+#include "stdio.h"
+#include <math.h>
+
+// 0 was -6600
+//int motor0_bias = -4500, motor1_bias = 100, motor2_bias = 5300, motor3_bias = 10300;
+int motor0_bias = -9900, motor1_bias = -200, motor2_bias = -10200, motor3_bias = 250;
+
+/**
+ * Takes the raw signal inputs from the receiver and filters it so the
+ * quadcopter doesn't flip or do something extreme
+ */
+void filter_PWMs(int* mixer) {
+
+}
+
+/**
+ * Converts PWM signals into 4 channel pitch, roll, yaw, throttle
+ */
+// javey: unused
+void PWMS_to_Aero(int* PWMs, int* aero) {
+	/**
+	 * Reference used to derive equations
+	 */
+	//	pwm0 = throttle_base - pitch_base + yaw_base;
+	//	pwm1 = throttle_base + roll_base - yaw_base;
+	//	pwm2 = throttle_base - roll_base - yaw_base;
+	//	pwm3 = throttle_base + pitch_base + yaw_base;
+
+	aero[THROTTLE] = (PWMs[0] + PWMs[1] + PWMs[2] + PWMs[3]) / 4;
+	aero[ROLL]     = (PWMs[1] - PWMs[2]) / 2;
+	aero[PITCH]    = (PWMs[3] - PWMs[0]) / 2;
+	aero[YAW]      = (PWMs[3] + PWMs[0] - PWMs[1] - PWMs[2]) / 4;
+}
+
+

quad/sw/modular_quad_pid/src/controllers.h

+/*
+ * controllers.h
+ *
+ *  Created on: Oct 11, 2014
+ *      Author: ucart
+ */
+#ifndef _CONTROLLERS_H
+#define _CONTROLLERS_H
+
+#include "util.h"
+#include "quadposition.h"
+
+/**
+ *
+ *  USING PLUS CONFIGURATION
+ *              0           R            CW               E
+ *            1 + 2       W   R      CCW    CCW        N     S
+ *              3           W            CW               W
+ *
+ *
+ *  USING X CONFIGURATION
+ *
+ *
+ *         0   2           R   R        CW    CCW
+ *           x
+ *         1   3           W   W        CCW   CW
+ */
+#define X_CONFIG
+
+/**
+ * Pin hook ups
+ *
+ * PWM Recorder port mapping
+ * 3.3 V || GND || PWM_REC_3 || PWM_REC_2 || PWM_REC_1 || PWM_REC_0
+ *
+ * Rx PINS
+ * GEAR     -> JD7
+ * THROTTLE -> JE1
+ * AILE     -> JE2
+ * ELEV     -> JE3
+ * RUDD     -> JE4
+ * GND      -> JE5
+ *
+ * JE PMOD  TOP PINS
+ * Unused   || GND  || YAW  || PITCH || ROLL || THROTTLE
+ *
+ * BOTTOM PINS
+ *
+ *  Unused  || GND  || PWM3 || PWM2  || PWM1 || PWM0
+ */
+
+/**
+ * Gear settings
+ * 1 - F mode = 171135
+ * 0 - Gear   = 118363
+ * Kill if gear is around 118363
+ */
+
+
+/*
+ * 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_MIN      129400
+#define ROLL_CENTER   149800
+#define ROLL_RANGE    ROLL_MAX - ROLL_MIN
+
+#define PITCH_MAX     169900
+#define PITCH_MIN     129500
+#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
+
+void filter_PWMs(int* mixer);
+void PWMS_to_Aero(int* PWMs, int* aero); // <= javey: unused
+void Aero_to_PWMS(int* PWMs, int* aero);
+
+#endif /* _CONTROLLERS_H */

quad/sw/modular_quad_pid/src/conversion.c

+
+#include "conversion.h"
+
+// takes a floating point percentage and converts to a
+// receiver command in the range min_rx_cmd to max_rx_cmd
+// if percentage is < 0 then returns a value less than
+//		center_rx_cmd but >= min_rx_cmd
+// if percentage is > 0 then returns a value greater than
+//		center_rx_cmd but <= max_rx_cmd
+// if percentage is = 0 then returns center_rx_cmd
+// acceptable range of values for percentage: [-100, 100]
+int map_to_rx_cmd(float percentage, int min_rx_cmd, int center_rx_cmd,
+	int max_rx_cmd)
+{
+	//bounds checking
+	// imagine a human flying and the stick is minimum
+	if(percentage >= 100.0)
+		return max_rx_cmd;
+
+	//bounds checking
+	// imagine a human flying and the stick is minimum
+	if(percentage <= -100.0)
+		return min_rx_cmd;
+
+	// 0 percentage is center cmd
+	// imagine a human flying and not touching the stick
+	if(percentage == 0)
+		return center_rx_cmd;
+
+	// calculate and return a percentage of the max/min command
+	if(percentage < 0)
+	{
+		return center_rx_cmd + ((int) (percentage/100.0 *
+				((float) max_rx_cmd - center_rx_cmd)));
+	}
+	else
+	{
+		return center_rx_cmd + ((int) (percentage/100.0 * (
+				(float) center_rx_cmd - min_rx_cmd)));
+	}
+
+	return 0;
+}
+
+int convert_to_receiver_cmd(int var_to_convert, float max_var_to_convert, float min_var_to_convert, int center_receiver_cmd,  int max_receiver_cmd, int min_receiver_cmd)
+{
+
+	if(var_to_convert <= 0) {
+		int ret = ((int) ((float)(min_receiver_cmd - center_receiver_cmd))/min_var_to_convert * var_to_convert) + center_receiver_cmd;
+
+		if(ret < min_receiver_cmd)
+			ret = min_receiver_cmd;
+
+		return ret;
+	}
+
+	else {
+		int ret = ((int) ((float)(max_receiver_cmd - center_receiver_cmd))/max_var_to_convert * var_to_convert) + center_receiver_cmd;
+
+		if(ret > max_receiver_cmd)
+			ret = max_receiver_cmd;
+
+		return ret;
+	}
+
+	return 0;
+}

quad/sw/modular_quad_pid/src/conversion.h

+#ifndef _CONVERSION_H
+#define _CONVERSION_H
+
+
+int convert_to_receiver_cmd(int var_to_convert, float max_var_to_convert, float min_var_to_convert, int center_receiver_cmd,  int max_receiver_cmd, int min_receiver_cmd);
+
+int map_to_rx_cmd(float percentage, int min_rx_cmd, int center_rx_cmd, int max_rx_cmd);
+
+#endif /* _CONVERSION_H */

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_ */