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

Refactored velocity configuration in control algorithm. Made default PID... 

Refactored velocity configuration in control algorithm. Made default PID constants the good ones. Corrected the velocity sign. There might be sign errors with the velocity PID constants.
parent c41fd619
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
quad/src/gen_diagram/network.dot
+ 33
− 37
View file @ 75dc98c3
digraph G {
rankdir="LR"
"Roll PID"[shape=record
label="<f0>Roll PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200] |<f7> [alpha=0.000]"]
label="<f0>Roll PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
"Roll" -> "Roll PID":f1 [label="Constant"]
"Y Vel PID" -> "Roll PID":f2 [label="Correction"]
"Ts_IMU" -> "Roll PID":f3 [label="Constant"]
"Pitch PID"[shape=record
label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200] |<f7> [alpha=0.000]"]
label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
"Pitch trim add" -> "Pitch PID":f1 [label="Sum"]
"X Vel PID" -> "Pitch PID":f2 [label="Correction"]
"Ts_IMU" -> "Pitch PID":f3 [label="Constant"]
......@@ -16,32 +16,32 @@ label="<f0>Yaw PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [K
"Yaw Setpoint" -> "Yaw PID":f2 [label="Constant"]
"Ts_VRPN" -> "Yaw PID":f3 [label="Constant"]
"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] |<f7> [alpha=0.000]"]
"dPhi" -> "Roll Rate PID":f1 [label="Constant"]
label="<f0>Roll Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=3000.000] |<f5> [Ki=0.000] |<f6> [Kd=500.000] |<f7> [alpha=0.880]"]
"Gyro X" -> "Roll Rate PID":f1 [label="Constant"]
"Roll PID" -> "Roll Rate PID":f2 [label="Correction"]
"Ts_IMU" -> "Roll Rate PID":f3 [label="Constant"]
"Pitch Rate PID"[shape=record
label="<f0>Pitch Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=4600.000] |<f5> [Ki=0.000] |<f6> [Kd=550.000] |<f7> [alpha=0.000]"]
"dTheta" -> "Pitch Rate PID":f1 [label="Constant"]
label="<f0>Pitch Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=3000.000] |<f5> [Ki=0.000] |<f6> [Kd=500.000] |<f7> [alpha=0.880]"]
"Gyro Y" -> "Pitch Rate PID":f1 [label="Constant"]
"Pitch PID" -> "Pitch Rate PID":f2 [label="Correction"]
"Ts_IMU" -> "Pitch Rate PID":f3 [label="Constant"]
"Yaw Rate PID"[shape=record
label="<f0>Yaw Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=29700.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
"dPsi" -> "Yaw Rate PID":f1 [label="Constant"]
"Gyro Z" -> "Yaw Rate PID":f1 [label="Constant"]
"Yaw PID" -> "Yaw Rate PID":f2 [label="Correction"]
"Ts_IMU" -> "Yaw Rate PID":f3 [label="Constant"]
"X pos PID"[shape=record
label="<f0>X pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.015] |<f5> [Ki=-0.005] |<f6> [Kd=-0.030] |<f7> [alpha=0.000]"]
label="<f0>X pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.550] |<f5> [Ki=-0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
"VRPN X" -> "X pos PID":f1 [label="Constant"]
"X Setpoint" -> "X pos PID":f2 [label="Constant"]
"Ts_VRPN" -> "X pos PID":f3 [label="Constant"]
"Y pos PID"[shape=record
label="<f0>Y pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.015] |<f5> [Ki=0.005] |<f6> [Kd=0.030] |<f7> [alpha=0.000]"]
label="<f0>Y pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.550] |<f5> [Ki=0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
"VRPN Y" -> "Y pos PID":f1 [label="Constant"]
"Y Setpoint" -> "Y pos PID":f2 [label="Constant"]
"Ts_VRPN" -> "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] |<f7> [alpha=0.000]"]
label="<f0>Altitude PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-9804.000] |<f5> [Ki=-817.000] |<f6> [Kd=-7353.000] |<f7> [alpha=0.000]"]
"VRPN Alt" -> "Altitude PID":f1 [label="Constant"]
"Alt Setpoint" -> "Altitude PID":f2 [label="Constant"]
"Ts_VRPN" -> "Altitude PID":f3 [label="Constant"]
......@@ -66,17 +66,17 @@ label="<f0>Roll |<f1> [Constant=0.000]"]
"Yaw"[shape=record
label="<f0>Yaw |<f1> [Constant=0.000]"]
"Pitch trim"[shape=record
label="<f0>Pitch trim |<f1> [Constant=0.020]"]
label="<f0>Pitch trim |<f1> [Constant=0.045]"]
"Pitch trim add"[shape=record
label="<f0>Pitch trim add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
"Pitch trim" -> "Pitch trim add":f1 [label="Constant"]
"Pitch" -> "Pitch trim add":f2 [label="Constant"]
"dTheta"[shape=record
label="<f0>dTheta |<f1> [Constant=0.000]"]
"dPhi"[shape=record
label="<f0>dPhi |<f1> [Constant=0.000]"]
"dPsi"[shape=record
label="<f0>dPsi |<f1> [Constant=0.000]"]
"Gyro Y"[shape=record
label="<f0>Gyro Y |<f1> [Constant=0.000]"]
"Gyro X"[shape=record
label="<f0>Gyro X |<f1> [Constant=0.000]"]
"Gyro Z"[shape=record
label="<f0>Gyro Z |<f1> [Constant=0.000]"]
"P PWM Clamp"[shape=record
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"]
......@@ -104,35 +104,23 @@ label="<f0>RC Roll |<f1> [Constant=0.000]"]
label="<f0>RC Yaw |<f1> [Constant=0.000]"]
"RC Throttle"[shape=record
label="<f0>RC Throttle |<f1> [Constant=0.000]"]
"Signal Mixer"[shape=record
label="<f0>Signal Mixer |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
"T trim add" -> "Signal Mixer":f1 [label="Sum"]
"P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
"R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
"Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
"Ts_IMU"[shape=record
label="<f0>Ts_IMU |<f1> [Constant=0.005]"]
"Ts_VRPN"[shape=record
label="<f0>Ts_VRPN |<f1> [Constant=0.040]"]
"zero"[shape=record
label="<f0>zero |<f1> [Constant=0.000]"]
"X Vel PID"[shape=record
label="<f0>X Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
label="<f0>X Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.100] |<f5> [Ki=0.000] |<f6> [Kd=0.020] |<f7> [alpha=0.880]"]
"X Vel" -> "X Vel PID":f1 [label="Correction"]
"X Vel Clamp" -> "X Vel PID":f2 [label="Bounded"]
"Ts_VRPN" -> "X Vel PID":f3 [label="Constant"]
"Y Vel PID"[shape=record
label="<f0>Y Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
label="<f0>Y Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.100] |<f5> [Ki=0.000] |<f6> [Kd=-0.020] |<f7> [alpha=0.880]"]
"Y Vel" -> "Y Vel PID":f1 [label="Correction"]
"Y vel Clamp" -> "Y Vel PID":f2 [label="Bounded"]
"Ts_VRPN" -> "Y Vel PID":f3 [label="Constant"]
"X Vel"[shape=record
label="<f0>X Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.000]"]
label="<f0>X Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=-1.000] |<f7> [alpha=0.000]"]
"VRPN X" -> "X Vel":f1 [label="Constant"]
"zero" -> "X Vel":f2 [label="Constant"]
"Ts_VRPN" -> "X Vel":f3 [label="Constant"]
"Y Vel"[shape=record
label="<f0>Y Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.000]"]
label="<f0>Y Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=-1.000] |<f7> [alpha=0.000]"]
"VRPN Y" -> "Y Vel":f1 [label="Constant"]
"zero" -> "Y Vel":f2 [label="Constant"]
"Ts_VRPN" -> "Y Vel":f3 [label="Constant"]
......@@ -142,8 +130,16 @@ label="<f0>X Vel Clamp |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]
"Y vel Clamp"[shape=record
label="<f0>Y vel Clamp |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]"]
"Y pos PID" -> "Y vel Clamp":f1 [label="Correction"]
"X Stick Gain"[shape=record
label="<f0>X Stick Gain |<f1> --\>Input |<f2> [Gain=5.000]"]
"Y Stick Gain"[shape=record
label="<f0>Y Stick Gain |<f1> --\>Input |<f2> [Gain=-5.000]"]
"Signal Mixer"[shape=record
label="<f0>Signal Mixer |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
"T trim add" -> "Signal Mixer":f1 [label="Sum"]
"P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
"R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
"Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
"Ts_IMU"[shape=record
label="<f0>Ts_IMU |<f1> [Constant=0.005]"]
"Ts_VRPN"[shape=record
label="<f0>Ts_VRPN |<f1> [Constant=0.040]"]
"zero"[shape=record
label="<f0>zero |<f1> [Constant=0.000]"]
}
\ No newline at end of file
quad/src/gen_diagram/network.png
+ 0
− 0
View replaced file @ c41fd619 View file @ 75dc98c3
quad/src/gen_diagram/network.png

