Moved Health to DCM, Centripetal correct

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

libraries/AP_DCM/AP_DCM.cpp

@@ -97,10 +97,11 @@ AP_DCM::matrix_update(float _G_Dt)
 		(fabs(_gyro_vector.z) >= radians(300)))
 		gyro_sat_count++;
 
-	if(_centrifugal){
-		_omega_integ_corr 	= _gyro_vector 		+ _omega_I;		// Used for centrep correction (theoretically better than _omega)
+		_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		
-		accel_adjust();				// Remove centrifugal acceleration.
+
+	if(_centripetal){
+		accel_adjust();				// Remove _centripetal acceleration.
 	}
 	
  #if OUTPUTMODE == 1				 
@@ -242,8 +243,8 @@ AP_DCM::drift_correction(void)
 	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
-	_imu->set_health(0.02 * (accel_weight-.5));
-	
+	_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???
 
@@ -338,5 +339,10 @@ AP_DCM::euler_angles(void)
 
 /**************************************************/
 
+float
+AP_DCM::get_health(void)
+{
+	return _health;
+}
 
 

libraries/AP_DCM/AP_DCM.h

@@ -42,11 +42,13 @@ public:
 	Vector3f	get_accel(void);
 	Matrix3f	get_dcm_matrix(void);
 	
-	void		set_centrifugal(bool b);
+	void		set_centripetal(bool b);
 
 	// Methods
 	void 		update_DCM(float _G_Dt);
 
+	float		get_health(void);
+
 	long		roll_sensor;					// Degrees * 100
 	long		pitch_sensor;					// Degrees * 100
 	long		yaw_sensor;						// Degrees * 100
@@ -88,7 +90,8 @@ private:
 	float 		_errorCourse;
 	float 		_course_over_ground_x; 		// Course overground X axis
 	float 		_course_over_ground_y; 		// Course overground Y axis
-	bool		_centrifugal;
+	float		_health;
+	bool		_centripetal;
 };
 
 #endif

libraries/AP_IMU/AP_IMU.cpp

@@ -55,18 +55,6 @@ const float   AP_IMU::_gyro_temp_curve[3][3] = {
 };	// To Do - make additional constructors to pass this in.
 
 
-/**************************************************/
-void
-AP_IMU::set_health(float health)
-{
-	_health += constrain(health, 0, 1);
-}
-
-float
-AP_IMU::get_health(void)
-{
-	return _health;
-}
 
 void
 AP_IMU::init(void)

libraries/AP_IMU/AP_IMU.h

@@ -32,9 +32,6 @@ public:
 	// raw ADC values - called by DCM
 	Vector3f	get_gyro(void);									// Radians/second
 	Vector3f	get_accel(void);								// meters/seconds squared
-
-	void		set_health(float health);
-	float		get_health(void);
 				
 	// Members
 	uint8_t 	adc_constraints;		// a check of how many times we get non-sensical values
@@ -48,7 +45,6 @@ private:
 	uint16_t	_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
-	float		_health;
 	Vector3f 	_accel_vector;				// Store the acceleration in a vector
 	Vector3f 	_gyro_vector;				// Store the gyros turn rate in a vector
 	AP_ADC		* _adc; 					// Analog to digital converter pointer