diff --git a/quad/src/gen_diagram/network.dot b/quad/src/gen_diagram/network.dot
index 25a5dc165c2dac141e2c6694b707ec96db8eaf4a..174e1564fe62c01c56a092bd73e22a4efa945275 100644
--- a/quad/src/gen_diagram/network.dot
+++ b/quad/src/gen_diagram/network.dot
@@ -1,12 +1,12 @@
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
diff --git a/quad/src/gen_diagram/network.png b/quad/src/gen_diagram/network.png
index 6e1c3ebc4db038a6d42839315233e453f2e123fe..c12bb2905d8e97d2c51cfb7c90f763352b264910 100644
Binary files a/quad/src/gen_diagram/network.png and b/quad/src/gen_diagram/network.png differ
diff --git a/quad/src/quad_app/PID.h b/quad/src/quad_app/PID.h
index 5a83225b0b9813e4d311bf61c8f1eae89c7f24a0..aea92ad5049acfe2f53a1b4e470befb9c39a4bca 100644
--- a/quad/src/quad_app/PID.h
+++ b/quad/src/quad_app/PID.h
@@ -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_ */
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 710f26b34144c2827d46aa0363575541a12aae97..9f2c8d5b9fdca403877c8a11112024a1fc75db17 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -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);
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index d016134432b8e058601904a5013a296b7ecd7b00..21a1dcc0b949dd1f4c434618a772f3f5142a5b1f 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -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;