From 2e594d0f433351d66f15a9c5fd505bebc23bc433 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 30 Jul 2024 20:38:36 +0900 Subject: [PATCH 1/3] SITL: slung payload affected by wind --- libraries/SITL/SIM_SlungPayload.cpp | 21 +++++++++++++-------- libraries/SITL/SIM_SlungPayload.h | 9 +++++---- 2 files changed, 18 insertions(+), 12 deletions(-) diff --git a/libraries/SITL/SIM_SlungPayload.cpp b/libraries/SITL/SIM_SlungPayload.cpp index 03b5363300d7c..f761185853688 100644 --- a/libraries/SITL/SIM_SlungPayload.cpp +++ b/libraries/SITL/SIM_SlungPayload.cpp @@ -79,8 +79,8 @@ SlungPayloadSim::SlungPayloadSim() AP_Param::setup_object_defaults(this, var_info); } -// update the SlungPayloadSim's state using the vehicle's earth-frame position, velocity and acceleration -void SlungPayloadSim::update(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef) +// update the SlungPayloadSim's state using the vehicle's earth-frame position, velocity, acceleration and wind +void SlungPayloadSim::update(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef, const Vector3f& wind_ef) { if (!enable) { return; @@ -104,7 +104,7 @@ void SlungPayloadSim::update(const Vector3p& veh_pos, const Vector3f& veh_vel_ef // calculate dt and update slung payload const float dt = (now_us - last_update_us)*1.0e-6; last_update_us = now_us; - update_payload(veh_pos, veh_vel_ef, veh_accel_ef, dt); + update_payload(veh_pos, veh_vel_ef, veh_accel_ef, wind_ef, dt); // send payload location to GCS at 5hz const uint32_t now_ms = AP_HAL::millis(); @@ -289,8 +289,9 @@ bool SlungPayloadSim::get_payload_location(Location& payload_loc) const } // update the slung payloads position, velocity, acceleration -// vehicle position, velocity and acceleration should be in earth-frame NED frame -void SlungPayloadSim::update_payload(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef, float dt) +// vehicle position, velocity, acceleration and wind should be in earth-frame NED frame +void SlungPayloadSim::update_payload(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef, + const Vector3f& wind_ef, float dt) { // how we calculate the payload's position, velocity and acceleration // 1. update the payload's position, velocity using the previous iterations acceleration @@ -373,11 +374,15 @@ void SlungPayloadSim::update_payload(const Vector3p& veh_pos, const Vector3f& ve // calculate drag force (0.5 * drag_coef * air_density * velocity^2 * surface area) Vector3f force_drag_NED; - if (drag_coef > 0 && !velocity_NED.is_zero()) { + Vector3f velocity_air_NED = velocity_NED; + if (!landed) { + velocity_air_NED -= wind_ef; + } + if (drag_coef > 0 && !velocity_air_NED.is_zero()) { const float air_density = 1.225; // 1.225 kg/m^3 (standard sea-level density) const float surface_area_m2 = 0.07; // 30cm diameter sphere - const float drag_force = 0.5 * drag_coef * air_density * velocity_NED.length_squared() * surface_area_m2; - force_drag_NED = -velocity_NED.normalized() * drag_force; + const float drag_force = 0.5 * drag_coef * air_density * velocity_air_NED.length_squared() * surface_area_m2; + force_drag_NED = -velocity_air_NED.normalized() * drag_force; } // sanity check payload distance from vehicle and calculate tension force diff --git a/libraries/SITL/SIM_SlungPayload.h b/libraries/SITL/SIM_SlungPayload.h index 3f668af547294..25c866f500c4d 100644 --- a/libraries/SITL/SIM_SlungPayload.h +++ b/libraries/SITL/SIM_SlungPayload.h @@ -37,8 +37,8 @@ class SlungPayloadSim { // constructor SlungPayloadSim(); - // update the SlungPayloadSim's state using thevehicle's earth-frame position, velocity and acceleration - void update(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef); + // update the SlungPayloadSim's state using thevehicle's earth-frame position, velocity, acceleration and wind + void update(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef, const Vector3f& wind_ef); // get earth-frame forces on the vehicle from slung payload // returns true on success and fills in forces_ef argument, false on failure @@ -67,8 +67,9 @@ class SlungPayloadSim { bool get_payload_location(Location& payload_loc) const; // update the slung payload's position, velocity, acceleration - // vehicle position, velocity and acceleration should be in earth-frame NED frame - void update_payload(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef, float dt); + // vehicle position, velocity, acceleration and wind should be in earth-frame NED frame + void update_payload(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef, + const Vector3f& wind_ef, float dt); // returns true if the two vectors point in the same direction, false if perpendicular or opposite bool vectors_same_direction(const Vector3f& v1, const Vector3f& v2) const; From 5cfcf46be5afa61de451651fcbf811742336ca26 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 27 Aug 2024 13:39:28 +0900 Subject: [PATCH 2/3] SITL: slung payload sends pos updates at 10hz --- libraries/SITL/SIM_SlungPayload.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_SlungPayload.h b/libraries/SITL/SIM_SlungPayload.h index 25c866f500c4d..e9a60aaac7e50 100644 --- a/libraries/SITL/SIM_SlungPayload.h +++ b/libraries/SITL/SIM_SlungPayload.h @@ -82,7 +82,7 @@ class SlungPayloadSim { uint32_t last_update_us; // system time of last update // mavlink reporting variables - const float reporting_period_ms = 200; // reporting period in ms + const float reporting_period_ms = 100; // reporting period in ms uint32_t last_report_ms; // system time of last MAVLink report sent to GCS uint32_t last_heartbeat_ms; // system time of last MAVLink heartbeat sent to GCS bool mavlink_connected; // true if a mavlink connection has been established From 9e1b3354058b7584e7b2e779747dd64f909eb397 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 30 Jul 2024 20:38:50 +0900 Subject: [PATCH 3/3] SITL: aircraft sends wind to slung payload --- libraries/SITL/SIM_Aircraft.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 9ca985a14591a..e3a10152af5d0 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -798,7 +798,7 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel) // update slung payload #if AP_SIM_SLUNGPAYLOAD_ENABLED - sitl->models.slung_payload_sim.update(get_position_relhome(), velocity_ef, accel_earth); + sitl->models.slung_payload_sim.update(get_position_relhome(), velocity_ef, accel_earth, wind_ef); #endif // allow for changes in physics step