diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 6cd32e4a4d..d7b95c700e 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -400,6 +400,8 @@ static boolean	motor_auto_armed;
 static Vector3f omega;
 // This is used to hold radio tuning values for in-flight CH6 tuning
 float tuning_value;
+// This will keep track of the percent of roll or pitch the user is applying
+float roll_scale_d, pitch_scale_d;
 
 ////////////////////////////////////////////////////////////////////////////////
 // LED output
@@ -1487,8 +1489,20 @@ void update_roll_pitch_mode(void)
 		//reset_stability_I();
 	}
 
-	// clear new radio frame info
-	new_radio_frame = false;
+	if(new_radio_frame){
+		// clear new radio frame info
+		new_radio_frame = false;
+
+		// These values can be used to scale the PID gains
+		// This allows for a simple gain scheduling implementation
+		roll_scale_d	= g.stabilize_d_schedule * (float)abs(g.rc_1.control_in);
+		roll_scale_d 	= (1 - (roll_scale_d / 4500.0));
+		roll_scale_d	= constrain(roll_scale_d, 0, 1) * g.stabilize_d;
+
+		pitch_scale_d	= g.stabilize_d_schedule * (float)abs(g.rc_2.control_in);
+		pitch_scale_d 	= (1 - (pitch_scale_d / 4500.0));
+		pitch_scale_d 	= constrain(pitch_scale_d, 0, 1) * g.stabilize_d;
+	}
 }
 
 // new radio frame is used to make sure we only call this at 50hz