AP_AHRS_MPU6000: first draft implementation of class that uses MPU6000's DMP to calculate attitude

libraries/AP_AHRS/AP_AHRS_MPU6000.cpp

@@ -0,0 +1,321 @@
+/*
+	AP_AHRS_MPU6000.cpp
+
+	AHRS system using MPU6000's internal calculations
+
+	Adapted for the general ArduPilot AHRS interface by Andrew Tridgell
+
+	This library is free software; you can redistribute it and/or
+	modify it under the terms of the GNU Lesser General Public License
+	as published by the Free Software Foundation; either version 2.1
+	of the License, or (at your option) any later version.
+*/
+#include <FastSerial.h>
+#include <AP_AHRS.h>
+
+// this is the speed in cm/s above which we first get a yaw lock with
+// the GPS
+#define GPS_SPEED_MIN 300
+
+// this is the speed in cm/s at which we stop using drift correction
+// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
+#define GPS_SPEED_RESET 100
+
+// the limit (in degrees/second) beyond which we stop integrating
+// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
+// which results in false gyro drift. See
+// http://gentlenav.googlecode.com/files/fastRotations.pdf
+#define SPIN_RATE_LIMIT 20
+
+// table of user settable parameters
+const AP_Param::GroupInfo AP_AHRS_MPU6000::var_info[] PROGMEM = {
+    // @Param: AHRS_YAW_P
+    // @DisplayName: Yaw P
+    // @Description: This controls the weight the compass has on the overall heading
+    // @Range: 0 .4
+    // @Increment: .01
+    AP_GROUPINFO("YAW_P", 0, AP_AHRS_MPU6000, _kp_yaw),
+
+    // @Param: AHRS_RP_P
+    // @DisplayName: AHRS RP_P
+    // @Description: This controls how fast the accelerometers correct the attitude
+    // @Range: 0 .4
+    // @Increment: .01
+    AP_GROUPINFO("RP_P",  1, AP_AHRS_MPU6000, _kp),
+    AP_GROUPEND
+};
+
+void
+AP_AHRS_MPU6000::init()
+{
+	_mpu6000->dmp_init();
+	push_gains_to_dmp();
+	push_offsets_to_ins();
+};
+
+// run a full MPU6000 update round
+void
+AP_AHRS_MPU6000::update(void)
+{
+	float delta_t;
+
+	// tell the IMU to grab some data.  is this necessary?
+	_imu->update();
+
+	// ask the IMU how much time this sensor reading represents
+	delta_t = _imu->get_delta_time();
+
+	// convert the quaternions into a DCM matrix
+	_mpu6000->quaternion.rotation_matrix(_dcm_matrix);
+	
+	// we run the gyro bias correction using gravity vector algorithm
+	drift_correction(delta_t);
+
+	// Calculate pitch, roll, yaw for stabilization and navigation
+	euler_angles();
+}
+
+// wrap_PI - ensure an angle (expressed in radians) is between -PI and PI
+// TO-DO: should remove and replace with more standard functions
+float AP_AHRS_MPU6000::wrap_PI(float angle_in_radians)
+{
+  if( angle_in_radians > M_PI ){
+    return(angle_in_radians - 2*M_PI);
+  }
+  else if( angle_in_radians < -M_PI ){
+    return(angle_in_radians + 2*M_PI);
+  }
+  else{
+    return(angle_in_radians);
+  }
+}
+
+// Function to correct the gyroX and gyroY bias (roll and pitch) using the gravity vector from accelerometers
+// We use the internal chip axis definition to make the bias correction because both sensors outputs (gyros and accels)
+// are in chip axis definition
+void AP_AHRS_MPU6000::drift_correction( float deltat )
+{
+	float errorRollPitch[2];
+
+	// Get current values for gyros
+	_accel_vector = _imu->get_accel();
+
+	// We take the accelerometer readings and cumulate to average them and obtain the gravity vector
+	_accel_filtered += _accel_vector;
+	_accel_filtered_samples++;
+
+	_gyro_bias_from_gravity_counter++;
+	// We make the bias calculation and correction at a lower rate (GYRO_BIAS_FROM_GRAVITY_RATE)
+	if( _gyro_bias_from_gravity_counter == GYRO_BIAS_FROM_GRAVITY_RATE ){
+		_gyro_bias_from_gravity_counter = 0;
+
+		_accel_filtered /= _accel_filtered_samples;  // average
+
+		// Adjust ground reference : Accel Cross Gravity to obtain the error between gravity from accels and gravity from attitude solution
+		// errorRollPitch are in Accel LSB units
+		errorRollPitch[0] = _accel_filtered.y * _dcm_matrix.c.z + _accel_filtered.z * _dcm_matrix.c.x;
+		errorRollPitch[1] = -_accel_filtered.z * _dcm_matrix.c.y - _accel_filtered.x * _dcm_matrix.c.z;
+
+		errorRollPitch[0] *= deltat * 1000;
+		errorRollPitch[1] *= deltat * 1000;
+
+		// we limit to maximum gyro drift rate on each axis
+		float drift_limit = _mpu6000->get_gyro_drift_rate() * deltat / _gyro_bias_from_gravity_gain;  //0.65*0.04 / 0.005 = 5.2
+		errorRollPitch[0] = constrain(errorRollPitch[0], -drift_limit, drift_limit);
+		errorRollPitch[1] = constrain(errorRollPitch[1], -drift_limit, drift_limit);
+
+		// We correct gyroX and gyroY bias using the error vector
+		_gyro_bias[0] += errorRollPitch[0]*_gyro_bias_from_gravity_gain;
+		_gyro_bias[1] += errorRollPitch[1]*_gyro_bias_from_gravity_gain;
+		
+		// TO-DO: fix this.  Currently it makes the roll and pitch drift more!
+		// If bias values are greater than 1 LSB we update the hardware offset registers
+		if( fabs(_gyro_bias[0])>1.0 ){
+			//_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[0],0,0);
+			//_mpu6000->set_gyro_offsets(0,-1*(int)_gyro_bias[0],0);
+			//_gyro_bias[0] -= (int)_gyro_bias[0];  // we remove the part that we have already corrected on registers...
+		}
+		if (fabs(_gyro_bias[1])>1.0){
+			//_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[1],0,0);
+			//_gyro_bias[1] -= (int)_gyro_bias[1];
+		}
+
+		// Reset the accelerometer variables
+		_accel_filtered.x = 0;
+		_accel_filtered.y = 0;
+		_accel_filtered.z = 0;
+		_accel_filtered_samples=0;
+	}
+
+	// correct the yaw
+	drift_correction_yaw();
+}
+
+
+/*
+  reset the DCM matrix and omega. Used on ground start, and on
+  extreme errors in the matrix
+ */
+void
+AP_AHRS_MPU6000::reset(bool recover_eulers)
+{
+	// if the caller wants us to try to recover to the current
+	// attitude then calculate the dcm matrix from the current
+	// roll/pitch/yaw values
+	if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
+		_dcm_matrix.from_euler(roll, pitch, yaw);
+	} else {
+		// otherwise make it flat
+		_dcm_matrix.from_euler(0, 0, 0);
+	}
+}
+
+// push offsets down from IMU to INS (required so MPU6000 can perform it's own attitude estimation)
+void
+AP_AHRS_MPU6000::push_offsets_to_ins()
+{
+	// push down gyro offsets (TO-DO: why are x and y offsets are reversed?!)
+	_mpu6000->set_gyro_offsets_scaled(((AP_IMU_INS*)_imu)->gy(),((AP_IMU_INS*)_imu)->gx(),((AP_IMU_INS*)_imu)->gz());
+	((AP_IMU_INS*)_imu)->gx(0);
+	((AP_IMU_INS*)_imu)->gy(0);
+	((AP_IMU_INS*)_imu)->gz(0);
+
+	// push down accelerometer offsets (TO-DO: why are x and y offsets are reversed?!)
+	_mpu6000->set_accel_offsets_scaled(((AP_IMU_INS*)_imu)->ay(), ((AP_IMU_INS*)_imu)->ax(), ((AP_IMU_INS*)_imu)->az());
+	//((AP_IMU_INS*)_imu)->ax(0);
+	//((AP_IMU_INS*)_imu)->ay(0);
+	//((AP_IMU_INS*)_imu)->az(0);
+}
+
+void
+AP_AHRS_MPU6000::push_gains_to_dmp()
+{
+	uint8_t gain;
+	if( _kp.get() >= 1.0 ) {
+		gain = 0xFF;
+	}else if( _kp.get() <= 0.0 ) {
+		gain = 0x00;
+	}else{
+		gain = (uint8_t)((float)0xFF * _kp.get());
+	}
+
+	_mpu6000->dmp_set_sensor_fusion_accel_gain(gain);
+}
+
+// produce a yaw error value. The returned value is proportional
+// to sin() of the current heading error in earth frame
+float
+AP_AHRS_MPU6000::yaw_error_compass(void)
+{
+	Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z);
+	// get the mag vector in the earth frame
+	Vector3f rb = _dcm_matrix * mag;
+
+	rb.normalize();
+	if (rb.is_inf()) {
+		// not a valid vector
+		return 0.0;
+	}
+
+	// get the earths magnetic field (only X and Y components needed)
+	Vector3f mag_earth = Vector3f(cos(_compass->get_declination()),
+				      sin(_compass->get_declination()), 0);
+
+	// calculate the error term in earth frame
+	Vector3f error = rb % mag_earth;
+
+	return error.z;
+}
+
+//
+// drift_correction_yaw - yaw drift correction using the compass
+//
+void
+AP_AHRS_MPU6000::drift_correction_yaw(void)
+{
+	static float yaw_last_uncorrected = HEADING_UNKNOWN;
+	static float yaw_corrected = HEADING_UNKNOWN;
+	float yaw_delta = 0;
+	bool new_value = false;
+	float yaw_error;
+	float yaw_deltat;
+	static float heading;
+
+	// we assume the DCM matrix is completely uncorrect for yaw
+	// retrieve how much heading has changed according to non-corrected dcm
+	if( yaw_last_uncorrected != HEADING_UNKNOWN ) {
+		yaw_delta = wrap_PI(yaw - yaw_last_uncorrected);	// the change in heading according to the gyros only since the last interation
+		yaw_last_uncorrected = yaw;
+	}
+
+	// initialise yaw_corrected (if necessary)
+	if( yaw_corrected != HEADING_UNKNOWN ) {
+		yaw_corrected = yaw;
+	}else{
+		yaw_corrected = wrap_PI(yaw_corrected + yaw_delta);	// best guess of current yaw is previous iterations corrected yaw + yaw change from gyro
+		_dcm_matrix.from_euler(roll, pitch, yaw_corrected);		// rebuild dcm matrix with best guess at current yaw
+	}
+
+	// if we have new compass data
+	if( _compass && _compass->use_for_yaw() ) {
+		if (_compass->last_update != _compass_last_update) {
+			yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6;
+			_compass_last_update = _compass->last_update;
+			heading = _compass->calculate_heading(_dcm_matrix);
+			if( !_have_initial_yaw ) {
+				yaw_corrected = heading;
+				_have_initial_yaw = true;
+				_dcm_matrix.from_euler(roll, pitch, yaw_corrected);		// rebuild dcm matrix with best guess at current yaw
+			}
+			new_value = true;
+		}
+	}
+
+	// perform the yaw correction
+	if( new_value ){
+		yaw_error = yaw_error_compass();
+		// the yaw error is a vector in earth frame
+		//Vector3f error = Vector3f(0,0, yaw_error);
+
+		// convert the error vector to body frame
+		//error = _dcm_matrix.mul_transpose(error);
+
+		// Update the differential component to reduce the error (it´s like a P control)
+		yaw_corrected += wrap_PI(yaw_error * _kp_yaw.get());		// probably completely wrong
+
+		// rebuild the dcm matrix yet again
+		_dcm_matrix.from_euler(roll, pitch, yaw_corrected);
+	}
+}
+
+// calculate the euler angles which will be used for high level
+// navigation control
+void
+AP_AHRS_MPU6000::euler_angles(void)
+{
+	_dcm_matrix.to_euler(&roll, &pitch, &yaw);
+	//quaternion.to_euler(&roll, &pitch, &yaw);	// cannot use this because the quaternion is not correct for yaw drift
+
+	roll_sensor 	= degrees(roll)  * 100;
+	pitch_sensor 	= degrees(pitch) * 100;
+	yaw_sensor 		= degrees(yaw)   * 100;
+
+	if (yaw_sensor < 0)
+		yaw_sensor += 36000;
+}
+
+/* reporting of DCM state for MAVLink */
+
+// average error_roll_pitch since last call
+float AP_AHRS_MPU6000::get_error_rp(void)
+{
+	// not yet supported with DMP
+	return 0.0;
+}
+
+// average error_yaw since last call
+float AP_AHRS_MPU6000::get_error_yaw(void)
+{
+	// not yet supported with DMP
+	return 0.0;
+}

