Added magnetometer support and magnetometer/gyro yaw complementary filter

quad/src/quad_app/control_algorithm.c

@@ -15,7 +15,7 @@
 
 #define USE_LIDAR
 #define USE_OF
-#define USE_FAKE_YAW
+#define USE_MAG_YAW
 #define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
 
 //10 degrees
@@ -128,6 +128,9 @@ int control_algorithm_init(parameter_t * ps)
     // gyroscope z integrator
     ps->gyro_yaw = graph_add_defined_block(graph, BLOCK_INTEGRATOR, "Integrated gyro z");
 
+    //Complementary yaw
+    ps->mag_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Mag Yaw");
+
     ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
 
     ps->angle_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_IMU");
@@ -154,7 +157,7 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->gyro_x, CONST_VAL);
     graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->angle_time, CONST_VAL);
     graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL);
-    //graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->vrpn_roll, CONST_VAL);
+    //graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->vrpn_roll, CONST_VAL);USE_FAKE_YAW
     graph_set_source(graph, ps->roll_pid, PID_DT, ps->angle_time, CONST_VAL);
 
     // Connect yaw PID chain
@@ -179,8 +182,8 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
 
     // X/Y global to local conversion
-#ifdef USE_FAKE_YAW
-    graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->gyro_yaw, CONST_VAL);
+#ifdef USE_MAG_YAW
+    graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->mag_yaw, CONST_VAL);
 #else
     graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
 #endif
@@ -230,8 +233,8 @@ int control_algorithm_init(parameter_t * ps)
 
 
     // X/Y global to local conversion
-#ifdef USE_FAKE_YAW
-    graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->gyro_yaw, CONST_VAL);
+#ifdef USE_MAG_YAW
+    graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->mag_yaw, CONST_VAL);
 #else
     graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
 #endif
@@ -251,9 +254,9 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND1, ps->alt_pid, PID_CORRECTION);
     graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND2, ps->throttle_trim, CONST_VAL);
     // Yaw angle
-#ifdef USE_FAKE_YAW
+#ifdef USE_MAG_YAW
     graph_set_source(graph, ps->yaw_pid, PID_DT, ps->angle_time, CONST_VAL);
-    graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->gyro_yaw, CONST_VAL);
+    graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->mag_yaw, CONST_VAL);
 #else
     graph_set_source(graph, ps->yaw_pid, PID_DT, ps->pos_time, CONST_VAL);
     graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->cur_yaw, CONST_VAL);
@@ -272,8 +275,8 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT);
 
     // Connect optical flow
-#ifdef USE_FAKE_YAW
-    graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->gyro_yaw, ADD_SUM);
+#ifdef USE_MAG_YAW
+    graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->mag_yaw, ADD_SUM);
 #else
     graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->cur_yaw, ADD_SUM);
 #endif
@@ -464,6 +467,7 @@ int control_algorithm_init(parameter_t * ps)
 	// Sensor values
     graph_set_param_val(graph, ps->cur_pitch, CONST_SET, sensor_struct->pitch_angle_filtered);
     graph_set_param_val(graph, ps->cur_roll, CONST_SET, sensor_struct->roll_angle_filtered);
+    graph_set_param_val(graph, ps->mag_yaw, CONST_SET, sensor_struct->yaw_angle_filtered);
     graph_set_param_val(graph, ps->gyro_y, CONST_SET, sensor_struct->gyr_y);
     graph_set_param_val(graph, ps->gyro_x, CONST_SET, sensor_struct->gyr_x);
     graph_set_param_val(graph, ps->gyro_z, CONST_SET, sensor_struct->gyr_z);

quad/src/quad_app/log_data.c

@@ -119,6 +119,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 	addOutputToLog(log_struct, ps->of_integ_x, INTEGRATED, m);
 	addOutputToLog(log_struct, ps->of_integ_y, INTEGRATED, m);
 	addOutputToLog(log_struct, ps->gyro_yaw, INTEGRATED, rad);
+	addOutputToLog(log_struct, ps->mag_yaw, CONST_VAL, rad);
 
 	addParamToLog(log_struct, ps->cur_roll, CONST_VAL, rad);
 	addParamToLog(log_struct, ps->cur_yaw, CONST_VAL, rad);
@@ -135,7 +136,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 	addParamToLog(log_struct, ps->flow_quality, CONST_VAL, "none");
 
 	// TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
