diff --git a/quad/src/gen_diagram/network.dot b/quad/src/gen_diagram/network.dot
index 07cb977d796610ef9c22de4c76891b48fee1514f..c7ebd3547075c6e2210eef8a3855bb61659a84c3 100644
--- a/quad/src/gen_diagram/network.dot
+++ b/quad/src/gen_diagram/network.dot
@@ -3,18 +3,18 @@ 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]"]
-"Yaw" -> "Yaw PID":f1 [label="Constant"]
+"Integrated gyro z" -> "Yaw PID":f1 [label="Integrated"]
"Yaw Setpoint" -> "Yaw PID":f2 [label="Constant"]
-"Ts_VRPN" -> "Yaw PID":f3 [label="Constant"]
+"Ts_IMU" -> "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"]
@@ -28,23 +28,23 @@ 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]"]
-"VRPN X" -> "X pos PID":f1 [label="Constant"]
+"OF X Trim Add" -> "X pos PID":f1 [label="Sum"]
"X Setpoint" -> "X pos PID":f2 [label="Constant"]
-"Ts_VRPN" -> "X pos PID":f3 [label="Constant"]
+"Ts_IMU" -> "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]"]
-"VRPN Y" -> "Y pos PID":f1 [label="Constant"]
+"OF Y Trim Add" -> "Y pos PID":f1 [label="Sum"]
"Y Setpoint" -> "Y pos PID":f2 [label="Constant"]
-"Ts_VRPN" -> "Y pos PID":f3 [label="Constant"]
+"Ts_IMU" -> "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]"]
-"VRPN Alt" -> "Altitude PID":f1 [label="Constant"]
+"Lidar" -> "Altitude PID":f1 [label="Constant"]
"Alt Setpoint" -> "Altitude PID":f2 [label="Constant"]
-"Ts_VRPN" -> "Altitude PID":f3 [label="Constant"]
+"Ts_IMU" -> "Altitude PID":f3 [label="Constant"]
"X Setpoint"[shape=record
label="<f0>X Setpoint |<f1> [Constant=0.000]"]
"Y Setpoint"[shape=record
@@ -116,35 +116,23 @@ 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]"]
-"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.100] |<f5> [Ki=0.000] |<f6> [Kd=0.020] |<f7> [alpha=0.000]"]
-"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.880]"]
-"VRPN X" -> "X Vel":f1 [label="Constant"]
-"zero" -> "X Vel":f2 [label="Constant"]
-"Ts_VRPN" -> "X Vel":f3 [label="Constant"]
+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=-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"]
+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=-2.000] |<f3> [Max=2.000]"]
+label="<f0>X Vel Clamp |<f1> --\>Bounds in |<f2> [Min=-0.174] |<f3> [Max=0.174]"]
"X pos PID" -> "X Vel Clamp":f1 [label="Correction"]
"Y vel Clamp"[shape=record
-label="<f0>Y vel Clamp |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]"]
+label="<f0>Y vel Clamp |<f1> --\>Bounds in |<f2> [Min=-0.174] |<f3> [Max=0.174]"]
"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"]
-"Yaw" -> "Yaw Correction":f1 [label="Constant"]
-"X Vel PID" -> "Yaw Correction":f2 [label="Correction"]
-"Y Vel PID" -> "Yaw Correction":f3 [label="Correction"]
+"Integrated gyro z" -> "Yaw Correction":f1 [label="Integrated"]
+"X Vel Clamp" -> "Yaw Correction":f2 [label="Bounded"]
+"Y vel Clamp" -> "Yaw Correction":f3 [label="Bounded"]
"OF Offset Angle"[shape=record
label="<f0>OF Offset Angle |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>Y Position"]
"OF Offset Add" -> "OF Offset Angle":f1 [label="Sum"]
@@ -155,7 +143,7 @@ label="<f0>OF Offset Rot |<f1> [Constant=0.622]"]
"OF Offset Add"[shape=record
label="<f0>OF Offset Add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
"OF Offset Rot" -> "OF Offset Add":f1 [label="Constant"]
-"Yaw" -> "OF Offset Add":f2 [label="Constant"]
+"Integrated gyro z" -> "OF Offset Add":f2 [label="Integrated"]
"OF Integrate X"[shape=record
label="<f0>OF Integrate X |<f1> --\>Integrator In |<f2> --\>Integrator dt"]
"OF Offset Angle" -> "OF Integrate X":f1 [label="Rotated X"]
@@ -176,9 +164,12 @@ 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"]
"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 f13041740aa8c01eec3c0d83b8ceaab251505c22..8a85dddca8320d59cc742c69825d449673b3577f 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 624c0642abfd47bc4de3cc586d9226dca28fa2a6..6f1f86a2f4e97fc8434e6ead0058ecb2d7d183ba 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -13,9 +13,14 @@
#include "util.h"
#include "timer.h"
-//#define USE_LIDAR
+#define USE_LIDAR
+#define USE_OF
+#define USE_FAKE_YAW
#define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
+//10 degrees
+#define ANGLE_CLAMP (0.1745)
+
#define PX4FLOW_QUAL_MIN (100)
#define OF_OFFSET_ANGLE (0.62204) // 35.64 degrees
@@ -29,8 +34,13 @@ 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->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
+#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);
+#else
+ graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
+#endif
}
void connect_manual(parameter_t* ps) {
@@ -118,6 +128,9 @@ 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");
+
ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
ps->angle_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_IMU");
@@ -152,6 +165,9 @@ 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);
+
+#ifndef USE_OF
// X velocity PID
// Use a PID block to compute the derivative
graph_set_param_val(graph, ps->x_vel, PID_KD, -1);
@@ -166,7 +182,11 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
// X/Y global to local conversion
+#ifdef USE_FAKE_YAW
+ graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->gyro_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);
@@ -194,6 +214,33 @@ int control_algorithm_init(parameter_t * ps)
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
+ // 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);
+
+
+ // X/Y global to local conversion
+#ifdef USE_FAKE_YAW
+ graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->gyro_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_clamp, PID_CORRECTION);
+ graph_set_source(graph, ps->yaw_correction, ROT_CUR_Y, ps->y_vel_clamp, PID_CORRECTION);
+#endif
// Alt autonomous
#ifdef USE_LIDAR
@@ -207,9 +254,14 @@ 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_FAKE_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->gyro_yaw, CONST_VAL);
+#else
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);
+#endif
+ graph_set_source(graph, ps->yaw_pid, PID_SETPOINT, ps->yaw_set, CONST_VAL);
// Connect PWM clamping blocks
graph_set_source(graph, ps->clamp_d_pwmP, BOUNDS_IN, ps->pitch_r_pid, PID_CORRECTION);
@@ -224,7 +276,11 @@ int control_algorithm_init(parameter_t * ps)
// Connect optical flow
graph_set_source(graph, ps->of_angle_add, ADD_SUMMAND1, ps->of_angle_offset, CONST_VAL);
+#ifdef USE_FAKE_YAW
+ graph_set_source(graph, ps->of_angle_add, ADD_SUMMAND2, ps->gyro_yaw, CONST_VAL);
+#else
graph_set_source(graph, ps->of_angle_add, ADD_SUMMAND2, ps->cur_yaw, CONST_VAL);
+#endif
graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->of_angle_add, ADD_SUM);
graph_set_source(graph, ps->of_angle_corr, ROT_CUR_X, ps->flow_vel_x, CONST_VAL);
graph_set_source(graph, ps->of_angle_corr, ROT_CUR_Y, ps->flow_vel_y, CONST_VAL);
@@ -299,11 +355,19 @@ 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);
@@ -354,10 +418,17 @@ int control_algorithm_init(parameter_t * ps)
// 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))
{
+#ifdef USE_OF
+ graph_set_param_val(graph, ps->x_set, CONST_SET, graph_get_output(graph, ps->of_trimmed_x, ADD_SUM));
+ graph_set_param_val(graph, ps->y_set, CONST_SET, graph_get_output(graph, ps->of_trimmed_y, ADD_SUM));
+ //graph_set_param_val(graph, ps->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
+ graph_set_param_val(graph, ps->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
+#else
graph_set_param_val(graph, ps->x_set, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
graph_set_param_val(graph, ps->y_set, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
graph_set_param_val(graph, ps->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
graph_set_param_val(graph, ps->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
+#endif
// reset the flag that engages auto mode
user_defined_struct->engaging_auto = 0;
@@ -407,10 +478,12 @@ int control_algorithm_init(parameter_t * ps)
//}
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
- if (sensor_struct->optical_flow.quality >= PX4FLOW_QUAL_MIN) {
+ //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);
- }
+ //}
graph_set_param_val(graph, ps->flow_distance, CONST_SET, sensor_struct->optical_flow.distance);
// RC values
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 2a29ddb8f798a1e9a80dfe6c879e196358621f67..dadb8194d29a6c6c677f4aea810ae9cfed1e32a2 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -118,11 +118,10 @@ 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);
addParamToLog(log_struct, ps->cur_roll, CONST_VAL, rad);
addParamToLog(log_struct, ps->cur_yaw, CONST_VAL, rad);
- addParamToLog(log_struct, ps->vrpn_pitch, CONST_VAL, rad);
- addParamToLog(log_struct, ps->vrpn_roll, CONST_VAL, rad);
addParamToLog(log_struct, ps->x_set, CONST_VAL, m);
addParamToLog(log_struct, ps->y_set, CONST_VAL, m);
addParamToLog(log_struct, ps->alt_set, CONST_VAL, m);
@@ -134,10 +133,6 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
addParamToLog(log_struct, ps->flow_vel_x, CONST_VAL, m_s);
addParamToLog(log_struct, ps->flow_vel_y, CONST_VAL, m_s);
addParamToLog(log_struct, ps->flow_quality, CONST_VAL, "none");
- addParamToLog(log_struct, ps->flow_distance, CONST_VAL, m);
- addParamToLog(log_struct, ps->rc_throttle, CONST_VAL, pwm_val);
- addParamToLog(log_struct, ps->rc_pitch, CONST_VAL, pwm_val);
- addParamToLog(log_struct, ps->rc_roll, CONST_VAL, pwm_val);
// TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
row_size = n_outputs + n_params + 6 + 1;
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index cdd5a8c347895ed6c557f37b4e72659a1af39ab3..59511fa9d349d6b274d5d5c32f183b85f05e8417 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -14,6 +14,8 @@
#include <math.h>
#define ALPHA 0.99
+#define PX4FLOW_QUAL_MIN (100)
+#define PX4FLOW_VEL_DECAY (0.99)
int sensor_processing_init(sensor_t* sensor_struct) {
float a0 = 0.0200833310260;
@@ -43,8 +45,14 @@ static float focal_length_px = 16.0 / (4.0f * 6.0f) * 1000.0f; //original focal
void flow_to_vel(px4flow_t* flow_data, double distance) {
double loop_time = get_last_loop_time();
if (loop_time != 0) {
- flow_data->xVel = distance * flow_data->flowX / focal_length_px / get_last_loop_time();
- flow_data->yVel = distance * flow_data->flowY / focal_length_px / get_last_loop_time();
+ if(flow_data->quality > PX4FLOW_QUAL_MIN) {
+ flow_data->xVel = distance * flow_data->flowX / focal_length_px / loop_time;
+ flow_data->yVel = distance * flow_data->flowY / focal_length_px / loop_time;
+ }
+ else {
+ flow_data->xVel *= PX4FLOW_VEL_DECAY;
+ flow_data->yVel *= PX4FLOW_VEL_DECAY;
+ }
}
}
@@ -124,8 +132,9 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
flow_to_vel(&sensor_struct->optical_flow, raw_sensor_struct->lidar_distance_m);
// sensor_struct->optical_flow.xVel = biquad_execute(&sensor_struct->flow_x_filt, -sensor_struct->optical_flow.xVel);
// sensor_struct->optical_flow.yVel = biquad_execute(&sensor_struct->flow_y_filt, -sensor_struct->optical_flow.yVel);
- sensor_struct->optical_flow.xVel *= -1;
- sensor_struct->optical_flow.yVel *= -1;
+ //Note: This was wrong and dumb
+ //sensor_struct->optical_flow.xVel *= -1;
+ //sensor_struct->optical_flow.yVel *= -1;
/*
* Altitude double complementary filter
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index d5bd1d45e240dd71e06ac784fc4c75e8b4e406d0..9c3e651305954b6a971485c1f24ad76f1acb9ff6 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -412,6 +412,7 @@ 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
} 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 da9ed7ffc6346f652d13391ae6acfd738171892b..bf94f23894fcebfc6b76d97b49b9e4d869426226 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
@@ -44,7 +44,7 @@
#define GYRO_X_BIAS 0.005f
#define GYRO_Y_BIAS -0.014f
-#define GYRO_Z_BIAS 0.045f
+#define GYRO_Z_BIAS 0.0541f
#define ACCEL_X_BIAS 0.023f
#define ACCEL_Y_BIAS 0.009f