Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

SITL: slung payload minor enhancements #27990

Merged
merged 3 commits into from
Sep 6, 2024
Merged

SITL: slung payload minor enhancements #27990

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
2e594d0
SITL: slung payload affected by wind
rmackay9 Jul 30, 2024
5cfcf46
SITL: slung payload sends pos updates at 10hz
rmackay9 Aug 27, 2024
9e1b335
SITL: aircraft sends wind to slung payload
rmackay9 Jul 30, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion libraries/SITL/SIM_Aircraft.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
21 changes: 13 additions & 8 deletions libraries/SITL/SIM_SlungPayload.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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();
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
11 changes: 6 additions & 5 deletions libraries/SITL/SIM_SlungPayload.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand All @@ -81,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
Expand Down
Loading