imu enhancements

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

libraries/AP_DCM/AP_DCM.cpp

@@ -94,11 +94,12 @@ AP_DCM::matrix_update(float _G_Dt)
 	//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)))
+		(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		
+	_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.
@@ -222,14 +223,14 @@ void
 AP_DCM::drift_correction(void)
 {
 	//Compensation the Roll, Pitch and Yaw drift. 
-	float mag_heading_x;
-	float mag_heading_y;
+	//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 float scaled_omega_P[3];
+	//static float scaled_omega_I[3];
 	static bool in_motion = false;
 	Matrix3f rot_mat;
 	
@@ -311,7 +312,7 @@ AP_DCM::drift_correction(void)
 	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("*");
 }
 
 

libraries/AP_IMU/AP_IMU.cpp

@@ -95,7 +95,10 @@ AP_IMU::init_gyro(void)
 			
 			// filter
 			_adc_offset[j] = _adc_offset[j] * 0.9 + _adc_in[j] * 0.1;
+			//Serial.print(_adc_offset[j], 1);
+			//Serial.print(", ");
 		}
+		//Serial.println(" ");
 
 		delay(20);
 		if(flashcount == 5) {
@@ -111,7 +114,8 @@ AP_IMU::init_gyro(void)
 		}
 		flashcount++;
 	}
-	
+	Serial.println(" ");
+
 	_adc_offset[5] += GRAVITY * _sensor_signs[5];
 	save_gyro_eeprom();
 }
@@ -125,20 +129,31 @@ AP_IMU::init_accel(void) // 3, 4, 5
  	delay(500);
 
 	Serial.println("Init Accel");
-	
-	for(int c = 0; c < 200; c++){
-		for (int i = 0; i < 6; i++)
-			_adc_in[i] = _adc->Ch(_sensors[i]);
-	}
 
+	for (int j = 3; j <= 5; j++){
+		_adc_in[j] 		= _adc->Ch(_sensors[j]);
+		_adc_in[j] 		-= 2025;
+		_adc_offset[j]	= _adc_in[j];
+	}
+	
 	for(int i = 0; i < 200; i++){		// We take some readings...
+		
+		delay(20);
+		
 		for (int j = 3; j <= 5; j++){
 			_adc_in[j] 		= _adc->Ch(_sensors[j]);
 			_adc_in[j] 		-= 2025;
 			_adc_offset[j]	= _adc_offset[j] * 0.9 + _adc_in[j] * 0.1;
+			//Serial.print(j);
+			//Serial.print(": ");
+			//Serial.print(_adc_in[j], 1);
+			//Serial.print(" | ");
+			//Serial.print(_adc_offset[j], 1);
+			//Serial.print(", ");
 		}
+		
+		//Serial.println(" ");
 
-		delay(20);
 		if(flashcount == 5) {
 			Serial.print("*");
 			digitalWrite(A_LED_PIN, LOW);
@@ -152,6 +167,7 @@ AP_IMU::init_accel(void) // 3, 4, 5
 		}
 		flashcount++;
 	}
+	Serial.println(" ");
 	_adc_offset[5] += GRAVITY * _sensor_signs[5];
 	save_accel_eeprom();
 }
@@ -254,6 +270,29 @@ AP_IMU::save_accel_eeprom(void)
 	write_EE_float(_adc_offset[5], _address + 20);
 }
 
+void 
+AP_IMU::print_accel_offsets(void)
+{
+	Serial.print("Accel offsets: ");
+	Serial.print(_adc_offset[3], 2);
+	Serial.print(", ");
+	Serial.print(_adc_offset[4], 2);
+	Serial.print(", ");
+	Serial.println(_adc_offset[5], 2);
+}
+
+void 
+AP_IMU::print_gyro_offsets(void)
+{
+	Serial.print("Gyro offsets: ");
+	Serial.print(_adc_offset[0], 2);
+	Serial.print(", ");
+	Serial.print(_adc_offset[1], 2);
+	Serial.print(", ");
+	Serial.println(_adc_offset[2], 2);
+}
+
+
 
 /********************************************************************************/
 

libraries/AP_IMU/AP_IMU.h

@@ -27,7 +27,9 @@ public:
 	void		load_gyro_eeprom(void);
 	void		save_gyro_eeprom(void);
 	void		load_accel_eeprom(void);
-	void		save_accel_eeprom(void);	
+	void		save_accel_eeprom(void);
+	void 		print_accel_offsets(void);
+	void 		print_gyro_offsets(void);
 
 	// raw ADC values - called by DCM
 	Vector3f	get_gyro(void);									// Radians/second