Much work on optical flow. Fixed issue with how we do logging, logging params now.

quad/src/quad_app/control_algorithm.c

@@ -67,8 +67,9 @@ int control_algorithm_init(parameter_t * ps)
     ps->cur_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Yaw");
     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_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel Y");
     ps->flow_quality = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Quality");
+    ps->flow_distance = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Distance");
     // Sensor trims
     ps->pitch_trim = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch trim");
     ps->pitch_trim_add = graph_add_defined_block(graph, BLOCK_ADD, "Pitch trim add");
@@ -372,6 +373,7 @@ int control_algorithm_init(parameter_t * ps)
         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
     graph_set_param_val(graph, ps->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
@@ -380,9 +382,8 @@ 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[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);
+    int outputs[1] = {ps->mixer};
+    graph_compute_nodes(graph, outputs, 1);
 
 	 // 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

@@ -316,6 +316,7 @@ int iic0_px4flow_update(px4flow_t *of) {
 		of->xVel = i2cFrame.flowCompX / 1000.;
 		of->yVel = i2cFrame.flowCompY / 1000.;
 		of->quality = i2cFrame.qual;
+		of->distance = i2cFrame.groundDistance / 1000.;
 	}
 
 	return status;

quad/src/quad_app/log_data.c

@@ -106,32 +106,34 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 	addOutputToLog(log_struct, ps->roll_r_pid, PID_CORRECTION, pwm_val);
 	addOutputToLog(log_struct, ps->yaw_r_pid, PID_CORRECTION, pwm_val);
 	addOutputToLog(log_struct, ps->pitch_trim_add, CONST_VAL, rad);
-	addOutputToLog(log_struct, ps->cur_roll, CONST_VAL, rad);
-	addOutputToLog(log_struct, ps->cur_yaw, CONST_VAL, rad);
-	addOutputToLog(log_struct, ps->vrpn_x, CONST_VAL, m);
-	addOutputToLog(log_struct, ps->vrpn_y, CONST_VAL, m);
-	addOutputToLog(log_struct, ps->vrpn_alt, CONST_VAL, m);
-	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);
 	addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR0, pwm_val);
 	addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR1, pwm_val);
 	addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR2, pwm_val);
 	addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR3, pwm_val);
-	addOutputToLog(log_struct, ps->rc_throttle, PID_CORRECTION, pwm_val);
-	addOutputToLog(log_struct, ps->rc_pitch, PID_CORRECTION, pwm_val);
-	addOutputToLog(log_struct, ps->rc_roll, PID_CORRECTION, pwm_val);
 	addOutputToLog(log_struct, ps->x_vel_pid, PID_CORRECTION, rad);
 	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);
 
+	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);
+	addParamToLog(log_struct, ps->lidar, CONST_VAL, m);
+	addParamToLog(log_struct, ps->vrpn_x, CONST_VAL, m);
+	addParamToLog(log_struct, ps->vrpn_y, CONST_VAL, m);
+	addParamToLog(log_struct, ps->vrpn_alt, CONST_VAL, m);
+	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;
 	size_t needed_memory = sizeof(float) * row_size * LOG_STARTING_SIZE;

quad/src/quad_app/type_def.h

@@ -117,6 +117,11 @@ typedef struct {
 
 typedef struct {
 	double xVel, yVel;
+	double distance;
+
+	double flowX, flowY;
+	double gyroX, gyroY, gyroZ;
+
 	int16_t quality;
 } px4flow_t;
 
@@ -339,6 +344,7 @@ typedef struct parameter_t {
 	int flow_vel_x; // optical flow
 	int flow_vel_y;
 	int flow_quality; // Quality value returned by optical flow sensor
+	int flow_distance;
 	// VRPN blocks
 	int vrpn_x;
 	int vrpn_y;