From c24e212d0902331a3f6d34b40ddbdc316e262d39 Mon Sep 17 00:00:00 2001
From: George Zogopoulos <tailwhipx4@gmail.com>
Date: Thu, 18 Jul 2024 09:54:02 +0200
Subject: [PATCH] SITL: Glider improvements

Fixed balloon going below ground.
Enabled glider torque logging.
---
 libraries/SITL/SIM_Aircraft.cpp | 2 +-
 libraries/SITL/SIM_Glider.cpp   | 8 ++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index a6baf91eb9da24..3e519576e43179 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -181,7 +181,7 @@ void Aircraft::update_position(void)
         uint32_t now = AP_HAL::millis();
         if (now - last_one_hz_ms >= 1000) {
             // shift origin of position at 1Hz to current location
-            // this prevents sperical errors building up in the GPS data
+            // this prevents spherical errors building up in the GPS data
             last_one_hz_ms = now;
             Vector2d diffNE = origin.get_distance_NE_double(location);
             position.xy() -= diffNE;
diff --git a/libraries/SITL/SIM_Glider.cpp b/libraries/SITL/SIM_Glider.cpp
index c86535b83cc5dc..78250dbbfec756 100644
--- a/libraries/SITL/SIM_Glider.cpp
+++ b/libraries/SITL/SIM_Glider.cpp
@@ -127,7 +127,7 @@ Vector3f Glider::getTorque(float inputAileron, float inputElevator, float inputR
     float Mz = Cn * qPa * m.Sref * m.refSpan;
 
 
-#if 0
+#if HAL_LOGGING_ENABLED
     AP::logger().Write("GLT", "TimeUS,Alpha,Beta,Cl,Cm,Cn", "Qfffff",
                        AP_HAL::micros64(),
                        degrees(alpharad),
@@ -231,7 +231,7 @@ void Glider::calculate_forces(const struct sitl_input &input, Vector3f &rot_acce
     float aileron  = 0.5*(filtered_servo_angle(input, 1) + filtered_servo_angle(input, 4));
     float elevator = filtered_servo_angle(input, 2);
     float rudder   = filtered_servo_angle(input, 3);
-    float balloon  = filtered_servo_range(input, 5);
+    float balloon  = MAX(0.0f, filtered_servo_range(input, 5)); // Don't let the balloon receive downwards commands.
     float balloon_cut = filtered_servo_range(input, 9);
 
     // Move balloon upwards using balloon velocity from channel 6
@@ -241,7 +241,7 @@ void Glider::calculate_forces(const struct sitl_input &input, Vector3f &rot_acce
         balloon_velocity = Vector3f(-wind_ef.x, -wind_ef.y, -wind_ef.z -balloon_rate * balloon);
         balloon_position += balloon_velocity * (1.0e-6 * (float)frame_time_us);
         const float height_AMSL = 0.01f * (float)home.alt - position.z;
-        // release at burst height or when channel 9 goes high
+        // release at burst height or when balloon cut output goes high
         if (hal.scheduler->is_system_initialized() &&
             (height_AMSL > balloon_burst_amsl || balloon_cut > 0.8)) {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "pre-release at %i m AMSL\n", (int)height_AMSL);
@@ -380,7 +380,7 @@ bool Glider::update_balloon(float balloon, Vector3f &force, Vector3f &rot_accel)
     // NED unit vector pointing from tether attachment on plane to attachment on balloon
     Vector3f tether_unit_vec_ef = relative_position.normalized();
 
-    // NED velocity of attahment point on plane
+    // NED velocity of attachment point on plane
     Vector3f attachment_velocity_ef = velocity_ef + dcm * (gyro % tether_pos_bf);
 
     // NED velocity of attachment point on balloon as seen by observer on attachemnt point on plane