libraries/AP_AHRS/AP_AHRS_MPU6000.h

@@ -0,0 +1,108 @@
+#ifndef AP_AHRS_MPU6000_H
+#define AP_AHRS_MPU6000_H
+/*
+  DCM based AHRS (Attitude Heading Reference System) interface for
+  ArduPilot
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+*/
+
+#define GYRO_BIAS_FROM_GRAVITY_RATE 4  // Rate of the gyro bias from gravity correction (200Hz/4) => 50Hz
+#define HEADING_UNKNOWN -9999             // Initial value to detect that compass correction is not initialized
+
+// max rate of gyro drift in gyro_LSB_units/s (16.4LSB = 1deg/s)
+static const float _MPU6000_gyro_drift_rate = 0.5; // This correspond to 0.03 degrees/s/s;
+
+class AP_AHRS_MPU6000 : public AP_AHRS
+{
+public:
+	// Constructors
+	AP_AHRS_MPU6000(AP_IMU_INS *imu, GPS *&gps, AP_InertialSensor_MPU6000 *mpu6000) : AP_AHRS(imu, gps), _mpu6000(mpu6000)
+	{
+		_dcm_matrix.identity();
+
+		// these are experimentally derived from the simulator
+		// with large drift levels
+		_ki = 0.0087;
+		_ki_yaw = 0.01;
+
+		_kp.set(0.4);
+		_kp_yaw.set(0.4);
+
+		// dmp related variable initialisation
+		_gyro_bias_from_gravity_gain = 0.008;
+		_compass_bias_time = 0;
+	}
+
+	// initialisation routine to start MPU6000's dmp
+	void init();
+
+	// return the smoothed gyro vector corrected for drift
+	Vector3f	get_gyro(void) {return _imu->get_gyro(); }
+	Matrix3f	get_dcm_matrix(void) {return _dcm_matrix; }
+
+	// return the current drift correction integrator value
+	Vector3f	get_gyro_drift(void) {return _omega_I; }
+
+	// Methods
+	void 		update(void);
+	void 		reset(bool recover_eulers = false);
+
+	// push offsets down from IMU to INS (required so MPU6000 can perform it's own attitude estimation)
+	void		push_offsets_to_ins();
+	void		push_gains_to_dmp();
+
+	// status reporting
+	float		get_error_rp(void);
+	float		get_error_yaw(void);
+
+	// settable parameters
+	AP_Float	_kp_yaw;
+	AP_Float	_kp;
+	AP_Float	gps_gain;	// not currently supported
+
+	// for holding parameters
+	static const struct AP_Param::GroupInfo var_info[];
+
+private:
+	float		_ki;
+	float		_ki_yaw;
+	bool		_have_initial_yaw;
+	AP_InertialSensor_MPU6000	*_mpu6000;
+
+	// Methods
+	void 		drift_correction(float deltat);
+
+	// Compass correction variables. TO-DO: move or replace?
+	float		wrap_PI(float angle_in_radians);	// TO-DO: move this to standard AP_AHRS methods
+    long		_compass_bias_time;
+	
+	void 		drift_correction_yaw(void);
+	float		yaw_error_compass();
+	void 		euler_angles(void);
+
+	Vector3f	_accel_filtered;
+	int16_t 	_accel_filtered_samples;
+	float		_gyro_bias[3];						// bias_tracking
+	float		_gyro_bias_from_gravity_gain;		// bias correction algorithm gain
+	uint8_t		_gyro_bias_from_gravity_counter;
+	
+	// primary representation of attitude
+	Matrix3f	_dcm_matrix;
+
+	Vector3f 	_accel_vector;		// current accel vector
+
+	Vector3f 	_omega_I;			// Omega Integrator correction
+	Vector3f	_omega_I_sum;
+	float		_omega_I_sum_time;
+
+	// state to support status reporting
+	float		_error_yaw_sum;
+	uint16_t	_error_yaw_count;
+	float		_error_yaw_last;
+};
+
+#endif // AP_AHRS_MPU6000_H