Skip to content
Snippets Groups Projects
Commit cf5e79fc authored by dawehr's avatar dawehr
Browse files

Changed logic in callbacks to reduce indentation. 

see cb_setparam, cb_getparam
parent 2349fd1b
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
quad/sw/modular_quad_pid/src/callbacks.c
+ 74
− 70
View file @ cf5e79fc
...@@ -112,35 +112,37 @@ int cb_setparam(modular_structs_t *structs) ...@@ -112,35 +112,37 @@ int cb_setparam(modular_structs_t *structs)
// Get some of the meta data // Get some of the meta data
u16 data_len = uart_buff_get_u16(6); u16 data_len = uart_buff_get_u16(6);
// Check if the data length is correct // Check if the data length is correct
if (data_len == 6) if (data_len != 6)
{ {
// Get the controller ID, parameter ID, parameter value return -1;
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); // Get the controller ID, parameter ID, parameter value
u8 controller_id = uart_buff_data_get_u8(0);
// Check to make sure the IDs are in bounds u8 param_id = uart_buff_data_get_u8(1);
if (controller_id < MAX_CONTROLLER_ID && float param_val = uart_buff_data_get_float(3);
param_id < MAX_CONTROL_PARAM_ID) // Check to make sure the IDs are in bounds
{ if (controller_id >= MAX_CONTROLLER_ID ||
// Set the param_val into the controller by controller_id, param_id param_id >= MAX_CONTROL_PARAM_ID)
switch(param_id) {
{ return -1;
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;
}
}
// 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; return 0;
...@@ -176,51 +178,53 @@ int cb_getparam(modular_structs_t* structs) ...@@ -176,51 +178,53 @@ int cb_getparam(modular_structs_t* structs)
u16 data_len = uart_buff_get_u16(6); u16 data_len = uart_buff_get_u16(6);
u16 msg_id = uart_buff_get_u16(3); u16 msg_id = uart_buff_get_u16(3);
// Check if the data length is correct // 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 case KP_ID:
u8 controller_id = uart_buff_data_get_u8(0); param_val = structs->parameter_struct.pid_controllers[controller_id].Kp;
u8 param_id = uart_buff_data_get_u8(1); break;
case KI_ID:
// Check to make sure the IDs are in bounds param_val = structs->parameter_struct.pid_controllers[controller_id].Ki;
if (controller_id < MAX_CONTROLLER_ID && break;
param_id < MAX_CONTROL_PARAM_ID) case KD_ID:
{ param_val = structs->parameter_struct.pid_controllers[controller_id].Kd;
// Make the variable to send break;
float param_val; case SP_ID:
// Set the param_val equal to the parameter value stored in the controller by param_val = structs->parameter_struct.pid_controllers[controller_id].setpoint;
// controller_id, param_id break;
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));
}
} }
// 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; return 0;
} }
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment