Implemented getcontrol, setcontrol in callbacks.c

- FIXME: there are still a few issues making it so that setting and getting setpoints may not work

quad/sw/modular_quad_pid/src/callbacks.c

@@ -157,53 +157,42 @@ int cb_setcontrol(modular_structs_t *structs)
   */
 int cb_getcontrol(modular_structs_t* structs)
 {
-	// Get the controller ID, value ID
-	u8 controller_id = uart_buff_data_get_u8(0);
-	u8 controller_value_id = uart_buff_data_get_u8(1);
-
-	// 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)
+	// Get the data length
+	u16 data_len = uart_buff_get_u16(6);
+	// Check if the data length is correct
+	if (data_len == 2)
 	{
-		case ROLL_ID:
-			// switch on the value ID
+		// Get the controller ID, value ID
+		u8 controller_id = uart_buff_data_get_u8(0);
+		u8 controller_value_id = uart_buff_data_get_u8(1);
+
+		// check to make sure all the values are in bounds
+		if (controller_id < MAX_CONTROLLER_ID &&
+			controller_value_id < MAX_CONTROL_VAL_ID)
+		{
+			// Make the variable to send
+			float controller_value;
+			// switch on the value ID to decide which one to send
 			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));
+					controller_value = structs->parameter_struct.pid_controllers[controller_id].Kp;
 					break;
 				case KI_ID:
-					//structs->parameter_struct.roll_angle_pid.Ki = controller_value;
+					controller_value = structs->parameter_struct.pid_controllers[controller_id].Ki;
 					break;
 				case KD_ID:
-					//structs->parameter_struct.roll_angle_pid.Kd = controller_value;
+					controller_value = structs->parameter_struct.pid_controllers[controller_id].Kd;
 					break;
 				case SP_ID:
-					//structs->setpoint_struct.desiredQuadPosition.roll = controller_value;
+					controller_value = structs->parameter_struct.pid_controllers[controller_id].setpoint;
 					break;
 			}
-			break;
-		case PITCH_ID:
-			// TODO
-		case YAW_ID:
-			// TODO
-		case ROLL_RATE_ID:
-			// TODO
-		case PITCH_RATE_ID:
-			// TODO
-		case YAW_RATE_ID:
-			// TODO
-		case LOCAL_X_ID:
-			// TODO
-		case LOCAL_Y_ID:
-			// TODO
-		case ALT_ID:
-			// TODO
-			break;
+
+			// Send the controller value
+			send_data(RESPCONTROL_ID, 0, (char *) &controller_value, sizeof(controller_value));
+		}
+
 	}
 
 	return 0;