From e170710de9ba36ff5341070a7bc7c724d4b59823 Mon Sep 17 00:00:00 2001 From: George Zogopoulos 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 | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index a6baf91eb9..3e519576e4 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 c86535b83c..a1334aac61 100644 --- a/libraries/SITL/SIM_Glider.cpp +++ b/libraries/SITL/SIM_Glider.cpp @@ -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