SITL_SA_GD2000: allow launch. still needs flight surface changes

magicrub · Sep 10, 2024 · 1a0b589 · 1a0b589
1 parent 56a392b
commit 1a0b589
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 7 deletions.
diff --git a/libraries/SITL/SIM_SA_GD2000.cpp b/libraries/SITL/SIM_SA_GD2000.cpp
@@ -34,7 +34,13 @@ const AP_Param::GroupInfo SA_GD2000::var_info[] = {
     // @DisplayName: mass
     // @Description: mass of SA_GD2000
     // @Units: kg
-    AP_GROUPINFO("MASS",     1, SA_GD2000,  param_mass, 907), // 907kg = 2000lbs
+    AP_GROUPINFO("MASS",     1, SA_GD2000,  params.mass, 5), // 907kg = 2000lbs
+
+    // @Param: ALT
+    // @DisplayName: launch alt MSL
+    // @Description: launch alt MSL
+    // @Units: m
+    AP_GROUPINFO("ALT",     2, SA_GD2000,  params.launch_alt, 3810), // 3810m == 125000 ft
 
     AP_GROUPEND
 };
@@ -46,12 +52,9 @@ SA_GD2000::SA_GD2000(const char *frame_str) :
 
     AP_Param::load_defaults_file("@ROMFS/models/sa_gd2000.parm", false);
 
-    mass = param_mass;
+    mass = params.mass.get();
     thrust_scale = 0;
 
-    launch_accel = 50;
-    launch_time = 1;
-
     coefficient.c_drag_p = 0.05;
 }
 
@@ -60,7 +63,23 @@ SA_GD2000::SA_GD2000(const char *frame_str) :
  */
 void SA_GD2000::update(const struct sitl_input &input)
 {
+    if (!has_launched) {
+        // we're in a pre-launch state cruising in the launch vehicle (like a C-130)
+        position.z = -1 * params.launch_alt; // 3810 == 12500ft 
+        velocity_ef.x = 10;
+
+        if (hal.util->get_soft_armed()) {
+            has_launched = true;
+        }
+    }
+
     Plane::update(input);
+
+    // constrain accelerations
+    accel_body.x = constrain_float(accel_body.x, -16*GRAVITY_MSS, 16*GRAVITY_MSS);
+    accel_body.y = constrain_float(accel_body.y, -16*GRAVITY_MSS, 16*GRAVITY_MSS);
+    accel_body.z = constrain_float(accel_body.z, -16*GRAVITY_MSS, 16*GRAVITY_MSS);
 }
 
-#endif // AP_SIM_SA_GD2000_ENABLED
+
+#endif // AP_SIM_SA_GD2000_ENABLED
diff --git a/libraries/SITL/SIM_SA_GD2000.h b/libraries/SITL/SIM_SA_GD2000.h
@@ -46,7 +46,13 @@ class SA_GD2000 : public Plane {
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
-    AP_Float param_mass;
+
+    bool has_launched;
+
+    struct {
+        AP_Float mass;
+        AP_Float launch_alt;
+    } params;
 };
 
 }