mirror of https://github.com/ArduPilot/ardupilot
104 lines
4.1 KiB
C++
104 lines
4.1 KiB
C++
/*
|
|
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, 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
|
|
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, 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;
|
|
|
|
// 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 = 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
|
|
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
|