diff --git a/quad/src/gen_diagram/network.dot b/quad/src/gen_diagram/network.dot
index 132e3de85641396dd4810289a15803411824e3db..401acb652fdf093d6198c64d26ffa37061e16f49 100644
--- a/quad/src/gen_diagram/network.dot
+++ b/quad/src/gen_diagram/network.dot
@@ -3,16 +3,16 @@ rankdir="LR"
"Roll PID"[shape=record
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"]
-"Yaw Correction" -> "Roll PID":f2 [label="Rotated Y"]
+"RC Roll" -> "Roll PID":f2 [label="Constant"]
"Ts_IMU" -> "Roll PID":f3 [label="Constant"]
"Pitch PID"[shape=record
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"]
-"Yaw Correction" -> "Pitch PID":f2 [label="Rotated X"]
+"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
"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]"]
-"Integrated gyro z" -> "Yaw PID":f1 [label="Integrated"]
+"PSI Sum" -> "Yaw PID":f1 [label="Sum"]
"Yaw Setpoint" -> "Yaw PID":f2 [label="Constant"]
"Ts_IMU" -> "Yaw PID":f3 [label="Constant"]
"Roll Rate PID"[shape=record
@@ -28,7 +28,7 @@ label="<f0>Pitch Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |
"Yaw Rate PID"[shape=record
label="<f0>Yaw Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.297] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
"Gyro Z" -> "Yaw Rate PID":f1 [label="Constant"]
-"Yaw PID" -> "Yaw Rate PID":f2 [label="Correction"]
+"RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
"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]"]
@@ -71,6 +71,10 @@ label="<f0>Lidar |<f1> [Constant=0.000]"]
label="<f0>Flow Vel X |<f1> [Constant=0.000]"]
"Flow Vel Y"[shape=record
label="<f0>Flow Vel Y |<f1> [Constant=0.000]"]
+"Flow Vel X Filt"[shape=record
+label="<f0>Flow Vel X Filt |<f1> [Constant=0.000]"]
+"Flow Vel Y Filt"[shape=record
+label="<f0>Flow Vel Y Filt |<f1> [Constant=0.000]"]
"Flow Quality"[shape=record
label="<f0>Flow Quality |<f1> [Constant=0.000]"]
"Flow Distance"[shape=record
@@ -116,26 +120,32 @@ 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 Clamp" -> "X Vel PID":f2 [label="Bounded"]
+"Ts_IMU" -> "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 Clamp" -> "Y Vel PID":f2 [label="Bounded"]
+"Ts_IMU" -> "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]"]
"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]"]
"X Vel Clamp"[shape=record
-label="<f0>X Vel Clamp |<f1> --\>Bounds in |<f2> [Min=-0.174] |<f3> [Max=0.174]"]
+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"]
"Y vel Clamp"[shape=record
-label="<f0>Y vel Clamp |<f1> --\>Bounds in |<f2> [Min=-0.174] |<f3> [Max=0.174]"]
+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"]
-"Integrated gyro z" -> "Yaw Correction":f1 [label="Integrated"]
-"X Vel Clamp" -> "Yaw Correction":f2 [label="Bounded"]
-"Y vel Clamp" -> "Yaw Correction":f3 [label="Bounded"]
+"PSI Sum" -> "Yaw Correction":f1 [label="Sum"]
+"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"]
-"Integrated gyro z" -> "OF Offset Angle":f1 [label="Integrated"]
+"PSI Sum" -> "OF Offset Angle":f1 [label="Sum"]
"Flow Vel X" -> "OF Offset Angle":f2 [label="Constant"]
"Flow Vel Y" -> "OF Offset Angle":f3 [label="Constant"]
"OF Integrate X"[shape=record
@@ -158,12 +168,29 @@ label="<f0>OF X Trim Add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
label="<f0>OF Y Trim Add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
"OF Integrate Y" -> "OF Y Trim Add":f1 [label="Integrated"]
"OF Trim Y" -> "OF Y Trim Add":f2 [label="Constant"]
-"Integrated gyro z"[shape=record
-label="<f0>Integrated gyro z |<f1> --\>Integrator In |<f2> --\>Integrator dt"]
-"Gyro Z" -> "Integrated gyro z":f1 [label="Constant"]
+"PSI Dot"[shape=record
+label="<f0>PSI Dot |<f1> [Constant=0.000]"]
+"PSI Dot Offset"[shape=record
+label="<f0>PSI Dot Offset |<f1> [Constant=0.000]"]
+"PSI Dot Sum"[shape=record
+label="<f0>PSI Dot Sum |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
+"PSI Dot" -> "PSI Dot Sum":f1 [label="Constant"]
+"PSI Dot Offset" -> "PSI Dot Sum":f2 [label="Constant"]
+"PSI Angle"[shape=record
+label="<f0>PSI Angle |<f1> --\>Integrator In |<f2> --\>Integrator dt"]
+"PSI Dot Sum" -> "PSI Angle":f1 [label="Sum"]
+"Ts_IMU" -> "PSI Angle":f2 [label="Constant"]
+"PSI Angle Offset"[shape=record
+label="<f0>PSI Angle Offset |<f1> [Constant=0.000]"]
+"PSI Sum"[shape=record
+label="<f0>PSI Sum |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
+"PSI Angle" -> "PSI Sum":f1 [label="Integrated"]
+"PSI Angle Offset" -> "PSI Sum":f2 [label="Constant"]
+"Mag Yaw"[shape=record
+label="<f0>Mag Yaw |<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"]
+"RC Throttle" -> "Signal Mixer":f1 [label="Constant"]
"P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
"R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
"Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
diff --git a/quad/src/gen_diagram/network.png b/quad/src/gen_diagram/network.png
index d0c2ae5e41d0e3108c5c5480eb0748e18ffd2de0..b45f77b3e859babf1d2aa3e249d765ca6b43eb49 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/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 1d6fad4bf0a9d961a1f1b6f2215495a3d06dfa37..15c500d31c9917de9a671c901e2592d42171d5d7 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -15,8 +15,8 @@
#define USE_LIDAR
#define USE_OF
-#define USE_MAG_YAW
-#define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
+#define USE_GYRO_YAW
+#define NO_VRPN
//10 degrees
#define ANGLE_CLAMP (0.1745)
@@ -79,6 +79,8 @@ int control_algorithm_init(parameter_t * ps)
ps->lidar = graph_add_defined_block(graph, BLOCK_CONSTANT, "Lidar");
ps->flow_vel_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel X");
ps->flow_vel_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel Y");
+ ps->flow_vel_x_filt = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel X Filt");
+ ps->flow_vel_y_filt = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel Y Filt");
ps->flow_quality = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Quality");
// Sensor trims
ps->pitch_trim = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch trim");
@@ -124,8 +126,13 @@ int control_algorithm_init(parameter_t * ps)
ps->of_trimmed_x = graph_add_defined_block(graph, BLOCK_ADD, "OF X Trim Add");
ps->of_trimmed_y = graph_add_defined_block(graph, BLOCK_ADD, "OF Y Trim Add");
- // gyroscope z integrator
- ps->gyro_yaw = graph_add_defined_block(graph, BLOCK_INTEGRATOR, "Integrated gyro z");
+ // psi dot integrator
+ ps->psi_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "PSI Dot");
+ ps->psi_dot_offset = graph_add_defined_block(graph, BLOCK_CONSTANT, "PSI Dot Offset");
+ ps->psi_dot_sum = graph_add_defined_block(graph, BLOCK_ADD, "PSI Dot Sum");
+ ps->psi = graph_add_defined_block(graph, BLOCK_INTEGRATOR, "PSI Angle");
+ ps->psi_offset = graph_add_defined_block(graph, BLOCK_CONSTANT, "PSI Angle Offset");
+ ps->psi_sum = graph_add_defined_block(graph, BLOCK_ADD, "PSI Sum");
//Complementary yaw
ps->mag_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Mag Yaw");
@@ -156,7 +163,7 @@ int control_algorithm_init(parameter_t * ps)
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);USE_FAKE_YAW
+ //graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->vrpn_roll, CONST_VAL);
graph_set_source(graph, ps->roll_pid, PID_DT, ps->angle_time, CONST_VAL);
// Connect yaw PID chain
@@ -164,7 +171,13 @@ int control_algorithm_init(parameter_t * ps)
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);
- graph_set_source(graph, ps->gyro_yaw, INTEGRATE_IN, ps->gyro_z, CONST_VAL);
+ //PSI-dot integration chain
+ graph_set_source(graph, ps->psi_dot_sum, ADD_SUMMAND1, ps->psi_dot, CONST_VAL);
+ graph_set_source(graph, ps->psi_dot_sum, ADD_SUMMAND2, ps->psi_dot_offset, CONST_VAL);
+ graph_set_source(graph, ps->psi, INTEGRATE_IN, ps->psi_dot_sum, CONST_VAL);
+ graph_set_source(graph, ps->psi, INTEGRATE_DT, ps->angle_time, CONST_VAL);
+ graph_set_source(graph, ps->psi_sum, ADD_SUMMAND1, ps->psi, INTEGRATED);
+ graph_set_source(graph, ps->psi_sum, ADD_SUMMAND2, ps->psi_offset, CONST_VAL);
#ifndef USE_OF
// X velocity PID
@@ -176,19 +189,6 @@ int control_algorithm_init(parameter_t * ps)
// 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_CUR_POINT, ps->of_angle_corr, ROT_OUT_X);
- 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/Y global to local conversion
-#ifdef USE_MAG_YAW
- graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->mag_yaw, CONST_VAL);
-#else
- graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
-#endif
- graph_set_source(graph, ps->yaw_correction, ROT_CUR_X, ps->x_vel_pid, PID_CORRECTION);
- graph_set_source(graph, ps->yaw_correction, ROT_CUR_Y, ps->y_vel_pid, PID_CORRECTION);
-
// Y velocity PID
// Use a PID block to compute the derivative
@@ -199,47 +199,50 @@ int control_algorithm_init(parameter_t * ps)
// 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_CUR_POINT, ps->of_angle_corr, ROT_OUT_Y);
- 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_CUR_POINT, ps->of_trimmed_x, CONST_VAL);
- graph_set_source(graph, ps->x_pos_pid, PID_SETPOINT, ps->x_set, CONST_VAL);
+
// 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_CUR_POINT, ps->of_trimmed_y, CONST_VAL);
- graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
-#endif
-
-#ifdef USE_OF
+#else
// X position
graph_set_source(graph, ps->x_pos_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->of_trimmed_x, ADD_SUM);
- //graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->of_trimmed_x, CONST_VAL);
- graph_set_source(graph, ps->x_pos_pid, PID_SETPOINT, ps->x_set, CONST_VAL);
+
// Y position
graph_set_source(graph, ps->y_pos_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->of_trimmed_y, ADD_SUM);
- //graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->of_trimmed_y, CONST_VAL);
- graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
- 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->x_vel_pid, PID_DT, ps->angle_time, CONST_VAL);
+ graph_set_source(graph, ps->x_vel_pid, PID_CUR_POINT, ps->flow_vel_x_filt, PID_CORRECTION);
+
+ graph_set_source(graph, ps->y_vel_pid, PID_DT, ps->angle_time, CONST_VAL);
+ graph_set_source(graph, ps->y_vel_pid, PID_CUR_POINT, ps->flow_vel_y_filt, PID_CORRECTION);
+#endif
+
+ 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/Y global to local conversion
-#ifdef USE_MAG_YAW
- graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->mag_yaw, CONST_VAL);
+#ifdef USE_GYRO_YAW
+ graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->psi_sum, ADD_SUM);
#else
graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
#endif
- graph_set_source(graph, ps->yaw_correction, ROT_CUR_X, ps->x_vel_clamp, PID_CORRECTION);
- graph_set_source(graph, ps->yaw_correction, ROT_CUR_Y, ps->y_vel_clamp, PID_CORRECTION);
-#endif
+ graph_set_source(graph, ps->yaw_correction, ROT_CUR_X, ps->x_vel_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->yaw_correction, ROT_CUR_Y, ps->y_vel_pid, 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);
+
+ graph_set_source(graph, ps->x_pos_pid, PID_SETPOINT, ps->x_set, CONST_VAL);
+
+ //graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->of_trimmed_y, CONST_VAL);
+ graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
// Alt autonomous
#ifdef USE_LIDAR
@@ -253,9 +256,9 @@ int control_algorithm_init(parameter_t * ps)
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 angle
-#ifdef USE_MAG_YAW
+#ifdef USE_GYRO_YAW
graph_set_source(graph, ps->yaw_pid, PID_DT, ps->angle_time, CONST_VAL);
- graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->mag_yaw, CONST_VAL);
+ graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->psi_sum, ADD_SUM);
#else
graph_set_source(graph, ps->yaw_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->cur_yaw, CONST_VAL);
@@ -274,8 +277,8 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT);
// Connect optical flow
-#ifdef USE_MAG_YAW
- graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->mag_yaw, ADD_SUM);
+#ifdef USE_GYRO_YAW
+ graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->psi_sum, ADD_SUM);
#else
graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->cur_yaw, ADD_SUM);
#endif
@@ -352,19 +355,11 @@ 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);
-#ifdef USE_OF
- //Set angle clamping limits
- graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MIN, -ANGLE_CLAMP);
- graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MAX, ANGLE_CLAMP);
- graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MIN, -ANGLE_CLAMP);
- graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MAX, ANGLE_CLAMP);
-#else
// 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);
-#endif
// Set trims
graph_set_param_val(graph, ps->pitch_trim, CONST_SET, PITCH_TRIM);
@@ -398,7 +393,11 @@ int control_algorithm_init(parameter_t * ps)
// flap switch was just toggled to auto flight mode
if((last_fm_switch != cur_fm_switch) && (cur_fm_switch == AUTO_FLIGHT_MODE))
{
+#ifdef NO_VRPN
+ user_defined_struct->engaging_auto = 2;
+#else
user_defined_struct->engaging_auto = 1;
+#endif
graph_set_param_val(graph, ps->throttle_trim, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
}
@@ -470,17 +469,15 @@ int control_algorithm_init(parameter_t * ps)
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);
- //if (fabs(sensor_struct->lidar_altitude) <= MAX_VALID_LIDAR) {
- graph_set_param_val(graph, ps->lidar, CONST_SET, sensor_struct->lidar_altitude);
- //}
+ graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->psi_dot);
+ graph_set_param_val(graph, ps->lidar, CONST_SET, sensor_struct->lidar_altitude);
graph_set_param_val(graph, ps->flow_quality, CONST_SET, sensor_struct->optical_flow.quality);
- //As per documentation, disregard frames with low quality, as they contain unreliable data
- //NOTE: As per the Wehrneyton(R) Method(TM), we will be exponentially decaying the
- //optical flow velocities when the quality is below the threshold
- //if (sensor_struct->optical_flow.quality >= PX4FLOW_QUAL_MIN) {
- graph_set_param_val(graph, ps->flow_vel_x, CONST_SET, sensor_struct->optical_flow.xVel);
- graph_set_param_val(graph, ps->flow_vel_y, CONST_SET, sensor_struct->optical_flow.yVel);
- //}
+
+ //Optical flow
+ graph_set_param_val(graph, ps->flow_vel_x, CONST_SET, sensor_struct->optical_flow.xVel);
+ graph_set_param_val(graph, ps->flow_vel_y, CONST_SET, sensor_struct->optical_flow.yVel);
+ graph_set_param_val(graph, ps->flow_vel_x_filt, CONST_SET, sensor_struct->optical_flow.xVelFilt);
+ graph_set_param_val(graph, ps->flow_vel_y_filt, CONST_SET, sensor_struct->optical_flow.yVelFilt);
// 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/log_data.c b/quad/src/quad_app/log_data.c
index 94566042e3ddc79352fe1d2ba89b49780ceacfa0..7079987a54d2288970d34af671f41d8b06e2cafd 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -118,7 +118,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
addOutputToLog(log_struct, ps->of_angle_corr, ROT_OUT_Y, m_s);
addOutputToLog(log_struct, ps->of_integ_x, INTEGRATED, m);
addOutputToLog(log_struct, ps->of_integ_y, INTEGRATED, m);
- addOutputToLog(log_struct, ps->gyro_yaw, INTEGRATED, rad);
+ addOutputToLog(log_struct, ps->psi_sum, ADD_SUM, rad);
addOutputToLog(log_struct, ps->mag_yaw, CONST_VAL, rad);
addParamToLog(log_struct, ps->cur_roll, CONST_VAL, rad);
@@ -146,7 +146,6 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
}
}
-
int log_data(log_t* log_struct, parameter_t* ps)
{
if(arrayIndex >= arraySize)
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 219a6bea3ef5af6526bfe5b14f4d8db0c5e8d2d8..4996c9117c605b9ee504d1e01d18a64b315a1e7d 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -23,6 +23,8 @@
#define GYRO_Z_OFFSET (0.0073)
+#define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
+
int sensor_processing_init(sensor_t* sensor_struct) {
float a0 = 0.0200833310260;
float a1 = 0.0401666620520;
@@ -32,11 +34,13 @@ int sensor_processing_init(sensor_t* sensor_struct) {
sensor_struct->accel_x_filt = filter_make_state(a0, a1, a2, b1, b2);
sensor_struct->accel_y_filt = filter_make_state(a0, a1, a2, b1, b2);
sensor_struct->accel_z_filt = filter_make_state(a0, a1, a2, b1, b2);
- float vel_a0 = 0.0098;
- float vel_a1 = 0.0195;
- float vel_a2 = 0.0098;
- float vel_b1 = -1.5816;
- float vel_b2 = 0.6853;
+
+ //1 Hert filter
+ float vel_a0 = 2.3921e-4;
+ float vel_a1 = 4.7841e-4;
+ float vel_a2 = 2.3921e-4;
+ float vel_b1 = -1.9381;
+ float vel_b2 = 0.9391;
sensor_struct->flow_x_filt = filter_make_state(vel_a0, vel_a1, vel_a2, vel_b1, vel_b2);
sensor_struct->flow_y_filt = filter_make_state(vel_a0, vel_a1, vel_a2, vel_b1, vel_b2);
@@ -209,6 +213,10 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
//sensor_struct->optical_flow.xVel *= -1;
//sensor_struct->optical_flow.yVel *= -1;
+ //Filter OF velocities
+ sensor_struct->optical_flow.xVelFilt = biquad_execute(&sensor_struct->flow_x_filt, sensor_struct->optical_flow.xVel);
+ sensor_struct->optical_flow.yVelFilt = biquad_execute(&sensor_struct->flow_y_filt, sensor_struct->optical_flow.yVel);
+
/*
* Altitude double complementary filter
*/
@@ -218,6 +226,10 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
static float last_lidar = 0;
float this_lidar = -raw_sensor_struct->lidar_distance_m;
+ if(this_lidar < (-MAX_VALID_LIDAR)) {
+ this_lidar = filtered_alt;
+ }
+
// Acceleration in m/s without gravity
float quad_z_accel = 9.8 * (accel_z + 1);
filtered_vel = alt_alpha*(filtered_vel + quad_z_accel*get_last_loop_time()) +
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index 280da1a58e12c09f32b72e23f9b217588b3479f5..cef24608aac3b2ca8095f570ba33f2e7a8df6c5b 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -132,6 +132,8 @@ typedef struct px4flow {
// Time since last readout in seconds
double dt;
+ double xVelFilt, yVelFilt;
+
int16_t quality;
SensorError_t error;
@@ -370,6 +372,8 @@ typedef struct parameter_t {
int lidar;
int flow_vel_x; // optical flow
int flow_vel_y;
+ int flow_vel_x_filt;
+ int flow_vel_y_filt;
int flow_quality; // Quality value returned by optical flow sensor
int flow_distance;
// VRPN blocks
@@ -419,7 +423,13 @@ typedef struct parameter_t {
int of_trim_y;
int of_trimmed_x; // Trimmed optical flow integrated value (of_integ_x + of_trim_x)
int of_trimmed_y;
- int gyro_yaw; // Integrates the gyro z value
+ //psi dot integration chain
+ int psi_dot;
+ int psi_dot_offset;
+ int psi_dot_sum;
+ int psi;
+ int psi_offset;
+ int psi_sum;
int mag_yaw; //Complementary filtered magnetometer/gyro yaw
} parameter_t;
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
index c7f2bfff3b7751ebf40346d52431eca8e15352f3..31b68f49d66c93896cb02f9f17afa4fe37f4cb34 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
@@ -46,7 +46,7 @@
#define GYRO_X_BIAS 0.005f
#define GYRO_Y_BIAS -0.014f
-#define GYRO_Z_BIAS 0.0614//0.0541f
+#define GYRO_Z_BIAS 0.0534//0.0541f
#define ACCEL_X_BIAS 0.023f
#define ACCEL_Y_BIAS 0.009f