fix bugs from factoring imu object.

Tested & working - integration and roll/pitch drift correction
Untested - yaw drift correction and centrepetal comp

git-svn-id: https://arducopter.googlecode.com/svn/trunk@729 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/AP_DCM/AP_DCM_FW.cpp

@@ -7,8 +7,8 @@
 #define ToRad(x) (x*0.01745329252)	// *pi/180
 #define ToDeg(x) (x*57.2957795131)	// *180/pi
 
-#define Kp_ROLLPITCH 0.0014	 		// .0014 Pitch&Roll Drift Correction Proportional Gain
-#define Ki_ROLLPITCH 0.0000003 		// 0.0000003 Pitch&Roll Drift Correction Integrator Gain
+#define Kp_ROLLPITCH 0.5852	 		// .0014 * 418 Pitch&Roll Drift Correction Proportional Gain
+#define Ki_ROLLPITCH 0.0001254 		// 0.0000003 * 418 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
 
@@ -154,7 +154,7 @@ AP_DCM_FW::_accel_adjust(void)
 	Vector3f _veloc, _temp;
 	float _vel;
 	
-	_veloc.x = _gps->ground_speed / 100;
+	_veloc.x = (_gps->ground_speed / 100) / 9.81;		// We are working with acceleration in g units
 	
 	//_accel_vector += _omega_integ_corr % _veloc;		// Equation 26  This line is giving the compiler a problem so we break it up below
 	_temp.y = _omega_integ_corr.z * _veloc.x; 			// only computing the non-zero terms
@@ -253,9 +253,9 @@ AP_DCM_FW::drift_correction(void)
 
 	// error_roll_pitch are in Accel ADC units
 	// Limit max error_roll_pitch to limit max omega_P and omega_I
-	_error_roll_pitch.x = constrain(_error_roll_pitch.x, -50, 50);
-	_error_roll_pitch.y = constrain(_error_roll_pitch.y, -50, 50);
-	_error_roll_pitch.z = constrain(_error_roll_pitch.z, -50, 50);
+	_error_roll_pitch.x = constrain(_error_roll_pitch.x, -.1196f, .1196f);
+	_error_roll_pitch.y = constrain(_error_roll_pitch.y, -.1196f, .1196f);
+	_error_roll_pitch.z = constrain(_error_roll_pitch.z, -.1196f, .1196f);
 
 	_omega_P = _error_roll_pitch * (Kp_ROLLPITCH * accel_weight);
 	_omega_I += _error_roll_pitch * (Ki_ROLLPITCH * accel_weight);

libraries/AP_IMU/AP_IMU.cpp

@@ -8,7 +8,7 @@
 // ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412
 // Tested value : 418
 #define GRAVITY 418 //this equivalent to 1G in the raw data coming from the accelerometer 
-#define accel_scale(x) x*(9.81/GRAVITY)//Scaling the raw data of the accel to actual acceleration in meters per second squared
+#define accel_scale(x) (x/GRAVITY)//Scaling the raw data of the accel to actual acceleration in meters per second squared
 
 #define ToRad(x) (x*0.01745329252)	// *pi/180
 #define ToDeg(x) (x*57.2957795131)	// *180/pi
@@ -74,20 +74,18 @@ AP_IMU::init(void)
 		digitalWrite(A_LED_PIN, HIGH);
 		delay(20);
 	}
-	
-	for(int y = 0; y <= 5; y++){	 // Read first initial ADC values for offset.
-		_adc_offset[y] = _adc_in[y];
-	}
 
 	for(int i = 0; i < 200; i++){		// We take some readings...
 		for (int j = 0; j < 6; j++) {
 			_adc_in[j] = APM_ADC.Ch(_sensors[j]);
-			_adc_in[j] -= _gyro_temp_curve[j][tc_temp];		// Subtract temp compensated typical gyro bias
-			if (_sensor_signs[j] < 0)
-				temp = (_adc_offset[j] - _adc_in[j]);
-			else
-				temp = (_adc_in[j] - _adc_offset[j]);
-			_adc_offset[j] = _adc_offset[j] * 0.9 + temp * 0.1;
+			if (j < 3) {	
+				_adc_in[j] -= _gyro_temp_comp(j, tc_temp);		// Subtract temp compensated typical gyro bias
+			} else {
+				_adc_in[j] -= 2025;
+			}
+				
+			_adc_offset[j] = _adc_offset[j] * 0.9 + _adc_in[j] * 0.1;
+
 		}
 
 		delay(20);
@@ -106,30 +104,39 @@ AP_IMU::init(void)
 	_adc_offset[5] += GRAVITY * _sensor_signs[5];
 	
 //  NOTE ***  Need to addd code to save values to EEPROM
-
 }
 
 
