corrections to imu and dcm libs, addition of vector member to compass class for magnetic vector

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

libraries/AP_Compass/AP_Compass.cpp

@@ -82,6 +82,9 @@ AP_Compass::update()
 		mag_Y =  ((((int)buff[2]) << 8) | buff[3]);		// Y axis
 		mag_Z = -((((int)buff[4]) << 8) | buff[5]);		// Z axis
 	}
+	mag.x = mag_X;
+	mag.y = mag_Y;
+	mag.z = mag_Z
 }
 
 void

libraries/AP_Compass/Compass.h

@@ -2,6 +2,7 @@
 #define Compass_h
 
 #include <inttypes.h>
+#include <AP_Math>
 
 class Compass
 {
@@ -10,6 +11,7 @@ class Compass
 	virtual void update();
 	virtual void calculate(float roll, float pitch);
 
+	Vector3f mag;
 	int16_t mag_X;
 	int16_t mag_Y;
 	int16_t mag_Z;

libraries/AP_DCM/AP_DCM_FW.cpp

@@ -82,33 +82,33 @@ AP_DCM_FW::update_DCM(float _G_Dt)
 
 /**************************************************/
 void
-AP_DCM_FW::quick_init(void)
+AP_DCM_FW::quick_init(uint16_t *_offset_address)
 {
-	_imu.quick_init();
+	_imu.quick_init(_offset_address);
 }
 /**************************************************/
 void
-AP_DCM_FW::init(void)
+AP_DCM_FW::init(uint16_t *_offset_address)
 {
-	_imu.init();
+	_imu.init(_offset_address);
 }
 
 
 /**************************************************/
 long
 AP_DCM_FW::get_roll_sensor(void)
-{	return degrees(roll) * 100;}
+{	return degrees(_roll) * 100;}
 
 /**************************************************/
 long
 AP_DCM_FW::get_pitch_sensor(void)
-{	return degrees(pitch) * 100;}
+{	return degrees(_pitch) * 100;}
 
 /**************************************************/
 long
 AP_DCM_FW::get_yaw_sensor(void)
 {	
-	long yaw_sensor = degrees(yaw) * 100;
+	long yaw_sensor = degrees(_yaw) * 100;
 	if (yaw_sensor < 0)	yaw_sensor += 36000;
 	return yaw_sensor;
 }
@@ -116,17 +116,17 @@ AP_DCM_FW::get_yaw_sensor(void)
 /**************************************************/
 float
 AP_DCM_FW::get_roll(void)
-{	return roll;}
+{	return _roll;}
 
 /**************************************************/
 float
 AP_DCM_FW::get_pitch(void)
-{	return pitch;}
+{	return _pitch;}
 
 /**************************************************/
 float
 AP_DCM_FW::get_yaw(void)
-{	return yaw;}
+{	return _yaw;}
 
 /**************************************************/
 Vector3f
@@ -138,12 +138,30 @@ Vector3f
 AP_DCM_FW::get_accels(void)
 {	return _accel_vector;}
 
+/**************************************************/
+Matrix3f
+AP_DCM_FW::get_dcm_matrix(void)
+{	return _dcm_matrix;}
+
+
+/*    For Debugging
+void
+printm(const char *l, Matrix3f &m)
+{ 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_FW::matrix_update(float _G_Dt)
 {
-	Matrix3f	_update_matrix;
-	static int8_t timer;
+	Matrix3f	update_matrix;
 	
 	//Record when you saturate any of the gyros.
 	if((abs(_gyro_vector.x) >= radians(300)) || 
@@ -154,115 +172,52 @@ AP_DCM_FW::matrix_update(float _G_Dt)
 	_omega_integ_corr = _gyro_vector + _omega_I;		// Used for centrep correction (theoretically better than _omega)
 	_omega = _omega_integ_corr + _omega_P;				// Equation 16, adding proportional and integral correction terms
 	
-	_accel_adjust();				// Remove centrifugal acceleration.
-	
-	
+	accel_adjust();				// Remove centrifugal 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;
+	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;
+	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
 	
-Serial.println("update matrix before");
-Serial.println(_update_matrix.a.x);
-Serial.println(_update_matrix.a.y);
-Serial.println(_update_matrix.a.z);
-Serial.println(_update_matrix.b.x);
-Serial.println(_update_matrix.b.y);
-Serial.println(_update_matrix.b.z);
-Serial.println(_update_matrix.c.x);
-Serial.println(_update_matrix.c.y);
-Serial.println(_update_matrix.c.z);
-Serial.println("dcm matrix before");
-Serial.println(_dcm_matrix.a.x);
-Serial.println(_dcm_matrix.a.y);
-Serial.println(_dcm_matrix.a.z);
-Serial.println(_dcm_matrix.b.x);
-Serial.println(_dcm_matrix.b.y);
-Serial.println(_dcm_matrix.b.z);
-Serial.println(_dcm_matrix.c.x);
-Serial.println(_dcm_matrix.c.y);
-Serial.println(_dcm_matrix.c.z);
-	// update
-	_update_matrix = _dcm_matrix * _update_matrix;		// Equation 17
-	
-Serial.println("update matrix middle");
-Serial.println(_update_matrix.a.x,12);
-Serial.println(_update_matrix.a.y,12);
-Serial.println(_update_matrix.a.z,12);
-Serial.println(_update_matrix.b.x,12);
-Serial.println(_update_matrix.b.y,12);
-Serial.println(_update_matrix.b.z,12);
-Serial.println(_update_matrix.c.x,12);
-Serial.println(_update_matrix.c.y,12);
-Serial.println(_update_matrix.c.z,12);
-Serial.println("dcm matrix middle");
-Serial.println(_dcm_matrix.a.x,12);
-Serial.println(_dcm_matrix.a.y,12);
-Serial.println(_dcm_matrix.a.z,12);
-Serial.println(_dcm_matrix.b.x,12);
-Serial.println(_dcm_matrix.b.y,12);
-Serial.println(_dcm_matrix.b.z,12);
-Serial.println(_dcm_matrix.c.x,12);
-Serial.println(_dcm_matrix.c.y,12);
-Serial.println(_dcm_matrix.c.z,12);
-
-	_dcm_matrix = _dcm_matrix + _update_matrix;		// Equation 17
+	_dcm_matrix = _dcm_matrix + update_matrix;		// Equation 17
 		
-Serial.println("update matrix after");
-Serial.println(_update_matrix.a.x);
-Serial.println(_update_matrix.a.y);
-Serial.println(_update_matrix.a.z);
-Serial.println(_update_matrix.b.x);
-Serial.println(_update_matrix.b.y);
-Serial.println(_update_matrix.b.z);
-Serial.println(_update_matrix.c.x);
-Serial.println(_update_matrix.c.y);
-Serial.println(_update_matrix.c.z);
-Serial.println("dcm matrix after");
-Serial.println(_dcm_matrix.a.x);
-Serial.println(_dcm_matrix.a.y);
-Serial.println(_dcm_matrix.a.z);
-Serial.println(_dcm_matrix.b.x);
-Serial.println(_dcm_matrix.b.y);
-Serial.println(_dcm_matrix.b.z);
-Serial.println(_dcm_matrix.c.x);
-Serial.println(_dcm_matrix.c.y);
-Serial.println(_dcm_matrix.c.z);
 }
 
 
 /**************************************************/
 void 
-AP_DCM_FW::_accel_adjust(void)
+AP_DCM_FW::accel_adjust(void)
 {
-	Vector3f _veloc, _temp;
-	float _vel;
+	Vector3f veloc, temp;
+	float vel;
 	
-	_veloc.x = _gps->ground_speed / 100;		// We are working with acceleration in m/s^2 units
+	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.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 
+	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;
+	_accel_vector -= temp;
 
 }
 
@@ -285,9 +240,9 @@ AP_DCM_FW::normalize(void)
 
 	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);
