ardupilot/libraries/AP_DCM/AP_DCM.cpp

/*
	APM_DCM_FW.cpp - DCM AHRS Library, fixed wing version, for Ardupilot Mega
		Code by Doug Weibel, Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com

	This library works with the ArduPilot Mega and "Oilpan"
	
	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.

        Methods:
				update_DCM(_G_Dt)	: Updates the AHRS by integrating the rotation matrix over time _G_Dt using the IMU object data
				get_gyro()			: Returns gyro vector corrected for bias
				get_accel()		: Returns accelerometer vector
				get_dcm_matrix()	: Returns dcm matrix

*/
#include <AP_DCM.h>

#define OUTPUTMODE 1				// This is just used for debugging, remove later
#define ToRad(x) (x*0.01745329252)	// *pi/180
#define ToDeg(x) (x*57.2957795131)	// *180/pi

#define Kp_ROLLPITCH 0.05967 		// .0014 * 418/9.81 Pitch&Roll Drift Correction Proportional Gain
#define Ki_ROLLPITCH 0.00001278		// 0.0000003 * 418/9.81 Pitch&Roll Drift Correction Integrator Gain
#define Kp_YAW 0.8		 			// Yaw Drift Correction Porportional Gain	
#define Ki_YAW 0.00004 				// Yaw Drift CorrectionIntegrator Gain

#define SPEEDFILT 300				// centimeters/second
#define ADC_CONSTRAINT 900


void
AP_DCM::set_compass(Compass *compass)
{
	_compass = compass;
}

/**************************************************/
void
AP_DCM::update_DCM(float _G_Dt)
{
	_gyro_vector 	= _imu->get_gyro();			// Get current values for IMU sensors
	_accel_vector 	= _imu->get_accel();			// Get current values for IMU sensors
	
	matrix_update(_G_Dt); 	// Integrate the DCM matrix
	normalize();			// Normalize the DCM matrix
	drift_correction();		// Perform drift correction
	
	euler_angles();			// Calculate pitch, roll, yaw for stabilization and navigation
}

/**************************************************/
Vector3f
AP_DCM::get_gyro(void)
{
	return _omega_integ_corr;
}	// We return the raw gyro vector corrected for bias

/**************************************************/
Vector3f
AP_DCM::get_accel(void)
{
	return _accel_vector;
}

/**************************************************/
Matrix3f
AP_DCM::get_dcm_matrix(void)
{
	return _dcm_matrix;
}


    //For Debugging
/*
void
printm(const char *l, Matrix3f &m)
{ 	Serial.println(" "); Serial.println(l);
	Serial.print(m.a.x, 12); Serial.print(" "); Serial.print(m.a.y, 12); Serial.print(" "); Serial.println(m.a.z, 12);
	Serial.print(m.b.x, 12); Serial.print(" "); Serial.print(m.b.y, 12); Serial.print(" "); Serial.println(m.b.z, 12);
	Serial.print(m.c.x, 12); Serial.print(" "); Serial.print(m.c.y, 12); Serial.print(" "); Serial.println(m.c.z, 12);  
	Serial.print(*(uint32_t *)&(m.a.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.a.y), HEX); Serial.print(" ");  Serial.println(*(uint32_t *)&(m.a.z), HEX);
	Serial.print(*(uint32_t *)&(m.b.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.b.y), HEX); Serial.print(" ");  Serial.println(*(uint32_t *)&(m.b.z), HEX);
	Serial.print(*(uint32_t *)&(m.c.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.c.y), HEX); Serial.print(" ");  Serial.println(*(uint32_t *)&(m.c.z), HEX);
}
*/
 
