Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: add SlungPayload

Browse Source
This commit is contained in:
Randy Mackay 2024-07-08 19:50:02 +09:00 committed by Andrew Tridgell
parent cb044d4718
commit 5c2b758f42
2 changed files with 560 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

458
libraries/SITL/SIM_SlungPayload.cpp Normal file
View File

@ -0,0 +1,458 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
simulate a slung payload
*/
#include "SIM_config.h"
#if AP_SIM_SLUNGPAYLOAD_ENABLED
#include "SIM_SlungPayload.h"
#include "SITL.h"
#include <stdio.h>
#include "SIM_Aircraft.h"
#include <AP_HAL_SITL/SITL_State.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
using namespace SITL;
// SlungPayloadSim parameters
const AP_Param::GroupInfo SlungPayloadSim::var_info[] = {
// @Param: ENABLE
// @DisplayName: Slung Payload Sim enable/disable
// @Description: Slung Payload Sim enable/disable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 1, SlungPayloadSim, enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: WEIGHT
// @DisplayName: Slung Payload weight
// @Description: Slung Payload weight in kg
// @Units: kg
// @Range: 0 15
// @User: Advanced
AP_GROUPINFO("WEIGHT", 2, SlungPayloadSim, weight_kg, 1.0),
// @Param: LINELEN
// @DisplayName: Slung Payload line length
// @Description: Slung Payload line length in meters
// @Units: m
// @Range: 0 100
// @User: Advanced
AP_GROUPINFO("LINELEN", 3, SlungPayloadSim, line_length, 30.0),
// @Param: DRAG
// @DisplayName: Slung Payload drag coefficient
// @Description: Slung Payload drag coefficient. Higher values increase drag and slow the payload more quickly
// @Units: m
// @Range: 0 10
// @User: Advanced
AP_GROUPINFO("DRAG", 4, SlungPayloadSim, drag_coef, 1),
// @Param: SYSID
// @DisplayName: Slung Payload MAVLink system ID
// @Description: Slung Payload MAVLink system id to distinguish it from others on the same network
// @Range: 0 255
// @User: Advanced
AP_GROUPINFO("SYSID", 5, SlungPayloadSim, sys_id, 2),
AP_GROUPEND
};
// SlungPayloadSim handles interaction with main vehicle
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)
{
if (!enable) {
return;
}
// initialise slung payload location
const uint32_t now_us = AP_HAL::micros();
if (!initialised) {
// capture EKF origin
auto *sitl = AP::sitl();
const Location ekf_origin = sitl->state.home;
if (ekf_origin.lat == 0 && ekf_origin.lng == 0) {
return;
}
// more initialisation
last_update_us = now_us;
initialised = true;
}
// 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);
// send payload location to GCS at 5hz
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - last_report_ms >= reporting_period_ms) {
last_report_ms = now_ms;
send_report();
write_log();
}
}
// get earth-frame forces on the vehicle from slung payload
// returns true on success and fills in forces_ef argument, false on failure
bool SlungPayloadSim::get_forces_on_vehicle(Vector3f& forces_ef) const
{
if (!enable) {
return false;
}
forces_ef = veh_forces_ef;
return true;
}
// send a report to the vehicle control code over MAVLink
void SlungPayloadSim::send_report(void)
{
if (!mavlink_connected && mav_socket.connect(target_address, target_port)) {
::printf("SlungPayloadSim connected to %s:%u\n", target_address, (unsigned)target_port);
mavlink_connected = true;
}
if (!mavlink_connected) {
return;
}
// get current time
uint32_t now_ms = AP_HAL::millis();
// send heartbeat at 1hz
const uint8_t component_id = MAV_COMP_ID_USER11;
if (now_ms - last_heartbeat_ms >= 1000) {
last_heartbeat_ms = now_ms;
const mavlink_heartbeat_t heartbeat{
custom_mode: 0,
type : MAV_TYPE_AIRSHIP,
autopilot : MAV_AUTOPILOT_INVALID,
base_mode: 0,
system_status: 0,
mavlink_version: 0,
};
mavlink_message_t msg;
mavlink_msg_heartbeat_encode_status(
sys_id.get(),
component_id,
&mav_status,
&msg,
&heartbeat);
uint8_t buf[300];
const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
mav_socket.send(buf, len);
}
// send a GLOBAL_POSITION_INT messages
{
Location payload_loc;
int32_t alt_amsl_cm, alt_rel_cm;
if (!get_payload_location(payload_loc) ||
!payload_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm) ||
!payload_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_rel_cm)) {
return;
}
const mavlink_global_position_int_t global_position_int{
time_boot_ms: now_ms,
lat: payload_loc.lat,
lon: payload_loc.lng,
alt: alt_amsl_cm * 10, // amsl alt in mm
relative_alt: alt_rel_cm * 10, // relative alt in mm
vx: int16_t(velocity_NED.x * 100), // velocity in cm/s
vy: int16_t(velocity_NED.y * 100), // velocity in cm/s
vz: int16_t(velocity_NED.z * 100), // velocity in cm/s
hdg: 0 // heading in centi-degrees
};
mavlink_message_t msg;
mavlink_msg_global_position_int_encode_status(sys_id, component_id, &mav_status, &msg, &global_position_int);
uint8_t buf[300];
const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
if (len > 0) {
mav_socket.send(buf, len);
}
}
// send ATTITUDE so MissionPlanner can display orientation
{
const mavlink_attitude_t attitude{
time_boot_ms: now_ms,
roll: 0,
pitch: 0,
yaw: 0, // heading in radians
rollspeed: 0,
pitchspeed: 0,
yawspeed: 0
};
mavlink_message_t msg;
mavlink_msg_attitude_encode_status(
sys_id,
component_id,
&mav_status,
&msg,
&attitude);
uint8_t buf[300];
const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
if (len > 0) {
mav_socket.send(buf, len);
}
}
}
// write onboard log
void SlungPayloadSim::write_log()
{
#if HAL_LOGGING_ENABLED
// write log of slung payload state
// @LoggerMessage: SLUP
// @Description: Slung payload
// @Field: TimeUS: Time since system startup
// @Field: Land: 1 if payload is landed, 0 otherwise
// @Field: Tens: Tension ratio, 1 if line is taut, 0 if slack
// @Field: Len: Line length
// @Field: PN: Payload position as offset from vehicle in North direction
// @Field: PE: Payload position as offset from vehicle in East direction
// @Field: PD: Payload position as offset from vehicle in Down direction
// @Field: VN: Payload velocity in North direction
// @Field: VE: Payload velocity in East direction
// @Field: VD: Payload velocity in Down direction
// @Field: AN: Payload acceleration in North direction
// @Field: AE: Payload acceleration in East direction
// @Field: AD: Payload acceleration in Down direction
// @Field: VFN: Force on vehicle in North direction
// @Field: VFE: Force on vehicle in East direction
// @Field: VFD: Force on vehicle in Down direction
AP::logger().WriteStreaming("SLUP",
"TimeUS,Land,Tens,Len,PN,PE,PD,VN,VE,VD,AN,AE,AD,VFN,VFE,VFD", // labels
"s-%mmmmnnnooo---", // units
"F-20000000000000", // multipliers
"Qbffffffffffffff", // format
AP_HAL::micros64(),
(uint8_t)landed,
(float)tension_ratio,
(float)payload_to_veh.length(),
(double)-payload_to_veh.x,
(double)-payload_to_veh.y,
(double)-payload_to_veh.z,
(double)velocity_NED.x,
(double)velocity_NED.y,
(double)velocity_NED.z,
(double)accel_NED.x,
(double)accel_NED.y,
(double)accel_NED.z,
(double)veh_forces_ef.x,
(double)veh_forces_ef.y,
(double)veh_forces_ef.z);
#endif
}
// returns true on success and fills in payload_loc argument, false on failure
bool SlungPayloadSim::get_payload_location(Location& payload_loc) const
{
// get EKF origin
auto *sitl = AP::sitl();
if (sitl == nullptr) {
return false;
}
const Location ekf_origin = sitl->state.home;
if (ekf_origin.lat == 0 && ekf_origin.lng == 0) {
return false;
}
// calculate location
payload_loc = ekf_origin;
payload_loc.offset(position_NED);
return true;
}
// 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)
{
// how we calculate the payload's position, velocity and acceleration
// 1. update the payload's position, velocity using the previous iterations acceleration
// 2. check that the payload does not fall below the terrain
// 3. check if the line is taught and that the payload does not move more than the line length from the vehicle
// 4. calculate gravity and drag forces on the payload
// 5. calculate the tension force between the payload and vehicle including force countering gravity, drag and centripetal force
// 6. update the payload's acceleration using the sum of the above forces
// initialise position_NED from vehicle position
if (position_NED.is_zero()) {
if (!veh_pos.is_zero()) {
position_NED = veh_pos;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: initialised at %f %f %f", position_NED.x, position_NED.y, position_NED.z);
}
return;
}
// integrate previous iterations acceleration into velocity and position
velocity_NED += accel_NED * dt;
position_NED += (velocity_NED * dt).todouble();
// calculate distance from payload to vehicle
payload_to_veh = veh_pos - position_NED;
float payload_to_veh_length = payload_to_veh.length();
// update landed state by checking if payload has dropped below terrain
Location payload_loc;
if (get_payload_location(payload_loc)) {
int32_t alt_terrain_cm;
bool landed_orig = landed;
if (payload_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_terrain_cm)) {
// landed if below terrain
if (alt_terrain_cm <= 0) {
landed = true;
// raise payload to match terrain
position_NED.z += (alt_terrain_cm * 0.01);
// zero out velocity and acceleration in horizontal and downward direction
velocity_NED.xy().zero();
velocity_NED.z = MIN(velocity_NED.z, 0);
accel_NED.xy().zero();
accel_NED.z = MIN(accel_NED.z, 0);
// zero out forces on vehicle
veh_forces_ef.zero();
}
// not landed if above terrain
if (landed && (alt_terrain_cm > 1)) {
landed = false;
}
}
// inform user if landed state has changed
if (landed != landed_orig) {
if (landed) {
// get payload location again in case it has moved
get_payload_location(payload_loc);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: landed lat:%f lon:%f alt:%4.1f",
(double)payload_loc.lat * 1e-7,
(double)payload_loc.lng * 1e-7,
(double)payload_loc.alt * 1e-2);
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: liftoff");
}
}
}
// calculate forces of gravity
Vector3f force_gravity_NED = Vector3f(0.0f, 0.0f, GRAVITY_MSS * weight_kg);
// tension force on payload (resists gravity, drag, centripetal force)
Vector3f tension_force_NED;
// tension ratio to smooth transition from line being taut to slack
tension_ratio = 0;
// 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()) {
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;
}
// sanity check payload distance from vehicle and calculate tension force
if (is_positive(payload_to_veh_length)) {
// calculate unit vector from payload to vehicle
const Vector3f payload_to_veh_norm = payload_to_veh.normalized().tofloat();
// ensure payload is no more than line_length from vehicle
if (payload_to_veh_length > line_length) {
payload_to_veh *= (line_length / payload_to_veh_length);
position_NED = veh_pos - payload_to_veh;
}
// calculate tension ratio as value between 0 and 1
// tension ratio is 0 when payload-to-vehicle distance is 10cm less than line length
// tension ratio is 1 when payload-to-vehicle distance is equal to line length
tension_ratio = constrain_float(1.0 - (line_length - payload_to_veh_length) * 10, 0, 1);
// calculate tension forces when line is taut
if (is_positive(tension_ratio)) {
// tension resists gravity if vehicle is above payload
if (is_negative(payload_to_veh_norm.z)) {
tension_force_NED += -force_gravity_NED.projected(payload_to_veh_norm);
}
// calculate tension force resulting from velocity difference between vehicle and payload
// use time constant to convert velocity to acceleration
const float velocity_to_accel_TC = 2.0;
Vector3f velocity_diff_NED = (veh_vel_ef - velocity_NED).projected(payload_to_veh_norm);
// add to tension force if the vehicle is moving faster than the payload
if (vectors_same_direction(velocity_diff_NED, payload_to_veh_norm)) {
tension_force_NED += velocity_diff_NED / velocity_to_accel_TC * weight_kg;
}
// tension force resisting payload drag
tension_force_NED += -force_drag_NED.projected(payload_to_veh_norm);
// calculate centripetal force
const Vector3f velocity_parallel = velocity_NED.projected(payload_to_veh_norm);
const Vector3f velocity_perpendicular = velocity_NED - velocity_parallel;
const float tension_force_centripetal = velocity_perpendicular.length_squared() * weight_kg / line_length;
const Vector3f tension_force_centripetal_NED = payload_to_veh_norm * tension_force_centripetal;
// add centripetal force to tension force
tension_force_NED += tension_force_centripetal_NED;
// scale tension force by tension ratio
tension_force_NED *= tension_ratio;
}
}
// force on vehicle is opposite to tension force on payload
veh_forces_ef = -tension_force_NED;
// convert force to acceleration (f=m*a => a=f/m)
accel_NED = (force_gravity_NED + force_drag_NED + tension_force_NED) / weight_kg;
// if slung payload is landed we zero out downward (e.g positive) acceleration
if (landed) {
accel_NED.z = MIN(accel_NED.z, 0);
// should probably zero out forces_ef vertical component as well?
}
}
// returns true if the two vectors point in the same direction, false if perpendicular or opposite
bool SlungPayloadSim::vectors_same_direction(const Vector3f& v1, const Vector3f& v2) const
{
// check both vectors are non-zero
if (v1.is_zero() || v2.is_zero()) {
return false;
}
return v1.dot(v2) > 0;
}
#endif

