diff --git a/groundStation/src/backend/commands.c b/groundStation/src/backend/commands.c
index 1cef27dc9cd60f1cf8ff4efc06912b609846e286..b88aebba678d28b8c62f0f5ddca142b959c6b1e5 100644
--- a/groundStation/src/backend/commands.c
+++ b/groundStation/src/backend/commands.c
@@ -68,6 +68,9 @@ command_cb cb_respcontrol __attribute__((weak, alias("cb_default")));
  *
  * There is one callback function pointer associated with each
  * element in this struct array.
+ *
+ * DO NOT change this struct without updating the
+ * "MessageTypeID" struct in commands.h as well
  */
 struct MessageType MessageTypes[MAX_TYPE_ID] =
 {
diff --git a/groundStation/src/backend/commands.h b/groundStation/src/backend/commands.h
index 9fda24a711d60750f733710f017bdf20b5f92103..094e3ac59c76e869c7d1420baa72d4279b5a1553 100644
--- a/groundStation/src/backend/commands.h
+++ b/groundStation/src/backend/commands.h
@@ -28,7 +28,10 @@ enum DataType
 /*
  * Message type IDs used to know what kind of message we are dealing with
  * Enumeration used to index the MessageTypes array in commands.c
+ *
  * DO NOT change this enum or you will break backwards compatibility.
+ * DO NOT change this enum without also updating the "MessageTypes" array
+ * in commands.c to match.
  */
 enum MessageTypeID{
 	DEBUG_ID,             // 00
@@ -57,6 +60,7 @@ enum ControllerID{
 	LOCAL_X_ID,           // 06 - Local X PID
 	LOCAL_Y_ID,           // 07 - Local Y PID
 	ALT_ID,               // 08 - Altitude PID
+	MAX_CONTROLLER_ID,    // 09 - Just used to keep track of the size
 };
 
 /*
@@ -67,6 +71,7 @@ enum ControllerValueID{
 	KI_ID,                 // 01 - I constant
 	KD_ID,                 // 02 - D constant
 	SP_ID,                 // 03 - Setpoint value
+	MAX_CONTROL_VAL_ID,    // 04 - Just used to keep track of the size
 };
 
 /*
diff --git a/quad/sw/modular_quad_pid/src/callbacks.c b/quad/sw/modular_quad_pid/src/callbacks.c
index 52705f5b46de2e5bd2dd1680a3b541f12788fb52..917bc4ef149ef7ac4d69dd53ccf0a519a15b6b1a 100644
--- a/quad/sw/modular_quad_pid/src/callbacks.c
+++ b/quad/sw/modular_quad_pid/src/callbacks.c
@@ -106,178 +106,38 @@ int cb_beginupdate(modular_structs_t *structs) {
   */
 int cb_setcontrol(modular_structs_t *structs)
 {
-	// Get the controller ID, value ID, float value
-	u8 controller_id = uart_buff_data_get_u8(0);
-	u8 controller_value_id = uart_buff_data_get_u8(1);
-	float controller_value = uart_buff_data_get_float(3);
-
-	// TODO check if the setpoints really need to be doubles
-	// TODO add rate setpoints
-	// Switch case on the controller ID
-	switch(controller_id)
+	// Get the data length
+	u16 data_len = uart_buff_get_u16(6);
+	// Check if the data length is correct
+	if (data_len == 6)
 	{
-		case ROLL_ID:
-			// switch on the value ID
-			switch(controller_value_id)
-			{
-				case KP_ID:
-					structs->parameter_struct.roll_angle_pid.Kp = controller_value;
-					break;
-				case KI_ID:
-					structs->parameter_struct.roll_angle_pid.Ki = controller_value;
-					break;
-				case KD_ID:
-					structs->parameter_struct.roll_angle_pid.Kd = controller_value;
-					break;
-				case SP_ID:
-					structs->setpoint_struct.desiredQuadPosition.roll = controller_value;
-					break;
-			}
-			break;
-		case PITCH_ID:
-			// switch on the value ID
-			switch(controller_value_id)
-			{
-				case KP_ID:
-					structs->parameter_struct.pitch_angle_pid.Kp = controller_value;
-					break;
-				case KI_ID:
-					structs->parameter_struct.pitch_angle_pid.Ki = controller_value;
-					break;
-				case KD_ID:
-					structs->parameter_struct.pitch_angle_pid.Kd = controller_value;
-					break;
-				case SP_ID:
-					structs->setpoint_struct.desiredQuadPosition.pitch = controller_value;
-					break;
-			}
-			break;
-		case YAW_ID:
+		// Get the controller ID, value ID, float value
+		u8 controller_id = uart_buff_data_get_u8(0);
+		u8 controller_value_id = uart_buff_data_get_u8(1);
+		float controller_value = uart_buff_data_get_float(3);
+
+		// check to make sure all the values are in bounds
+		if (controller_id < MAX_CONTROLLER_ID &&
+			controller_value_id < MAX_CONTROL_VAL_ID)
+		{
 			// switch on the value ID
 			switch(controller_value_id)
 			{
 				case KP_ID:
-					structs->parameter_struct.yaw_angle_pid.Kp = controller_value;
+					structs->parameter_struct.pid_controllers[controller_id].Kp = controller_value;
 					break;
 				case KI_ID:
-					structs->parameter_struct.yaw_angle_pid.Ki = controller_value;
+					structs->parameter_struct.pid_controllers[controller_id].Ki = controller_value;
 					break;
 				case KD_ID:
-					structs->parameter_struct.yaw_angle_pid.Kd = controller_value;
+					structs->parameter_struct.pid_controllers[controller_id].Kd = controller_value;
 					break;
 				case SP_ID:
-					structs->setpoint_struct.desiredQuadPosition.yaw = controller_value;
+					structs->parameter_struct.pid_controllers[controller_id].setpoint = controller_value;
 					break;
 			}
-			break;
-		case ROLL_RATE_ID:
-			// switch on the value ID
-			switch(controller_value_id)
-			{
-				case KP_ID:
-					structs->parameter_struct.roll_ang_vel_pid.Kp = controller_value;
-					break;
-				case KI_ID:
-					structs->parameter_struct.roll_ang_vel_pid.Ki = controller_value;
-					break;
-				case KD_ID:
-					structs->parameter_struct.roll_ang_vel_pid.Kd = controller_value;
-					break;
-				case SP_ID:
-					// do nothing for now because there is no set point for this conroller
-					break;
-			}
-			break;
-		case PITCH_RATE_ID:
-			// switch on the value ID
-			switch(controller_value_id)
-			{
-				case KP_ID:
-					structs->parameter_struct.pitch_ang_vel_pid.Kp = controller_value;
-					break;
-				case KI_ID:
-					structs->parameter_struct.pitch_ang_vel_pid.Ki = controller_value;
-					break;
-				case KD_ID:
-					structs->parameter_struct.pitch_ang_vel_pid.Kd = controller_value;
-					break;
-				case SP_ID:
-					// do nothing for now because there is no set point for this conroller
-					break;
-			}
-			break;
-		case YAW_RATE_ID:
-			// switch on the value ID
-			switch(controller_value_id)
-			{
-				case KP_ID:
-					structs->parameter_struct.yaw_ang_vel_pid.Kp = controller_value;
-					break;
-				case KI_ID:
-					structs->parameter_struct.yaw_ang_vel_pid.Ki = controller_value;
-					break;
-				case KD_ID:
-					structs->parameter_struct.yaw_ang_vel_pid.Kd = controller_value;
-					break;
-				case SP_ID:
-					// do nothing for now because there is no set point for this conroller
-					break;
-			}
-			break;
-		case LOCAL_X_ID:
-			// switch on the value ID
-			switch(controller_value_id)
-			{
-				case KP_ID:
-					structs->parameter_struct.local_x_pid.Kp = controller_value;
-					break;
-				case KI_ID:
-					structs->parameter_struct.local_x_pid.Ki = controller_value;
-					break;
-				case KD_ID:
-					structs->parameter_struct.local_x_pid.Kd = controller_value;
-					break;
-				case SP_ID:
-					structs->setpoint_struct.desiredQuadPosition.x_pos = controller_value;
-					break;
-			}
-			break;
-		case LOCAL_Y_ID:
-			// switch on the value ID
-			switch(controller_value_id)
-			{
-				case KP_ID:
-					structs->parameter_struct.local_y_pid.Kp = controller_value;
-					break;
-				case KI_ID:
-					structs->parameter_struct.local_y_pid.Ki = controller_value;
-					break;
-				case KD_ID:
-					structs->parameter_struct.local_y_pid.Kd = controller_value;
-					break;
-				case SP_ID:
-					structs->setpoint_struct.desiredQuadPosition.y_pos = controller_value;
-					break;
-			}
-			break;
-		case ALT_ID:
-			// switch on the value ID
-			switch(controller_value_id)
-			{
-				case KP_ID:
-					structs->parameter_struct.alt_pid.Kp = controller_value;
-					break;
-				case KI_ID:
-					structs->parameter_struct.alt_pid.Ki = controller_value;
-					break;
-				case KD_ID:
-					structs->parameter_struct.alt_pid.Kd = controller_value;
-					break;
-				case SP_ID:
-					structs->setpoint_struct.desiredQuadPosition.alt_pos = controller_value;
-					break;
-			}
-			break;
+		}
+
 	}
 
 	return 0;
@@ -304,6 +164,7 @@ int cb_getcontrol(modular_structs_t* structs)
 	// TODO check if the setpoints really need to be doubles
 	// TODO add rate setpoints
 	// TODO figure out what the message ID should be
+	// make sure to send the same ID information back like in setval
 	// Switch case on the controller ID
 	switch(controller_id)
 	{
@@ -312,17 +173,17 @@ int cb_getcontrol(modular_structs_t* structs)
 			switch(controller_value_id)
 			{
 				case KP_ID:
-					float controller_value = structs->parameter_struct.roll_angle_pid.Kp;
-					send_data(RESPCONTROL_ID, 0, ((char *) &controller_value), sizeof(controller_value));
+					//float controller_value = structs->parameter_struct.roll_angle_pid.Kp;
+					//send_data(RESPCONTROL_ID, 0, ((char *) &controller_value), sizeof(controller_value));
 					break;
 				case KI_ID:
-					structs->parameter_struct.roll_angle_pid.Ki = controller_value;
+					//structs->parameter_struct.roll_angle_pid.Ki = controller_value;
 					break;
 				case KD_ID:
-					structs->parameter_struct.roll_angle_pid.Kd = controller_value;
+					//structs->parameter_struct.roll_angle_pid.Kd = controller_value;
 					break;
 				case SP_ID:
-					structs->setpoint_struct.desiredQuadPosition.roll = controller_value;
+					//structs->setpoint_struct.desiredQuadPosition.roll = controller_value;
 					break;
 			}
 			break;
@@ -342,6 +203,7 @@ int cb_getcontrol(modular_structs_t* structs)
 			// TODO
 		case ALT_ID:
 			// TODO
+			break;
 	}
 
 	return 0;
diff --git a/quad/sw/modular_quad_pid/src/callbacks.h b/quad/sw/modular_quad_pid/src/callbacks.h
index 6b6dee02a2e6020df1e0ee47a44c724a044cf1c0..7e67ddac5b59b5e824faf8a2a6bb736609242fca 100644
--- a/quad/sw/modular_quad_pid/src/callbacks.h
+++ b/quad/sw/modular_quad_pid/src/callbacks.h
@@ -1,10 +1,11 @@
-#ifndef __callbacks_
+#ifndef __callbacks_h
 #define __callbacks_h
 
 /* Grab some stupid stuff from legacy code */
-#include "type_def.h"
+
+struct modular_structs;
 
 /* Make commands.c happy */
-typedef int (command_cb)(modular_structs_t *structs);
+typedef int (command_cb)(struct modular_structs *structs);
 
 #endif
diff --git a/quad/sw/modular_quad_pid/src/cb_default.h b/quad/sw/modular_quad_pid/src/cb_default.h
index 99982e63f73c37f9fdd1175bdf94c1f4b268a6c9..aa308108b499e2cb8f4e723a59b81b02abe6d29c 100644
--- a/quad/sw/modular_quad_pid/src/cb_default.h
+++ b/quad/sw/modular_quad_pid/src/cb_default.h
@@ -1,4 +1,5 @@
 #include "commands.h"
+#include "type_def.h"
 
 /* The cb_default used on the groundStation. This file MUST NOT BE INCLUDED
  * by anything except for commands.c */
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index 2e79f58fb0ba983ca258cd543dc05e32740d038a..63ba756137739578a39f2609048a3728027266c7 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -16,42 +16,42 @@
  {
 	// 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
+	parameter_struct->pid_controllers[ROLL_ID].dt = 0.005; parameter_struct->pid_controllers[ROLL_RATE_ID].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
+	parameter_struct->pid_controllers[PITCH_ID].dt = 0.005; parameter_struct->pid_controllers[PITCH_RATE_ID].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
+	parameter_struct->pid_controllers[YAW_RATE_ID].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;
+	parameter_struct->pid_controllers[LOCAL_X_ID].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;
+	parameter_struct->pid_controllers[LOCAL_Y_ID].dt = 0.100;
 
 	// CAM PIDs for yaw (2 loops angle --> angular velocity)
-	parameter_struct->yaw_angle_pid.dt = 0.100;
+	parameter_struct->pid_controllers[YAW_ID].dt = 0.100;
 
 	// CAM PID for altitude (1 loop altitude)
-	parameter_struct->alt_pid.dt = 0.100;
+	parameter_struct->pid_controllers[ALT_ID].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);
+	setPIDCoeff(&(parameter_struct->pid_controllers[LOCAL_Y_ID]), YPOS_KP, YPOS_KI, YPOS_KD);
+	setPIDCoeff(&(parameter_struct->pid_controllers[LOCAL_X_ID]), XPOS_KP, XPOS_KI, XPOS_KD);
+	setPIDCoeff(&(parameter_struct->pid_controllers[ALT_ID]), 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);
+	setPIDCoeff(&(parameter_struct->pid_controllers[PITCH_ID]), PITCH_ANGLE_KP, PITCH_ANGLE_KI, PITCH_ANGLE_KD);
+	setPIDCoeff(&(parameter_struct->pid_controllers[ROLL_ID]), ROLL_ANGLE_KP, ROLL_ANGLE_KI, ROLL_ANGLE_KD);
+	setPIDCoeff(&(parameter_struct->pid_controllers[YAW_ID]), 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);
+	setPIDCoeff(&(parameter_struct->pid_controllers[PITCH_RATE_ID]), PITCH_ANGULAR_VELOCITY_KP, PITCH_ANGULAR_VELOCITY_KI, PITCH_ANGULAR_VELOCITY_KD);
+	setPIDCoeff(&(parameter_struct->pid_controllers[ROLL_RATE_ID]), ROLL_ANGULAR_VELOCITY_KP, ROLL_ANGULAR_VELOCITY_KI, ROLL_ANGULAR_VELOCITY_KD);
+	setPIDCoeff(&(parameter_struct->pid_controllers[YAW_RATE_ID]), YAW_ANGULAR_VELOCITY_KP, YAW_ANGULAR_VELOCITY_KI, YAW_ANGULAR_VELOCITY_KD);
 
 	return 0;
  }
@@ -102,14 +102,14 @@
 	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;
+		parameter_struct->pid_controllers[ALT_ID].acc_error = 0.0;
+		parameter_struct->pid_controllers[LOCAL_X_ID].acc_error = 0.0;
+		parameter_struct->pid_controllers[LOCAL_Y_ID].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;
+		parameter_struct->pid_controllers[ALT_ID].prev_error = 0.0;
+		parameter_struct->pid_controllers[LOCAL_X_ID].prev_error = 0.0;
+		parameter_struct->pid_controllers[LOCAL_Y_ID].prev_error = 0.0;
 
 		setpoint_struct->desiredQuadPosition.alt_pos = sensor_struct->currentQuadPosition.alt_pos;
 		setpoint_struct->desiredQuadPosition.x_pos = sensor_struct->currentQuadPosition.x_pos;
@@ -135,26 +135,26 @@
 
 	if(user_input_struct->locationFresh)
 	{
-		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->pid_controllers[LOCAL_Y_ID].current_point = sensor_struct->currentQuadPosition.y_pos;
+		parameter_struct->pid_controllers[LOCAL_Y_ID].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->pid_controllers[LOCAL_X_ID].current_point = sensor_struct->currentQuadPosition.x_pos;
+		parameter_struct->pid_controllers[LOCAL_X_ID].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;
+		parameter_struct->pid_controllers[ALT_ID].current_point = sensor_struct->currentQuadPosition.alt_pos;
+		parameter_struct->pid_controllers[ALT_ID].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));
+		log_struct->local_y_PID_values = pid_computation(&(parameter_struct->pid_controllers[LOCAL_Y_ID]));
+		log_struct->local_x_PID_values = pid_computation(&(parameter_struct->pid_controllers[LOCAL_X_ID]));
+		log_struct->altitude_PID_values = pid_computation(&(parameter_struct->pid_controllers[ALT_ID]));
 
 		// 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
+		parameter_struct->pid_controllers[YAW_ID].current_point = sensor_struct->currentQuadPosition.yaw;// in radians
+		parameter_struct->pid_controllers[YAW_ID].setpoint = setpoint_struct->desiredQuadPosition.yaw; // constant setpoint
 
 		//logging and PID computation
-		log_struct->angle_yaw_PID_values = pid_computation(&(parameter_struct->yaw_angle_pid));
+		log_struct->angle_yaw_PID_values = pid_computation(&(parameter_struct->pid_controllers[YAW_ID]));
 
 	}
 
