Added Scaling to yaw response on Auto, RTL, and Guided modes

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

ArduCopterMega/ArduCopterMega.pde

@@ -1027,7 +1027,7 @@ void update_current_flight_mode(void)
 				}
 
 				// Yaw control
-				g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
+				g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, .5);
 				break;
 		}
 
@@ -1063,7 +1063,7 @@ void update_current_flight_mode(void)
 				g.rc_3.servo_out = get_throttle(g.rc_3.control_in);
 
 				// Yaw control
-				g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
+				g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
 
 				//Serial.printf("%u\t%d\n", nav_yaw, g.rc_4.servo_out);
 				break;
@@ -1140,7 +1140,7 @@ void update_current_flight_mode(void)
 				g.rc_3.servo_out = get_throttle(g.rc_3.control_in);
 
 				// Yaw control
-				g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
+				g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
 				//Serial.printf("%d \t %d\n", g.rc_3.servo_out, throttle_slew);
 
 			break;
@@ -1164,7 +1164,7 @@ void update_current_flight_mode(void)
 				}
 
 				// Yaw control
-				g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
+				g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
 				break;
 
 			case GUIDED:
@@ -1185,7 +1185,7 @@ void update_current_flight_mode(void)
 				}
 
 				// Yaw control
-				g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
+				g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 0.5);
 				break;
 
 			case LOITER:
@@ -1211,7 +1211,7 @@ void update_current_flight_mode(void)
 				}
 
 				// Yaw control
-				g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
+				g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
 				break;
 
 			default:

ArduCopterMega/Attitude.pde

@@ -50,7 +50,7 @@ get_stabilize_pitch(long target_angle)
 }
 
 int
-get_stabilize_yaw(long target_angle)
+get_stabilize_yaw(long target_angle, float scaler)
 {
 	long error;
 	long rate;
@@ -62,7 +62,7 @@ get_stabilize_yaw(long target_angle)
 	error 		= constrain(error, -4500, 4500);
 
 	// desired Rate:
-	rate 		= g.pid_stabilize_yaw.get_pi((float)error, delta_ms_fast_loop, 1.0);
+	rate 		= g.pid_stabilize_yaw.get_pi((float)error, delta_ms_fast_loop, scaler);
 	//Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate);
 
 	// Rate P: