Added yaw rate clamp.

quad/src/gen_diagram/network.dot

@@ -12,9 +12,9 @@ label="<f0>Pitch PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4>
 "Ts_IMU" -> "Pitch PID":f3 [label="Constant"]
 "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] |<f7> [alpha=0.000]"]
-"PSI Sum" -> "Yaw PID":f1 [label="Sum"]
+"Yaw" -> "Yaw PID":f1 [label="Constant"]
 "Yaw Setpoint" -> "Yaw PID":f2 [label="Constant"]
-"Ts_IMU" -> "Yaw PID":f3 [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=0.030] |<f5> [Ki=0.000] |<f6> [Kd=0.005] |<f7> [alpha=0.880]"]
 "Gyro X" -> "Roll Rate PID":f1 [label="Constant"]
@@ -32,19 +32,19 @@ label="<f0>Yaw Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f
 "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.550] |<f5> [Ki=0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
-"OF X Trim Add" -> "X pos PID":f1 [label="Sum"]
+"VRPN X" -> "X pos PID":f1 [label="Constant"]
 "X Setpoint" -> "X pos PID":f2 [label="Constant"]
-"Ts_IMU" -> "X pos PID":f3 [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.550] |<f5> [Ki=0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
-"OF Y Trim Add" -> "Y pos PID":f1 [label="Sum"]
+"VRPN Y" -> "Y pos PID":f1 [label="Constant"]
 "Y Setpoint" -> "Y pos PID":f2 [label="Constant"]
-"Ts_IMU" -> "Y pos PID":f3 [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=-0.098] |<f5> [Ki=-0.008] |<f6> [Kd=-0.074] |<f7> [alpha=0.880]"]
-"Lidar" -> "Altitude PID":f1 [label="Constant"]
+"VRPN Alt" -> "Altitude PID":f1 [label="Constant"]
 "Alt Setpoint" -> "Altitude PID":f2 [label="Constant"]
-"Ts_IMU" -> "Altitude PID":f3 [label="Constant"]
+"Ts_VRPN" -> "Altitude PID":f3 [label="Constant"]
 "X Setpoint"[shape=record
 label="<f0>X Setpoint  |<f1> [Constant=0.000]"]
 "Y Setpoint"[shape=record
@@ -100,6 +100,9 @@ label="<f0>R PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20
 "Y PWM Clamp"[shape=record
 label="<f0>Y PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"]
 "Yaw Rate PID" -> "Y PWM Clamp":f1 [label="Correction"]
+"Yaw Rate Clamp"[shape=record
+label="<f0>Yaw Rate Clamp  |<f1> --\>Bounds in |<f2> [Min=-1.000] |<f3> [Max=1.000]"]
+"Yaw PID" -> "Yaw Rate Clamp":f1 [label="Correction"]
 "VRPN X"[shape=record
 label="<f0>VRPN X  |<f1> [Constant=0.000]"]
 "VRPN Y"[shape=record
@@ -120,18 +123,24 @@ label="<f0>RC Yaw  |<f1> [Constant=0.000]"]
 label="<f0>RC Throttle  |<f1> [Constant=0.000]"]
 "X Vel PID"[shape=record
 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.000]"]
-"Flow Vel X Filt" -> "X Vel PID":f1 [label="Constant"]
+"X Vel" -> "X Vel PID":f1 [label="Correction"]
 "X Vel Clamp" -> "X Vel PID":f2 [label="Bounded"]
-"Ts_IMU" -> "X Vel PID":f3 [label="Constant"]
+"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.100] |<f5> [Ki=0.000] |<f6> [Kd=0.020] |<f7> [alpha=0.000]"]
-"Flow Vel Y Filt" -> "Y Vel PID":f1 [label="Constant"]
+"Y Vel" -> "Y Vel PID":f1 [label="Correction"]
 "Y vel Clamp" -> "Y Vel PID":f2 [label="Bounded"]
-"Ts_IMU" -> "Y Vel PID":f3 [label="Constant"]
+"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=0.000] |<f7> [alpha=0.880]"]
+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.880]"]
+"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=0.000] |<f7> [alpha=0.880]"]
+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.880]"]
+"VRPN Y" -> "Y Vel":f1 [label="Constant"]
+"zero" -> "Y Vel":f2 [label="Constant"]
+"Ts_VRPN" -> "Y Vel":f3 [label="Constant"]
 "X Vel Clamp"[shape=record
 label="<f0>X Vel Clamp  |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]"]
 "X pos PID" -> "X Vel Clamp":f1 [label="Correction"]
@@ -140,12 +149,12 @@ 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"]
 "Yaw Correction"[shape=record
 label="<f0>Yaw Correction  |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>Y Position"]
-"PSI Sum" -> "Yaw Correction":f1 [label="Sum"]
+"Yaw" -> "Yaw Correction":f1 [label="Constant"]
 "X Vel PID" -> "Yaw Correction":f2 [label="Correction"]
 "Y Vel PID" -> "Yaw Correction":f3 [label="Correction"]
 "OF Offset Angle"[shape=record
 label="<f0>OF Offset Angle  |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>Y Position"]
-"PSI Sum" -> "OF Offset Angle":f1 [label="Sum"]
+"Yaw" -> "OF Offset Angle":f1 [label="Constant"]
 "Flow Vel X" -> "OF Offset Angle":f2 [label="Constant"]
 "Flow Vel Y" -> "OF Offset Angle":f3 [label="Constant"]
 "OF Integrate X"[shape=record

quad/src/quad_app/control_algorithm.c

@@ -13,10 +13,10 @@
 #include "util.h"
 #include "timer.h"
 
-#define USE_LIDAR
-#define USE_OF
-#define USE_GYRO_YAW
-#define NO_VRPN
+//#define USE_LIDAR
+//#define USE_OF
+//#define USE_GYRO_YAW
+//#define NO_VRPN
 
 //10 degrees
 #define ANGLE_CLAMP		(0.1745)
@@ -33,7 +33,7 @@ void connect_autonomous(parameter_t* ps) {
 	//graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_pos_pid, PID_CORRECTION);
 	graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->yaw_correction, ROT_OUT_X);
 	graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->yaw_correction, ROT_OUT_Y);
-	graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
+    graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_clamp, BOUNDS_OUT);
 #ifdef USE_OF
 	//graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, ADD_SUM);
 	graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
@@ -95,6 +95,9 @@ int control_algorithm_init(parameter_t * ps)
     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");
 
+    // Yaw clamp
+    ps->yaw_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "Yaw Rate Clamp");
+
     // Create blocks for VRPN data
     ps->vrpn_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN X");
     ps->vrpn_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Y");
@@ -168,7 +171,7 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->roll_pid, PID_DT, ps->angle_time, CONST_VAL);
 
     // 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_clamp, BOUNDS_IN, ps->yaw_pid, PID_CORRECTION);
     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);
 
@@ -362,6 +365,10 @@ int control_algorithm_init(parameter_t * ps)
     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 yaw clamping limits
+    graph_set_param_val(graph, ps->yaw_clamp, BOUNDS_MIN, -1.5);
+    graph_set_param_val(graph, ps->yaw_clamp, BOUNDS_MAX, 1.5);
+
     // Set trims
     graph_set_param_val(graph, ps->pitch_trim, CONST_SET, PITCH_TRIM);
 

quad/src/quad_app/type_def.h

@@ -392,6 +392,7 @@ typedef struct parameter_t {
 	int clamp_d_pwmP;
 	int clamp_d_pwmR;
 	int clamp_d_pwmY;
+	int yaw_clamp;
 	// Loop times
 	int angle_time;
 	int pos_time;