+/**************************************************/
+// Returns the temperature compensated raw gyro value
+//---------------------------------------------------
+float
+AP_IMU::_gyro_temp_comp(int i, int temp) const
+{
+	// We use a 2nd order curve of the form Gtc = A + B * Graw + C * (Graw)**2
+	//------------------------------------------------------------------------
+	return _gyro_temp_curve[i][0] + _gyro_temp_curve[i][1] * temp + _gyro_temp_curve[i][2] * temp * temp;	
+}
 /**************************************************/
 Vector3f
 AP_IMU::get_gyro(void)
 {	
-	float temp;
 	int tc_temp = APM_ADC.Ch(_gyro_temp_ch);
 	
 	for (int i = 0; i < 3; i++) {
 		_adc_in[i] = APM_ADC.Ch(_sensors[i]);
-		_adc_in[i] -= _gyro_temp_curve[i][tc_temp];		// Subtract temp compensated typical gyro bias
+		_adc_in[i] -= _gyro_temp_comp(i,tc_temp);		// Subtract temp compensated typical gyro bias
 		if (_sensor_signs[i] < 0)
-			temp = (_adc_offset[i] - _adc_in[i]);
+			_adc_in[i] = (_adc_offset[i] - _adc_in[i]);
 		else
-			temp = (_adc_in[i] - _adc_offset[i]);		
-		if (abs(temp) > ADC_CONSTRAINT) {
-			adc_constraints++; 												// We keep track of the number of times		
-			_adc_in[i] = constrain(temp, -ADC_CONSTRAINT, ADC_CONSTRAINT);	// Throw out nonsensical values
+			_adc_in[i] = (_adc_in[i] - _adc_offset[i]);
+			
+		if (abs(_adc_in[i]) > ADC_CONSTRAINT) {
+			adc_constraints++; 														// We keep track of the number of times		
+			_adc_in[i] = constrain(_adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT);	// Throw out nonsensical values
 		}
 	}
-	
+
 	_gyro_vector.x = ToRad(_gyro_gain_x) * _adc_in[0];
 	_gyro_vector.y = ToRad(_gyro_gain_y) * _adc_in[1];
 	_gyro_vector.z = ToRad(_gyro_gain_z) * _adc_in[2];
@@ -141,18 +148,16 @@ AP_IMU::get_gyro(void)
 Vector3f
 AP_IMU::get_accel(void)
 {	
-	float temp;
-	
 	for (int i = 3; i < 6; i++) {
 		_adc_in[i] = APM_ADC.Ch(_sensors[i]);
 		_adc_in[i] -= 2025;								// Subtract typical accel bias
 		if (_sensor_signs[i] < 0)
-			temp = (_adc_offset[i] - _adc_in[i]);
+			_adc_in[i] = (_adc_offset[i] - _adc_in[i]);
 		else
-			temp = (_adc_in[i] - _adc_offset[i]);		
-		if (abs(temp) > ADC_CONSTRAINT) {
+			_adc_in[i] = (_adc_in[i] - _adc_offset[i]);		
+		if (abs(_adc_in[i]) > ADC_CONSTRAINT) {
 			adc_constraints++; 												// We keep track of the number of times		
-			_adc_in[i] = constrain(temp, -ADC_CONSTRAINT, ADC_CONSTRAINT);	// Throw out nonsensical values
+			_adc_in[i] = constrain(_adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT);	// Throw out nonsensical values
 		}
 	}