Lots of work with gyroscope compensation. Currently disabled, but using...

Lots of work with gyroscope compensation. Currently disabled, but using integral frame properly now.

quad/src/quad_app/sensor_processing.c

@@ -26,6 +26,7 @@
 #define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
 
 int sensor_processing_init(sensor_t* sensor_struct) {
+	// 10Hz cutoff at 200hz sampling
 	float a0 = 0.0200833310260;
 	float a1 = 0.0401666620520;
 	float a2 = 0.0200833310260;
@@ -35,6 +36,11 @@ int sensor_processing_init(sensor_t* sensor_struct) {
 	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);
 
+	// 10Hz filters for bias-corrected euler rates
+	sensor_struct->phi_dot_filt = filter_make_state(a0, a1, a2, b1, b2);
+	sensor_struct->theta_dot_filt = filter_make_state(a0, a1, a2, b1, b2);
+	sensor_struct->psi_dot_filt = filter_make_state(a0, a1, a2, b1, b2);
+
 	//1 Hert filter
 	float vel_a0 = 2.3921e-4;
 	float vel_a1 = 4.7841e-4;
@@ -56,19 +62,34 @@ int sensor_processing_init(sensor_t* sensor_struct) {
 }
 
 
- // Focal length in mm = 16
+// Focal length in mm = 16, in pixels is below
 // static float focal_length_px = 16.0 / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
 /*
- * Modifies the pixel flow values to account for rotations
+ * Convert integral frame flow in radians to velocity in m/s
  * Theta = pitch, phi = roll
+ * Note that we pass phi and theta as angles, but psi dot, because we don't have a complementary-filtered psi (yaw)
  */
- void flow_gyro_compensation(px4flow_t* flow_data, double distance, double phi, double theta, double psi_d) {
+ void flow_gyro_compensation(sensor_t* sensor_struct, double distance,
+		                     double phi, double theta, double psi_d_new) {
+	 //------ Gyro compensation stuff. It seems to make the quadcopter unstable, although all the math checks out and data seems better
+	/*
+	// The reason for converting back to euler rates instead of using raw gyroscope data is so that we can use
+	// The complementary-filtered angles, which will prevent gyroscope drift from creating position drift
 	static double last_phi = 0;
 	static double last_theta = 0;
 
 	// Calculate difference in angles
-	double phi_d = phi - last_phi;
-	double theta_d = theta - last_theta;
+	double phi_d_new, theta_d_new;
+	float loop_dt = get_last_loop_time();
+	if (loop_dt != 0) { // divide by zero check
+		phi_d_new = (phi - last_phi) / loop_dt;
+		theta_d_new = (theta - last_theta) / loop_dt;
+	} else {phi_d_new = theta_d_new = 0;}
+
+	// Run low-pass filters on euler angle rates
+	float phi_d = biquad_execute(&sensor_struct->phi_dot_filt, phi_d_new);
+	float theta_d = biquad_execute(&sensor_struct->theta_dot_filt, theta_d_new);
+	float psi_d = biquad_execute(&sensor_struct->psi_dot_filt, psi_d_new);
 
 	// Convert angles to body rotations (gyroscope equivalents)
 	///////////////////-------  Inverse of AEB matrix  -------//////////////////
@@ -80,33 +101,45 @@ int sensor_processing_init(sensor_t* sensor_struct) {
 	double cos_theta = cos(theta);
 	double cos_phi = cos(phi);
 
+	// We re-calculate p, q, r instead of using the gyroscope values because these are calculated using
+	// the complementary filter pitch and roll, which eliminates drift over time
 	double p = phi_d - sin_phi*psi_d;
 	double q = cos_phi*theta_d + cos_theta*sin_phi*psi_d;
 	double r = -sin_phi*theta_d + cos_phi*cos_theta*psi_d;
+	*/
 
-	// TODO: Clamp correction to maximum search radius
-	// TODO: Verify p vs q here
-	// Compensates for angle rotation using the gyroscope
-	double flow_x_rad_corr = flow_data->flow_x_rad - p;
-	double flow_y_rad_corr = flow_data->flow_y_rad - q;
+	// Convert rotations to rotation rates
+	double flow_x_rad_rate = sensor_struct->optical_flow.flow_x_rad / sensor_struct->optical_flow.dt;
+	double flow_y_rad_rate = sensor_struct->optical_flow.flow_y_rad / sensor_struct->optical_flow.dt;
+
+	// Add p to flow_x_rad_rate to add gyro compensation (Currently disabled)
+	// Add q to flow_y_rad_rate
+	double x_rad_rate_corr = flow_x_rad_rate;// + p;
+	double y_rad_rate_corr = flow_y_rad_rate;// + q;
 	
-	// Swap x and y to switch from rotation around an axis to movement along an axis
-	double x_dist = -flow_y_rad_corr * distance;
-	double y_dist = flow_x_rad_corr * distance;
 
-	if (flow_data->quality > PX4FLOW_QUAL_MIN) {
-		flow_data->xVel = x_dist / flow_data->dt;
-		flow_data->yVel = y_dist / flow_data->dt;
+	// Only accumulate if the quality is good
+	if (sensor_struct->optical_flow.quality > PX4FLOW_QUAL_MIN) {
+		// Swap x and y to switch from rotation around an axis to movement along an axis
+		// Y is negative because some reason?
+		// We simply multiply by distance, because for small angles tan(theta) = theta.
+		//     Also, the internal PX4Flow code works under the small angle assumption,
+		//     so not doing trig here makes it more accurate than doing trig
+		sensor_struct->optical_flow.xVel = -y_rad_rate_corr * distance;
+		sensor_struct->optical_flow.yVel = x_rad_rate_corr * distance;
 	}
 	// Gradually decay towards 0 if quality is bad
 	else {
-		flow_data->xVel *= PX4FLOW_VEL_DECAY;
-		flow_data->yVel *= PX4FLOW_VEL_DECAY;
+		sensor_struct->optical_flow.xVel *= PX4FLOW_VEL_DECAY;
+		sensor_struct->optical_flow.yVel *= PX4FLOW_VEL_DECAY;
 	}
 
-	// Store angles for next call
+	// Un-comment if using gyroscope compensation
+	/*
+	// Store angles for next time
 	last_phi = phi;
 	last_theta = theta;
+	*/
 }
 
 
@@ -188,29 +221,35 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 	sensor_struct->gyr_y = raw_sensor_struct->gam.gyro_yVel_q;
 	sensor_struct->gyr_z = raw_sensor_struct->gam.gyro_zVel_r;
 
+	double loop_dt = get_last_loop_time();
 	// Complementary Filter Calculations
-	sensor_struct->pitch_angle_filtered = ALPHA * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * get_last_loop_time())
+	sensor_struct->pitch_angle_filtered = ALPHA * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * loop_dt)
 			+ (1. - ALPHA) * accel_pitch;
 
-	sensor_struct->roll_angle_filtered = ALPHA * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* get_last_loop_time())
+	sensor_struct->roll_angle_filtered = ALPHA * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* loop_dt)
 			+ (1. - ALPHA) * accel_roll;
 
 	// Z-axis points upward, so negate distance
 	//sensor_struct->lidar_altitude = -raw_sensor_struct->lidar_distance_m;
 