482 KiB | W: | H:

quad/src/gen_diagram/network.png

514 KiB | W: | H:

quad/src/gen_diagram/network.png
quad/src/gen_diagram/network.png
quad/src/gen_diagram/network.png
quad/src/gen_diagram/network.png
  • 2-up
  • Swipe
  • Onion skin
quad/src/quad_app/PID.h
+ 32
− 48
View file @ 75dc98c3
......@@ -10,16 +10,10 @@
#include "type_def.h"
// Yaw constants
// when using units of degrees
//#define YAW_ANGULAR_VELOCITY_KP 40.0f
//#define YAW_ANGULAR_VELOCITY_KI 0.0f
//#define YAW_ANGULAR_VELOCITY_KD 0.0f
//#define YAW_ANGLE_KP 2.6f
//#define YAW_ANGLE_KI 0.0f
//#define YAW_ANGLE_KD 0.0f
#define VEL_CLAMP 2.0
#define PITCH_TRIM 0.045
// Yaw constants
// when using units of radians
#define YAW_ANGULAR_VELOCITY_KP 29700//200.0f * 2292.0f
#define YAW_ANGULAR_VELOCITY_KI 0.0f
......@@ -28,59 +22,49 @@
#define YAW_ANGLE_KI 0.0f
#define YAW_ANGLE_KD 0.0f
// Roll constants
//#define ROLL_ANGULAR_VELOCITY_KP 0.95f
//#define ROLL_ANGULAR_VELOCITY_KI 0.0f
//#define ROLL_ANGULAR_VELOCITY_KD 0.13f//0.4f//0.7f
//#define ROLL_ANGLE_KP 17.0f //9.0f
//#define ROLL_ANGLE_KI 0.0f
//#define ROLL_ANGLE_KD 0.3f // 0.2f
//#define YPOS_KP 0.0f
//#define YPOS_KI 0.0f
//#define YPOS_KD 0.0f
// when using units of radians
#define ROLL_ANGULAR_VELOCITY_KP 100.0f*46.0f//102.0f*46.0f//9384.0f//204.0f * 46.0f
#define ROLL_ANGULAR_VELOCITY_KP 3000
#define ROLL_ANGULAR_VELOCITY_KI 0.0f
#define ROLL_ANGULAR_VELOCITY_KD 100.f*5.5f//102.0f*6.8f//1387.2//204.0f * 6.8f
#define ROLL_ANGLE_KP 15.0f
#define ROLL_ANGULAR_VELOCITY_KD 500
#define ROLL_ANGULAR_VELOCITY_ALPHA 0.88
#define ROLL_ANGLE_KP 35
#define ROLL_ANGLE_KI 0.0f
#define ROLL_ANGLE_KD 0.2f
#define YPOS_KP 0.015f
#define YPOS_KI 0.005f
#define YPOS_KD 0.03f
#define ROLL_ANGLE_KD 1.0
#define ROLL_ANGLE_ALPHA 0.88
#define YVEL_KP -0.1
#define YVEL_KD -0.02
#define YVEL_ALPHA 0.88
#define YPOS_KP 0.55
#define YPOS_KI 0.0075
#define YPOS_KD 0.0
#define YPOS_ALPHA 0
//Pitch constants
// when using units of degrees
//#define PITCH_ANGULAR_VELOCITY_KP 0.95f
//#define PITCH_ANGULAR_VELOCITY_KI 0.0f
//#define PITCH_ANGULAR_VELOCITY_KD 0.13f//0.35f//0.7f
//#define PITCH_ANGLE_KP 17.0f // 7.2f
//#define PITCH_ANGLE_KI 0.0f
//#define PITCH_ANGLE_KD 0.3f //0.3f
//#define XPOS_KP 40.0f
//#define XPOS_KI 0.0f
//#define XPOS_KD 10.0f//0.015f
// when using units of radians
#define PITCH_ANGULAR_VELOCITY_KP 100.0f*46.0f//101.0f*46.0f//9292.0f//202.0f * 46.0f
#define PITCH_ANGULAR_VELOCITY_KP 3000
#define PITCH_ANGULAR_VELOCITY_KI 0.0f
#define PITCH_ANGULAR_VELOCITY_KD 100.0f*5.5f//101.0f*6.8f//1373.6//202.0f * 6.8f
#define PITCH_ANGLE_KP 15.0f
#define PITCH_ANGULAR_VELOCITY_KD 500
#define PITCH_ANGULAR_VELOCITY_ALPHA 0.88
#define PITCH_ANGLE_KP 35
#define PITCH_ANGLE_KI 0.0f
#define PITCH_ANGLE_KD 0.2f
#define XPOS_KP -0.015f
#define XPOS_KI -0.005f
#define XPOS_KD -0.03f
#define PITCH_ANGLE_KD 1.0
#define PITCH_ANGLE_ALPHA 0.88
#define XVEL_KP 0.1
#define XVEL_KD 0.02
#define XVEL_ALPHA 0.88
#define XPOS_KP -0.55
#define XPOS_KI -0.0075
#define XPOS_KD 0.0
#define XPOS_ALPHA 0
//Throttle constants
#define ALT_ZPOS_KP 9804.0f
#define ALT_ZPOS_KI 817.0f
#define ALT_ZPOS_KD 7353.0f
#define ALT_ZPOS_KP -9804.0f
#define ALT_ZPOS_KI -817.0f
#define ALT_ZPOS_KD -7353.0f
#define ALT_ZPOS_ALPHA 0.88
#endif /* PID_H_ */
quad/src/quad_app/control_algorithm.c
+ 66
− 72
View file @ 75dc98c3
......@@ -66,13 +66,9 @@ int control_algorithm_init(parameter_t * ps)
ps->pitch_trim_add = graph_add_defined_block(graph, BLOCK_ADD, "Pitch trim add");
// Yaw angular velocity PID
// theta_dot is the angular velocity about the y-axis
// phi_dot is the angular velocity about the x-axis
// psi_dot is the angular velocity about the z-axis
// These are calculated from using the gimbal equations
ps->theta_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "dTheta");
ps->phi_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "dPhi");
ps->psi_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "dPsi");
ps->gyro_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "Gyro Y");
ps->gyro_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "Gyro X");
ps->gyro_z = graph_add_defined_block(graph, BLOCK_CONSTANT, "Gyro Z");
ps->clamp_d_pwmP = graph_add_defined_block(graph, BLOCK_BOUNDS, "P PWM Clamp");
ps->clamp_d_pwmR = graph_add_defined_block(graph, BLOCK_BOUNDS, "R PWM Clamp");
ps->clamp_d_pwmY = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y PWM Clamp");
......@@ -90,18 +86,30 @@ int control_algorithm_init(parameter_t * ps)
ps->rc_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Yaw");
ps->rc_throttle = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Throttle");
// Create blocks for velocity controllers
ps->x_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "X Vel PID");
ps->y_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "Y Vel PID");
ps->x_vel = graph_add_defined_block(graph, BLOCK_PID, "X Vel");
ps->y_vel = graph_add_defined_block(graph, BLOCK_PID, "Y Vel");
ps->x_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "X Vel Clamp");
ps->y_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y vel Clamp");
ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
ps->angle_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_IMU");
ps->pos_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_VRPN");
// Useful zero block
int zero_block = graph_add_defined_block(graph, BLOCK_CONSTANT, "zero");
graph_set_param_val(graph, zero_block, CONST_SET, 0);
// Connect pitch PID chain
// Trims
graph_set_source(graph, ps->pitch_trim_add, ADD_SUMMAND1, ps->pitch_trim, CONST_VAL);
graph_set_source(graph, ps->pitch_trim_add, ADD_SUMMAND2, ps->cur_pitch, CONST_VAL);
// Controllers
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->gyro_y, CONST_VAL);
graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->pitch_trim_add, ADD_SUM);
//graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->vrpn_pitch, CONST_VAL);
......@@ -109,7 +117,7 @@ int control_algorithm_init(parameter_t * ps)
// Connect roll PID chain
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->gyro_x, CONST_VAL);
graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->angle_time, 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->vrpn_roll, CONST_VAL);
......@@ -117,26 +125,49 @@ int control_algorithm_init(parameter_t * ps)
// Connect yaw PID chain
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->gyro_z, CONST_VAL);
graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->angle_time, CONST_VAL);
// X velocity PID
// Use a PID block to compute the derivative
graph_set_param_val(graph, ps->x_vel, PID_KD, -1);
graph_set_source(graph, ps->x_vel, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->x_vel, PID_CUR_POINT, ps->vrpn_x, CONST_VAL);
graph_set_source(graph, ps->x_vel, PID_SETPOINT, zero_block, CONST_VAL);
// Connect velocity PID itself
graph_set_source(graph, ps->x_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->x_vel_pid, PID_CUR_POINT, ps->x_vel, PID_CORRECTION);
graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel_clamp, BOUNDS_OUT);
graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
// X autonomous
// Y velocity PID
// Use a PID block to compute the derivative
graph_set_param_val(graph, ps->y_vel, PID_KD, -1);
graph_set_source(graph, ps->y_vel, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->y_vel, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
graph_set_source(graph, ps->y_vel, PID_SETPOINT, zero_block, CONST_VAL);
// Connect velocity PID itself
graph_set_source(graph, ps->y_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->y_vel_pid, PID_CUR_POINT, ps->y_vel, PID_CORRECTION);
graph_set_source(graph, ps->y_vel_pid, PID_SETPOINT, ps->y_vel_clamp, BOUNDS_OUT);
graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
// X position
graph_set_source(graph, ps->x_pos_pid, PID_DT, ps->pos_time, CONST_VAL);
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
// Y position
graph_set_source(graph, ps->y_pos_pid, PID_DT, ps->pos_time, CONST_VAL);
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
// Alt position
graph_set_source(graph, ps->alt_pid, PID_DT, ps->pos_time, 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
// Yaw angle
graph_set_source(graph, ps->yaw_pid, PID_DT, ps->pos_time, 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);
......@@ -156,17 +187,21 @@ int control_algorithm_init(parameter_t * ps)
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_KD, PITCH_ANGLE_KD);
graph_set_param_val(graph, ps->pitch_pid, PID_ALPHA, PITCH_ANGLE_ALPHA);
graph_set_param_val(graph, ps->pitch_r_pid, PID_KP, PITCH_ANGULAR_VELOCITY_KP);
graph_set_param_val(graph, ps->pitch_r_pid, PID_KI, PITCH_ANGULAR_VELOCITY_KI);
graph_set_param_val(graph, ps->pitch_r_pid, PID_KD, PITCH_ANGULAR_VELOCITY_KD);
graph_set_param_val(graph, ps->pitch_r_pid, PID_ALPHA, PITCH_ANGULAR_VELOCITY_ALPHA);
// Set roll PID constants
graph_set_param_val(graph, ps->roll_pid, PID_KP, ROLL_ANGLE_KP);
graph_set_param_val(graph, ps->roll_pid, PID_KI, ROLL_ANGLE_KI);
graph_set_param_val(graph, ps->roll_pid, PID_KD, ROLL_ANGLE_KD);
graph_set_param_val(graph, ps->roll_pid, PID_ALPHA, ROLL_ANGLE_ALPHA);
graph_set_param_val(graph, ps->roll_r_pid, PID_KP, ROLL_ANGULAR_VELOCITY_KP);
graph_set_param_val(graph, ps->roll_r_pid, PID_KI, ROLL_ANGULAR_VELOCITY_KI);
graph_set_param_val(graph, ps->roll_r_pid, PID_KD, ROLL_ANGULAR_VELOCITY_KD);
graph_set_param_val(graph, ps->roll_r_pid, PID_ALPHA, ROLL_ANGULAR_VELOCITY_ALPHA);
// Set yaw PID constants
graph_set_param_val(graph, ps->yaw_pid, PID_KP, YAW_ANGLE_KP);
......@@ -176,6 +211,14 @@ int control_algorithm_init(parameter_t * ps)
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);
// Set velocity constants
graph_set_param_val(graph, ps->x_vel_pid, PID_KP, XVEL_KP);
graph_set_param_val(graph, ps->x_vel_pid, PID_KD, XVEL_KD);
graph_set_param_val(graph, ps->x_vel_pid, PID_ALPHA, XVEL_ALPHA);
graph_set_param_val(graph, ps->y_vel_pid, PID_KP, YVEL_KP);
graph_set_param_val(graph, ps->y_vel_pid, PID_KD, YVEL_KD);
graph_set_param_val(graph, ps->y_vel_pid, PID_ALPHA, YVEL_ALPHA);
// 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);
......@@ -197,60 +240,14 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, PWM_DIFF_BOUNDS);
// Velocity stuff
int zero_block = graph_add_defined_block(graph, BLOCK_CONSTANT, "zero");
ps->x_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "X Vel PID");
ps->y_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "Y Vel PID");
ps->x_vel = graph_add_defined_block(graph, BLOCK_PID, "X Vel");
ps->y_vel = graph_add_defined_block(graph, BLOCK_PID, "Y Vel");
ps->x_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "X Vel Clamp");
ps->y_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y vel Clamp");
ps->vel_x_gain = graph_add_defined_block(graph, BLOCK_GAIN, "X Stick Gain");
ps->vel_y_gain = graph_add_defined_block(graph, BLOCK_GAIN, "Y Stick Gain");
graph_set_source(graph, ps->x_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->y_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->x_vel, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->y_vel, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel, PID_CORRECTION);
graph_set_source(graph, ps->y_vel_pid, PID_SETPOINT, ps->y_vel, PID_CORRECTION);
graph_set_source(graph, ps->x_vel_pid, PID_CUR_POINT, ps->x_vel, PID_CORRECTION);
graph_set_source(graph, ps->y_vel_pid, PID_CUR_POINT, ps->y_vel, PID_CORRECTION);
graph_set_source(graph, ps->x_vel, PID_CUR_POINT, ps->vrpn_x, CONST_VAL);
graph_set_source(graph, ps->y_vel, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
graph_set_source(graph, ps->x_vel, PID_SETPOINT, zero_block, CONST_VAL);
graph_set_source(graph, ps->y_vel, PID_SETPOINT, zero_block, CONST_VAL);
graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel_clamp, GAIN_RESULT);
graph_set_source(graph, ps->y_vel_pid, PID_SETPOINT, ps->y_vel_clamp, GAIN_RESULT);
graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
//graph_set_source(graph, ps->vel_x_gain, GAIN_INPUT, ps->rc_pitch, CONST_VAL);
//graph_set_source(graph, ps->vel_y_gain, GAIN_INPUT, ps->rc_roll, CONST_VAL);
//graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->vel_x_gain, GAIN_RESULT);
//graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->vel_y_gain, GAIN_RESULT);
float vel_clamps = 2;
graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MIN, -vel_clamps);
graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MAX, vel_clamps);
graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MIN, -vel_clamps);
graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MAX, vel_clamps);
graph_set_param_val(graph, ps->x_vel, PID_KD, 1);
graph_set_param_val(graph, ps->y_vel, PID_KD, 1);
graph_set_param_val(graph, ps->vel_x_gain, GAIN_GAIN, 5);
graph_set_param_val(graph, ps->vel_y_gain, GAIN_GAIN, -5);
// Set velocity clamping limits
graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MIN, -VEL_CLAMP);
graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MAX, VEL_CLAMP);
graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MIN, -VEL_CLAMP);
graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MAX, VEL_CLAMP);
// Set trims
graph_set_param_val(graph, ps->pitch_trim, CONST_SET, 0.02);
graph_set_param_val(graph, ps->pitch_trim, CONST_SET, PITCH_TRIM);
// Initial value for sampling periods
graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.04);
......@@ -341,12 +338,9 @@ int control_algorithm_init(parameter_t * ps)
// Sensor values
graph_set_param_val(graph, ps->cur_pitch, CONST_SET, sensor_struct->pitch_angle_filtered);
graph_set_param_val(graph, ps->cur_roll, CONST_SET, sensor_struct->roll_angle_filtered);
//graph_set_param_val(graph, ps->theta_dot, CONST_SET, sensor_struct->theta_dot);
//graph_set_param_val(graph, ps->phi_dot, CONST_SET, sensor_struct->phi_dot);
//graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->psi_dot);
graph_set_param_val(graph, ps->theta_dot, CONST_SET, sensor_struct->gyr_y);
graph_set_param_val(graph, ps->phi_dot, CONST_SET, sensor_struct->gyr_x);
graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->gyr_z);
graph_set_param_val(graph, ps->gyro_y, CONST_SET, sensor_struct->gyr_y);
graph_set_param_val(graph, ps->gyro_x, CONST_SET, sensor_struct->gyr_x);
graph_set_param_val(graph, ps->gyro_z, CONST_SET, sensor_struct->gyr_z);
// RC values
graph_set_param_val(graph, ps->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
......
quad/src/quad_app/type_def.h
+ 3
− 3
View file @ 75dc98c3
......@@ -316,9 +316,9 @@ typedef struct parameter_t {
int cur_pitch;
int cur_roll;
int cur_yaw;
int theta_dot;
int phi_dot;
int psi_dot;
int gyro_y;
int gyro_x;
int gyro_z;
// VRPN blocks
int vrpn_x;
int vrpn_y;
......
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