Added dynamic setting of kp_rollpitch, ki_rollpitch, kp_yaw.

Added three constants for kp_rollpitch, (high, med -default, low)
Functionally equivalent to prior version.

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

libraries/AP_DCM/AP_DCM.cpp

@@ -22,9 +22,11 @@
 #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 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 Ki_ROLLPITCH 0.0			// 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
@@ -239,8 +241,8 @@ AP_DCM::drift_correction(void)
 	_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);
+	_omega_P =  _error_roll_pitch * (_kp_roll_pitch * accel_weight);
+	_omega_I += _error_roll_pitch * (_ki_roll_pitch * accel_weight);
 
 
 	//*****YAW***************
@@ -253,6 +255,7 @@ AP_DCM::drift_correction(void)
 
 		// 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) {
@@ -280,8 +283,8 @@ AP_DCM::drift_correction(void)
 				_dcm_matrix = rot_mat * _dcm_matrix;
 				in_motion =  true;
 				error_course = 0;
-				
 			}
+
 		} else {
 			error_course = 0;
 			in_motion = false;
@@ -290,7 +293,7 @@ AP_DCM::drift_correction(void)
 
 	_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_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

libraries/AP_DCM/AP_DCM.h

@@ -30,7 +30,10 @@ public:
 					0, 0, 1),
 		_course_over_ground_x(0),
 		_course_over_ground_y(1),
-		_health(1.)
+		_health(1.),
+		_kp_roll_pitch(0.05967),
+		_ki_roll_pitch(0.00001278),
+		_kp_yaw(0.8)
 	{}
 
 	// Accessors
@@ -60,7 +63,24 @@ public:
 	uint8_t 	renorm_sqrt_count;
 	uint8_t 	renorm_blowup_count;
 
+	float	kp_roll_pitch() 		{ return _kp_roll_pitch; }
+	void	kp_roll_pitch(float v) 	{ _kp_roll_pitch = v; }
+
+	float	ki_roll_pitch() 		{ return _ki_roll_pitch; }
+	void	ki_roll_pitch(float v) 	{ _ki_roll_pitch = v; }
+
+	float	kp_yaw() 				{ return _kp_yaw; }
+	void	kp_yaw(float v) 		{ _kp_yaw = v; }
+
+	static const float kDCM_kp_rp_high 		= 0.15;
+	static const float kDCM_kp_rp_medium	= 0.05967;
+	static const float kDCM_kp_rp_low		= 0.01;
+
+
 private:
+	float		_kp_roll_pitch;
+	float		_ki_roll_pitch;
+	float		_kp_yaw;
 	// Methods
 	void 		read_adc_raw(void);
 	void 		accel_adjust(void);