From 1a6ba71af7edca1a1153a50474e401c946c03876 Mon Sep 17 00:00:00 2001 From: "ucart@co3050-12" <dawehr@iastate.edu> Date: Sat, 22 Apr 2017 17:57:57 -0500 Subject: [PATCH] Adjustments to LiDAR complementary filter. --- quad/src/quad_app/sensor_processing.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c index 80e2c22d3..ad1ee5e5e 100644 --- a/quad/src/quad_app/sensor_processing.c +++ b/quad/src/quad_app/sensor_processing.c @@ -125,14 +125,14 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se /* * Altitude double complementary filter */ - static float alt_alpha = 0.9975; + static float alt_alpha = 0.98; static float filtered_vel = 0; static float filtered_alt = 0; static float last_lidar = 0; - float this_lidar = raw_sensor_struct->lidar_distance_m; + float this_lidar = -raw_sensor_struct->lidar_distance_m; // Acceleration in m/s without gravity - float quad_z_accel = -9.8 * (accel_z + 1); + float quad_z_accel = 9.8 * (accel_z + 1); filtered_vel = alt_alpha*(filtered_vel + quad_z_accel*get_last_loop_time()) + (1 - alt_alpha)*(this_lidar - last_lidar); filtered_alt = alt_alpha*(filtered_alt + filtered_vel*get_last_loop_time()) + -- GitLab