+	_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;
@@ -299,24 +254,12 @@ AP_DCM_FW::normalize(void)
 		_dcm_matrix.c.x = 0.0f;
 		_dcm_matrix.c.y = 0.0f;
 		_dcm_matrix.c.z = 1.0f;
-Serial.println("Solution blew up");	
-/*
-Serial.println(_dcm_matrix.a.x);
-Serial.println(_dcm_matrix.a.y);
-Serial.println(_dcm_matrix.a.z);
-Serial.println(_dcm_matrix.b.x);
-Serial.println(_dcm_matrix.b.y);
-Serial.println(_dcm_matrix.b.z);
-Serial.println(_dcm_matrix.c.x);
-Serial.println(_dcm_matrix.c.y);
-Serial.println(_dcm_matrix.c.z);
-*/
 	}
 }
 
 /**************************************************/
 Vector3f
-AP_DCM_FW::_renorm(Vector3f const &a, int &problem)
+AP_DCM_FW::renorm(Vector3f const &a, int &problem)
 {
 	float	renorm;
 
@@ -349,7 +292,7 @@ AP_DCM_FW::drift_correction(void)
 	float accel_weight;
 	float integrator_magnitude;
 	static bool in_motion = FALSE;
-	Matrix3f _rot_mat;
+	Matrix3f rot_mat;
 	
 	//*****Roll and Pitch***************
 
@@ -384,48 +327,39 @@ AP_DCM_FW::drift_correction(void)
 		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));
