From 6243cf77a8168a623c200ade51876c5104ffee8c 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 c03b628bf..df7b64b4b 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 3bac4730b..b97e2f604 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 9c43b277c..f52c43495 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 a0860fd2b..e12130b1c 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
--
GitLab