Low pass filtering flow at 7Hz.

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