Calculating velocity from flow onboard.

quad/src/quad_app/sensor_processing.c

@@ -35,6 +35,17 @@ int sensor_processing_init(sensor_t* sensor_struct) {
 	return 0;
 }
 
+/*
+ * Populates the xVel and yVel fields of flow_data,
+ * using the flowX and flowY, and the given distance
+ */
+// Focal length in mm = 16
+static float focal_length_px = 16.0 / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
+void flow_to_vel(px4flow_t* flow_data, double distance) {
+	flow_data->xVel = distance * -flow_data->flowX / focal_length_px / get_last_loop_time();
+	flow_data->yVel = distance * -flow_data->flowY / focal_length_px / get_last_loop_time();
+}
+
 int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_sensor_t* raw_sensor_struct, sensor_t* sensor_struct)
 {
 	// Filter accelerometer values
@@ -111,8 +122,8 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 
 	//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);
-	sensor_struct->optical_flow.xVel = -sensor_struct->optical_flow.xVel;
-	sensor_struct->optical_flow.yVel = -sensor_struct->optical_flow.yVel;
+	flow_to_vel(&sensor_struct->optical_flow, raw_sensor_struct->lidar_distance_m);
+
 	return 0;
 }
 

quad/xsdk_workspace/real_quad/src/hw_impl_zybo_lidar.c

@@ -4,7 +4,7 @@
 #include <string.h>
 
 #define LIDARLITE_DEVICE_ADDR		0x62
-#define LIDAR_OFFSET 0.02 // Distance from LiDAR sensor to ground, in meters
+#define LIDAR_OFFSET (0.016666) // Distance from LiDAR sensor to ground, in meters
 
 int lidarlite_write(struct I2CDriver *i2c, u8 register_addr, u8 data);
 int lidarlite_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int size);
@@ -37,8 +37,8 @@ int zybo_lidar_read(struct LidarDriver *self, lidar_t *lidar) {
   // Read the sensor value
   error = lidarlite_read(i2c, buf, 0x8f, 2);
   if (error) return error;
-  float distance_cm = (float)(buf[0] << 8 | buf[1]) - LIDAR_OFFSET;
-  lidar->distance_m = distance_cm * 0.01;
+  float distance_cm = (float)(buf[0] << 8 | buf[1]);
+  lidar->distance_m = (distance_cm * 0.01) + LIDAR_OFFSET;
 
   return error;
 }

quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c

@@ -46,6 +46,9 @@ int zybo_optical_flow_read(struct OpticalFlowDriver *self, px4flow_t *of) {
 
     of->xVel = i2cFrame.flowCompX / 1000.;
     of->yVel = i2cFrame.flowCompY / 1000.;
+    // They call it "sum", but it's just the latest pixel flow * 10
+    of->flowX = (double)i2cFrame.pixelFlowXSum / 10.;
+    of->flowY = (double)i2cFrame.pixelFlowYSum / 10.;
     of->quality = i2cFrame.qual;
     of->distance = i2cFrame.groundDistance / 1000.;
   }