From d9157f100b508061faabd5d4b3a5823208745c41 Mon Sep 17 00:00:00 2001
From: David Wehr <dawehr@iastate.edu>
Date: Fri, 21 Apr 2017 19:39:50 -0500
Subject: [PATCH] Calculating velocity from flow onboard.
---
quad/src/quad_app/sensor_processing.c | 15 +++++++++++++--
.../real_quad/src/hw_impl_zybo_lidar.c | 6 +++---
.../real_quad/src/hw_impl_zybo_optical_flow.c | 3 +++
3 files changed, 19 insertions(+), 5 deletions(-)
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 43dd61146..99a3cfbac 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/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;
}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_lidar.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_lidar.c
index cd49ca365..32d0fad0c 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_lidar.c
+++ b/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;
}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
index f3ab4d9e6..8a7e17d3a 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
+++ b/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.;
}
--
GitLab