/**************************************************/
void 
AP_DCM::matrix_update(float _G_Dt)
{
	Matrix3f	update_matrix;
	Matrix3f	temp_matrix;
	
	//Record when you saturate any of the gyros.
	if((fabs(_gyro_vector.x) >= radians(300)) || 
		(fabs(_gyro_vector.y) >= radians(300)) || 
		(fabs(_gyro_vector.z) >= radians(300))){
		gyro_sat_count++;
	}

	_omega_integ_corr 	= _gyro_vector 		+ _omega_I;		// Used for _centripetal correction (theoretically better than _omega)
	_omega 				= _omega_integ_corr + _omega_P;		// Equation 16, adding proportional and integral correction terms		

	if(_centripetal){
		accel_adjust();				// Remove _centripetal acceleration.
	}
	
 #if OUTPUTMODE == 1				 
	update_matrix.a.x = 0;
	update_matrix.a.y = -_G_Dt * _omega.z; 		// -delta Theta z
	update_matrix.a.z =  _G_Dt * _omega.y; 		// delta Theta y
	update_matrix.b.x =  _G_Dt * _omega.z; 		// delta Theta z
	update_matrix.b.y = 0;
	update_matrix.b.z = -_G_Dt * _omega.x; 		// -delta Theta x
	update_matrix.c.x = -_G_Dt * _omega.y; 		// -delta Theta y
	update_matrix.c.y =  _G_Dt * _omega.x; 		// delta Theta x
	update_matrix.c.z = 0;
 #else										// Uncorrected data (no drift correction)				 
	update_matrix.a.x = 0;
	update_matrix.a.y = -_G_Dt * _gyro_vector.z; 		
	update_matrix.a.z =  _G_Dt * _gyro_vector.y;
	update_matrix.b.x =  _G_Dt * _gyro_vector.z; 		
	update_matrix.b.y = 0;
	update_matrix.b.z = -_G_Dt * _gyro_vector.x; 		
	update_matrix.c.x = -_G_Dt * _gyro_vector.y; 		
	update_matrix.c.y =  _G_Dt * _gyro_vector.x; 		
	update_matrix.c.z = 0;
 #endif

	temp_matrix = _dcm_matrix * update_matrix;
	_dcm_matrix = _dcm_matrix + temp_matrix;		// Equation 17
		
}


/**************************************************/
void 
AP_DCM::accel_adjust(void)
{
	Vector3f veloc, temp;
	float vel;
	
	veloc.x = _gps->ground_speed / 100;		// We are working with acceleration in m/s^2 units
	
	// We are working with a modified version of equation 26 as our IMU object reports acceleration in the positive axis direction as positive

	//_accel_vector -= _omega_integ_corr % _veloc;		// Equation 26  This line is giving the compiler a problem so we break it up below
	temp.x = 0;
	temp.y = _omega_integ_corr.z * veloc.x; 			// only computing the non-zero terms
	temp.z = -1.0f * _omega_integ_corr.y * veloc.x;	// After looking at the compiler issue lets remove _veloc and simlify 

	_accel_vector -= temp;
}


/*************************************************
Direction Cosine Matrix IMU: Theory
William Premerlani and Paul Bizard

Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5 
to approximations rather than identities. In effect, the axes in the two frames of reference no 
longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a 
simple matter to stay ahead of it.
We call the process of enforcing the orthogonality conditions <EFBFBD>renormalization<EFBFBD>.
*/
void 
AP_DCM::normalize(void)
{
	float error = 0;
	Vector3f	temporary[3];
	
	int problem = 0;
	
	error = _dcm_matrix.a * _dcm_matrix.b; 							// eq.18

	temporary[0] = _dcm_matrix.b;	
	temporary[1] = _dcm_matrix.a;		
	temporary[0] = _dcm_matrix.a - (temporary[0] * (0.5f * error));		// eq.19
	temporary[1] = _dcm_matrix.b - (temporary[1] * (0.5f * error));		// eq.19

	temporary[2] = temporary[0] % temporary[1];							// c= a x b // eq.20

	_dcm_matrix.a = renorm(temporary[0], problem);
	_dcm_matrix.b = renorm(temporary[1], problem);
	_dcm_matrix.c = renorm(temporary[2], problem);
	
	if (problem == 1) {		// Our solution is blowing up and we will force back to initial condition.	Hope we are not upside down!
		_dcm_matrix.a.x = 1.0f;
		_dcm_matrix.a.y = 0.0f;
		_dcm_matrix.a.z = 0.0f;
		_dcm_matrix.b.x = 0.0f;
		_dcm_matrix.b.y = 1.0f;
		_dcm_matrix.b.z = 0.0f;
		_dcm_matrix.c.x = 0.0f;
		_dcm_matrix.c.y = 0.0f;
		_dcm_matrix.c.z = 1.0f;
	}
}

/**************************************************/
Vector3f
AP_DCM::renorm(Vector3f const &a, int &problem)
{
	float	renorm;

	renorm = a * a;
	
	if (renorm < 1.5625f && renorm > 0.64f) {			// Check if we are OK to use Taylor expansion
		renorm = 0.5 * (3 - renorm);					// eq.21
	} else if (renorm < 100.0f && renorm > 0.01f) {
		renorm = 1.0 / sqrt(renorm);
		renorm_sqrt_count++;
	} else {
		problem = 1;
		renorm_blowup_count++;
	}

	return(a * renorm);
}