102
libraries/SITL/SIM_SlungPayload.h Normal file
View File

@ -0,0 +1,102 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
simulate a payload slung from a line under a vehicle
*/
#pragma once
#include "SIM_config.h"
#if AP_SIM_SLUNGPAYLOAD_ENABLED
#include <AP_HAL/utility/Socket_native.h>
#include <AP_Math/AP_Math.h>
#include <AP_Common/Location.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
namespace SITL {
// SlungPayloadSim handles interaction with main vehicle
class SlungPayloadSim {
public:
friend class SlungPayload;
// 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);
// get earth-frame forces on the vehicle from slung payload
// returns true on success and fills in forces_ef argument, false on failure
bool get_forces_on_vehicle(Vector3f& forces_ef) const;
// parameter table
static const struct AP_Param::GroupInfo var_info[];
private:
// parameters
AP_Int8 enable; // enable parameter
AP_Float weight_kg; // payload weight in kg
AP_Float line_length; // line length in meters
AP_Int8 sys_id; // mavlink system id for reporting to GCS
AP_Float drag_coef; // drag coefficient (spheres=0.5, cubes=1.05, barrels=0.8~1.2)
// send MAVLink messages to GCS
void send_report();
// write onboard log
void write_log();
// get payload location
// returns true on success and fills in payload_loc argument, false on failure
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);
// 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;
// socket connection variables
const char *target_address = "127.0.0.1";
const uint16_t target_port = 5763;
SocketAPM_native mav_socket { false };
bool initialised; // true if this class has been initialised
uint32_t last_update_us; // system time of last update
// mavlink reporting variables
const float reporting_period_ms = 200; // 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
mavlink_status_t mav_status; // reported mavlink status
// payload variables
bool landed = true; // true if the payload is on the ground
float tension_ratio; // 0 if line is loose, 1 if completely taut
Vector3p payload_to_veh;// distance vector (in meters in NED frame) from payload to vehicle (used for reporting purposes)
Vector3p position_NED; // payload's position (as an offset from EKF origin? offset from vehicle?) in meters
Vector3f velocity_NED; // payload velocity
Vector3f accel_NED; // payload's acceleration
Vector3f veh_forces_ef; // earth-frame forces on the vehicle caused by the payload
};
} // namespace SITL
#endif // AP_SIM_SLUNGPAYLOAD_ENABLED