From 1ac62e7538bb8a5bc59bd350a2ff158f5df7ab0b Mon Sep 17 00:00:00 2001
From: "ucart@co3050-12" <dawehr@iastate.edu>
Date: Sun, 29 Jan 2017 20:30:27 -0600
Subject: [PATCH] Changed the parameter struct in modular_structs to have an
array of PID_t types
- added some comments in commands.c/h about usage of the file
- added a MAX_CONTROLLER_ID and MAX_CONTROL_VAL_ID to the enums in commands.h
- implemented cb_set_control in callbacks.c to work with the new parameter struct
- changes the parameter_struct to now contain an array of pid_controllers instead of a bunch of random PID_t members
- the new parameter_struct array can now be accessed by controller ID as defined in commands.h
---
groundStation/src/backend/commands.c | 3 +
groundStation/src/backend/commands.h | 5 +
quad/sw/modular_quad_pid/src/callbacks.c | 190 +++---------------
quad/sw/modular_quad_pid/src/callbacks.h | 7 +-
quad/sw/modular_quad_pid/src/cb_default.h | 1 +
.../modular_quad_pid/src/control_algorithm.c | 164 +++++++--------
quad/sw/modular_quad_pid/src/type_def.h | 24 +--
7 files changed, 131 insertions(+), 263 deletions(-)
diff --git a/groundStation/src/backend/commands.c b/groundStation/src/backend/commands.c
index 1cef27dc9..b88aebba6 100644
--- a/groundStation/src/backend/commands.c
+++ b/groundStation/src/backend/commands.c
@@ -68,6 +68,9 @@ command_cb cb_respcontrol __attribute__((weak, alias("cb_default")));
*
* There is one callback function pointer associated with each
* element in this struct array.
+ *
+ * DO NOT change this struct without updating the
+ * "MessageTypeID" struct in commands.h as well
*/
struct MessageType MessageTypes[MAX_TYPE_ID] =
{
diff --git a/groundStation/src/backend/commands.h b/groundStation/src/backend/commands.h
index 9fda24a71..094e3ac59 100644
--- a/groundStation/src/backend/commands.h
+++ b/groundStation/src/backend/commands.h
@@ -28,7 +28,10 @@ enum DataType
/*
* Message type IDs used to know what kind of message we are dealing with
* Enumeration used to index the MessageTypes array in commands.c
+ *
* DO NOT change this enum or you will break backwards compatibility.
+ * DO NOT change this enum without also updating the "MessageTypes" array
+ * in commands.c to match.
*/
enum MessageTypeID{
DEBUG_ID, // 00
@@ -57,6 +60,7 @@ enum ControllerID{
LOCAL_X_ID, // 06 - Local X PID
LOCAL_Y_ID, // 07 - Local Y PID
ALT_ID, // 08 - Altitude PID
+ MAX_CONTROLLER_ID, // 09 - Just used to keep track of the size
};
/*
@@ -67,6 +71,7 @@ enum ControllerValueID{
KI_ID, // 01 - I constant
KD_ID, // 02 - D constant
SP_ID, // 03 - Setpoint value
+ MAX_CONTROL_VAL_ID, // 04 - Just used to keep track of the size
};
/*
diff --git a/quad/sw/modular_quad_pid/src/callbacks.c b/quad/sw/modular_quad_pid/src/callbacks.c
index 52705f5b4..917bc4ef1 100644
--- a/quad/sw/modular_quad_pid/src/callbacks.c
+++ b/quad/sw/modular_quad_pid/src/callbacks.c
@@ -106,178 +106,38 @@ int cb_beginupdate(modular_structs_t *structs) {
*/
int cb_setcontrol(modular_structs_t *structs)
{
- // Get the controller ID, value ID, float value
- u8 controller_id = uart_buff_data_get_u8(0);
- u8 controller_value_id = uart_buff_data_get_u8(1);
- float controller_value = uart_buff_data_get_float(3);
-
- // TODO check if the setpoints really need to be doubles
- // TODO add rate setpoints
- // 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 == 6)
{
- case ROLL_ID:
- // switch on the value ID
- switch(controller_value_id)
- {
- case KP_ID:
- structs->parameter_struct.roll_angle_pid.Kp = controller_value;
- break;
- case KI_ID:
- structs->parameter_struct.roll_angle_pid.Ki = controller_value;
- break;
- case KD_ID:
- structs->parameter_struct.roll_angle_pid.Kd = controller_value;
- break;
- case SP_ID:
- structs->setpoint_struct.desiredQuadPosition.roll = controller_value;
- break;
- }
- break;
- case PITCH_ID:
- // switch on the value ID
- switch(controller_value_id)
- {
- case KP_ID:
- structs->parameter_struct.pitch_angle_pid.Kp = controller_value;
- break;
- case KI_ID:
- structs->parameter_struct.pitch_angle_pid.Ki = controller_value;
- break;
- case KD_ID:
- structs->parameter_struct.pitch_angle_pid.Kd = controller_value;
- break;
- case SP_ID:
- structs->setpoint_struct.desiredQuadPosition.pitch = controller_value;
- break;
- }
- break;
- case YAW_ID:
+ // Get the controller ID, value ID, float value
+ u8 controller_id = uart_buff_data_get_u8(0);
+ 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
+ if (controller_id < MAX_CONTROLLER_ID &&
+ controller_value_id < MAX_CONTROL_VAL_ID)
+ {
// switch on the value ID
switch(controller_value_id)
{
case KP_ID:
- structs->parameter_struct.yaw_angle_pid.Kp = controller_value;
+ structs->parameter_struct.pid_controllers[controller_id].Kp = controller_value;
break;
case KI_ID:
- structs->parameter_struct.yaw_angle_pid.Ki = controller_value;
+ structs->parameter_struct.pid_controllers[controller_id].Ki = controller_value;
break;
case KD_ID:
- structs->parameter_struct.yaw_angle_pid.Kd = controller_value;
+ structs->parameter_struct.pid_controllers[controller_id].Kd = controller_value;
break;
case SP_ID:
- structs->setpoint_struct.desiredQuadPosition.yaw = controller_value;
+ structs->parameter_struct.pid_controllers[controller_id].setpoint = controller_value;
break;
}
- break;
- case ROLL_RATE_ID:
- // switch on the value ID
- switch(controller_value_id)
- {
- case KP_ID:
- structs->parameter_struct.roll_ang_vel_pid.Kp = controller_value;
- break;
- case KI_ID:
- structs->parameter_struct.roll_ang_vel_pid.Ki = controller_value;
- break;
- case KD_ID:
- structs->parameter_struct.roll_ang_vel_pid.Kd = controller_value;
- break;
- case SP_ID:
- // do nothing for now because there is no set point for this conroller
- break;
- }
- break;
- case PITCH_RATE_ID:
- // switch on the value ID
- switch(controller_value_id)
- {
- case KP_ID:
- structs->parameter_struct.pitch_ang_vel_pid.Kp = controller_value;
- break;
- case KI_ID:
- structs->parameter_struct.pitch_ang_vel_pid.Ki = controller_value;
- break;
- case KD_ID:
- structs->parameter_struct.pitch_ang_vel_pid.Kd = controller_value;
- break;
- case SP_ID:
- // do nothing for now because there is no set point for this conroller
- break;
- }
- break;
- case YAW_RATE_ID:
- // switch on the value ID
- switch(controller_value_id)
- {
- case KP_ID:
- structs->parameter_struct.yaw_ang_vel_pid.Kp = controller_value;
- break;
- case KI_ID:
- structs->parameter_struct.yaw_ang_vel_pid.Ki = controller_value;
- break;
- case KD_ID:
- structs->parameter_struct.yaw_ang_vel_pid.Kd = controller_value;
- break;
- case SP_ID:
- // do nothing for now because there is no set point for this conroller
- break;
- }
- break;
- case LOCAL_X_ID:
- // switch on the value ID
- switch(controller_value_id)
- {
- case KP_ID:
- structs->parameter_struct.local_x_pid.Kp = controller_value;
- break;
- case KI_ID:
- structs->parameter_struct.local_x_pid.Ki = controller_value;
- break;
- case KD_ID:
- structs->parameter_struct.local_x_pid.Kd = controller_value;
- break;
- case SP_ID:
- structs->setpoint_struct.desiredQuadPosition.x_pos = controller_value;
- break;
- }
- break;
- case LOCAL_Y_ID:
- // switch on the value ID
- switch(controller_value_id)
- {
- case KP_ID:
- structs->parameter_struct.local_y_pid.Kp = controller_value;
- break;
- case KI_ID:
- structs->parameter_struct.local_y_pid.Ki = controller_value;
- break;
- case KD_ID:
- structs->parameter_struct.local_y_pid.Kd = controller_value;
- break;
- case SP_ID:
- structs->setpoint_struct.desiredQuadPosition.y_pos = controller_value;
- break;
- }
- break;
- case ALT_ID:
- // switch on the value ID
- switch(controller_value_id)
- {
- case KP_ID:
- structs->parameter_struct.alt_pid.Kp = controller_value;
- break;
- case KI_ID:
- structs->parameter_struct.alt_pid.Ki = controller_value;
- break;
- case KD_ID:
- structs->parameter_struct.alt_pid.Kd = controller_value;
- break;
- case SP_ID:
- structs->setpoint_struct.desiredQuadPosition.alt_pos = controller_value;
- break;
- }
- break;
+ }
+
}
return 0;
@@ -304,6 +164,7 @@ int cb_getcontrol(modular_structs_t* structs)
// 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)
{
@@ -312,17 +173,17 @@ int cb_getcontrol(modular_structs_t* structs)
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));
+ //float controller_value = structs->parameter_struct.roll_angle_pid.Kp;
+ //send_data(RESPCONTROL_ID, 0, ((char *) &controller_value), sizeof(controller_value));
break;
case KI_ID:
- structs->parameter_struct.roll_angle_pid.Ki = controller_value;
+ //structs->parameter_struct.roll_angle_pid.Ki = controller_value;
break;
case KD_ID:
- structs->parameter_struct.roll_angle_pid.Kd = controller_value;
+ //structs->parameter_struct.roll_angle_pid.Kd = controller_value;
break;
case SP_ID:
- structs->setpoint_struct.desiredQuadPosition.roll = controller_value;
+ //structs->setpoint_struct.desiredQuadPosition.roll = controller_value;
break;
}
break;
@@ -342,6 +203,7 @@ int cb_getcontrol(modular_structs_t* structs)
// TODO
case ALT_ID:
// TODO
+ break;
}
return 0;
diff --git a/quad/sw/modular_quad_pid/src/callbacks.h b/quad/sw/modular_quad_pid/src/callbacks.h
index 6b6dee02a..7e67ddac5 100644
--- a/quad/sw/modular_quad_pid/src/callbacks.h
+++ b/quad/sw/modular_quad_pid/src/callbacks.h
@@ -1,10 +1,11 @@
-#ifndef __callbacks_
+#ifndef __callbacks_h
#define __callbacks_h
/* Grab some stupid stuff from legacy code */
-#include "type_def.h"
+
+struct modular_structs;
/* Make commands.c happy */
-typedef int (command_cb)(modular_structs_t *structs);
+typedef int (command_cb)(struct modular_structs *structs);
#endif
diff --git a/quad/sw/modular_quad_pid/src/cb_default.h b/quad/sw/modular_quad_pid/src/cb_default.h
index 99982e63f..aa308108b 100644
--- a/quad/sw/modular_quad_pid/src/cb_default.h
+++ b/quad/sw/modular_quad_pid/src/cb_default.h
@@ -1,4 +1,5 @@
#include "commands.h"
+#include "type_def.h"
/* The cb_default used on the groundStation. This file MUST NOT BE INCLUDED
* by anything except for commands.c */
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index 2e79f58fb..63ba75613 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -16,42 +16,42 @@
{
// HUMAN Piloted (RC) PID DEFINITIONS //////
// RC PIDs for roll (2 loops: angle --> angular velocity)
- parameter_struct->roll_angle_pid.dt = 0.005; parameter_struct->roll_ang_vel_pid.dt = 0.005; // 5 ms calculation period
+ parameter_struct->pid_controllers[ROLL_ID].dt = 0.005; parameter_struct->pid_controllers[ROLL_RATE_ID].dt = 0.005; // 5 ms calculation period
// RC PIDs for pitch (2 loops: angle --> angular velocity)
- parameter_struct->pitch_angle_pid.dt = 0.005; parameter_struct->pitch_ang_vel_pid.dt = 0.005; // 5 ms calculation period
+ parameter_struct->pid_controllers[PITCH_ID].dt = 0.005; parameter_struct->pid_controllers[PITCH_RATE_ID].dt = 0.005; // 5 ms calculation period
// initialize Yaw PID_t and PID constants
// RC PID for yaw (1 loop angular velocity)
- parameter_struct->yaw_ang_vel_pid.dt = 0.005; // 5 ms calculation period
+ parameter_struct->pid_controllers[YAW_RATE_ID].dt = 0.005; // 5 ms calculation period
// AUTOMATIC Pilot (Position) PID DEFINITIONS //////
// Local X PID using a translation from X camera system data to quad local X position (3 loops: local y position --> angle --> angular velocity)
- parameter_struct->local_x_pid.dt = 0.100;
+ parameter_struct->pid_controllers[LOCAL_X_ID].dt = 0.100;
// Local Y PID using a translation from Y camera system data to quad local Y position(3 loops: local x position --> angle --> angular velocity)
- parameter_struct->local_y_pid.dt = 0.100;
+ parameter_struct->pid_controllers[LOCAL_Y_ID].dt = 0.100;
// CAM PIDs for yaw (2 loops angle --> angular velocity)
- parameter_struct->yaw_angle_pid.dt = 0.100;
+ parameter_struct->pid_controllers[YAW_ID].dt = 0.100;
// CAM PID for altitude (1 loop altitude)
- parameter_struct->alt_pid.dt = 0.100;
+ parameter_struct->pid_controllers[ALT_ID].dt = 0.100;
// PID coeffiecients (Position)
- setPIDCoeff(&(parameter_struct->local_y_pid), YPOS_KP, YPOS_KI, YPOS_KD);
- setPIDCoeff(&(parameter_struct->local_x_pid), XPOS_KP, XPOS_KI, XPOS_KD);
- setPIDCoeff(&(parameter_struct->alt_pid), ALT_ZPOS_KP, ALT_ZPOS_KI, ALT_ZPOS_KD);
+ setPIDCoeff(&(parameter_struct->pid_controllers[LOCAL_Y_ID]), YPOS_KP, YPOS_KI, YPOS_KD);
+ setPIDCoeff(&(parameter_struct->pid_controllers[LOCAL_X_ID]), XPOS_KP, XPOS_KI, XPOS_KD);
+ setPIDCoeff(&(parameter_struct->pid_controllers[ALT_ID]), ALT_ZPOS_KP, ALT_ZPOS_KI, ALT_ZPOS_KD);
// PID coefficients (Angle)
- setPIDCoeff(&(parameter_struct->pitch_angle_pid), PITCH_ANGLE_KP, PITCH_ANGLE_KI, PITCH_ANGLE_KD);
- setPIDCoeff(&(parameter_struct->roll_angle_pid), ROLL_ANGLE_KP, ROLL_ANGLE_KI, ROLL_ANGLE_KD);
- setPIDCoeff(&(parameter_struct->yaw_angle_pid), YAW_ANGLE_KP, YAW_ANGLE_KI, YAW_ANGLE_KD);
+ setPIDCoeff(&(parameter_struct->pid_controllers[PITCH_ID]), PITCH_ANGLE_KP, PITCH_ANGLE_KI, PITCH_ANGLE_KD);
+ setPIDCoeff(&(parameter_struct->pid_controllers[ROLL_ID]), ROLL_ANGLE_KP, ROLL_ANGLE_KI, ROLL_ANGLE_KD);
+ setPIDCoeff(&(parameter_struct->pid_controllers[YAW_ID]), YAW_ANGLE_KP, YAW_ANGLE_KI, YAW_ANGLE_KD);
// PID coefficients (Angular Velocity)
- setPIDCoeff(&(parameter_struct->pitch_ang_vel_pid), PITCH_ANGULAR_VELOCITY_KP, PITCH_ANGULAR_VELOCITY_KI, PITCH_ANGULAR_VELOCITY_KD);
- setPIDCoeff(&(parameter_struct->roll_ang_vel_pid), ROLL_ANGULAR_VELOCITY_KP, ROLL_ANGULAR_VELOCITY_KI, ROLL_ANGULAR_VELOCITY_KD);
- setPIDCoeff(&(parameter_struct->yaw_ang_vel_pid), YAW_ANGULAR_VELOCITY_KP, YAW_ANGULAR_VELOCITY_KI, YAW_ANGULAR_VELOCITY_KD);
+ setPIDCoeff(&(parameter_struct->pid_controllers[PITCH_RATE_ID]), PITCH_ANGULAR_VELOCITY_KP, PITCH_ANGULAR_VELOCITY_KI, PITCH_ANGULAR_VELOCITY_KD);
+ setPIDCoeff(&(parameter_struct->pid_controllers[ROLL_RATE_ID]), ROLL_ANGULAR_VELOCITY_KP, ROLL_ANGULAR_VELOCITY_KI, ROLL_ANGULAR_VELOCITY_KD);
+ setPIDCoeff(&(parameter_struct->pid_controllers[YAW_RATE_ID]), YAW_ANGULAR_VELOCITY_KP, YAW_ANGULAR_VELOCITY_KI, YAW_ANGULAR_VELOCITY_KD);
return 0;
}
@@ -102,14 +102,14 @@
if((cur_fm_switch == AUTO_FLIGHT_MODE) && (user_defined_struct->engaging_auto == 2))
{
// zero out the accumulated error so the I terms don't cause wild things to happen
- parameter_struct->alt_pid.acc_error = 0.0;
- parameter_struct->local_x_pid.acc_error = 0.0;
- parameter_struct->local_y_pid.acc_error = 0.0;
+ parameter_struct->pid_controllers[ALT_ID].acc_error = 0.0;
+ parameter_struct->pid_controllers[LOCAL_X_ID].acc_error = 0.0;
+ parameter_struct->pid_controllers[LOCAL_Y_ID].acc_error = 0.0;
// make previous error equal to the current so the D term doesn't spike
- parameter_struct->alt_pid.prev_error = 0.0;
- parameter_struct->local_x_pid.prev_error = 0.0;
- parameter_struct->local_y_pid.prev_error = 0.0;
+ parameter_struct->pid_controllers[ALT_ID].prev_error = 0.0;
+ parameter_struct->pid_controllers[LOCAL_X_ID].prev_error = 0.0;
+ parameter_struct->pid_controllers[LOCAL_Y_ID].prev_error = 0.0;
setpoint_struct->desiredQuadPosition.alt_pos = sensor_struct->currentQuadPosition.alt_pos;
setpoint_struct->desiredQuadPosition.x_pos = sensor_struct->currentQuadPosition.x_pos;
@@ -135,26 +135,26 @@
if(user_input_struct->locationFresh)
{
- parameter_struct->local_y_pid.current_point = sensor_struct->currentQuadPosition.y_pos;
- parameter_struct->local_y_pid.setpoint = setpoint_struct->desiredQuadPosition.y_pos;
+ parameter_struct->pid_controllers[LOCAL_Y_ID].current_point = sensor_struct->currentQuadPosition.y_pos;
+ parameter_struct->pid_controllers[LOCAL_Y_ID].setpoint = setpoint_struct->desiredQuadPosition.y_pos;
- parameter_struct->local_x_pid.current_point = sensor_struct->currentQuadPosition.x_pos;
- parameter_struct->local_x_pid.setpoint = setpoint_struct->desiredQuadPosition.x_pos;
+ parameter_struct->pid_controllers[LOCAL_X_ID].current_point = sensor_struct->currentQuadPosition.x_pos;
+ parameter_struct->pid_controllers[LOCAL_X_ID].setpoint = setpoint_struct->desiredQuadPosition.x_pos;
- parameter_struct->alt_pid.current_point = sensor_struct->currentQuadPosition.alt_pos;
- parameter_struct->alt_pid.setpoint = setpoint_struct->desiredQuadPosition.alt_pos;
+ parameter_struct->pid_controllers[ALT_ID].current_point = sensor_struct->currentQuadPosition.alt_pos;
+ parameter_struct->pid_controllers[ALT_ID].setpoint = setpoint_struct->desiredQuadPosition.alt_pos;
//logging and PID computation
- log_struct->local_y_PID_values = pid_computation(&(parameter_struct->local_y_pid));
- log_struct->local_x_PID_values = pid_computation(&(parameter_struct->local_x_pid));
- log_struct->altitude_PID_values = pid_computation(&(parameter_struct->alt_pid));
+ log_struct->local_y_PID_values = pid_computation(&(parameter_struct->pid_controllers[LOCAL_Y_ID]));
+ log_struct->local_x_PID_values = pid_computation(&(parameter_struct->pid_controllers[LOCAL_X_ID]));
+ log_struct->altitude_PID_values = pid_computation(&(parameter_struct->pid_controllers[ALT_ID]));
// yaw angular position PID calculation
- parameter_struct->yaw_angle_pid.current_point = sensor_struct->currentQuadPosition.yaw;// in radians
- parameter_struct->yaw_angle_pid.setpoint = setpoint_struct->desiredQuadPosition.yaw; // constant setpoint
+ parameter_struct->pid_controllers[YAW_ID].current_point = sensor_struct->currentQuadPosition.yaw;// in radians
+ parameter_struct->pid_controllers[YAW_ID].setpoint = setpoint_struct->desiredQuadPosition.yaw; // constant setpoint
//logging and PID computation
- log_struct->angle_yaw_PID_values = pid_computation(&(parameter_struct->yaw_angle_pid));
+ log_struct->angle_yaw_PID_values = pid_computation(&(parameter_struct->pid_controllers[YAW_ID]));
}
@@ -165,37 +165,37 @@
*/
//angle boundaries
- if(parameter_struct->local_x_pid.pid_correction > ROLL_PITCH_MAX_ANGLE)
+ if(parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction > ROLL_PITCH_MAX_ANGLE)
{
- parameter_struct->local_x_pid.pid_correction = ROLL_PITCH_MAX_ANGLE;
+ parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction = ROLL_PITCH_MAX_ANGLE;
}
- if(parameter_struct->local_x_pid.pid_correction < -ROLL_PITCH_MAX_ANGLE)
+ if(parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction < -ROLL_PITCH_MAX_ANGLE)
{
- parameter_struct->local_x_pid.pid_correction = -ROLL_PITCH_MAX_ANGLE;
+ parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction = -ROLL_PITCH_MAX_ANGLE;
}
- if(parameter_struct->local_y_pid.pid_correction > ROLL_PITCH_MAX_ANGLE)
+ if(parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction > ROLL_PITCH_MAX_ANGLE)
{
- parameter_struct->local_y_pid.pid_correction = ROLL_PITCH_MAX_ANGLE;
+ parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction = ROLL_PITCH_MAX_ANGLE;
}
- if(parameter_struct->local_y_pid.pid_correction < -ROLL_PITCH_MAX_ANGLE)
+ if(parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction < -ROLL_PITCH_MAX_ANGLE)
{
- parameter_struct->local_y_pid.pid_correction = -ROLL_PITCH_MAX_ANGLE;
+ parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction = -ROLL_PITCH_MAX_ANGLE;
}
- parameter_struct->pitch_angle_pid.current_point = sensor_struct->pitch_angle_filtered;
- parameter_struct->pitch_angle_pid.setpoint =
+ parameter_struct->pid_controllers[PITCH_ID].current_point = sensor_struct->pitch_angle_filtered;
+ parameter_struct->pid_controllers[PITCH_ID].setpoint =
/*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
- (parameter_struct->local_x_pid.pid_correction) + pitch_trim :*/ user_input_struct->pitch_angle_manual_setpoint;
+ (parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction) + pitch_trim :*/ user_input_struct->pitch_angle_manual_setpoint;
- parameter_struct->roll_angle_pid.current_point = sensor_struct->roll_angle_filtered;
- parameter_struct->roll_angle_pid.setpoint =
+ parameter_struct->pid_controllers[ROLL_ID].current_point = sensor_struct->roll_angle_filtered;
+ parameter_struct->pid_controllers[ROLL_ID].setpoint =
/*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
- (parameter_struct->local_y_pid.pid_correction) + roll_trim :*/ user_input_struct->roll_angle_manual_setpoint;
+ (parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction) + roll_trim :*/ user_input_struct->roll_angle_manual_setpoint;
//logging and PID computation
- log_struct->angle_pitch_PID_values = pid_computation(&(parameter_struct->pitch_angle_pid));
- log_struct->angle_roll_PID_values = pid_computation(&(parameter_struct->roll_angle_pid));
+ log_struct->angle_pitch_PID_values = pid_computation(&(parameter_struct->pid_controllers[PITCH_ID]));
+ log_struct->angle_roll_PID_values = pid_computation(&(parameter_struct->pid_controllers[ROLL_ID]));
/* Angular Velocity Loop
@@ -205,25 +205,25 @@
// theta_dot is the angular velocity about the y-axis
// it is calculated from using the gimbal equations
- parameter_struct->pitch_ang_vel_pid.current_point = sensor_struct->theta_dot;
- parameter_struct->pitch_ang_vel_pid.setpoint = parameter_struct->pitch_angle_pid.pid_correction;
+ parameter_struct->pid_controllers[PITCH_RATE_ID].current_point = sensor_struct->theta_dot;
+ parameter_struct->pid_controllers[PITCH_RATE_ID].setpoint = parameter_struct->pid_controllers[PITCH_ID].pid_correction;
// phi_dot is the angular velocity about the x-axis
// it is calculated from using the gimbal equations
- parameter_struct->roll_ang_vel_pid.current_point = sensor_struct->phi_dot;
- parameter_struct->roll_ang_vel_pid.setpoint = parameter_struct->roll_angle_pid.pid_correction;
+ parameter_struct->pid_controllers[ROLL_RATE_ID].current_point = sensor_struct->phi_dot;
+ parameter_struct->pid_controllers[ROLL_RATE_ID].setpoint = parameter_struct->pid_controllers[ROLL_ID].pid_correction;
// Yaw angular velocity PID
// psi_dot is the angular velocity about the z-axis
// it is calculated from using the gimbal equations
- parameter_struct->yaw_ang_vel_pid.current_point = sensor_struct->psi_dot;
- parameter_struct->yaw_ang_vel_pid.setpoint = /*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
- parameter_struct->yaw_angle_pid.pid_correction :*/ user_input_struct->yaw_manual_setpoint; // no trim added because the controller already works well
+ parameter_struct->pid_controllers[YAW_RATE_ID].current_point = sensor_struct->psi_dot;
+ parameter_struct->pid_controllers[YAW_RATE_ID].setpoint = /*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
+ parameter_struct->pid_controllers[YAW_ID].pid_correction :*/ user_input_struct->yaw_manual_setpoint; // no trim added because the controller already works well
//logging and PID computation
- log_struct->ang_vel_pitch_PID_values = pid_computation(&(parameter_struct->pitch_ang_vel_pid));
- log_struct->ang_vel_roll_PID_values = pid_computation(&(parameter_struct->roll_ang_vel_pid));
- log_struct->ang_vel_yaw_PID_values = pid_computation(&(parameter_struct->yaw_ang_vel_pid));
+ log_struct->ang_vel_pitch_PID_values = pid_computation(&(parameter_struct->pid_controllers[PITCH_RATE_ID]));
+ log_struct->ang_vel_roll_PID_values = pid_computation(&(parameter_struct->pid_controllers[ROLL_RATE_ID]));
+ log_struct->ang_vel_yaw_PID_values = pid_computation(&(parameter_struct->pid_controllers[YAW_RATE_ID]));
//END PIDs///////////////////////////////////////////////////////////////////////
@@ -240,43 +240,43 @@
{
//THROTTLE
raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] =
- ((int)(parameter_struct->alt_pid.pid_correction)) + sensor_struct->trims.throttle;
+ ((int)(parameter_struct->pid_controllers[ALT_ID].pid_correction)) + sensor_struct->trims.throttle;
//ROLL
raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
- parameter_struct->roll_ang_vel_pid.pid_correction; // + sensor_struct->trims.roll;
+ parameter_struct->pid_controllers[ROLL_RATE_ID].pid_correction; // + sensor_struct->trims.roll;
//PITCH
raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
- parameter_struct->pitch_ang_vel_pid.pid_correction; // + sensor_struct->trims.pitch;
+ parameter_struct->pid_controllers[PITCH_RATE_ID].pid_correction; // + sensor_struct->trims.pitch;
//YAW
raw_actuator_struct->controller_corrected_motor_commands[YAW] =
- parameter_struct->yaw_ang_vel_pid.pid_correction;// + sensor_struct->trims.yaw;
+ parameter_struct->pid_controllers[YAW_RATE_ID].pid_correction;// + sensor_struct->trims.yaw;
// static int slow_down = 0;
// slow_down++;
// if(slow_down % 50 == 0)
// printf("X: %.3f\tY: %.3f\tZ: %.3f\tX_s: %.3f\tX_c: %.3f\tY_s: %.3f\tY_c: %.3f\tZ_s: %.3f\tZ_c: %.3f\t\n",
-// parameter_struct->local_x_pid.pid_correction,
-// parameter_struct->local_y_pid.pid_correction,
-// parameter_struct->alt_pid.pid_correction,
-// parameter_struct->local_x_pid.setpoint, parameter_struct->local_x_pid.current_point,
-// parameter_struct->local_y_pid.setpoint, parameter_struct->local_y_pid.current_point,
-// parameter_struct->alt_pid.setpoint, parameter_struct->alt_pid.current_point);
+// parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction,
+// parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction,
+// parameter_struct->pid_controllers[ALT_ID].pid_correction,
+// parameter_struct->pid_controllers[LOCAL_X_ID].setpoint, parameter_struct->pid_controllers[LOCAL_X_ID].current_point,
+// parameter_struct->pid_controllers[LOCAL_Y_ID].setpoint, parameter_struct->pid_controllers[LOCAL_Y_ID].current_point,
+// parameter_struct->pid_controllers[ALT_ID].setpoint, parameter_struct->pid_controllers[ALT_ID].current_point);
}
else{
//ROLL
raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
- parameter_struct->roll_ang_vel_pid.pid_correction;
+ parameter_struct->pid_controllers[ROLL_RATE_ID].pid_correction;
//PITCH
raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
- parameter_struct->pitch_ang_vel_pid.pid_correction;
+ parameter_struct->pid_controllers[PITCH_RATE_ID].pid_correction;
//YAW
raw_actuator_struct->controller_corrected_motor_commands[YAW] =
- parameter_struct->yaw_ang_vel_pid.pid_correction;
+ parameter_struct->pid_controllers[YAW_RATE_ID].pid_correction;
}
//BOUNDS CHECKING
@@ -314,17 +314,17 @@
// here we are not actually duplicating the logging from the PID computation
// the PID computation logs PID_values struct where this logs the PID struct
// they contain different sets of data
- log_struct->local_y_PID = parameter_struct->local_y_pid;
- log_struct->local_x_PID = parameter_struct->local_x_pid;
- log_struct->altitude_PID = parameter_struct->alt_pid;
+ log_struct->local_y_PID = parameter_struct->pid_controllers[LOCAL_Y_ID];
+ log_struct->local_x_PID = parameter_struct->pid_controllers[LOCAL_X_ID];
+ log_struct->altitude_PID = parameter_struct->pid_controllers[ALT_ID];
- log_struct->angle_roll_PID = parameter_struct->roll_angle_pid;
- log_struct->angle_pitch_PID = parameter_struct->pitch_angle_pid;
- log_struct->angle_yaw_PID = parameter_struct->yaw_angle_pid;
+ log_struct->angle_roll_PID = parameter_struct->pid_controllers[ROLL_ID];
+ log_struct->angle_pitch_PID = parameter_struct->pid_controllers[PITCH_ID];
+ log_struct->angle_yaw_PID = parameter_struct->pid_controllers[YAW_ID];
- log_struct->ang_vel_roll_PID = parameter_struct->roll_ang_vel_pid;
- log_struct->ang_vel_pitch_PID = parameter_struct->pitch_ang_vel_pid;
- log_struct->ang_vel_yaw_PID = parameter_struct->yaw_ang_vel_pid;
+ log_struct->ang_vel_roll_PID = parameter_struct->pid_controllers[ROLL_RATE_ID];
+ log_struct->ang_vel_pitch_PID = parameter_struct->pid_controllers[PITCH_RATE_ID];
+ log_struct->ang_vel_yaw_PID = parameter_struct->pid_controllers[YAW_RATE_ID];
last_fm_switch = cur_fm_switch;
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index 279d3d413..c6f89b496 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -9,6 +9,8 @@
#define TYPE_DEF_H_
#include <stdint.h>
+#include "commands.h"
+
/**
* @brief
* The modes for autonomous and manual flight.
@@ -106,14 +108,14 @@ typedef struct {
}gam_t;
typedef struct PID_t {
- double current_point; // Current value of the system
- double setpoint; // Desired value of the system
+ float current_point; // Current value of the system
+ float setpoint; // Desired value of the system
float Kp; // Proportional constant
float Ki; // Integral constant
float Kd; // Derivative constant
- double prev_error; // Previous error
- double acc_error; // Accumulated error
- double pid_correction; // Correction factor computed by the PID
+ float prev_error; // Previous error
+ float acc_error; // Accumulated error
+ float pid_correction; // Correction factor computed by the PID
float dt; // sample period
} PID_t;
@@ -285,13 +287,7 @@ typedef struct setpoint_t {
*
*/
typedef struct parameter_t {
- PID_t roll_angle_pid, roll_ang_vel_pid;
- PID_t pitch_angle_pid, pitch_ang_vel_pid;
- PID_t yaw_ang_vel_pid;
- PID_t local_x_pid;
- PID_t local_y_pid;
- PID_t yaw_angle_pid;
- PID_t alt_pid;
+ PID_t pid_controllers[MAX_CONTROLLER_ID];
} parameter_t;
/**
@@ -328,7 +324,7 @@ typedef struct actuator_command_t {
* @brief
* Structures to be used throughout
*/
-typedef struct {
+typedef struct modular_structs {
user_input_t user_input_struct;
log_t log_struct;
raw_sensor_t raw_sensor_struct;
@@ -338,7 +334,7 @@ typedef struct {
user_defined_t user_defined_struct;
raw_actuator_t raw_actuator_struct;
actuator_command_t actuator_command_struct;
-}modular_structs_t;
+} modular_structs_t;
//////// END MAIN MODULAR STRUCTS
--
GitLab