Added lidar complementary filter and updated accelerometer Z bias.

quad/src/quad_app/sensor_processing.c

@@ -112,9 +112,8 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 	sensor_struct->roll_angle_filtered = ALPHA * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* get_last_loop_time())
 			+ (1. - ALPHA) * accel_roll;
 
-
 	// Z-axis points upward, so negate distance
-	sensor_struct->lidar_altitude = -raw_sensor_struct->lidar_distance_m;
+	//sensor_struct->lidar_altitude = -raw_sensor_struct->lidar_distance_m;
 
 	// Simply copy optical flow data
 	sensor_struct->optical_flow = raw_sensor_struct->optical_flow;
@@ -123,6 +122,25 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 	//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);
 
+	/*
+	 * Altitude double complementary filter
+	 */
+	static float alt_alpha = 0.9975;
+	static float filtered_vel = 0;
+	static float filtered_alt = 0;
+	static float last_lidar = 0;
+	
+	float this_lidar = raw_sensor_struct->lidar_distance_m;
+	// Acceleration in m/s without gravity
+	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()) +
+	              (1 - alt_alpha)*(this_lidar);
+
+	last_lidar = this_lidar;
+	sensor_struct->lidar_altitude = filtered_alt;
+
 	return 0;
 }
 

quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c

@@ -48,7 +48,7 @@
 
 #define ACCEL_X_BIAS	0.023f
 #define ACCEL_Y_BIAS	0.009f
-#define ACCEL_Z_BIAS	0.087f
+#define ACCEL_Z_BIAS	0.0686f
 
 int mpu9150_write(struct I2CDriver *i2c, u8 register_addr, u8 data);
 int mpu9150_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int size);