From 655006892ff954571701be5f658f5ecbf37ebab6 Mon Sep 17 00:00:00 2001
From: "ucart@co3050-12" <dawehr@iastate.edu>
Date: Sun, 23 Apr 2017 00:51:57 -0500
Subject: [PATCH] Low pass filtering flow at 7Hz.
---
quad/src/quad_app/sensor_processing.c | 21 ++++++++++++---------
1 file changed, 12 insertions(+), 9 deletions(-)
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index ad1ee5e5e..e3e2c3d61 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -24,11 +24,11 @@ int sensor_processing_init(sensor_t* sensor_struct) {
sensor_struct->accel_x_filt = filter_make_state(a0, a1, a2, b1, b2);
sensor_struct->accel_y_filt = filter_make_state(a0, a1, a2, b1, b2);
sensor_struct->accel_z_filt = filter_make_state(a0, a1, a2, b1, b2);
- float vel_a0 = 0.04125344;
- float vel_a1 = 0.08250688;
- float vel_a2 = 0.04125344;
- float vel_b1 = -1.3489646;
- float vel_b2 = 0.51397836;
+ float vel_a0 = 0.0098;
+ float vel_a1 = 0.0195;
+ float vel_a2 = 0.0098;
+ float vel_b1 = -1.5816;
+ float vel_b2 = 0.6853;
sensor_struct->flow_x_filt = filter_make_state(vel_a0, vel_a1, vel_a2, vel_b1, vel_b2);
sensor_struct->flow_y_filt = filter_make_state(vel_a0, vel_a1, vel_a2, vel_b1, vel_b2);
return 0;
@@ -41,8 +41,11 @@ int sensor_processing_init(sensor_t* sensor_struct) {
// 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();
+ double loop_time = get_last_loop_time();
+ if (loop_time != 0) {
+ 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)
@@ -118,9 +121,9 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
// Simply copy optical flow data
sensor_struct->optical_flow = raw_sensor_struct->optical_flow;
- //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);
flow_to_vel(&sensor_struct->optical_flow, raw_sensor_struct->lidar_distance_m);
+ 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);
/*
* Altitude double complementary filter
--
GitLab