Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: Glider improvements 

Fixed balloon going below ground.
Enabled glider torque logging.
This commit is contained in:
George Zogopoulos 2024-07-18 09:54:02 +02:00 committed by Andrew Tridgell
parent bedc76e0e8
commit e170710de9
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/SITL/SIM_Aircraft.cpp
View File

@ -181,7 +181,7 @@ void Aircraft::update_position(void)
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if (now - last_one_hz_ms >= 1000) { if (now - last_one_hz_ms >= 1000) {
// shift origin of position at 1Hz to current location // 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; last_one_hz_ms = now;
Vector2d diffNE = origin.get_distance_NE_double(location); Vector2d diffNE = origin.get_distance_NE_double(location);
position.xy() -= diffNE; position.xy() -= diffNE;

6
libraries/SITL/SIM_Glider.cpp
View File

@ -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 aileron = 0.5*(filtered_servo_angle(input, 1) + filtered_servo_angle(input, 4));
float elevator = filtered_servo_angle(input, 2); float elevator = filtered_servo_angle(input, 2);
float rudder = filtered_servo_angle(input, 3); 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); float balloon_cut = filtered_servo_range(input, 9);
// Move balloon upwards using balloon velocity from channel 6 // 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_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); balloon_position += balloon_velocity * (1.0e-6 * (float)frame_time_us);
const float height_AMSL = 0.01f * (float)home.alt - position.z; 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() && if (hal.scheduler->is_system_initialized() &&
(height_AMSL > balloon_burst_amsl || balloon_cut > 0.8)) { (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); 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 // NED unit vector pointing from tether attachment on plane to attachment on balloon
Vector3f tether_unit_vec_ef = relative_position.normalized(); 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); 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 // NED velocity of attachment point on balloon as seen by observer on attachemnt point on plane