From 6bcfe913f76dfbcdbbd533aeefe9442647ff6492 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 26 Jul 2012 11:30:23 +1000
Subject: [PATCH] APM: added a SCALING_SPEED parameter

this allows users to adjust the base speed used for scaling roll/pitch
PIDs. This can be used to make PIDs work for both airspeed and
non-airspeed control
---
 ArduPlane/Attitude.pde   | 4 ++--
 ArduPlane/Parameters.h   | 5 +++++
 ArduPlane/Parameters.pde | 7 +++++++
 ArduPlane/config.h       | 6 ++----
 4 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index c03b628bf6..df7b64b4bd 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -14,7 +14,7 @@ static void stabilize()
 	if (airspeed.use()) {
         float aspeed = airspeed.get_airspeed();
 		if (aspeed > 0) {
-			speed_scaler = STANDARD_SPEED / aspeed;
+			speed_scaler = g.scaling_speed / aspeed;
         } else {
 			speed_scaler = 2.0;
         }
@@ -226,7 +226,7 @@ static void calc_nav_roll()
 #else
     // this is the old nav_roll calculation. We will use this for 2.50
     // then remove for a future release
-    float nav_gain_scaler = (float)g_gps->ground_speed / (STANDARD_SPEED * 100.0);
+    float nav_gain_scaler = 0.01 * g_gps->ground_speed / g.scaling_speed;
     nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
     nav_roll = g.pidNavRoll.get_pid(bearing_error, nav_gain_scaler); //returns desired bank angle in degrees*100
 #endif
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index 3bac4730bf..b97e2f604e 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -163,6 +163,7 @@ public:
         k_param_kff_rudder_mix,
         k_param_kff_pitch_to_throttle,
         k_param_kff_throttle_to_pitch,
+        k_param_scaling_speed,
 
         //
         // 210: flight modes
@@ -265,6 +266,9 @@ public:
     AP_Float    kff_pitch_to_throttle;
     AP_Float    kff_throttle_to_pitch;
 
+    // speed used for speed scaling
+    AP_Float    scaling_speed;
+
     // Crosstrack navigation
     //
     AP_Float    crosstrack_gain;
@@ -402,6 +406,7 @@ public:
         kff_rudder_mix          (RUDDER_MIX),
         kff_pitch_to_throttle   (P_TO_T),
         kff_throttle_to_pitch   (T_TO_P),
+        scaling_speed           (SCALING_SPEED),
 
         crosstrack_gain         (XTRACK_GAIN_SCALED),
         crosstrack_entry_angle  (XTRACK_ENTRY_ANGLE_CENTIDEGREE),
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index 9c43b277cc..f52c434954 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -392,6 +392,13 @@ static const AP_Param::Info var_info[] PROGMEM = {
 	// @User: User
 	GSCALAR(airspeed_cruise_cm,        "TRIM_ARSPD_CM"),
 
+    // @Param: SCALING_SPEED
+	// @DisplayName: speed used for speed scaling calculations
+	// @Description: Airspeed in m/s to use when calculating surface speed scaling. Note that changing this value will affect all PID values
+	// @Units: m/s
+	// @User: Advanced
+	GSCALAR(scaling_speed,        "SCALING_SPEED"),
+
     // @Param: MIN_GNDSPD_CM
 	// @DisplayName: Minimum ground speed
 	// @Description: Minimum ground speed in cm/s when under airspeed control
diff --git a/ArduPlane/config.h b/ArduPlane/config.h
index a0860fd2bf..e12130b1ca 100644
--- a/ArduPlane/config.h
+++ b/ArduPlane/config.h
@@ -817,11 +817,9 @@
 // Developer Items
 //
 
-#ifndef STANDARD_SPEED
-# define STANDARD_SPEED		15.0
-#define STANDARD_SPEED_SQUARED (STANDARD_SPEED * STANDARD_SPEED)
+#ifndef SCALING_SPEED
+# define SCALING_SPEED		15.0
 #endif
-#define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE)
 
 // use this to enable servos in HIL mode
 #ifndef HIL_SERVOS