From 1e4c011ea7fe9152d1bd5afd8c9acd0619a2f02a Mon Sep 17 00:00:00 2001
From: Daniel Field <daniel@fieldfam.com>
Date: Tue, 30 Apr 2024 10:33:30 +1000
Subject: [PATCH] SITL: modifications to lift in SIM_StratoBlimp (#3)

* Update SIM_StratoBlimp.cpp

* Update SIM_StratoBlimp.h
---
 libraries/SITL/SIM_StratoBlimp.cpp | 37 +++++++++++++++++++-----------
 libraries/SITL/SIM_StratoBlimp.h   |  5 +++-
 2 files changed, 28 insertions(+), 14 deletions(-)

diff --git a/libraries/SITL/SIM_StratoBlimp.cpp b/libraries/SITL/SIM_StratoBlimp.cpp
index 5748fb55446e19..66bfcc837a9680 100644
--- a/libraries/SITL/SIM_StratoBlimp.cpp
+++ b/libraries/SITL/SIM_StratoBlimp.cpp
@@ -60,10 +60,15 @@ const AP_Param::GroupInfo StratoBlimp::var_info[] = {
     // @Description: drag on X axis
     AP_GROUPINFO("DRAG_FWD", 5, StratoBlimp,  drag_fwd, 0.27),
 
+    // @Param: DRAG_SIDE
+    // @DisplayName: drag in sidewards direction
+    // @Description: drag on Y axis
+    AP_GROUPINFO("DRAG_SIDE",  16, StratoBlimp,  drag_side, 0.5),
+
     // @Param: DRAG_UP
     // @DisplayName: drag in upward direction
     // @Description: drag on Z axis
-    AP_GROUPINFO("DRAG_UP",  6, StratoBlimp,  drag_up, 0.1),
+    AP_GROUPINFO("DRAG_UP",  6, StratoBlimp,  drag_up, 0.4),
 
     // @Param: MOI_YAW
     // @DisplayName: moment of inertia in yaw
@@ -87,10 +92,10 @@ const AP_Param::GroupInfo StratoBlimp::var_info[] = {
     AP_GROUPINFO("ALT_TARG", 10, StratoBlimp,  altitude_target, 20000),
 
     // @Param: CLMB_RT
-    // @DisplayName: climb rate
-    // @Description: climb rate
+    // @DisplayName: target climb rate
+    // @Description: target climb rate
     // @Units: m/s
-    AP_GROUPINFO("CLMB_RT",  11, StratoBlimp, climb_rate, 5),
+    AP_GROUPINFO("CLMB_RT",  11, StratoBlimp, target_climb_rate, 5),
 
     // @Param: YAW_RT
     // @DisplayName: yaw rate
@@ -114,7 +119,12 @@ const AP_Param::GroupInfo StratoBlimp::var_info[] = {
     // @DisplayName: weathervaning offset
     // @Description: center of drag for weathervaning
     // @Units: m
-    AP_GROUPINFO("WVANE",    15, StratoBlimp,  center_of_drag, 2),
+    AP_GROUPINFO("WVANE",    15, StratoBlimp,  center_of_drag, 0.3),
+
+    // @Param: FLR
+    // @DisplayName: free lift rate
+    // @Description: amount of additional lift generated by the helper balloon (for the purpose of ascent), as a proportion of the 'neutral buoyancy' lift
+    AP_GROUPINFO("FLR",  17, StratoBlimp,  free_lift_rate, 0.12),
     
     AP_GROUPEND
 };
@@ -201,14 +211,15 @@ float StratoBlimp::get_lift(float altitude)
     // start with neutral buoyancy
     float lift_accel = GRAVITY_MSS;
 
-    // assume lift will equal drag at the given climb rate
-    float climb_accel = 0.5 * air_density * sq(climb_rate) * drag_up;
-
-    // ramp down lift as we approach target alt
-    lift_accel += linear_interpolate(climb_accel, 0,
-                                     altitude,
-                                     altitude_target-1000, altitude_target);
-
+    // add lift from helper balloon if still attached
+    if (helper_balloon_attached) {
+        // helper balloon additional lift amount based on Free Lift Ratio
+        lift_accel += GRAVITY_MSS*free_lift_rate;
+        // detach helper balloon if the target altitude has been reached
+        if (altitude >= altitude_target) {
+            helper_balloon_attached = false;
+        }
+    }
     return mass * lift_accel;
 }
 
diff --git a/libraries/SITL/SIM_StratoBlimp.h b/libraries/SITL/SIM_StratoBlimp.h
index 26a8af095ce134..9165770aa6f969 100644
--- a/libraries/SITL/SIM_StratoBlimp.h
+++ b/libraries/SITL/SIM_StratoBlimp.h
@@ -61,15 +61,17 @@ class StratoBlimp : public Aircraft {
     float EAS2TAS;
     float drag_yaw;
     bool released;
+    bool helper_balloon_attached = true;
 
     AP_Float mass;
     AP_Float helium_mass;
     AP_Float arm_length;
     AP_Float motor_thrust;
     AP_Float drag_fwd;
+    AP_Float drag_side;
     AP_Float drag_up;
     AP_Float altitude_target;
-    AP_Float climb_rate;
+    AP_Float target_climb_rate;
     AP_Float turn_rate;
     AP_Float motor_angle;
     AP_Float yaw_rate_max;
@@ -78,6 +80,7 @@ class StratoBlimp : public Aircraft {
     AP_Float moi_pitch;
     AP_Float center_of_lift;
     AP_Float center_of_drag;
+    AP_Float free_lift_rate;
 };
 
 }