SITL: add param SIM_THRUST_FAIL for Plane

magicrub · Oct 24, 2023 · 8abb099 · 8abb099
1 parent 4dc9d1e
commit 8abb099
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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 {