-	// Simply copy optical flow data
+
+	//-------- Optical flow -----------//
+	// Copy over optical flow data
 	sensor_struct->optical_flow = raw_sensor_struct->optical_flow;
 
-	flow_gyro_compensation(&sensor_struct->optical_flow,
+	flow_gyro_compensation(sensor_struct,
 						   raw_sensor_struct->lidar_distance_m,
 						   sensor_struct->roll_angle_filtered,
 						   sensor_struct->pitch_angle_filtered,
-						   sensor_struct->currentQuadPosition.yaw);
+						   sensor_struct->psi_dot);
+
 
 	//Filter OF velocities
 	sensor_struct->optical_flow.xVelFilt = biquad_execute(&sensor_struct->flow_x_filt, sensor_struct->optical_flow.xVel);
 	sensor_struct->optical_flow.yVelFilt = biquad_execute(&sensor_struct->flow_y_filt, sensor_struct->optical_flow.yVel);
 
+
+
 	/*
 	 * Altitude double complementary filter
 	 */

quad/src/quad_app/type_def.h

@@ -328,6 +328,9 @@ typedef struct sensor {
 	struct biquadState accel_z_filt;
 	struct biquadState flow_x_filt;
 	struct biquadState flow_y_filt;
+	struct biquadState phi_dot_filt;
+	struct biquadState psi_dot_filt;
+	struct biquadState theta_dot_filt;
 	struct biquadState mag_x_filt;
 	struct biquadState mag_y_filt;
 

quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c

@@ -36,7 +36,7 @@ int zybo_optical_flow_read(struct OpticalFlowDriver *self, px4flow_t *of) {
   u8 buf[sizeof(i2c_integral_frame)];
 
   // Read the sensor value
-  error = px4flow_read(i2c, buf, 0x16, sizeof(i2c_integral_frame));
+  error = px4flow_read(i2c, buf, 0x16, 26);
 
   if(error == 0) {
     //Copy into struct
@@ -47,7 +47,6 @@ int zybo_optical_flow_read(struct OpticalFlowDriver *self, px4flow_t *of) {
     of->quality = i2c_integral_frame.quality;
     of->dt = (double)i2c_integral_frame.integration_timespan / 1000000;
   }
-
   return error;
 }
 
@@ -66,7 +65,9 @@ int px4flow_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int s
 	int error = 0;
 
 	error = i2c->write(i2c, PX4FLOW_DEVICE_ADDR, buf, 1);
-	if (error) return error;
+	if (error) {
+		return error;
+	}
 	error = i2c->read(i2c, PX4FLOW_DEVICE_ADDR, recv_buffer, size);
 	return error;
 }

quad/xsdk_workspace/real_quad/src/main.c

@@ -49,7 +49,7 @@ int main()
   //test_zybo_mio7_led_and_system();
   //test_zybo_i2c();
   //test_zybo_i2c_imu();
-  //test_zybo_i2c_px4flow();
+  test_zybo_i2c_px4flow();
   //test_zybo_i2c_lidar();
   //test_zybo_i2c_all();
   //test_zybo_rc_receiver();