Added rotation correction for optical flow. Uses both the static offset and the quad's yaw.

quad/src/gen_diagram/network.dot

@@ -3,12 +3,12 @@ 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]"]
@@ -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=29700.000] |<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]"]
@@ -69,10 +69,12 @@ label="<f0>Yaw  |<f1> [Constant=0.000]"]
 label="<f0>Lidar  |<f1> [Constant=0.000]"]
 "Flow Vel X"[shape=record
 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 Y"[shape=record
+label="<f0>Flow Vel Y  |<f1> [Constant=0.000]"]
 "Flow Quality"[shape=record
 label="<f0>Flow Quality  |<f1> [Constant=0.000]"]
+"Flow Distance"[shape=record
+label="<f0>Flow Distance  |<f1> [Constant=0.000]"]
 "Pitch trim"[shape=record
 label="<f0>Pitch trim  |<f1> [Constant=0.045]"]
 "Pitch trim add"[shape=record
@@ -143,9 +145,20 @@ label="<f0>Yaw Correction  |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>
 "Yaw" -> "Yaw Correction":f1 [label="Constant"]
 "X Vel PID" -> "Yaw Correction":f2 [label="Correction"]
 "Y Vel PID" -> "Yaw Correction":f3 [label="Correction"]
+"OF Offset"[shape=record
+label="<f0>OF Offset  |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>Y Position"]
+"OF Offset Add" -> "OF Offset":f1 [label="Sum"]
+"Flow Vel X" -> "OF Offset":f2 [label="Constant"]
+"Flow Vel Y" -> "OF Offset":f3 [label="Constant"]
+"OF Offset Rot"[shape=record
+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"]
 "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"]

quad/src/quad_app/control_algorithm.c

@@ -16,6 +16,7 @@
 //#define USE_LIDAR
 
 #define PX4FLOW_QUAL_MIN			(100)
+#define OF_OFFSET_ANGLE             (-0.62204) // -35.64 degrees
 
 #define ROLL_PITCH_MAX_ANGLE 0.35 // 20 degrees
 #define PWM_DIFF_BOUNDS 20000
@@ -105,6 +106,11 @@ int control_algorithm_init(parameter_t * ps)
         // Converts global X/Y to local X/Y
     ps->yaw_correction = graph_add_defined_block(graph, BLOCK_YAW_ROT, "Yaw Correction");
 
+    // Optical Flow
+    ps->of_angle_corr = graph_add_defined_block(graph, BLOCK_YAW_ROT, "OF Offset");
+    ps->of_angle_offset = graph_add_defined_block(graph, BLOCK_CONSTANT, "OF Offset Rot");
+    ps->of_angle_add = graph_add_defined_block(graph, BLOCK_ADD, "OF Offset Add");
+
     ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
 
     ps->angle_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_IMU");
@@ -205,6 +211,14 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->mixer, MIXER_ROLL, ps->clamp_d_pwmR, BOUNDS_OUT);
     graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT);
 
+    // Connect optical flow
+    graph_set_source(graph, ps->of_angle_add, ADD_SUMMAND1, ps->of_angle_offset, CONST_VAL);
+    graph_set_source(graph, ps->of_angle_add, ADD_SUMMAND2, ps->cur_yaw, CONST_VAL);
+    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);
+
+
     // Set pitch PID constants
     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);
@@ -272,6 +286,7 @@ int control_algorithm_init(parameter_t * ps)
 
     // Set trims
     graph_set_param_val(graph, ps->pitch_trim, CONST_SET, PITCH_TRIM);
+    graph_set_param_val(graph, ps->of_angle_offset, CONST_SET, OF_OFFSET_ANGLE);
 
     // Initial value for sampling periods
     graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.04);

quad/src/quad_app/log_data.c

@@ -114,6 +114,8 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 	addOutputToLog(log_struct, ps->y_vel_pid, PID_CORRECTION, rad);
 	addOutputToLog(log_struct, ps->x_vel, PID_CORRECTION, m_s);
 	addOutputToLog(log_struct, ps->y_vel, PID_CORRECTION, m_s);
+	addOutputToLog(log_struct, ps->of_angle_corr, ROT_OUT_X, m_s);
+	addOutputToLog(log_struct, ps->of_angle_corr, ROT_OUT_Y, m_s);
 
 	addParamToLog(log_struct, ps->cur_roll, CONST_VAL, rad);
 	addParamToLog(log_struct, ps->cur_yaw, CONST_VAL, rad);

quad/src/quad_app/type_def.h

@@ -385,6 +385,9 @@ typedef struct parameter_t {
 	int vel_y_gain;
 	// Sensor processing
 	int yaw_correction;
+	int of_angle_corr; // Corrects for the optical flow mounting angle
+	int of_angle_offset; // Offset for optical flow angle
+	int of_angle_add; // Adds optical flow static offset to current yaw
 } parameter_t;
 
 /**