Using optical flow for autonomous flight. Filtering velocity at 15hz.

quad/src/computation_graph/computation_graph.c

@@ -34,7 +34,7 @@ static void reset_node_rec(struct computation_graph* graph, int node_id, int dep
     struct graph_node* node = &graph->nodes[node_id];
     // Don't reset nodes that are already connected to something else
     // Don't reset nodes that have already been reset or discovered
-    if (node->n_children != 1 || node->processed_state != UNPROCESSED) {
+    if (node->processed_state != UNPROCESSED) {
         return;
     }
     node->processed_state = DISCOVERED;

quad/src/gen_diagram/network.dot

@@ -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=3000.000] |<f5> [Ki=0.000] |<f6> [Kd=500.000] |<f7> [alpha=0.880]"]
+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"]
 "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=3000.000] |<f5> [Ki=0.000] |<f6> [Kd=500.000] |<f7> [alpha=0.880]"]
+label="<f0>Pitch 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 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]"]
+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"]
 "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=-9804.000] |<f5> [Ki=-817.000] |<f6> [Kd=-7353.000] |<f7> [alpha=0.880]"]
+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"]
 "Alt Setpoint" -> "Altitude PID":f2 [label="Constant"]
 "Ts_VRPN" -> "Altitude PID":f3 [label="Constant"]
@@ -116,14 +116,14 @@ 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"]
+"OF Offset Angle" -> "X Vel PID":f1 [label="Rotated X"]
 "X Vel Clamp" -> "X Vel PID":f2 [label="Bounded"]
-"Ts_VRPN" -> "X Vel PID":f3 [label="Constant"]
+"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]"]
-"Y Vel" -> "Y Vel PID":f1 [label="Correction"]
+"OF Offset Angle" -> "Y Vel PID":f1 [label="Rotated Y"]
 "Y vel Clamp" -> "Y Vel PID":f2 [label="Bounded"]
-"Ts_VRPN" -> "Y Vel PID":f3 [label="Constant"]
+"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=-1.000] |<f7> [alpha=0.880]"]
 "VRPN X" -> "X Vel":f1 [label="Constant"]
@@ -151,7 +151,7 @@ label="<f0>OF Offset Angle  |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\
 "Flow Vel X" -> "OF Offset Angle":f2 [label="Constant"]
 "Flow Vel Y" -> "OF Offset Angle":f3 [label="Constant"]
 "OF Offset Rot"[shape=record
-label="<f0>OF Offset Rot  |<f1> [Constant=-0.622]"]
+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"]

quad/src/quad_app/control_algorithm.c

@@ -159,8 +159,8 @@ int control_algorithm_init(parameter_t * ps)
     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_DT, ps->angle_time, CONST_VAL);
+    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);
 
@@ -177,18 +177,18 @@ int control_algorithm_init(parameter_t * ps)
     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_DT, ps->angle_time, CONST_VAL);
+    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_DT, ps->angle_time, 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_DT, ps->angle_time, 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
@@ -350,8 +350,8 @@ 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))
 	{
-		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->x_set, CONST_SET, 0);
+		graph_set_param_val(graph, ps->y_set, CONST_SET, 0);
 		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);
 
@@ -398,9 +398,9 @@ 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) {
+    //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->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) {

quad/src/quad_app/sensor_processing.c

@@ -25,6 +25,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.04125344;
+	float vel_a1 = 0.08250688;
+	float vel_a2 = 0.04125344;
+	float vel_b1 = -1.3489646;
+	float vel_b2 = 0.51397836;
+	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);
 	return 0;
 }
 
@@ -101,8 +108,9 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 
 	// Simply copy optical flow data
 	sensor_struct->optical_flow = raw_sensor_struct->optical_flow;
-	sensor_struct->optical_flow.xVel = -sensor_struct->optical_flow.xVel;
-	sensor_struct->optical_flow.yVel = -sensor_struct->optical_flow.yVel;
+
+	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);
 	return 0;
 }
 

quad/src/quad_app/type_def.h

@@ -301,6 +301,8 @@ typedef struct sensor {
 	struct biquadState accel_x_filt;
 	struct biquadState accel_y_filt;
 	struct biquadState accel_z_filt;
+	struct biquadState flow_x_filt;
+	struct biquadState flow_y_filt;
 
 	// Information obtained from optical flow sensor 
 	px4flow_t optical_flow;

quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c

@@ -283,7 +283,8 @@ int XIicPs_MasterRecvPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
 			((IntrStatusReg & Intrs) == 0) && !(IntrStatusReg & XIICPS_IXR_COMP_MASK)) {
 /* <--- MicroCART additions (iic watchdog timer hack) ---> */
 		u64 usec_passed = timer_get_count() - start_time;
-		if (usec_passed > max_usec_per_byte * (ByteCount - InstancePtr->RecvByteCount)) {
+		// Add 1 so it has a chance to read
+		if (usec_passed > max_usec_per_byte * (1 + ByteCount - InstancePtr->RecvByteCount)) {
 			return IIC_RX_TIMEOUT_FAILURE;
 		}
 /* <--- End hack ---> */