Optical flow data is being polled, part of the graph, and being logged now. It...

Optical flow data is being polled, part of the graph, and being logged now. It is not used for flight.

quad/src/quad_app/control_algorithm.c

@@ -15,6 +15,8 @@
 
 //#define USE_LIDAR
 
+#define PX4FLOW_QUAL_MIN			(100)
+
 #define ROLL_PITCH_MAX_ANGLE 0.35 // 20 degrees
 #define PWM_DIFF_BOUNDS 20000
 #define VRPN_REFRESH_TIME 0.01f // 10ms
@@ -352,6 +354,7 @@ int control_algorithm_init(parameter_t * ps)
 	    graph_set_param_val(graph, ps->pos_time, CONST_SET, VRPN_REFRESH_TIME * (current_vrpn_id - last_vrpn_id));
 	    last_vrpn_id = current_vrpn_id;
 	}
+    
 
 	// Loop time
     graph_set_param_val(graph, ps->angle_time, CONST_SET, get_last_loop_time());
@@ -363,6 +366,12 @@ int control_algorithm_init(parameter_t * ps)
     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);
     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) {
+        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);
+    }
 
     // RC values
     graph_set_param_val(graph, ps->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
@@ -371,8 +380,9 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_param_val(graph, ps->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
 
     // Compute VRPN blocks so they can be logged
-    int outputs[5] = {ps->mixer, ps->vrpn_alt, ps->vrpn_pitch, ps->vrpn_roll, ps->lidar};
-    graph_compute_nodes(graph, outputs, 5);
+    int outputs[8] = {ps->mixer, ps->vrpn_alt, ps->vrpn_pitch, ps->vrpn_roll, ps->lidar,
+                      ps->flow_quality, ps->flow_vel_x, ps->flow_vel_y};
+    graph_compute_nodes(graph, outputs, 8);
 
 	 // here for now so in case any flight command is not PID controlled, it will default to rc_command value:
 	//memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);

quad/src/quad_app/iic_utils.c

@@ -313,7 +313,6 @@ int iic0_px4flow_update(px4flow_t *of) {
 		//Copy into struct
 		memcpy(&i2cFrame, buf, sizeof(i2cFrame));
 
-		//As per documentation, disregard frames with low quality, as they contain unreliable data
 		of->xVel = i2cFrame.flowCompX / 1000.;
 		of->yVel = i2cFrame.flowCompY / 1000.;
 		of->quality = i2cFrame.qual;

quad/src/quad_app/iic_utils.h

@@ -134,7 +134,6 @@ int iic0_lidarlite_sleep();
 ////////////////////////////////////////////////////////////////////////////////////////////
 
 #define PX4FLOW_DEVICE_ADDR			0x42
-#define PX4FLOW_QUAL_MIN			(100)
 
 
 int iic0_px4flow_write(u8 register_addr, u8 data);

quad/src/quad_app/log_data.c

@@ -114,6 +114,9 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 	addOutputToLog(log_struct, ps->vrpn_pitch, CONST_VAL, rad);
 	addOutputToLog(log_struct, ps->vrpn_roll, CONST_VAL, rad);
 	addOutputToLog(log_struct, ps->lidar, CONST_VAL, m);
+	addOutputToLog(log_struct, ps->flow_vel_x, CONST_VAL, m_s);
+	addOutputToLog(log_struct, ps->flow_vel_y, CONST_VAL, m_s);
+	addOutputToLog(log_struct, ps->flow_quality, CONST_VAL, "none");
 	addOutputToLog(log_struct, ps->x_set, CONST_VAL, m);
 	addOutputToLog(log_struct, ps->y_set, CONST_VAL, m);
 	addOutputToLog(log_struct, ps->alt_set, CONST_VAL, m);