From 8abb099afaa47657a3448203064a163a35d08d9a Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tom.pittenger@krausaerospace.com>
Date: Thu, 7 Sep 2023 10:19:00 -0700
Subject: [PATCH] SITL: add param SIM_THRUST_FAIL for Plane

---
 libraries/SITL/SIM_Plane.cpp | 11 +++++++++++
 libraries/SITL/SITL.cpp      |  9 +++++++++
 libraries/SITL/SITL.h        |  3 +++
 3 files changed, 23 insertions(+)

diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp
index 4b32c10406267..eb37abfe2a6c9 100644
--- a/libraries/SITL/SIM_Plane.cpp
+++ b/libraries/SITL/SIM_Plane.cpp
@@ -323,6 +323,17 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel
         thrust = icengine.update(input);
     }
 
+    // simulated thrust failure
+    if (sitl->thrust_fail > 0) {
+        if (sitl->thrust_fail < 1) {
+            // partial loss
+            thrust *= sitl->thrust_fail;
+        } else {
+            // total loss
+            thrust = 0;
+        }
+    }
+
     // calculate angle of attack
     angle_of_attack = atan2f(velocity_air_bf.z, velocity_air_bf.x);
     beta = atan2f(velocity_air_bf.y,velocity_air_bf.x);
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 3d3add9002385..8b4e8936e4f09 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -439,6 +439,15 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
     // @User: Advanced
     AP_GROUPINFO("UART_LOSS", 42, SIM,  uart_byte_loss_pct, 0),
 
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+    // @Param: THRUST_FAIL
+    // @DisplayName: Simulated thrust failure
+    // @Description: Simulated full or partial forward thrust failure for Plane. A value of >= 1 is full loss. Values between 0 and 1 simulate degraded thrust where 0.1 would mean only 10% of normal thrust is being generated. A value <= 0 means no failure.
+    // @Range: 0 1
+    // @Increment: 0.1
+    AP_GROUPINFO("THRUST_FAIL", 43, SIM, thrust_fail, 0),
+#endif
+
     // @Group: ARSPD_
     // @Path: ./SITL_Airspeed.cpp
     AP_SUBGROUPINFO(airspeed[0], "ARSPD_", 50, SIM, AirspeedParm),
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 4dfd9b251facb..c56b8d3d21d4c 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -456,6 +456,9 @@ class SIM {
     // RPM when motors are armed
     AP_Float esc_rpm_armed;
 
+    // ESC/Motor failure that degrades Plane forward thrust
+    AP_Float thrust_fail;
+
     struct {
         // LED state, for serial LED emulation
         struct {