From 09fb9a8e586b8ed0e13d5f01d9de059e5f76c112 Mon Sep 17 00:00:00 2001
From: Hwurzburg <hwurzburg@yahoo.com>
Date: Fri, 16 Apr 2021 15:01:59 -0500
Subject: [PATCH] Plane: limit speed scaling in Autotakeoff wo AS sensor option

---
 ArduPlane/Attitude.cpp   | 5 +++++
 ArduPlane/Parameters.cpp | 2 +-
 ArduPlane/defines.h      | 1 +
 3 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
index 6937b6ca90..d6ec4a6179 100644
--- a/ArduPlane/Attitude.cpp
+++ b/ArduPlane/Attitude.cpp
@@ -46,6 +46,11 @@ float Plane::get_speed_scaler(void)
         // no speed estimate and not armed, use a unit scaling
         speed_scaler = 1;
     }
+    if (!plane.ahrs.airspeed_sensor_enabled()  && 
+        (plane.g2.flight_options & FlightOptions::SURPRESS_TKOFF_SCALING) &&
+        (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF)) { //scaling is surpressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates
+        return MIN(speed_scaler, 1.0f) ;
+    }
     return speed_scaler;
 }
 
diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
index b5df76d210..bbe03825e4 100644
--- a/ArduPlane/Parameters.cpp
+++ b/ArduPlane/Parameters.cpp
@@ -1120,7 +1120,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Param: FLIGHT_OPTIONS
     // @DisplayName: Flight mode options
     // @Description: Flight mode specific options
-    // @Bitmask: 0:Rudder mixing in direct flight modes only (Manual / Stabilize / Acro),1:Use centered throttle in Cruise or FBWB to indicate trim airspeed, 2:Disable attitude check for takeoff arming, 3:Force target airspeed to trim airspeed in Cruise or FBWB, 4: Climb to ALT_HOLD_RTL before turning for RTL, 5:Enable yaw damper in acro mode
+    // @Bitmask: 0:Rudder mixing in direct flight modes only (Manual / Stabilize / Acro),1:Use centered throttle in Cruise or FBWB to indicate trim airspeed, 2:Disable attitude check for takeoff arming, 3:Force target airspeed to trim airspeed in Cruise or FBWB, 4: Climb to ALT_HOLD_RTL before turning for RTL, 5: Enable yaw damper in acro mode, 6: Surpress speed scaling during auto takeoffs to be 1 or less to prevent oscillations without airpseed sensor.
     // @User: Advanced
     AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0),
 
diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
index 2adee01a9a..748c00a7dd 100644
--- a/ArduPlane/defines.h
+++ b/ArduPlane/defines.h
@@ -154,6 +154,7 @@ enum FlightOptions {
     CRUISE_TRIM_AIRSPEED = (1 << 3),
     CLIMB_BEFORE_TURN = (1 << 4),
     ACRO_YAW_DAMPER = (1 << 5),
+    SURPRESS_TKOFF_SCALING = (1<<6),
 };
 
 enum CrowFlapOptions {