wip: getting optical flow into graph.

quad/src/quad_app/control_algorithm.c

@@ -64,6 +64,9 @@ int control_algorithm_init(parameter_t * ps)
     ps->cur_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "Roll");
     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_quality = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Quality");
     // 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");

quad/src/quad_app/iic_utils.c

@@ -212,7 +212,10 @@ int iic0_lidarlite_read(u8* recv_buffer, u8 register_addr, int size) {
 	int status = 0;
 
 	status |= i2c->write(i2c, LIDARLITE_DEVICE_ADDR, buf, 1);
-	status |= i2c->read(i2c, LIDARLITE_DEVICE_ADDR, recv_buffer, size);
+	// Only read if write succeeded
+	if (status == 0) {
+		status |= i2c->read(i2c, LIDARLITE_DEVICE_ADDR, recv_buffer, size);
+	}
 	return status;
 }
 
@@ -263,29 +266,24 @@ int iic0_px4flow_read(u8* recv_buffer, u8 register_addr, int size) {
 	int status = 0;
 
 	status |= i2c->write(i2c, PX4FLOW_DEVICE_ADDR, buf, 1);
-	status |= i2c->read(i2c, PX4FLOW_DEVICE_ADDR, recv_buffer, size);
+	if (status == 0) {
+		status |= i2c->read(i2c, PX4FLOW_DEVICE_ADDR, recv_buffer, size);
+	}
 	return status;
 }
 
 int iic0_px4flow_init(px4flow_t *of) {
 	//Clear struct
-	of->xPos = 0;
-	of->yPos = 0;
 	of->xVel = 0;
 	of->yVel = 0;
 
 	//Perform initial update
-	return iic0_px4flow_update(of, 0.);
+	// TODO: Re-enable this initial read once virtual mock-up is made
+	//return iic0_px4flow_update(of);
+	return 0;
 }
 
-int iic0_px4flow_update(px4flow_t *of, double dt) {
-	static double time = 0.;
-
-	time += dt;
-	if(time >= 10.) {
-		time = 0.;
-	}
-
+int iic0_px4flow_update(px4flow_t *of) {
 	int status = 0;
 
 	struct {
@@ -316,13 +314,9 @@ int iic0_px4flow_update(px4flow_t *of, double dt) {
 		memcpy(&i2cFrame, buf, sizeof(i2cFrame));
 
 		//As per documentation, disregard frames with low quality, as they contain unreliable data
-		if(i2cFrame.qual >= PX4FLOW_QUAL_MIN) {
-			of->xVel = i2cFrame.flowCompX / 1000.;
-			of->yVel = i2cFrame.flowCompY / 1000.;
-
-			of->xPos += of->xVel * dt;
-			of->yPos += of->yVel * dt;
-		}
+		of->xVel = i2cFrame.flowCompX / 1000.;
+		of->yVel = i2cFrame.flowCompY / 1000.;
+		of->quality = i2cFrame.qual;
 	}
 
 	return status;

quad/src/quad_app/iic_utils.h

@@ -140,7 +140,7 @@ int iic0_lidarlite_sleep();
 int iic0_px4flow_write(u8 register_addr, u8 data);
 int iic0_px4flow_read(u8* recv_buffer, u8 register_addr, int size);
 
-int iic0_px4flow_update(px4flow_t *of, double dt);
+int iic0_px4flow_update(px4flow_t *of);
 
 int iic0_px4flow_init(px4flow_t *of);
 

quad/src/quad_app/sensor.c

@@ -16,7 +16,11 @@ int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
 //		return -1;
 //	}
 
-	if(iic0_mpu9150_start() == -1) {
+	if(iic0_mpu9150_start()) {
+		return -1;
+	}
+
+	if (iic0_px4flow_init(&raw_sensor_struct->optical_flow)) {
 		return -1;
 	}
 
@@ -70,14 +74,18 @@ int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t
 	int status = 0;
 
 	// the the sensor board and fill in the readings into the GAM struct
-	iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
-
+	status |= iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
 	log_struct->gam = raw_sensor_struct->gam;
 
-//	static lidar_t lidar_val;
-//	iic0_lidarlite_read_distance(&lidar_val);
-//	raw_sensor_struct->lidar_distance_m = lidar_val.distance_m;
+	static lidar_t lidar_val;
+	int lidar_status = iic0_lidarlite_read_distance(&lidar_val);
+	if (lidar_status == 0) {
+		raw_sensor_struct->lidar_distance_m = lidar_val.distance_m;
+	}
+	status |= lidar_status;
+	
+	status |= iic0_px4flow_update(&raw_sensor_struct->optical_flow);
 
-    return 0;
+    return status;
 }
  

quad/src/quad_app/sensor_processing.c

@@ -98,6 +98,9 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 
 	// Z-axis points upward, so negate distance
 	sensor_struct->lidar_altitude = -raw_sensor_struct->lidar_distance_m;
+
+	// Simply copy optical flow data
+	sensor_struct->optical_flow = raw_sensor_struct->optical_flow;
 	return 0;
 }
 

quad/src/quad_app/type_def.h

@@ -117,7 +117,7 @@ typedef struct {
 
 typedef struct {
 	double xVel, yVel;
-	double xPos, yPos;
+	int16_t quality;
 } px4flow_t;
 
 typedef struct PID_t {
@@ -248,6 +248,9 @@ typedef struct raw_sensor {
 	// Ideally equals 0 when landed
 	float lidar_distance_m;
 
+	// Information obtained from optical flow sensor 
+	px4flow_t optical_flow;
+
 } raw_sensor_t;
 
 /**
@@ -294,6 +297,8 @@ typedef struct sensor {
 	struct biquadState accel_y_filt;
 	struct biquadState accel_z_filt;
 
+	// Information obtained from optical flow sensor 
+	px4flow_t optical_flow;
 } sensor_t;
 
 /**
@@ -331,6 +336,9 @@ typedef struct parameter_t {
 	int gyro_x;
 	int gyro_z;
 	int lidar;
+	int flow_vel_x; // optical flow
+	int flow_vel_y;
+	int flow_quality; // Quality value returned by optical flow sensor
 	// VRPN blocks
 	int vrpn_x;
 	int vrpn_y;

quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c

@@ -99,7 +99,7 @@ int test_zybo_i2c_px4flow() {
 
     sys.sleep(&sys, delay * 1000);
 
-    iic0_px4flow_update(&of, delay / 1000.);
+    iic0_px4flow_update(&of);
   }
   return 0;
 }
@@ -129,7 +129,7 @@ int test_zybo_i2c_all() {
 
     sys.sleep(&sys, delay * 1000);
 
-    if (iic0_px4flow_update(&of, delay / 1000.)) {
+    if (iic0_px4flow_update(&of)) {
     	of_errors += 1;
     }