/**************************************************/
void 
AP_DCM::drift_correction(void)
{
	//Compensation the Roll, Pitch and Yaw drift. 
	//float mag_heading_x;
	//float mag_heading_y;
	float error_course;
	float accel_magnitude;
	float accel_weight;
	float integrator_magnitude;
	//static float scaled_omega_P[3];
	//static float scaled_omega_I[3];
	static bool in_motion = false;
	Matrix3f rot_mat;
	
	//*****Roll and Pitch***************

	// Calculate the magnitude of the accelerometer vector
	accel_magnitude = _accel_vector.length() / 9.80665f;

	// Dynamic weighting of accelerometer info (reliability filter)
	// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
	accel_weight = constrain(1 - 2 * fabs(1 - accel_magnitude), 0, 1);	//	
	
	//	We monitor the amount that the accelerometer based drift correction is deweighted for performance reporting
	_health += constrain((0.02 * (accel_weight - .5)), 0, 1);

	// adjust the ground of reference 
	_error_roll_pitch =  _dcm_matrix.c % _accel_vector;			// Equation 27  *** sign changed from prev implementation???

	// error_roll_pitch are in Accel m/s^2 units
	// Limit max error_roll_pitch to limit max omega_P and omega_I
	_error_roll_pitch.x = constrain(_error_roll_pitch.x, -1.17f, 1.17f);
	_error_roll_pitch.y = constrain(_error_roll_pitch.y, -1.17f, 1.17f);
	_error_roll_pitch.z = constrain(_error_roll_pitch.z, -1.17f, 1.17f);

	_omega_P = _error_roll_pitch * (Kp_ROLLPITCH * accel_weight);
	_omega_I += _error_roll_pitch * (Ki_ROLLPITCH * accel_weight);

	
	//*****YAW***************
	
	if (_compass) {
		// We make the gyro YAW drift correction based on compass magnetic heading
		error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);	// Equation 23, Calculating YAW error	
		
	} else {
	
		// Use GPS Ground course to correct yaw gyro drift
		if (_gps->ground_speed >= SPEEDFILT) {
			_course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
			_course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
			if(in_motion) {
				error_course = (_dcm_matrix.a.x * _course_over_ground_y) - (_dcm_matrix.b.x * _course_over_ground_x);	// Equation 23, Calculating YAW error
			} else  {
				float cos_psi_err, sin_psi_err;
				// This is the case for when we first start moving and reset the DCM so that yaw matches the gps ground course
				// This is just to get a reasonable estimate faster
				yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
				cos_psi_err = cos(ToRad(_gps->ground_course/100.0) - yaw);
				sin_psi_err = sin(ToRad(_gps->ground_course/100.0) - yaw);
				// Rxx = cos psi err, Rxy = - sin psi err, Rxz = 0
				// Ryx = sin psi err, R	yy = cos psi err,   Ryz = 0
				// Rzx = Rzy = 0, Rzz = 1
				rot_mat.a.x = cos_psi_err;
				rot_mat.a.y = - sin_psi_err;
				rot_mat.b.x = sin_psi_err;
				rot_mat.b.y = cos_psi_err;
				rot_mat.a.z = 0;
				rot_mat.b.z = 0;
				rot_mat.c.x = 0;
				rot_mat.c.y = 0;
				rot_mat.c.z = 1.0;
				
				_dcm_matrix = rot_mat * _dcm_matrix;
				in_motion =  true;
				error_course = 0;
				
			}	
		} else {
			error_course = 0;
			in_motion = false;
		}	
	}
	
	_error_yaw = _dcm_matrix.c * error_course;	// Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
	
	_omega_P += _error_yaw * Kp_YAW;			// Adding yaw correction to proportional correction vector.
	_omega_I += _error_yaw * Ki_YAW;			// adding yaw correction to integrator correction vector.	 

	//	Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
	integrator_magnitude = _omega_I.length();
	if (integrator_magnitude > radians(300)) {
		_omega_I *= (0.5f * radians(300) / integrator_magnitude);		// Why do we have this discontinuous?  EG, why the 0.5?
	}
	//Serial.print("*");
}


/**************************************************/
void 
AP_DCM::euler_angles(void)
{
	#if (OUTPUTMODE == 2)				 // Only accelerometer info (debugging purposes)
	roll 		= atan2(_accel_vector.y, -_accel_vector.z);		// atan2(acc_y, acc_z)
	pitch 		= asin((_accel_vector.x) / (double)9.81); // asin(acc_x)
	yaw 			= 0;
	#else
	pitch 		= -asin(_dcm_matrix.c.x);
	roll 		= atan2(_dcm_matrix.c.y, _dcm_matrix.c.z);
	yaw 		= atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
	#endif
	
	roll_sensor 	= degrees(roll)  * 100;
	pitch_sensor 	= degrees(pitch) * 100;
	yaw_sensor 		= degrees(yaw)   * 100;

	if (yaw_sensor < 0)
		yaw_sensor += 36000;
}

/**************************************************/

float
AP_DCM::get_health(void)
{
	return _health;
}