diff --git a/quad/sw/modular_quad_pid/src/callbacks.c b/quad/sw/modular_quad_pid/src/callbacks.c
index 8c6661dad66de3b07db273c85753b97ef34e36fd..3555fb45826a4dbcb2c1859e06d206cffaa07b18 100644
--- a/quad/sw/modular_quad_pid/src/callbacks.c
+++ b/quad/sw/modular_quad_pid/src/callbacks.c
@@ -112,35 +112,37 @@ int cb_setparam(modular_structs_t *structs)
 	// Get some of the meta data
 	u16 data_len = uart_buff_get_u16(6);
 	// Check if the data length is correct
-	if (data_len == 6)
+	if (data_len != 6)
 	{
-		// Get the controller ID, parameter ID, parameter value
-		u8 controller_id = uart_buff_data_get_u8(0);
-		u8 param_id = uart_buff_data_get_u8(1);
-		float param_val = uart_buff_data_get_float(3);
-
-		// Check to make sure the IDs are in bounds
-		if (controller_id < MAX_CONTROLLER_ID &&
-			param_id < MAX_CONTROL_PARAM_ID)
-		{
-			// Set the param_val into the controller by controller_id, param_id
-			switch(param_id)
-			{
-				case KP_ID:
-					structs->parameter_struct.pid_controllers[controller_id].Kp = param_val;
-					break;
-				case KI_ID:
-					structs->parameter_struct.pid_controllers[controller_id].Ki = param_val;
-					break;
-				case KD_ID:
-					structs->parameter_struct.pid_controllers[controller_id].Kd = param_val;
-					break;
-				case SP_ID:
-					structs->parameter_struct.pid_controllers[controller_id].setpoint = param_val;
-					break;
-			}
-		}
+		return -1;
+	}
+
+	// Get the controller ID, parameter ID, parameter value
+	u8 controller_id = uart_buff_data_get_u8(0);
+	u8 param_id = uart_buff_data_get_u8(1);
+	float param_val = uart_buff_data_get_float(3);
+	// Check to make sure the IDs are in bounds
+	if (controller_id >= MAX_CONTROLLER_ID ||
+		param_id >= MAX_CONTROL_PARAM_ID)
+	{
+		return -1;
+	}
 
+	// Set the param_val into the controller by controller_id, param_id
+	switch(param_id)
+	{
+		case KP_ID:
+			structs->parameter_struct.pid_controllers[controller_id].Kp = param_val;
+			break;
+		case KI_ID:
+			structs->parameter_struct.pid_controllers[controller_id].Ki = param_val;
+			break;
+		case KD_ID:
+			structs->parameter_struct.pid_controllers[controller_id].Kd = param_val;
+			break;
+		case SP_ID:
+			structs->parameter_struct.pid_controllers[controller_id].setpoint = param_val;
+			break;
 	}
 
 	return 0;
@@ -176,51 +178,53 @@ int cb_getparam(modular_structs_t* structs)
 	u16 data_len = uart_buff_get_u16(6);
 	u16 msg_id = uart_buff_get_u16(3);
 	// Check if the data length is correct
-	if (data_len == 2)
+	if (data_len != 2)
+	{
+		return -1;
+	}
+
+	// Get the controller ID, parameter ID
+	u8 controller_id = uart_buff_data_get_u8(0);
+	u8 param_id = uart_buff_data_get_u8(1);
+	// Check to make sure the IDs are in bounds
+	if (controller_id >= MAX_CONTROLLER_ID ||
+		param_id >= MAX_CONTROL_PARAM_ID)
+	{
+		return -1;
+	}
+
+	// Make the variable to send
+	float param_val;
+	// Set the param_val equal to the parameter value stored in the controller by
+	// controller_id, param_id
+	switch(param_id)
 	{
-		// Get the controller ID, parameter ID
-		u8 controller_id = uart_buff_data_get_u8(0);
-		u8 param_id = uart_buff_data_get_u8(1);
-
-		// Check to make sure the IDs are in bounds
-		if (controller_id < MAX_CONTROLLER_ID &&
-			param_id < MAX_CONTROL_PARAM_ID)
-		{
-			// Make the variable to send
-			float param_val;
-			// Set the param_val equal to the parameter value stored in the controller by
-			// controller_id, param_id
-			switch(param_id)
-			{
-				case KP_ID:
-					param_val = structs->parameter_struct.pid_controllers[controller_id].Kp;
-					break;
-				case KI_ID:
-					param_val = structs->parameter_struct.pid_controllers[controller_id].Ki;
-					break;
-				case KD_ID:
-					param_val = structs->parameter_struct.pid_controllers[controller_id].Kd;
-					break;
-				case SP_ID:
-					param_val = structs->parameter_struct.pid_controllers[controller_id].setpoint;
-					break;
-			}
-			
-			// Format the response data
-			char resp_data[6];
-
-			// Controller ID
-			resp_data[0] = controller_id;
-			// Parameter ID
-			resp_data[1] = param_id;
-			// Parameter value (4 byte float)
-			// TODO set a strict byte ordering for communication between the ground station and the quad
-			memcpy(&resp_data[2], &param_val, sizeof(param_val));
-			
-			// Send the response
-			send_data(RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
-		}
+		case KP_ID:
+			param_val = structs->parameter_struct.pid_controllers[controller_id].Kp;
+			break;
+		case KI_ID:
+			param_val = structs->parameter_struct.pid_controllers[controller_id].Ki;
+			break;
+		case KD_ID:
+			param_val = structs->parameter_struct.pid_controllers[controller_id].Kd;
+			break;
+		case SP_ID:
+			param_val = structs->parameter_struct.pid_controllers[controller_id].setpoint;
+			break;
 	}
 
+	// Format the response data
+	char resp_data[6];
+	// Controller ID
+	resp_data[0] = controller_id;
+	// Parameter ID
+	resp_data[1] = param_id;
+	// Parameter value (4 byte float)
+	// TODO set a strict byte ordering for communication between the ground station and the quad
+	memcpy(&resp_data[2], &param_val, sizeof(param_val));
+
+	// Send the response
+	send_data(RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
+
 	return 0;
 }