@@ -165,37 +165,37 @@
 	 */
 
 	//angle boundaries
-	if(parameter_struct->local_x_pid.pid_correction > ROLL_PITCH_MAX_ANGLE)
+	if(parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction > ROLL_PITCH_MAX_ANGLE)
 	{
-		parameter_struct->local_x_pid.pid_correction = ROLL_PITCH_MAX_ANGLE;
+		parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction = ROLL_PITCH_MAX_ANGLE;
 	}
-	if(parameter_struct->local_x_pid.pid_correction < -ROLL_PITCH_MAX_ANGLE)
+	if(parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction < -ROLL_PITCH_MAX_ANGLE)
 	{
-		parameter_struct->local_x_pid.pid_correction = -ROLL_PITCH_MAX_ANGLE;
+		parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction = -ROLL_PITCH_MAX_ANGLE;
 	}
-	if(parameter_struct->local_y_pid.pid_correction > ROLL_PITCH_MAX_ANGLE)
+	if(parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction > ROLL_PITCH_MAX_ANGLE)
 	{
-		parameter_struct->local_y_pid.pid_correction = ROLL_PITCH_MAX_ANGLE;
+		parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction = ROLL_PITCH_MAX_ANGLE;
 	}
-	if(parameter_struct->local_y_pid.pid_correction < -ROLL_PITCH_MAX_ANGLE)
+	if(parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction < -ROLL_PITCH_MAX_ANGLE)
 	{
-		parameter_struct->local_y_pid.pid_correction = -ROLL_PITCH_MAX_ANGLE;
+		parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction = -ROLL_PITCH_MAX_ANGLE;
 	}
 
