Skip to content
Snippets Groups Projects
Unverified Commit 54862713 authored by Jake Drahos's avatar Jake Drahos
Browse files

Merge remote-tracking branch 'origin/commands-dev' into commands-dev-backend

parents 5683f8ac 5b1b7aa7
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 193 additions and 308 deletions
groundStation/src/backend/commands.c
+ 3
− 0
View file @ 54862713
...@@ -68,6 +68,9 @@ command_cb cb_respcontrol __attribute__((weak, alias("cb_default"))); ...@@ -68,6 +68,9 @@ command_cb cb_respcontrol __attribute__((weak, alias("cb_default")));
* *
* There is one callback function pointer associated with each * There is one callback function pointer associated with each
* element in this struct array. * 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] = struct MessageType MessageTypes[MAX_TYPE_ID] =
{ {
......
groundStation/src/backend/commands.h
+ 5
− 0
View file @ 54862713
...@@ -28,7 +28,10 @@ enum DataType ...@@ -28,7 +28,10 @@ enum DataType
/* /*
* Message type IDs used to know what kind of message we are dealing with * Message type IDs used to know what kind of message we are dealing with
* Enumeration used to index the MessageTypes array in commands.c * 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 or you will break backwards compatibility.
* DO NOT change this enum without also updating the "MessageTypes" array
* in commands.c to match.
*/ */
enum MessageTypeID{ enum MessageTypeID{
DEBUG_ID, // 00 DEBUG_ID, // 00
...@@ -57,6 +60,7 @@ enum ControllerID{ ...@@ -57,6 +60,7 @@ enum ControllerID{
LOCAL_X_ID, // 06 - Local X PID LOCAL_X_ID, // 06 - Local X PID
LOCAL_Y_ID, // 07 - Local Y PID LOCAL_Y_ID, // 07 - Local Y PID
ALT_ID, // 08 - Altitude PID ALT_ID, // 08 - Altitude PID
MAX_CONTROLLER_ID, // 09 - Just used to keep track of the size
}; };
/* /*
...@@ -67,6 +71,7 @@ enum ControllerValueID{ ...@@ -67,6 +71,7 @@ enum ControllerValueID{
KI_ID, // 01 - I constant KI_ID, // 01 - I constant
KD_ID, // 02 - D constant KD_ID, // 02 - D constant
SP_ID, // 03 - Setpoint value SP_ID, // 03 - Setpoint value
MAX_CONTROL_VAL_ID, // 04 - Just used to keep track of the size
}; };
/* /*
......
quad/sw/modular_quad_pid/src/callbacks.c
+ 88
− 209
View file @ 54862713
...@@ -95,189 +95,52 @@ int cb_beginupdate(modular_structs_t *structs) { ...@@ -95,189 +95,52 @@ int cb_beginupdate(modular_structs_t *structs) {
/** /**
* Handles a command to set a controller value on the quad. * Handles a command to set a controller value on the quad.
* *
* NOTE: expects the uart buff to have some data in the following format: * NOTE:
* |------------------------------------------------------| * Expects the uart buff to have data in the following format:
* | data index|| 0 | 1 | 3 - 6 | * |--------------------------------------------------------|
* |------------------------------------------------------| * | data index || 0 | 1 | 2 - 5 |
* | param|| control ID | ctrl val ID | float val | * |--------------------------------------------------------|
* |------------------------------------------------------| * | param || control ID | ctrl val ID | float val |
* | bytes|| 1 | 1 | 4 | * |--------------------------------------------------------|
* |------------------------------------------------------| * | bytes || 1 | 1 | 4 |
* |--------------------------------------------------------|
*
* Does not send anything in response.
*/ */
int cb_setcontrol(modular_structs_t *structs) int cb_setcontrol(modular_structs_t *structs)
{ {
// Get the controller ID, value ID, float value // Get some of the meta data
u8 controller_id = uart_buff_data_get_u8(0); u16 data_len = uart_buff_get_u16(6);
u8 controller_value_id = uart_buff_data_get_u8(1); // Check if the data length is correct
float controller_value = uart_buff_data_get_float(3); if (data_len == 6)
// TODO check if the setpoints really need to be doubles
// TODO add rate setpoints
// Switch case on the controller ID
switch(controller_id)
{ {
case ROLL_ID: // Get the controller ID, value ID, float value
// switch on the value ID u8 controller_id = uart_buff_data_get_u8(0);
switch(controller_value_id) u8 controller_value_id = uart_buff_data_get_u8(1);
{ float controller_value = uart_buff_data_get_float(3);
case KP_ID:
structs->parameter_struct.roll_angle_pid.Kp = controller_value; // Check to make sure the IDs are in bounds
break; if (controller_id < MAX_CONTROLLER_ID &&
case KI_ID: controller_value_id < MAX_CONTROL_VAL_ID)
structs->parameter_struct.roll_angle_pid.Ki = controller_value; {
break; // Set the controller_value into the controller by controller_id, controller_value_id
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) switch(controller_value_id)
{ {
case KP_ID: case KP_ID:
structs->parameter_struct.pitch_angle_pid.Kp = controller_value; structs->parameter_struct.pid_controllers[controller_id].Kp = controller_value;
break; break;
case KI_ID: case KI_ID:
structs->parameter_struct.pitch_angle_pid.Ki = controller_value; structs->parameter_struct.pid_controllers[controller_id].Ki = controller_value;
break; break;
case KD_ID: case KD_ID:
structs->parameter_struct.pitch_angle_pid.Kd = controller_value; structs->parameter_struct.pid_controllers[controller_id].Kd = controller_value;
break; break;
case SP_ID: case SP_ID:
structs->setpoint_struct.desiredQuadPosition.pitch = controller_value; structs->parameter_struct.pid_controllers[controller_id].setpoint = controller_value;
break; break;
} }
break; }
case YAW_ID:
// switch on the value ID
switch(controller_value_id)
{
case KP_ID:
structs->parameter_struct.yaw_angle_pid.Kp = controller_value;
break;
case KI_ID:
structs->parameter_struct.yaw_angle_pid.Ki = controller_value;
break;
case KD_ID:
structs->parameter_struct.yaw_angle_pid.Kd = controller_value;
break;
case SP_ID:
structs->setpoint_struct.desiredQuadPosition.yaw = 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; return 0;
...@@ -286,62 +149,78 @@ int cb_setcontrol(modular_structs_t *structs) ...@@ -286,62 +149,78 @@ int cb_setcontrol(modular_structs_t *structs)
/** /**
* Handles a command to get a controller value from the quad. * Handles a command to get a controller value from the quad.
* *
* NOTE: expects the uart buff to have some data in the following format: * NOTE:
* |----------------------------------------| * Expects the uart buff to have data in the following format:
* | data index|| 0 | 1 | * |------------------------------------------|
* |----------------------------------------| * | data index || 0 | 1 |
* | param|| control ID | ctrl val ID | * |------------------------------------------|
* |----------------------------------------| * | param || control ID | ctrl val ID |
* | bytes|| 1 | 1 | * |------------------------------------------|
* |----------------------------------------| * | 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) int cb_getcontrol(modular_structs_t* structs)
{ {
// Get the controller ID, value ID // Get some of the meta data
u8 controller_id = uart_buff_data_get_u8(0); u16 data_len = uart_buff_get_u16(6);
u8 controller_value_id = uart_buff_data_get_u8(1); u16 msg_id = uart_buff_get_u16(3);
// Check if the data length is correct
// TODO check if the setpoints really need to be doubles if (data_len == 2)
// TODO add rate setpoints
// TODO figure out what the message ID should be
// Switch case on the controller ID
switch(controller_id)
{ {
case ROLL_ID: // Get the controller ID, value ID
// switch on the 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 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;
// Set the controller_value equal to the controller value stored in the controller by
// controllerid, controller_value_id
switch(controller_value_id) switch(controller_value_id)
{ {
case KP_ID: case KP_ID:
float controller_value = structs->parameter_struct.roll_angle_pid.Kp; controller_value = structs->parameter_struct.pid_controllers[controller_id].Kp;
send_data(RESPCONTROL_ID, 0, ((char *) &controller_value), sizeof(controller_value));
break; break;
case KI_ID: case KI_ID:
structs->parameter_struct.roll_angle_pid.Ki = controller_value; controller_value = structs->parameter_struct.pid_controllers[controller_id].Ki;
break; break;
case KD_ID: case KD_ID:
structs->parameter_struct.roll_angle_pid.Kd = controller_value; controller_value = structs->parameter_struct.pid_controllers[controller_id].Kd;
break; break;
case SP_ID: case SP_ID:
structs->setpoint_struct.desiredQuadPosition.roll = controller_value; controller_value = structs->parameter_struct.pid_controllers[controller_id].setpoint;
break; break;
} }
break;
case PITCH_ID: // Format the response data
// TODO char resp_data[6];
case YAW_ID:
// TODO // Controller ID
case ROLL_RATE_ID: resp_data[0] = controller_id;
// TODO // Controller value ID
case PITCH_RATE_ID: resp_data[1] = controller_value_id;
// TODO // Controller value (4 byte float)
case YAW_RATE_ID: // TODO set a strict byte ordering for communication between the ground station and the quad
// TODO memcpy(&resp_data[2], &controller_value, sizeof(controller_value));
case LOCAL_X_ID:
// TODO // Send the data
case LOCAL_Y_ID: send_data(RESPCONTROL_ID, msg_id, resp_data, sizeof(resp_data));
// TODO }
case ALT_ID:
// TODO
} }
return 0; return 0;
......
quad/sw/modular_quad_pid/src/callbacks.h
+ 4
− 3
View file @ 54862713
#ifndef __callbacks_ #ifndef __callbacks_h
#define __callbacks_h #define __callbacks_h
/* Grab some stupid stuff from legacy code */ /* Grab some stupid stuff from legacy code */
#include "type_def.h"
struct modular_structs;
/* Make commands.c happy */ /* Make commands.c happy */
typedef int (command_cb)(modular_structs_t *structs); typedef int (command_cb)(struct modular_structs *structs);
#endif #endif
quad/sw/modular_quad_pid/src/cb_default.h
+ 1
− 0
View file @ 54862713
#include "commands.h" #include "commands.h"
#include "type_def.h"
/* The cb_default used on the groundStation. This file MUST NOT BE INCLUDED /* The cb_default used on the groundStation. This file MUST NOT BE INCLUDED
* by anything except for commands.c */ * by anything except for commands.c */
......
quad/sw/modular_quad_pid/src/control_algorithm.c
+ 82
− 82
View file @ 54862713
...@@ -16,42 +16,42 @@ ...@@ -16,42 +16,42 @@
{ {
// HUMAN Piloted (RC) PID DEFINITIONS ////// // HUMAN Piloted (RC) PID DEFINITIONS //////
// RC PIDs for roll (2 loops: angle --> angular velocity) // 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) // 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 // initialize Yaw PID_t and PID constants
// RC PID for yaw (1 loop angular velocity) // 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 ////// // 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) // 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) // 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) // 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) // 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) // PID coeffiecients (Position)
setPIDCoeff(&(parameter_struct->local_y_pid), YPOS_KP, YPOS_KI, YPOS_KD); setPIDCoeff(&(parameter_struct->pid_controllers[LOCAL_Y_ID]), YPOS_KP, YPOS_KI, YPOS_KD);
setPIDCoeff(&(parameter_struct->local_x_pid), XPOS_KP, XPOS_KI, XPOS_KD); setPIDCoeff(&(parameter_struct->pid_controllers[LOCAL_X_ID]), 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[ALT_ID]), ALT_ZPOS_KP, ALT_ZPOS_KI, ALT_ZPOS_KD);
// PID coefficients (Angle) // PID coefficients (Angle)
setPIDCoeff(&(parameter_struct->pitch_angle_pid), PITCH_ANGLE_KP, PITCH_ANGLE_KI, PITCH_ANGLE_KD); setPIDCoeff(&(parameter_struct->pid_controllers[PITCH_ID]), 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->pid_controllers[ROLL_ID]), 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[YAW_ID]), YAW_ANGLE_KP, YAW_ANGLE_KI, YAW_ANGLE_KD);
// PID coefficients (Angular Velocity) // 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->pid_controllers[PITCH_RATE_ID]), 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->pid_controllers[ROLL_RATE_ID]), 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[YAW_RATE_ID]), YAW_ANGULAR_VELOCITY_KP, YAW_ANGULAR_VELOCITY_KI, YAW_ANGULAR_VELOCITY_KD);
return 0; return 0;
} }
...@@ -102,14 +102,14 @@ ...@@ -102,14 +102,14 @@
if((cur_fm_switch == AUTO_FLIGHT_MODE) && (user_defined_struct->engaging_auto == 2)) 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 // 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->pid_controllers[ALT_ID].acc_error = 0.0;
parameter_struct->local_x_pid.acc_error = 0.0; parameter_struct->pid_controllers[LOCAL_X_ID].acc_error = 0.0;
parameter_struct->local_y_pid.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 // make previous error equal to the current so the D term doesn't spike
parameter_struct->alt_pid.prev_error = 0.0; parameter_struct->pid_controllers[ALT_ID].prev_error = 0.0;
parameter_struct->local_x_pid.prev_error = 0.0; parameter_struct->pid_controllers[LOCAL_X_ID].prev_error = 0.0;
parameter_struct->local_y_pid.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.alt_pos = sensor_struct->currentQuadPosition.alt_pos;
setpoint_struct->desiredQuadPosition.x_pos = sensor_struct->currentQuadPosition.x_pos; setpoint_struct->desiredQuadPosition.x_pos = sensor_struct->currentQuadPosition.x_pos;
...@@ -135,26 +135,26 @@ ...@@ -135,26 +135,26 @@
if(user_input_struct->locationFresh) if(user_input_struct->locationFresh)
{ {
parameter_struct->local_y_pid.current_point = sensor_struct->currentQuadPosition.y_pos; parameter_struct->pid_controllers[LOCAL_Y_ID].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].setpoint = setpoint_struct->desiredQuadPosition.y_pos;
parameter_struct->local_x_pid.current_point = sensor_struct->currentQuadPosition.x_pos; parameter_struct->pid_controllers[LOCAL_X_ID].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].setpoint = setpoint_struct->desiredQuadPosition.x_pos;
parameter_struct->alt_pid.current_point = sensor_struct->currentQuadPosition.alt_pos; parameter_struct->pid_controllers[ALT_ID].current_point = sensor_struct->currentQuadPosition.alt_pos;
parameter_struct->alt_pid.setpoint = setpoint_struct->desiredQuadPosition.alt_pos; parameter_struct->pid_controllers[ALT_ID].setpoint = setpoint_struct->desiredQuadPosition.alt_pos;
//logging and PID computation //logging and PID computation
log_struct->local_y_PID_values = pid_computation(&(parameter_struct->local_y_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->local_x_pid)); log_struct->local_x_PID_values = pid_computation(&(parameter_struct->pid_controllers[LOCAL_X_ID]));
log_struct->altitude_PID_values = pid_computation(&(parameter_struct->alt_pid)); log_struct->altitude_PID_values = pid_computation(&(parameter_struct->pid_controllers[ALT_ID]));
// yaw angular position PID calculation // yaw angular position PID calculation
parameter_struct->yaw_angle_pid.current_point = sensor_struct->currentQuadPosition.yaw;// in radians parameter_struct->pid_controllers[YAW_ID].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].setpoint = setpoint_struct->desiredQuadPosition.yaw; // constant setpoint
//logging and PID computation //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 @@ ...@@ -165,37 +165,37 @@
*/ */
//angle boundaries //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->pid_controllers[PITCH_ID].current_point = sensor_struct->pitch_angle_filtered;
parameter_struct->pitch_angle_pid.setpoint = parameter_struct->pid_controllers[PITCH_ID].setpoint =
/*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)? /*(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->pid_controllers[ROLL_ID].current_point = sensor_struct->roll_angle_filtered;
parameter_struct->roll_angle_pid.setpoint = parameter_struct->pid_controllers[ROLL_ID].setpoint =
/*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)? /*(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 //logging and PID computation
log_struct->angle_pitch_PID_values = pid_computation(&(parameter_struct->pitch_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->roll_angle_pid)); log_struct->angle_roll_PID_values = pid_computation(&(parameter_struct->pid_controllers[ROLL_ID]));
/* Angular Velocity Loop /* Angular Velocity Loop
...@@ -205,25 +205,25 @@ ...@@ -205,25 +205,25 @@
// theta_dot is the angular velocity about the y-axis // theta_dot is the angular velocity about the y-axis
// it is calculated from using the gimbal equations // it is calculated from using the gimbal equations
parameter_struct->pitch_ang_vel_pid.current_point = sensor_struct->theta_dot; parameter_struct->pid_controllers[PITCH_RATE_ID].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].setpoint = parameter_struct->pid_controllers[PITCH_ID].pid_correction;
// phi_dot is the angular velocity about the x-axis // phi_dot is the angular velocity about the x-axis
// it is calculated from using the gimbal equations // it is calculated from using the gimbal equations
parameter_struct->roll_ang_vel_pid.current_point = sensor_struct->phi_dot; parameter_struct->pid_controllers[ROLL_RATE_ID].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].setpoint = parameter_struct->pid_controllers[ROLL_ID].pid_correction;
// Yaw angular velocity PID // Yaw angular velocity PID
// psi_dot is the angular velocity about the z-axis // psi_dot is the angular velocity about the z-axis
// it is calculated from using the gimbal equations // it is calculated from using the gimbal equations
parameter_struct->yaw_ang_vel_pid.current_point = sensor_struct->psi_dot; parameter_struct->pid_controllers[YAW_RATE_ID].current_point = sensor_struct->psi_dot;
parameter_struct->yaw_ang_vel_pid.setpoint = /*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)? parameter_struct->pid_controllers[YAW_RATE_ID].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_ID].pid_correction :*/ user_input_struct->yaw_manual_setpoint; // no trim added because the controller already works well
//logging and PID computation //logging and PID computation
log_struct->ang_vel_pitch_PID_values = pid_computation(&(parameter_struct->pitch_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->roll_ang_vel_pid)); 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->yaw_ang_vel_pid)); log_struct->ang_vel_yaw_PID_values = pid_computation(&(parameter_struct->pid_controllers[YAW_RATE_ID]));
//END PIDs/////////////////////////////////////////////////////////////////////// //END PIDs///////////////////////////////////////////////////////////////////////
...@@ -240,43 +240,43 @@ ...@@ -240,43 +240,43 @@
{ {
//THROTTLE //THROTTLE
raw_actuator_struct->controller_corrected_motor_commands[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 //ROLL
raw_actuator_struct->controller_corrected_motor_commands[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 //PITCH
raw_actuator_struct->controller_corrected_motor_commands[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 //YAW
raw_actuator_struct->controller_corrected_motor_commands[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; // static int slow_down = 0;
// slow_down++; // slow_down++;
// if(slow_down % 50 == 0) // 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", // 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->pid_controllers[LOCAL_X_ID].pid_correction,
// parameter_struct->local_y_pid.pid_correction, // parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction,
// parameter_struct->alt_pid.pid_correction, // parameter_struct->pid_controllers[ALT_ID].pid_correction,
// parameter_struct->local_x_pid.setpoint, parameter_struct->local_x_pid.current_point, // parameter_struct->pid_controllers[LOCAL_X_ID].setpoint, parameter_struct->pid_controllers[LOCAL_X_ID].current_point,
// parameter_struct->local_y_pid.setpoint, parameter_struct->local_y_pid.current_point, // parameter_struct->pid_controllers[LOCAL_Y_ID].setpoint, parameter_struct->pid_controllers[LOCAL_Y_ID].current_point,
// parameter_struct->alt_pid.setpoint, parameter_struct->alt_pid.current_point); // parameter_struct->pid_controllers[ALT_ID].setpoint, parameter_struct->pid_controllers[ALT_ID].current_point);
} }
else{ else{
//ROLL //ROLL
raw_actuator_struct->controller_corrected_motor_commands[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 //PITCH
raw_actuator_struct->controller_corrected_motor_commands[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 //YAW
raw_actuator_struct->controller_corrected_motor_commands[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 //BOUNDS CHECKING
...@@ -314,17 +314,17 @@ ...@@ -314,17 +314,17 @@
// here we are not actually duplicating the logging from the PID computation // 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 // the PID computation logs PID_values struct where this logs the PID struct
// they contain different sets of data // they contain different sets of data
log_struct->local_y_PID = parameter_struct->local_y_pid; log_struct->local_y_PID = parameter_struct->pid_controllers[LOCAL_Y_ID];
log_struct->local_x_PID = parameter_struct->local_x_pid; log_struct->local_x_PID = parameter_struct->pid_controllers[LOCAL_X_ID];
log_struct->altitude_PID = parameter_struct->alt_pid; log_struct->altitude_PID = parameter_struct->pid_controllers[ALT_ID];
log_struct->angle_roll_PID = parameter_struct->roll_angle_pid; log_struct->angle_roll_PID = parameter_struct->pid_controllers[ROLL_ID];
log_struct->angle_pitch_PID = parameter_struct->pitch_angle_pid; log_struct->angle_pitch_PID = parameter_struct->pid_controllers[PITCH_ID];
log_struct->angle_yaw_PID = parameter_struct->yaw_angle_pid; 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_roll_PID = parameter_struct->pid_controllers[ROLL_RATE_ID];
log_struct->ang_vel_pitch_PID = parameter_struct->pitch_ang_vel_pid; log_struct->ang_vel_pitch_PID = parameter_struct->pid_controllers[PITCH_RATE_ID];
log_struct->ang_vel_yaw_PID = parameter_struct->yaw_ang_vel_pid; log_struct->ang_vel_yaw_PID = parameter_struct->pid_controllers[YAW_RATE_ID];
last_fm_switch = cur_fm_switch; last_fm_switch = cur_fm_switch;
......
quad/sw/modular_quad_pid/src/type_def.h
+ 10
− 14
View file @ 54862713
...@@ -9,6 +9,8 @@ ...@@ -9,6 +9,8 @@
#define TYPE_DEF_H_ #define TYPE_DEF_H_
#include <stdint.h> #include <stdint.h>
#include "commands.h"
/** /**
* @brief * @brief
* The modes for autonomous and manual flight. * The modes for autonomous and manual flight.
...@@ -106,14 +108,14 @@ typedef struct { ...@@ -106,14 +108,14 @@ typedef struct {
}gam_t; }gam_t;
typedef struct PID_t { typedef struct PID_t {
double current_point; // Current value of the system float current_point; // Current value of the system
double setpoint; // Desired value of the system float setpoint; // Desired value of the system
float Kp; // Proportional constant float Kp; // Proportional constant
float Ki; // Integral constant float Ki; // Integral constant
float Kd; // Derivative constant float Kd; // Derivative constant
double prev_error; // Previous error float prev_error; // Previous error
double acc_error; // Accumulated error float acc_error; // Accumulated error
double pid_correction; // Correction factor computed by the PID float pid_correction; // Correction factor computed by the PID
float dt; // sample period float dt; // sample period
} PID_t; } PID_t;
...@@ -285,13 +287,7 @@ typedef struct setpoint_t { ...@@ -285,13 +287,7 @@ typedef struct setpoint_t {
* *
*/ */
typedef struct parameter_t { typedef struct parameter_t {
PID_t roll_angle_pid, roll_ang_vel_pid; PID_t pid_controllers[MAX_CONTROLLER_ID];
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;
} parameter_t; } parameter_t;
/** /**
...@@ -328,7 +324,7 @@ typedef struct actuator_command_t { ...@@ -328,7 +324,7 @@ typedef struct actuator_command_t {
* @brief * @brief
* Structures to be used throughout * Structures to be used throughout
*/ */
typedef struct { typedef struct modular_structs {
user_input_t user_input_struct; user_input_t user_input_struct;
log_t log_struct; log_t log_struct;
raw_sensor_t raw_sensor_struct; raw_sensor_t raw_sensor_struct;
...@@ -338,7 +334,7 @@ typedef struct { ...@@ -338,7 +334,7 @@ typedef struct {
user_defined_t user_defined_struct; user_defined_t user_defined_struct;
raw_actuator_t raw_actuator_struct; raw_actuator_t raw_actuator_struct;
actuator_command_t actuator_command_struct; actuator_command_t actuator_command_struct;
}modular_structs_t; } modular_structs_t;
//////// END MAIN MODULAR STRUCTS //////// END MAIN MODULAR STRUCTS
......
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