From 00686bfd21dbf2ccdc13566cff7eedb881af41c2 Mon Sep 17 00:00:00 2001
From: "ucart@co3050-12" <dawehr@iastate.edu>
Date: Sun, 29 Jan 2017 20:42:23 -0600
Subject: [PATCH] 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 | 59 ++++++++++--------------
1 file changed, 24 insertions(+), 35 deletions(-)
diff --git a/quad/sw/modular_quad_pid/src/callbacks.c b/quad/sw/modular_quad_pid/src/callbacks.c
index 917bc4ef1..1d4604781 100644
--- a/quad/sw/modular_quad_pid/src/callbacks.c
+++ b/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;
--
GitLab