-Serial.println(_dcm_matrix.a.x);
-Serial.println(_dcm_matrix.a.y);
-Serial.println(_dcm_matrix.a.z);
-Serial.println(_dcm_matrix.b.x);
-Serial.println(_dcm_matrix.b.y);
-Serial.println(_dcm_matrix.b.z);
-Serial.println(_dcm_matrix.c.x);
-Serial.println(_dcm_matrix.c.y);
-Serial.println(_dcm_matrix.c.z);
-			_course_over_ground_x = 1;
-			_course_over_ground_y = 0;
-Serial.print("in motion = ");
-Serial.print(in_motion);
-Serial.print("\t");
+		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  {
-Serial.println("in yaw reset");
 				float cos_psi_err, sin_psi_err;
 				
-				yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
-				cos_psi_err = cos(yaw - ToRad(_gps->ground_course/100.0));
-				sin_psi_err = sin(yaw - ToRad(_gps->ground_course/100.0));
+				_yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
+				cos_psi_err = cos(_yaw - ToRad(_gps->ground_course/100.0));
+				sin_psi_err = sin(_yaw - ToRad(_gps->ground_course/100.0));
 				// Rxx = cos psi err, Rxy = - sin psi err, Rxz = 0
 				// Ryx = sin psi err, Ryy = cos psi err,   Ryz = 0
 				// Rzx = Rzy = Rzz = 0
-				_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;
-				_dcm_matrix = _rot_mat * _dcm_matrix;
+				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 = 0;
+				
+				_dcm_matrix = rot_mat * _dcm_matrix;
 				in_motion =  TRUE;
 				error_course = 0;
 				
 			}	
-	/*	} else {
+		} 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.
@@ -447,13 +381,13 @@ void
 AP_DCM_FW::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;
+	_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);
+	_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
 
 }

libraries/AP_DCM/AP_DCM_FW.h

@@ -27,10 +27,11 @@ public:
 	float	get_yaw(void);						// Radians
 	Vector3f	get_gyros(void);
 	Vector3f	get_accels(void);
+	Matrix3f	get_dcm_matrix(void);
 	
 	// Methods
-	void	quick_init(void);
-	void	init(void);
+	void	quick_init(uint16_t *_offset_address);
+	void	init(uint16_t *_offset_address);
 	void 	update_DCM(float _G_Dt);   
 
 	float 		imu_health;						//Metric based on accel gain deweighting
@@ -42,12 +43,11 @@ public:
 private:
 	// Methods
 	void 		read_adc_raw(void);
-	void 		_accel_adjust(void);
-	float 		_gyro_temp_comp(int i, int temp) const;
+	void 		accel_adjust(void);
 	float 		read_adc(int select);
 	void 		matrix_update(float _G_Dt);
 	void 		normalize(void);
-	Vector3f 	_renorm(Vector3f const &a, int &problem);
+	Vector3f 	renorm(Vector3f const &a, int &problem);
 	void 		drift_correction(void);
 	void 		euler_angles(void);
 
@@ -56,13 +56,9 @@ private:
 	GPS 		*_gps;
 	AP_IMU		_imu;
 
-	long	roll_sensor;						// degrees * 100
-	long	pitch_sensor;						// degrees * 100
-	long	yaw_sensor;							// degrees * 100
-
-	float 		roll;						// radians
-	float 		pitch;						// radians
-	float 		yaw;						// radians
+	float 		_roll;						// radians
+	float 		_pitch;						// radians
+	float 		_yaw;						// radians
 	
 	Matrix3f	_dcm_matrix;
 

libraries/AP_IMU/AP_IMU.cpp

@@ -64,16 +64,16 @@ AP_IMU::AP_IMU(void)
 
 /**************************************************/
 void