-	row_size = n_outputs + n_params + 6 + 1;
+	row_size = n_outputs + n_params + 9 + 1;
 	size_t needed_memory = sizeof(float) * row_size * LOG_STARTING_SIZE;
 	logArray = malloc(needed_memory);
 	if (!logArray) { // malloc failed
@@ -161,6 +162,10 @@ int log_data(log_t* log_struct, parameter_t* ps)
 	thisRow[offset++] = log_struct->gam.gyro_xVel_p;
 	thisRow[offset++] = log_struct->gam.gyro_yVel_q;
 	thisRow[offset++] = log_struct->gam.gyro_zVel_r;
+	thisRow[offset++] = log_struct->gam.mag_x;
+	thisRow[offset++] = log_struct->gam.mag_y;
+	thisRow[offset++] = log_struct->gam.mag_z;
+
 
 	int i;
 	for (i = 0; i < n_params; i++) {
@@ -218,7 +223,7 @@ void printLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, r
 	}
 
 	// Header names for the pre-defined values
-	safe_sprintf_cat(&buf, "%%Time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z");
+	safe_sprintf_cat(&buf, "%%Time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z\tmag_x\tmag_y\tmag_z");
 
 	// Print all the recorded block parameters
 	for (i = 0; i < n_params; i++) {
@@ -239,7 +244,7 @@ void printLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, r
 
 	// Send units header
 	buf.size = 0;
-	safe_sprintf_cat(&buf, "&s\tG\tG\tG\trad/s\trad/s\trad/s"); // The pre-defined ones
+	safe_sprintf_cat(&buf, "&s\tG\tG\tG\trad/s\trad/s\trad/s\tuT\tuT\tuT"); // The pre-defined ones
 	safe_sprintf_cat(&buf, units_output.str);
 	safe_sprintf_cat(&buf, units_param.str);
 	safe_sprintf_cat(&buf, "\n");

quad/src/quad_app/sensor_processing.c

@@ -14,9 +14,15 @@
 #include <math.h>
 
 #define ALPHA 0.99
+#define YAW_ALPHA	(0.995)
 #define PX4FLOW_QUAL_MIN			(100)
 #define PX4FLOW_VEL_DECAY					(0.99)
 
+#define MAG_OFFSET_X				(-33.9844)
+#define MAG_OFFSET_Y				(40.4922)
+
+#define GYRO_Z_OFFSET				(0.0073)
+
 int sensor_processing_init(sensor_t* sensor_struct) {
 	float a0 = 0.0200833310260;
 	float a1 = 0.0401666620520;
@@ -33,6 +39,15 @@ int sensor_processing_init(sensor_t* sensor_struct) {
 	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);
+
+	float mag_a0 = 2.3921e-4;
+	float mag_a1 = 4.7841e-4;
+	float mag_a2 = 2.3921e-4;
+	float mag_b1 = -1.9281;
+	float mag_b2 = 0.9391;
+	sensor_struct->mag_x_filt = filter_make_state(mag_a0, mag_a1, mag_a2, mag_b1, mag_b2);
+	sensor_struct->mag_y_filt = filter_make_state(mag_a0, mag_a1, mag_a2, mag_b1, mag_b2);
+
 	return 0;
 }
 
@@ -155,6 +170,15 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 	last_lidar = this_lidar;
 	sensor_struct->lidar_altitude = filtered_alt;
 
+	//Magnetometer filter
+	float magX_filt = biquad_execute(&sensor_struct->mag_x_filt, gam->mag_x - MAG_OFFSET_X);
+	float magY_filt = biquad_execute(&sensor_struct->mag_y_filt, gam->mag_y - MAG_OFFSET_Y);
+	float mag_yaw = atan2(-magY_filt, -magX_filt);
+
+	//Heading complementary filter
+	sensor_struct->yaw_angle_filtered = YAW_ALPHA * (sensor_struct->yaw_angle_filtered +
+				(sensor_struct->psi_dot)*get_last_loop_time()) + (1. - YAW_ALPHA) * mag_yaw;
+
 	return 0;
 }
 

quad/src/quad_app/type_def.h

@@ -102,15 +102,18 @@ typedef struct gam {
 	float accel_roll;
 	float accel_pitch;
 
-	// MAG
-	//Xint16 raw_mag_x, raw_mag_y, raw_mag_z;
-
-	float heading; // In degrees
+	//float heading; // In degrees
 
 	float mag_x; //Magnetic north: ~50 uT
 	float mag_y;
 	float mag_z;
 
+	float magX_correction;
+	float magY_correction;
+	float magZ_correction;
+
+	int magDRDYCount;
+
 	SensorError_t error;
 } gam_t;
 
@@ -303,6 +306,7 @@ typedef struct sensor {
 	// Complementary filter outputs
 	float pitch_angle_filtered;
 	float roll_angle_filtered;
+	float yaw_angle_filtered;
 
 	// Z-axis value obtained from LiDAR
 	// Note that this is not distance, as our Z-axis points upwards.
@@ -319,6 +323,8 @@ typedef struct sensor {
 	struct biquadState accel_z_filt;
 	struct biquadState flow_x_filt;
 	struct biquadState flow_y_filt;
+	struct biquadState mag_x_filt;
+	struct biquadState mag_y_filt;
 
 	// Information obtained from optical flow sensor 
 	px4flow_t optical_flow;
@@ -411,6 +417,7 @@ typedef struct parameter_t {
 	int of_trimmed_x; // Trimmed optical flow integrated value (of_integ_x + of_trim_x)
 	int of_trimmed_y;
 	int gyro_yaw; // Integrates the gyro z value
+	int mag_yaw; //Complementary filtered magnetometer/gyro yaw
 } parameter_t;
 
 /**

quad/xsdk_workspace/real_quad/.cproject

@@ -109,7 +109,7 @@
 							</tool>
 							<tool id="xilinx.gnu.arm.c.toolchain.compiler.release.85270120" name="ARM gcc compiler" superClass="xilinx.gnu.arm.c.toolchain.compiler.release">
 								<option defaultValue="gnu.c.optimization.level.more" id="xilinx.gnu.compiler.option.optimization.level.515686013" name="Optimization Level" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
-								<option id="xilinx.gnu.compiler.option.debugging.level.1121150517" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.option.debugging.level.1121150517" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
 								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.687694973" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
 									<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
 								</option>

quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c

@@ -13,6 +13,8 @@
 #define MAG_READ_SIZE 				6
 #define MAG_BASE_ADDR 				0x03
 
+#define MAG_DRDY_TIMEOUT			(10)
+
 #define RAD_TO_DEG 57.29578
 #define DEG_TO_RAD 0.0174533
 
@@ -44,7 +46,7 @@
 
 #define GYRO_X_BIAS	0.005f
 #define GYRO_Y_BIAS	-0.014f
-#define GYRO_Z_BIAS	0.0541f
+#define GYRO_Z_BIAS	0.0614//0.0541f
 
 #define ACCEL_X_BIAS	0.023f
 #define ACCEL_Y_BIAS	0.009f
@@ -53,6 +55,7 @@
 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);
 
+int mpu9150_calc_mag_sensitivity(struct IMUDriver *self, gam_t *gam);
 int mpu9150_read_mag(struct IMUDriver *self, gam_t* gam);
 int mpu9150_read_gyro_accel(gam_t* gam);
 
@@ -77,23 +80,57 @@ int zybo_imu_reset(struct IMUDriver *self, gam_t *gam) {
   // Enable I2C bypass for AUX I2C (Magnetometer)
   mpu9150_write(i2c, 0x37, 0x02);
 
-  // Setup Mag
-  mpu9150_write(i2c, 0x37, 0x02);             //INT_PIN_CFG   -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=0 ; CLKOUT_EN=0
-
   usleep(100000);
 
+  //Calculate magnetometer sensitivities
+  mpu9150_calc_mag_sensitivity(self, gam);
+
+  usleep(10000);
+
+  //Enable single measurement mode
+  mpu9150_write(i2c, 0x0A, 0x00);
+  mpu9150_write(i2c, 0x0A, 0x01);
+
   int i;
-  gam_t temp_gam;
 
   // Do about 20 reads to warm up the device
   for(i=0; i < 20; ++i){
-    self->read(self, &temp_gam);
+    self->read(self, gam);
     usleep(1000);
   }
 
   return 0;
 }
 
+int mpu9150_calc_mag_sensitivity(struct IMUDriver *self, gam_t *gam) {
+	u8 buf[3];
+	u8 ASAX, ASAY, ASAZ;
+
+	// Quickly read from the factory ROM to get correction coefficents
+	int status = mpu9150_write(self->i2c, 0x0A, 0x0F);
+	if(status != 0) {
+		return status;
+	}
+
+	usleep(10000);
+
+	// Read raw adjustment values
+	status = mpu9150_read(self->i2c, buf, 0x10,3);
+	if(status != 0) {
+		return status;
+	}
+	ASAX = buf[0];
+	ASAY = buf[1];
+	ASAZ = buf[2];
+
+	// Set the correction coefficients
+	gam->magX_correction = (ASAX-128)*0.5/128 + 1;
+	gam->magY_correction = (ASAY-128)*0.5/128 + 1;
+	gam->magZ_correction = (ASAZ-128)*0.5/128 + 1;
+
+	return 0;
+}
+
 int zybo_imu_read(struct IMUDriver *self, gam_t *gam) {
   struct I2CDriver *i2c = self->i2c;
   i16 raw_accel_x, raw_accel_y, raw_accel_z;
@@ -126,7 +163,7 @@ int zybo_imu_read(struct IMUDriver *self, gam_t *gam) {
   gam->gyro_zVel_r = ((gyro_z / GYRO_SENS) * DEG_TO_RAD) + GYRO_Z_BIAS;
 
   // Magnometer
-  //mpu9150_read_mag(self, gam);
+  mpu9150_read_mag(self, gam);
 
   return error;
 }
@@ -174,56 +211,55 @@ int mpu9150_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int s
 
 
 int mpu9150_read_mag(struct IMUDriver *self, gam_t* gam){
-
-  static double magX_correction = -1, magY_correction, magZ_correction;
-
-  struct I2CDriver *i2c = self->i2c;
-
-  u8 mag_data[6];
-  i16 raw_magX, raw_magY, raw_magZ;
-
-  // Grab calibrations if not done already
-  if(magX_correction == -1){
-    u8 buf[3];
-    u8 ASAX, ASAY, ASAZ;
-
-    // Quickly read from the factory ROM to get correction coefficents
-    mpu9150_write(i2c, 0x0A, 0x0F);
-    usleep(10000);
-
-    // Read raw adjustment values
-    mpu9150_read(i2c, buf, 0x10,3);
-    ASAX = buf[0];
-    ASAY = buf[1];
-    ASAZ = buf[2];
-
-    // Set the correction coefficients
-    magX_correction = (ASAX-128)*0.5/128 + 1;
-    magY_correction = (ASAY-128)*0.5/128 + 1;
-    magZ_correction = (ASAZ-128)*0.5/128 + 1;
-  }
-
-  // Set Mag to single read mode
-  mpu9150_write(i2c, 0x0A, 0x01);
-  usleep(10000);
-  mag_data[0] = 0;
-
-  // Keep checking if data is ready before reading new mag data
-  while(mag_data[0] == 0x00){
-    mpu9150_read(i2c, mag_data, 0x02, 1);
-  }
-
-  // Get mag data
-  mpu9150_read(i2c, mag_data, 0x03, 6);
-
-  raw_magX = (mag_data[1] << 8) | mag_data[0];
-  raw_magY = (mag_data[3] << 8) | mag_data[2];
-  raw_magZ = (mag_data[5] << 8) | mag_data[4];
-
-  // Set magnetometer data to output
-  gam->mag_x = raw_magX * magX_correction;
-  gam->mag_y = raw_magY * magY_correction;
-  gam->mag_z = raw_magZ * magZ_correction;
-
-  return 0;
+	u8 mag_data[6];
+	u8 mag_status;
+	i16 raw_magX, raw_magY, raw_magZ;
+
+	int trigger = 0;
+
+	int status = mpu9150_read(self->i2c, mag_data, 0x02, 1);
+	if(status != 0) {
+		return status;
+	}
+
+	if(mag_data[0] & 0x01) {
+		// Get mag data
+		status = mpu9150_read(self->i2c, mag_data, 0x03, 6);
+		if(status != 0) {
+			return status;
+		}
+
+		status = mpu9150_read(self->i2c, &mag_status, 0x09, 1);
+		if(status != 0) {
+			return status;
+		}
+
+		raw_magX = (mag_data[1] << 8) | mag_data[0];
+		raw_magY = (mag_data[3] << 8) | mag_data[2];
+		raw_magZ = (mag_data[5] << 8) | mag_data[4];
+
+		// Set magnetometer data to output
+		gam->mag_x = raw_magX * gam->magX_correction;
+		gam->mag_y = raw_magY * gam->magY_correction;
+		gam->mag_z = raw_magZ * gam->magZ_correction;
+
+		trigger = 1;
+	}
+	else {
+		gam->magDRDYCount++;
+
+		if(gam->magDRDYCount > MAG_DRDY_TIMEOUT) {
+			gam->magDRDYCount = 0;
+
+			trigger = 1;
+		}
+	}
+
+	if(trigger) {
+		//Start next reading
+		mpu9150_write(self->i2c, 0x0A, 0x00);
+		mpu9150_write(self->i2c, 0x0A, 0x01);
+	}
+
+	return 0;
 }

quad/xsdk_workspace/real_quad/src/main.c

@@ -48,7 +48,7 @@ int main()
 #ifdef RUN_TESTS
   //test_zybo_mio7_led_and_system();
   //test_zybo_i2c();
-  //test_zybo_i2c_imu();
+  test_zybo_i2c_imu();
   //test_zybo_i2c_px4flow();
   //test_zybo_i2c_lidar();
   //test_zybo_i2c_all();