fixed up the data packing in cb_getcontrol

- the desired format is in the comment above the funciton
- format should be controller ID, controller value ID, controller value
- should now be sending the data in the correct format

quad/sw/modular_quad_pid/src/callbacks.c

@@ -95,18 +95,21 @@ int cb_beginupdate(modular_structs_t *structs) {
 /**
   * Handles a command to set a controller value on the quad.
   *
-  * NOTE: expects the uart buff to have some data in the following format:
-  * |------------------------------------------------------|
-  * | data index||      0      |      1      |    3 - 6    |
-  * |------------------------------------------------------|
-  * |      param||  control ID | ctrl val ID |  float val  |
-  * |------------------------------------------------------|
-  * |      bytes||      1      |      1      |      4      |
-  * |------------------------------------------------------|
+  * NOTE:
+  * Expects the uart buff to have data in the following format:
+  * |--------------------------------------------------------|
+  * |  data index ||      0      |      1      |    2 - 5    |
+  * |--------------------------------------------------------|
+  * |       param ||  control ID | ctrl val ID |  float val  |
+  * |--------------------------------------------------------|
+  * |       bytes ||      1      |      1      |      4      |
+  * |--------------------------------------------------------|
+  * 
+  * Does not send anything in response.
   */
 int cb_setcontrol(modular_structs_t *structs)
 {
-	// Get the data length
+	// 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)
@@ -116,11 +119,11 @@ int cb_setcontrol(modular_structs_t *structs)
 		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
+		// Check to make sure the IDs are in bounds
 		if (controller_id < MAX_CONTROLLER_ID &&
 			controller_value_id < MAX_CONTROL_VAL_ID)
 		{
-			// switch on the value ID
+			// Set the controller_value into the controller by controller_id, controller_value_id
 			switch(controller_value_id)
 			{
 				case KP_ID:
@@ -146,18 +149,30 @@ int cb_setcontrol(modular_structs_t *structs)
 /**
   * Handles a command to get a controller value from the quad.
   *
-  * NOTE: expects the uart buff to have some data in the following format:
-  * |----------------------------------------|
-  * | data index||      0      |      1      |
-  * |----------------------------------------|
-  * |      param||  control ID | ctrl val ID |
-  * |----------------------------------------|
-  * |      bytes||      1      |      1      |
-  * |----------------------------------------|
+  * NOTE:
+  * Expects the uart buff to have data in the following format:
+  * |------------------------------------------|
+  * |  data index ||      0      |      1      |
+  * |------------------------------------------|
+  * |       param ||  control ID | ctrl val ID |
+  * |------------------------------------------|
+  * |       bytes ||      1      |      1      |
+  * |------------------------------------------|
+  *
+  * Sends a response of type RESPONSECONTROL_ID.
+  * The response will have a message ID equal to the one originally received.
+  * The data of the response will be in the following format:
+  * |--------------------------------------------------------|
+  * |  data index ||      0      |      1      |    2 - 5    |
+  * |--------------------------------------------------------|
+  * |       param ||  control ID | ctrl val ID |  float val  |
+  * |--------------------------------------------------------|
+  * |       bytes ||      1      |      1      |      4      |
+  * |--------------------------------------------------------|
   */
 int cb_getcontrol(modular_structs_t* structs)
 {
-	// Get the data length
+	// Get some of the meta data
 	u16 data_len = uart_buff_get_u16(6);
 	u16 msg_id = uart_buff_get_u16(3);
 	// Check if the data length is correct
@@ -167,13 +182,14 @@ int cb_getcontrol(modular_structs_t* structs)
 		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
+		// Check to make sure the IDs 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
+			// Set the controller_value equal to the controller value stored in the controller by
+			// controllerid, controller_value_id
 			switch(controller_value_id)
 			{
 				case KP_ID:
@@ -189,9 +205,22 @@ int cb_getcontrol(modular_structs_t* structs)
 					controller_value = structs->parameter_struct.pid_controllers[controller_id].setpoint;
 					break;
 			}
-
-			// Send the controller value
-			send_data(RESPCONTROL_ID, msg_id, (char *) &controller_value, sizeof(controller_value));
+			
+			// Format the response data
+			char resp_data[6];
+
+			// Controller ID
+			resp_data[0] = controller_id;
+			// Controller value ID
+			resp_data[1] = controller_value_id;
+			// Controller value (4 byte float)
+			resp_data[2] = (controller_value) & 0x000000ff;
+			resp_data[3] = (controller_value >> 8) & 0x000000ff;
+			resp_data[4] = (controller_value >> 16) & 0x000000ff;
+			resp_data[5] = (controller_value >> 24) & 0x000000ff;
+			
+			// Send the data
+			send_data(RESPCONTROL_ID, msg_id, resp_data, sizeof(resp_data);
 		}
 
 	}