-AP_IMU::quick_init(void)
+AP_IMU::quick_init(uint16_t *_offset_address)
 {
-	
-//  NOTE ***  Need to addd code to retrieve values from EEPROM
+	// We read the imu offsets from EEPROM for a quick air restart
+	eeprom_read_block((void*)&_adc_offset, _offset_address, sizeof(_adc_offset));
 }
 
 
 /**************************************************/
 void
-AP_IMU::init(void)
+AP_IMU::init(uint16_t *_offset_address)
 {
 	
 	float temp;
@@ -97,7 +97,7 @@ AP_IMU::init(void)
 		for (int j = 0; j < 6; j++) {
 			_adc_in[j] = APM_ADC.Ch(_sensors[j]);
 			if (j < 3) {	
-				_adc_in[j] -= _gyro_temp_comp(j, tc_temp);		// Subtract temp compensated typical gyro bias
+				_adc_in[j] -= gyro_temp_comp(j, tc_temp);		// Subtract temp compensated typical gyro bias
 			} else {
 				_adc_in[j] -= 2025;
 			}
@@ -121,7 +121,9 @@ AP_IMU::init(void)
 	
 	_adc_offset[5] += GRAVITY * _sensor_signs[5];
 	
-//  NOTE ***  Need to addd code to save values to EEPROM
+	//  Save offset values to EEPROM for use in an air restart
+ 	eeprom_write_block((const void *)&_adc_offset, _offset_address, sizeof(_adc_offset));
+	
 }
 
 
@@ -129,7 +131,7 @@ AP_IMU::init(void)
 // Returns the temperature compensated raw gyro value
 //---------------------------------------------------
 float
-AP_IMU::_gyro_temp_comp(int i, int temp) const
+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
 	//------------------------------------------------------------------------
@@ -143,7 +145,7 @@ AP_IMU::get_gyro(void)
 	
 	for (int i = 0; i < 3; i++) {
 		_adc_in[i] = APM_ADC.Ch(_sensors[i]);
-		_adc_in[i] -= _gyro_temp_comp(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)
 			_adc_in[i] = (_adc_offset[i] - _adc_in[i]);
 		else

libraries/AP_IMU/AP_IMU.h

@@ -6,6 +6,7 @@
 #include <inttypes.h>
 #include "WProgram.h"
 #include <APM_ADC.h>
+#include <avr/eeprom.h>
 
 
 class AP_IMU
@@ -15,8 +16,8 @@ public:
 	AP_IMU();									// Default Constructor
 	
 	// Methods
-	void		quick_init(void);				// For air start
-	void		init(void);						// For ground start
+	void		quick_init(uint16_t *_offset_address);				// For air start
+	void		init(uint16_t *_offset_address);						// For ground start
 	Vector3f	get_gyro(void);					// Radians/second
 	Vector3f	get_accel(void);				// meters/seconds squared
 
@@ -27,10 +28,11 @@ public:
 private:
 	// Methods
 	void 		read_adc_raw(void);
-	float 		_gyro_temp_comp(int i, int temp) const;
+	float 		gyro_temp_comp(int i, int temp) const;
 	float 		read_adc(int select);
 
 	// members
+	//uint16_t	_offset_address;			// EEPROM start address for saving/retrieving offsets
 	float 		_adc_in[6];					// array that store the 6 ADC channels used by IMU
 	float 		_adc_offset[6]; 			// Array that store the Offset of the gyros and accelerometers
 	Vector3f 	_accel_vector;				// Store the acceleration in a vector