From 6358a3c962af31d00829e1fa69bbaaa1108cc39a Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Mon, 5 Sep 2022 00:10:01 +0100
Subject: [PATCH] Plane: add min airspeed arming check and constrain for speed
 scailing

---
 ArduPlane/AP_Arming.cpp | 5 +++++
 ArduPlane/Attitude.cpp  | 5 +++--
 ArduPlane/defines.h     | 2 ++
 3 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp
index 9a08598773..2abe4e4f19 100644
--- a/ArduPlane/AP_Arming.cpp
+++ b/ArduPlane/AP_Arming.cpp
@@ -78,6 +78,11 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
         ret = false;
     }
 
+    if (plane.aparm.airspeed_min < MIN_AIRSPEED_MIN) {
+        check_failed(display_failure, "ARSPD_FBW_MIN too low (%i < %i)", plane.aparm.airspeed_min.get(), MIN_AIRSPEED_MIN);
+        ret = false;
+    }
+
     if (plane.channel_throttle->get_reverse() && 
         Plane::ThrFailsafe(plane.g.throttle_fs_enabled.get()) != Plane::ThrFailsafe::Disabled &&
         plane.g.throttle_fs_value < 
diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
index 061a5d9890..ba3315f577 100644
--- a/ArduPlane/Attitude.cpp
+++ b/ArduPlane/Attitude.cpp
@@ -13,8 +13,9 @@ float Plane::calc_speed_scaler(void)
             auto_state.highest_airspeed = aspeed;
         }
         // ensure we have scaling over the full configured airspeed
+        const float airspeed_min = MAX(aparm.airspeed_min, MIN_AIRSPEED_MIN);
         const float scale_min = MIN(0.5, g.scaling_speed / (2.0 * aparm.airspeed_max));
-        const float scale_max = MAX(2.0, g.scaling_speed / (0.7 * aparm.airspeed_min));
+        const float scale_max = MAX(2.0, g.scaling_speed / (0.7 * airspeed_min));
         if (aspeed > 0.0001f) {
             speed_scaler = g.scaling_speed / aspeed;
         } else {
@@ -25,7 +26,7 @@ float Plane::calc_speed_scaler(void)
 #if HAL_QUADPLANE_ENABLED
         if (quadplane.in_vtol_mode() && hal.util->get_soft_armed()) {
             // when in VTOL modes limit surface movement at low speed to prevent instability
-            float threshold = aparm.airspeed_min * 0.5;
+            float threshold = airspeed_min * 0.5;
             if (aspeed < threshold) {
                 float new_scaler = linear_interpolate(0.001, g.scaling_speed / threshold, aspeed, 0, threshold);
                 speed_scaler = MIN(speed_scaler, new_scaler);
diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
index 2e92f68f5b..fdb9c65692 100644
--- a/ArduPlane/defines.h
+++ b/ArduPlane/defines.h
@@ -6,6 +6,8 @@
 #define SERVO_MAX 4500.0  // This value represents 45 degrees and is just an
                         // arbitrary representation of servo max travel.
 
+#define MIN_AIRSPEED_MIN 5 // m/s, used for arming check and speed scaling
+
 // failsafe
 // ----------------------
 enum failsafe_state {