diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp
index 9a8a10e2fb..6e297ca49e 100644
--- a/libraries/AP_DCM/AP_DCM.cpp
+++ b/libraries/AP_DCM/AP_DCM.cpp
@@ -27,7 +27,7 @@
 //#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 Ki_YAW 0.00004 				// Yaw Drift CorrectionIntegrator Gain
 
 #define SPEEDFILT 300				// centimeters/second
 #define ADC_CONSTRAINT 900
@@ -302,7 +302,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_I += _error_yaw * Ki_YAW;			// adding yaw correction to integrator 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
 	integrator_magnitude = _omega_I.length();
diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h
index 909c96a0b9..bd705da8f8 100644
--- a/libraries/AP_DCM/AP_DCM.h
+++ b/libraries/AP_DCM/AP_DCM.h
@@ -33,7 +33,8 @@ public:
 		_health(1.),
 		_kp_roll_pitch(0.05967),
 		_ki_roll_pitch(0.00001278),
-		_kp_yaw(0.8),
+		_kp_yaw(0.8), // .8
+		_ki_yaw(0.00004), // 0.00004
 		_toggle(0)
 	{}
 
@@ -73,6 +74,9 @@ public:
 	float	kp_yaw() 				{ return _kp_yaw; }
 	void	kp_yaw(float v) 		{ _kp_yaw = v; }
 
+	float	ki_yaw() 				{ return _ki_yaw; }
+	void	ki_yaw(float v) 		{ _ki_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;
@@ -82,6 +86,8 @@ private:
 	float		_kp_roll_pitch;
 	float		_ki_roll_pitch;
 	float		_kp_yaw;
+	float		_ki_yaw;
+
 	// Methods
 	void 		read_adc_raw(void);
 	void 		accel_adjust(void);