diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c index 80e2c22d38f100d47ae5fe2560ede1602da18305..ad1ee5e5e2da5679fa732d5fdf61ab895656ffcc 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()) +