-	parameter_struct->pitch_angle_pid.current_point = sensor_struct->pitch_angle_filtered;
-	parameter_struct->pitch_angle_pid.setpoint =
+	parameter_struct->pid_controllers[PITCH_ID].current_point = sensor_struct->pitch_angle_filtered;
+	parameter_struct->pid_controllers[PITCH_ID].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->pid_controllers[LOCAL_X_ID].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 =
+	parameter_struct->pid_controllers[ROLL_ID].current_point = sensor_struct->roll_angle_filtered;
+	parameter_struct->pid_controllers[ROLL_ID].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;
+			(parameter_struct->pid_controllers[LOCAL_Y_ID].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));
+	log_struct->angle_pitch_PID_values = pid_computation(&(parameter_struct->pid_controllers[PITCH_ID]));
+	log_struct->angle_roll_PID_values = pid_computation(&(parameter_struct->pid_controllers[ROLL_ID]));
 
 
 	/* 				Angular Velocity Loop
@@ -205,25 +205,25 @@
 
 	// 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;
+	parameter_struct->pid_controllers[PITCH_RATE_ID].current_point = sensor_struct->theta_dot;
+	parameter_struct->pid_controllers[PITCH_RATE_ID].setpoint = parameter_struct->pid_controllers[PITCH_ID].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;
+	parameter_struct->pid_controllers[ROLL_RATE_ID].current_point = sensor_struct->phi_dot;
+	parameter_struct->pid_controllers[ROLL_RATE_ID].setpoint = parameter_struct->pid_controllers[ROLL_ID].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
+	parameter_struct->pid_controllers[YAW_RATE_ID].current_point = sensor_struct->psi_dot;
+	parameter_struct->pid_controllers[YAW_RATE_ID].setpoint = /*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
+			parameter_struct->pid_controllers[YAW_ID].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));
+	log_struct->ang_vel_pitch_PID_values = pid_computation(&(parameter_struct->pid_controllers[PITCH_RATE_ID]));
+	log_struct->ang_vel_roll_PID_values = pid_computation(&(parameter_struct->pid_controllers[ROLL_RATE_ID]));
+	log_struct->ang_vel_yaw_PID_values = pid_computation(&(parameter_struct->pid_controllers[YAW_RATE_ID]));
 
 	//END PIDs///////////////////////////////////////////////////////////////////////
 
@@ -240,43 +240,43 @@
 		{
 			//THROTTLE
 			raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] =
-				((int)(parameter_struct->alt_pid.pid_correction)) + sensor_struct->trims.throttle;
+				((int)(parameter_struct->pid_controllers[ALT_ID].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;
+					parameter_struct->pid_controllers[ROLL_RATE_ID].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;
+					parameter_struct->pid_controllers[PITCH_RATE_ID].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;
+					parameter_struct->pid_controllers[YAW_RATE_ID].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);
+//					parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction,
+//					parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction,
+//					parameter_struct->pid_controllers[ALT_ID].pid_correction,
+//					parameter_struct->pid_controllers[LOCAL_X_ID].setpoint, parameter_struct->pid_controllers[LOCAL_X_ID].current_point,
+//					parameter_struct->pid_controllers[LOCAL_Y_ID].setpoint, parameter_struct->pid_controllers[LOCAL_Y_ID].current_point,
+//					parameter_struct->pid_controllers[ALT_ID].setpoint, parameter_struct->pid_controllers[ALT_ID].current_point);
 		}
 		else{
 			//ROLL
 			raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
-					parameter_struct->roll_ang_vel_pid.pid_correction;
+					parameter_struct->pid_controllers[ROLL_RATE_ID].pid_correction;
 
 			//PITCH
 			raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
-					parameter_struct->pitch_ang_vel_pid.pid_correction;
+					parameter_struct->pid_controllers[PITCH_RATE_ID].pid_correction;
 
 			//YAW
 			raw_actuator_struct->controller_corrected_motor_commands[YAW] =
-					parameter_struct->yaw_ang_vel_pid.pid_correction;
+					parameter_struct->pid_controllers[YAW_RATE_ID].pid_correction;
 		}
 
 		//BOUNDS CHECKING
@@ -314,17 +314,17 @@
 	// 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->local_y_PID = parameter_struct->pid_controllers[LOCAL_Y_ID];
+	log_struct->local_x_PID = parameter_struct->pid_controllers[LOCAL_X_ID];
+	log_struct->altitude_PID = parameter_struct->pid_controllers[ALT_ID];
 
-	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->angle_roll_PID = parameter_struct->pid_controllers[ROLL_ID];
+	log_struct->angle_pitch_PID = parameter_struct->pid_controllers[PITCH_ID];
+	log_struct->angle_yaw_PID = parameter_struct->pid_controllers[YAW_ID];
 
-	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;
+	log_struct->ang_vel_roll_PID = parameter_struct->pid_controllers[ROLL_RATE_ID];
+	log_struct->ang_vel_pitch_PID = parameter_struct->pid_controllers[PITCH_RATE_ID];
+	log_struct->ang_vel_yaw_PID = parameter_struct->pid_controllers[YAW_RATE_ID];
 
 	last_fm_switch = cur_fm_switch;
 
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index 279d3d4138b058f850cce854b34cde4029470784..c6f89b496581f5241b2bfa392d50f1fbda319fd5 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -9,6 +9,8 @@
 #define TYPE_DEF_H_
 
 #include <stdint.h>
+#include "commands.h"
+
 /**
  * @brief
  *      The modes for autonomous and manual flight.
@@ -106,14 +108,14 @@ typedef struct {
 }gam_t;
 
 typedef struct PID_t {
-	double current_point;	// Current value of the system
-	double setpoint;		// Desired value of the system
+	float current_point;	// Current value of the system
+	float setpoint;		// Desired value of the system
 	float Kp;				// Proportional constant
 	float Ki;				// Integral constant
 	float Kd;				// Derivative constant
-	double prev_error;		// Previous error
-	double acc_error;		// Accumulated error
-	double pid_correction;	// Correction factor computed by the PID
+	float prev_error;		// Previous error
+	float acc_error;		// Accumulated error
+	float pid_correction;	// Correction factor computed by the PID
 	float dt; 				// sample period
 } PID_t;
 
@@ -285,13 +287,7 @@ typedef struct setpoint_t {
  *
  */
 typedef struct parameter_t {
-	PID_t roll_angle_pid, roll_ang_vel_pid;
-	PID_t pitch_angle_pid, pitch_ang_vel_pid;
-	PID_t yaw_ang_vel_pid;
-	PID_t local_x_pid;
-	PID_t local_y_pid;
-	PID_t yaw_angle_pid;
-	PID_t alt_pid;
+	PID_t pid_controllers[MAX_CONTROLLER_ID];
 } parameter_t;
 
 /**
@@ -328,7 +324,7 @@ typedef struct actuator_command_t {
  * @brief
  * 		Structures to be used throughout
  */
-typedef struct {
+typedef struct modular_structs {
 	user_input_t user_input_struct;
 	log_t log_struct;
 	raw_sensor_t raw_sensor_struct;
@@ -338,7 +334,7 @@ typedef struct {
 	user_defined_t user_defined_struct;
 	raw_actuator_t raw_actuator_struct;
 	actuator_command_t actuator_command_struct;
-}modular_structs_t;
+} modular_structs_t;
 
 //////// END MAIN MODULAR STRUCTS