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

Added support for autonomous flight. Untested

parent a5594362
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 133 additions and 151 deletions
quad/sw/modular_quad_pid/gen_diagram/gen_diagram
+ 0
− 0
View file @ 5bf7ac34
No preview for this file type
quad/sw/modular_quad_pid/gen_diagram/network.dot
+ 36
− 4
View file @ 5bf7ac34
...@@ -12,6 +12,9 @@ label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> ...@@ -12,6 +12,9 @@ label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4>
"dT" -> "Pitch PID":f3 [label="Constant"] "dT" -> "Pitch PID":f3 [label="Constant"]
"Yaw PID"[shape=record "Yaw PID"[shape=record
label="<f0>Yaw PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=2.600] |<f5> [Ki=0.000] |<f6> [Kd=0.000]"] label="<f0>Yaw PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=2.600] |<f5> [Ki=0.000] |<f6> [Kd=0.000]"]
"Yaw" -> "Yaw PID":f1 [label="Constant"]
"Yaw Setpoint" -> "Yaw PID":f2 [label="Constant"]
"dT" -> "Yaw PID":f3 [label="Constant"]
"Roll Rate PID"[shape=record "Roll Rate PID"[shape=record
label="<f0>Roll Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=4600.000] |<f5> [Ki=0.000] |<f6> [Kd=550.000]"] label="<f0>Roll Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=4600.000] |<f5> [Ki=0.000] |<f6> [Kd=550.000]"]
"dPhi" -> "Roll Rate PID":f1 [label="Constant"] "dPhi" -> "Roll Rate PID":f1 [label="Constant"]
...@@ -28,11 +31,34 @@ label="<f0>Yaw Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f ...@@ -28,11 +31,34 @@ label="<f0>Yaw Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f
"RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"] "RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
"dT" -> "Yaw Rate PID":f3 [label="Constant"] "dT" -> "Yaw Rate PID":f3 [label="Constant"]
"X pos PID"[shape=record "X pos PID"[shape=record
label="<f0>X pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000]"] label="<f0>X pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.015] |<f5> [Ki=-0.005] |<f6> [Kd=-0.030]"]
"VRPN X" -> "X pos PID":f1 [label="Constant"]
"X Setpoint" -> "X pos PID":f2 [label="Constant"]
"dT" -> "X pos PID":f3 [label="Constant"]
"Y pos PID"[shape=record "Y pos PID"[shape=record
label="<f0>Y pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000]"] label="<f0>Y pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.015] |<f5> [Ki=0.005] |<f6> [Kd=0.030]"]
"Altitude"[shape=record "VRPN Y" -> "Y pos PID":f1 [label="Constant"]
label="<f0>Altitude |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000]"] "Y Setpoint" -> "Y pos PID":f2 [label="Constant"]
"dT" -> "Y pos PID":f3 [label="Constant"]
"Altitude PID"[shape=record
label="<f0>Altitude PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=9804.000] |<f5> [Ki=817.000] |<f6> [Kd=7353.000]"]
"VRPN Alt" -> "Altitude PID":f1 [label="Constant"]
"Alt Setpoint" -> "Altitude PID":f2 [label="Constant"]
"dT" -> "Altitude PID":f3 [label="Constant"]
"X Setpoint"[shape=record
label="<f0>X Setpoint |<f1> [Constant=0.000]"]
"Y Setpoint"[shape=record
label="<f0>Y Setpoint |<f1> [Constant=0.000]"]
"Alt Setpoint"[shape=record
label="<f0>Alt Setpoint |<f1> [Constant=0.000]"]
"Yaw Setpoint"[shape=record
label="<f0>Yaw Setpoint |<f1> [Constant=0.000]"]
"Throttle trim"[shape=record
label="<f0>Throttle trim |<f1> [Constant=0.000]"]
"T trim add"[shape=record
label="<f0>T trim add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
"Altitude PID" -> "T trim add":f1 [label="Correction"]
"Throttle trim" -> "T trim add":f2 [label="Constant"]
"P PWM Clamp"[shape=record "P PWM Clamp"[shape=record
label="<f0>P PWM Clamp |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"] label="<f0>P PWM Clamp |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"]
"Pitch Rate PID" -> "P PWM Clamp":f1 [label="Correction"] "Pitch Rate PID" -> "P PWM Clamp":f1 [label="Correction"]
...@@ -60,6 +86,12 @@ label="<f0>dTheta |<f1> [Constant=0.000]"] ...@@ -60,6 +86,12 @@ label="<f0>dTheta |<f1> [Constant=0.000]"]
label="<f0>dPhi |<f1> [Constant=0.000]"] label="<f0>dPhi |<f1> [Constant=0.000]"]
"dPsi"[shape=record "dPsi"[shape=record
label="<f0>dPsi |<f1> [Constant=0.000]"] label="<f0>dPsi |<f1> [Constant=0.000]"]
"VRPN X"[shape=record
label="<f0>VRPN X |<f1> [Constant=0.000]"]
"VRPN Y"[shape=record
label="<f0>VRPN Y |<f1> [Constant=0.000]"]
"VRPN Alt"[shape=record
label="<f0>VRPN Alt |<f1> [Constant=0.000]"]
"RC Pitch"[shape=record "RC Pitch"[shape=record
label="<f0>RC Pitch |<f1> [Constant=0.000]"] label="<f0>RC Pitch |<f1> [Constant=0.000]"]
"RC Roll"[shape=record "RC Roll"[shape=record
......
quad/sw/modular_quad_pid/gen_diagram/network.png
+ 0
− 0
View replaced file @ a5594362 View file @ 5bf7ac34
quad/sw/modular_quad_pid/gen_diagram/network.png

236 KiB | W: | H:

quad/sw/modular_quad_pid/gen_diagram/network.png

321 KiB | W: | H:

quad/sw/modular_quad_pid/gen_diagram/network.png
quad/sw/modular_quad_pid/gen_diagram/network.png
quad/sw/modular_quad_pid/gen_diagram/network.png
quad/sw/modular_quad_pid/gen_diagram/network.png
  • 2-up
  • Swipe
  • Onion skin
quad/sw/modular_quad_pid/src/control_algorithm.c
+ 83
− 147
View file @ 5bf7ac34
...@@ -12,13 +12,30 @@ ...@@ -12,13 +12,30 @@
#include "graph_blocks/node_bounds.h" #include "graph_blocks/node_bounds.h"
#include "graph_blocks/node_constant.h" #include "graph_blocks/node_constant.h"
#include "graph_blocks/node_mixer.h" #include "graph_blocks/node_mixer.h"
#include "graph_blocks/node_add.h"
#include "PID.h" #include "PID.h"
#include "util.h" #include "util.h"
#define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees #define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees
int control_algorithm_init(parameter_t * ps) void connect_autonomous(parameter_t* ps) {
{ struct computation_graph* graph = ps->graph;
graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->clamp_pitch, PID_CORRECTION);
graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->clamp_roll, PID_CORRECTION);
graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
}
void connect_manual(parameter_t* ps) {
struct computation_graph* graph = ps->graph;
graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->rc_pitch, CONST_VAL);
graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL);
graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, CONST_VAL);
}
int control_algorithm_init(parameter_t * ps)
{
struct computation_graph* graph = create_graph(); struct computation_graph* graph = create_graph();
ps->graph = graph; ps->graph = graph;
...@@ -31,7 +48,13 @@ ...@@ -31,7 +48,13 @@
ps->yaw_r_pid = graph_add_node_pid(graph, "Yaw Rate PID"); ps->yaw_r_pid = graph_add_node_pid(graph, "Yaw Rate PID");
ps->x_pos_pid = graph_add_node_pid(graph, "X pos PID"); ps->x_pos_pid = graph_add_node_pid(graph, "X pos PID");
ps->y_pos_pid = graph_add_node_pid(graph, "Y pos PID"); ps->y_pos_pid = graph_add_node_pid(graph, "Y pos PID");
ps->alt_pid = graph_add_node_pid(graph, "Altitude"); ps->alt_pid = graph_add_node_pid(graph, "Altitude PID");
ps->x_set = graph_add_node_const(graph, "X Setpoint");
ps->y_set = graph_add_node_const(graph, "Y Setpoint");
ps->alt_set = graph_add_node_const(graph, "Alt Setpoint");
ps->yaw_set = graph_add_node_const(graph, "Yaw Setpoint");
ps->throttle_trim = graph_add_node_const(graph, "Throttle trim");
ps->throttle_trim_add = graph_add_node_add(graph, "T trim add");
// Create blocks for bounds checking // Create blocks for bounds checking
ps->clamp_d_pwmP = graph_add_node_bounds(graph, "P PWM Clamp"); ps->clamp_d_pwmP = graph_add_node_bounds(graph, "P PWM Clamp");
...@@ -53,6 +76,11 @@ ...@@ -53,6 +76,11 @@
ps->phi_dot = graph_add_node_const(graph, "dPhi"); ps->phi_dot = graph_add_node_const(graph, "dPhi");
ps->psi_dot = graph_add_node_const(graph, "dPsi"); ps->psi_dot = graph_add_node_const(graph, "dPsi");
// Create blocks for VRPN data
ps->vrpn_x = graph_add_node_const(graph, "VRPN X");
ps->vrpn_y = graph_add_node_const(graph, "VRPN Y");
ps->vrpn_alt = graph_add_node_const(graph, "VRPN Alt");
// Create blocks for RC controller // Create blocks for RC controller
ps->rc_pitch = graph_add_node_const(graph, "RC Pitch"); ps->rc_pitch = graph_add_node_const(graph, "RC Pitch");
ps->rc_roll = graph_add_node_const(graph, "RC Roll"); ps->rc_roll = graph_add_node_const(graph, "RC Roll");
...@@ -67,7 +95,7 @@ ...@@ -67,7 +95,7 @@
graph_set_source(graph, ps->pitch_r_pid, PID_SETPOINT, ps->pitch_pid, PID_CORRECTION); graph_set_source(graph, ps->pitch_r_pid, PID_SETPOINT, ps->pitch_pid, PID_CORRECTION);
graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->theta_dot, CONST_VAL); graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->theta_dot, CONST_VAL);
graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->dt, CONST_VAL); graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->dt, CONST_VAL);
graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->rc_pitch, CONST_VAL); //graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->rc_pitch, CONST_VAL);
graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->cur_pitch, CONST_VAL); graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->cur_pitch, CONST_VAL);
graph_set_source(graph, ps->pitch_pid, PID_DT, ps->dt, CONST_VAL); graph_set_source(graph, ps->pitch_pid, PID_DT, ps->dt, CONST_VAL);
...@@ -75,7 +103,7 @@ ...@@ -75,7 +103,7 @@
graph_set_source(graph, ps->roll_r_pid, PID_SETPOINT, ps->roll_pid, PID_CORRECTION); graph_set_source(graph, ps->roll_r_pid, PID_SETPOINT, ps->roll_pid, PID_CORRECTION);
graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->phi_dot, CONST_VAL); graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->phi_dot, CONST_VAL);
graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->dt, CONST_VAL); graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->dt, CONST_VAL);
graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL); //graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL);
graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL); graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL);
graph_set_source(graph, ps->roll_pid, PID_DT, ps->dt, CONST_VAL); graph_set_source(graph, ps->roll_pid, PID_DT, ps->dt, CONST_VAL);
...@@ -83,15 +111,30 @@ ...@@ -83,15 +111,30 @@
graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, PID_CORRECTION); graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, PID_CORRECTION);
graph_set_source(graph, ps->yaw_r_pid, PID_CUR_POINT, ps->psi_dot, CONST_VAL); graph_set_source(graph, ps->yaw_r_pid, PID_CUR_POINT, ps->psi_dot, CONST_VAL);
graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->dt, CONST_VAL); graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->dt, CONST_VAL);
/*
graph_set_source(graph, ps->yaw_pid, PID_SETPOINT, ps->rc_yaw, CONST_VAL);
graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->yaw, CONST_VAL);
graph_set_source(graph, ps->yaw_pid, PID_DT, ps->dt, CONST_VAL);
*/
// Connect angle clamping blocks
graph_set_source(graph, ps->clamp_pitch, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION); // X autonomous
graph_set_source(graph, ps->x_pos_pid, PID_DT, ps->dt, CONST_VAL);
graph_set_source(graph, ps->clamp_roll, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION); graph_set_source(graph, ps->clamp_roll, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->vrpn_x, CONST_VAL);
graph_set_source(graph, ps->x_pos_pid, PID_SETPOINT, ps->x_set, CONST_VAL);
// Y autonomous
graph_set_source(graph, ps->y_pos_pid, PID_DT, ps->dt, CONST_VAL);
graph_set_source(graph, ps->clamp_pitch, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
// Alt autonomous
graph_set_source(graph, ps->alt_pid, PID_DT, ps->dt, CONST_VAL);
graph_set_source(graph, ps->alt_pid, PID_CUR_POINT, ps->vrpn_alt, CONST_VAL);
graph_set_source(graph, ps->alt_pid, PID_SETPOINT, ps->alt_set, CONST_VAL);
graph_set_source(graph, ps->alt_set, CONST_VAL, ps->vrpn_alt, CONST_VAL);
graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND1, ps->alt_pid, PID_CORRECTION);
graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND2, ps->throttle_trim, CONST_VAL);
// Yaw autonomous
graph_set_source(graph, ps->yaw_pid, PID_DT, ps->dt, CONST_VAL);
graph_set_source(graph, ps->yaw_pid, PID_SETPOINT, ps->yaw_set, CONST_VAL);
graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->cur_yaw, CONST_VAL);
// Connect PWM clamping blocks // Connect PWM clamping blocks
graph_set_source(graph, ps->clamp_d_pwmP, BOUNDS_IN, ps->pitch_r_pid, PID_CORRECTION); graph_set_source(graph, ps->clamp_d_pwmP, BOUNDS_IN, ps->pitch_r_pid, PID_CORRECTION);
...@@ -99,11 +142,14 @@ ...@@ -99,11 +142,14 @@
graph_set_source(graph, ps->clamp_d_pwmY, BOUNDS_IN, ps->yaw_r_pid, PID_CORRECTION); graph_set_source(graph, ps->clamp_d_pwmY, BOUNDS_IN, ps->yaw_r_pid, PID_CORRECTION);
// Connect signal mixer // Connect signal mixer
graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL); //graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
graph_set_source(graph, ps->mixer, MIXER_PITCH, ps->clamp_d_pwmP, BOUNDS_OUT); graph_set_source(graph, ps->mixer, MIXER_PITCH, ps->clamp_d_pwmP, BOUNDS_OUT);
graph_set_source(graph, ps->mixer, MIXER_ROLL, ps->clamp_d_pwmR, BOUNDS_OUT); graph_set_source(graph, ps->mixer, MIXER_ROLL, ps->clamp_d_pwmR, BOUNDS_OUT);
graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT); graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT);
// Set initial mode
connect_manual(ps);
// Set pitch PID constants // Set pitch PID constants
graph_set_param_val(graph, ps->pitch_pid, PID_KP, PITCH_ANGLE_KP); graph_set_param_val(graph, ps->pitch_pid, PID_KP, PITCH_ANGLE_KP);
graph_set_param_val(graph, ps->pitch_pid, PID_KI, PITCH_ANGLE_KI); graph_set_param_val(graph, ps->pitch_pid, PID_KI, PITCH_ANGLE_KI);
...@@ -128,6 +174,18 @@ ...@@ -128,6 +174,18 @@
graph_set_param_val(graph, ps->yaw_r_pid, PID_KI, YAW_ANGULAR_VELOCITY_KI); graph_set_param_val(graph, ps->yaw_r_pid, PID_KI, YAW_ANGULAR_VELOCITY_KI);
graph_set_param_val(graph, ps->yaw_r_pid, PID_KD, YAW_ANGULAR_VELOCITY_KD); graph_set_param_val(graph, ps->yaw_r_pid, PID_KD, YAW_ANGULAR_VELOCITY_KD);
// Set X/Y/Alt constants
graph_set_param_val(graph, ps->x_pos_pid, PID_KP, XPOS_KP);
graph_set_param_val(graph, ps->x_pos_pid, PID_KI, XPOS_KI);
graph_set_param_val(graph, ps->x_pos_pid, PID_KD, XPOS_KD);
graph_set_param_val(graph, ps->y_pos_pid, PID_KP, YPOS_KP);
graph_set_param_val(graph, ps->y_pos_pid, PID_KI, YPOS_KI);
graph_set_param_val(graph, ps->y_pos_pid, PID_KD, YPOS_KD);
graph_set_param_val(graph, ps->alt_pid, PID_KP, ALT_ZPOS_KP);
graph_set_param_val(graph, ps->alt_pid, PID_KI, ALT_ZPOS_KI);
graph_set_param_val(graph, ps->alt_pid, PID_KD, ALT_ZPOS_KD);
// Set angle clamping limits // Set angle clamping limits
graph_set_param_val(graph, ps->clamp_pitch, BOUNDS_MIN, -ROLL_PITCH_MAX_ANGLE); graph_set_param_val(graph, ps->clamp_pitch, BOUNDS_MIN, -ROLL_PITCH_MAX_ANGLE);
graph_set_param_val(graph, ps->clamp_pitch, BOUNDS_MAX, ROLL_PITCH_MAX_ANGLE); graph_set_param_val(graph, ps->clamp_pitch, BOUNDS_MAX, ROLL_PITCH_MAX_ANGLE);
...@@ -161,23 +219,12 @@ ...@@ -161,23 +219,12 @@
if(cur_fm_switch == MANUAL_FLIGHT_MODE) if(cur_fm_switch == MANUAL_FLIGHT_MODE)
user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE; user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
static float roll_trim = 0.0;
static float pitch_trim = 0.0;
// flap switch was just toggled to auto flight mode // flap switch was just toggled to auto flight mode
if((last_fm_switch != cur_fm_switch) && (cur_fm_switch == AUTO_FLIGHT_MODE)) if((last_fm_switch != cur_fm_switch) && (cur_fm_switch == AUTO_FLIGHT_MODE))
{ {
user_defined_struct->engaging_auto = 1; user_defined_struct->engaging_auto = 1;
// Read in trimmed values because it should read trim values right when the pilot flips the flight mode switch graph_set_param_val(graph, parameter_struct->throttle_trim, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
pitch_trim = user_input_struct->pitch_angle_manual_setpoint; //rc_commands[PITCH] - PITCH_CENTER;
roll_trim = user_input_struct->roll_angle_manual_setpoint; //rc_commands[ROLL] - ROLL_CENTER;
//sensor_struct->trimmedRCValues.yaw = yaw_manual_setpoint; //rc_commands[YAW] - YAW_CENTER;
// sensor_struct->trims.roll = raw_actuator_struct->controller_corrected_motor_commands[ROLL];
// sensor_struct->trims.pitch = raw_actuator_struct->controller_corrected_motor_commands[PITCH];
// sensor_struct->trims.yaw = raw_actuator_struct->controller_corrected_motor_commands[YAW];
sensor_struct->trims.throttle = user_input_struct->rc_commands[THROTTLE];
log_struct->trims.roll = sensor_struct->trims.roll; log_struct->trims.roll = sensor_struct->trims.roll;
log_struct->trims.pitch = sensor_struct->trims.pitch; log_struct->trims.pitch = sensor_struct->trims.pitch;
...@@ -194,26 +241,13 @@ ...@@ -194,26 +241,13 @@
// also reset the previous error and accumulated error from the position PIDs // also reset the previous error and accumulated error from the position PIDs
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))
{ {
// TODO: Change this to reconnect PIDs graph_set_param_val(graph, parameter_struct->x_set, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
/* graph_set_param_val(graph, parameter_struct->y_set, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
// zero out the accumulated error so the I terms don't cause wild things to happen graph_set_param_val(graph, parameter_struct->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
parameter_struct->pid_controllers[ALT_ID].acc_error = 0.0; graph_set_param_val(graph, parameter_struct->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
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->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;
setpoint_struct->desiredQuadPosition.y_pos = sensor_struct->currentQuadPosition.y_pos;
setpoint_struct->desiredQuadPosition.yaw = 0.0;//currentQuadPosition.yaw;
// reset the flag that engages auto mode // reset the flag that engages auto mode
user_defined_struct->engaging_auto = 0; user_defined_struct->engaging_auto = 0;
*/
// finally engage the AUTO_FLIGHT_MODE // finally engage the AUTO_FLIGHT_MODE
// this ensures that we've gotten a new update packet right after the switch was set to auto mode // this ensures that we've gotten a new update packet right after the switch was set to auto mode
user_defined_struct->flight_mode = AUTO_FLIGHT_MODE; user_defined_struct->flight_mode = AUTO_FLIGHT_MODE;
...@@ -226,61 +260,26 @@ ...@@ -226,61 +260,26 @@
* a pitch or roll for the angle loop PIDs * a pitch or roll for the angle loop PIDs
*/ */
// static int counter_between_packets = 0;
if(user_input_struct->locationFresh) if(user_input_struct->locationFresh)
{ {
// TODO: Change this to populate VRPN block // VRPN values
/* graph_set_param_val(graph, parameter_struct->vrpn_x, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
parameter_struct->pid_controllers[LOCAL_Y_ID].current_point = sensor_struct->currentQuadPosition.y_pos; graph_set_param_val(graph, parameter_struct->vrpn_y, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
parameter_struct->pid_controllers[LOCAL_Y_ID].setpoint = setpoint_struct->desiredQuadPosition.y_pos; graph_set_param_val(graph, parameter_struct->vrpn_alt, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
graph_set_param_val(graph, parameter_struct->cur_yaw, CONST_SET, sensor_struct->currentQuadPosition.yaw);
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->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->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
graph_set_param_val(graph, parameter_struct->cur_yaw, CONST_SET, sensor_struct->currentQuadPosition.yaw);
// TODO: Set yaw 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->pid_controllers[YAW_ID]));
} }
// Sensor values
graph_set_param_val(graph, parameter_struct->cur_pitch, CONST_SET, sensor_struct->pitch_angle_filtered); graph_set_param_val(graph, parameter_struct->cur_pitch, CONST_SET, sensor_struct->pitch_angle_filtered);
graph_set_param_val(graph, parameter_struct->cur_roll, CONST_SET, sensor_struct->roll_angle_filtered); graph_set_param_val(graph, parameter_struct->cur_roll, CONST_SET, sensor_struct->roll_angle_filtered);
graph_set_param_val(graph, parameter_struct->theta_dot, CONST_SET, sensor_struct->theta_dot); graph_set_param_val(graph, parameter_struct->theta_dot, CONST_SET, sensor_struct->theta_dot);
graph_set_param_val(graph, parameter_struct->phi_dot, CONST_SET, sensor_struct->phi_dot); graph_set_param_val(graph, parameter_struct->phi_dot, CONST_SET, sensor_struct->phi_dot);
graph_set_param_val(graph, parameter_struct->psi_dot, CONST_SET, sensor_struct->psi_dot); graph_set_param_val(graph, parameter_struct->psi_dot, CONST_SET, sensor_struct->psi_dot);
// RC values
graph_set_param_val(graph, parameter_struct->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint); graph_set_param_val(graph, parameter_struct->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
graph_set_param_val(graph, parameter_struct->rc_roll, CONST_SET, user_input_struct->roll_angle_manual_setpoint); graph_set_param_val(graph, parameter_struct->rc_roll, CONST_SET, user_input_struct->roll_angle_manual_setpoint);
graph_set_param_val(graph, parameter_struct->rc_yaw, CONST_SET, user_input_struct->yaw_manual_setpoint); graph_set_param_val(graph, parameter_struct->rc_yaw, CONST_SET, user_input_struct->yaw_manual_setpoint);
graph_set_param_val(graph, parameter_struct->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]); graph_set_param_val(graph, parameter_struct->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
/*parameter_struct->pid_controllers[PITCH_ID].setpoint =
(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
(parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction) + pitch_trim : user_input_struct->pitch_angle_manual_setpoint;
parameter_struct->pid_controllers[ROLL_ID].setpoint =
(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
(parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction) + roll_trim : user_input_struct->roll_angle_manual_setpoint;
*/
/*
int outputs[3] = {parameter_struct->pitch_r_pid,
parameter_struct->roll_r_pid,
parameter_struct->yaw_r_pid};*/
int outputs[1] = {parameter_struct->mixer}; int outputs[1] = {parameter_struct->mixer};
graph_compute_nodes(graph, outputs, 1); graph_compute_nodes(graph, outputs, 1);
...@@ -291,39 +290,6 @@ ...@@ -291,39 +290,6 @@
if((user_input_struct->rc_commands[THROTTLE] > if((user_input_struct->rc_commands[THROTTLE] >
118000) || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)) 118000) || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE))
{ {
if(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)
{
/*
//THROTTLE
raw_actuator_struct->controller_corrected_motor_commands[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->pid_controllers[ROLL_RATE_ID].pid_correction; // + sensor_struct->trims.roll;
//PITCH
raw_actuator_struct->controller_corrected_motor_commands[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->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->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{
//THROTTLE //THROTTLE
actuator_struct->pwms[0] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM0); actuator_struct->pwms[0] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM0);
...@@ -332,32 +298,6 @@ ...@@ -332,32 +298,6 @@
actuator_struct->pwms[2] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM2); actuator_struct->pwms[2] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM2);
actuator_struct->pwms[3] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM3); actuator_struct->pwms[3] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM3);
}
/*
//BOUNDS CHECKING
if(raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] < 0)
raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] = 0;
//BOUNDS CHECKING
if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] > 20000)
raw_actuator_struct->controller_corrected_motor_commands[ROLL] = 20000;
if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] < -20000)
raw_actuator_struct->controller_corrected_motor_commands[ROLL] = -20000;
if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] > 20000)
raw_actuator_struct->controller_corrected_motor_commands[PITCH] = 20000;
if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] < -20000)
raw_actuator_struct->controller_corrected_motor_commands[PITCH] = -20000;
if(raw_actuator_struct->controller_corrected_motor_commands[YAW] > 20000)
raw_actuator_struct->controller_corrected_motor_commands[YAW] = 20000;
if(raw_actuator_struct->controller_corrected_motor_commands[YAW] < -20000)
raw_actuator_struct->controller_corrected_motor_commands[YAW] = -20000;
*/
} }
else else
{ {
...@@ -365,10 +305,6 @@ ...@@ -365,10 +305,6 @@
actuator_struct->pwms[1] = user_input_struct->rc_commands[THROTTLE]; actuator_struct->pwms[1] = user_input_struct->rc_commands[THROTTLE];
actuator_struct->pwms[2] = user_input_struct->rc_commands[THROTTLE]; actuator_struct->pwms[2] = user_input_struct->rc_commands[THROTTLE];
actuator_struct->pwms[3] = user_input_struct->rc_commands[THROTTLE]; actuator_struct->pwms[3] = user_input_struct->rc_commands[THROTTLE];
/*
raw_actuator_struct->controller_corrected_motor_commands[ROLL] = 0;
raw_actuator_struct->controller_corrected_motor_commands[PITCH] = 0;
raw_actuator_struct->controller_corrected_motor_commands[YAW] = 0;*/
} }
//logging //logging
......
quad/sw/modular_quad_pid/src/graph_blocks/node_add.c 0 → 120000
+ 1
− 0
View file @ 5bf7ac34
../../../../computation_graph/src/graph_blocks/node_add.c
\ No newline at end of file
quad/sw/modular_quad_pid/src/graph_blocks/node_add.h 0 → 120000
+ 1
− 0
View file @ 5bf7ac34
../../../../computation_graph/src/graph_blocks/node_add.h
\ No newline at end of file
quad/sw/modular_quad_pid/src/type_def.h
+ 12
− 0
View file @ 5bf7ac34
...@@ -307,11 +307,20 @@ typedef struct parameter_t { ...@@ -307,11 +307,20 @@ typedef struct parameter_t {
int theta_dot; int theta_dot;
int phi_dot; int phi_dot;
int psi_dot; int psi_dot;
// VRPN blocks
int vrpn_x;
int vrpn_y;
int vrpn_alt;
// RC blocks // RC blocks
int rc_pitch; int rc_pitch;
int rc_roll; int rc_roll;
int rc_yaw; int rc_yaw;
int rc_throttle; int rc_throttle;
// Desired positions
int x_set;
int y_set;
int alt_set;
int yaw_set;
// Loop time // Loop time
int dt; int dt;
// Signal mixer // Signal mixer
...@@ -322,6 +331,9 @@ typedef struct parameter_t { ...@@ -322,6 +331,9 @@ typedef struct parameter_t {
int clamp_d_pwmR; // Clamp the change in PWM values for roll int clamp_d_pwmR; // Clamp the change in PWM values for roll
int clamp_d_pwmP; // ... pitch int clamp_d_pwmP; // ... pitch
int clamp_d_pwmY; // ... yaw int clamp_d_pwmY; // ... yaw
// "trim" for autonomous
int throttle_trim;
int throttle_trim_add;
} parameter_t; } parameter_t;
/** /**
......
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