From e25cc41b0093bc11e30f52f3a81831b8d47ef625 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 8 Jul 2024 19:50:02 +0900 Subject: [PATCH 1/2] SITL: add SlungPayload --- libraries/SITL/SIM_SlungPayload.cpp | 458 ++++++++++++++++++++++++++++ libraries/SITL/SIM_SlungPayload.h | 102 +++++++ 2 files changed, 560 insertions(+) create mode 100644 libraries/SITL/SIM_SlungPayload.cpp create mode 100644 libraries/SITL/SIM_SlungPayload.h diff --git a/libraries/SITL/SIM_SlungPayload.cpp b/libraries/SITL/SIM_SlungPayload.cpp new file mode 100644 index 0000000000000..03b5363300d7c --- /dev/null +++ b/libraries/SITL/SIM_SlungPayload.cpp @@ -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 . + */ +/* + simulate a slung payload +*/ + +#include "SIM_config.h" + +#if AP_SIM_SLUNGPAYLOAD_ENABLED + +#include "SIM_SlungPayload.h" +#include "SITL.h" +#include +#include "SIM_Aircraft.h" +#include +#include +#include + +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 diff --git a/libraries/SITL/SIM_SlungPayload.h b/libraries/SITL/SIM_SlungPayload.h new file mode 100644 index 0000000000000..3f668af547294 --- /dev/null +++ b/libraries/SITL/SIM_SlungPayload.h @@ -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 . + */ +/* + simulate a payload slung from a line under a vehicle +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_SLUNGPAYLOAD_ENABLED + +#include +#include +#include +#include + +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 From 8e8146d710ef14a591325410c49ada3191efa548 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 12 Jul 2024 15:18:16 +0900 Subject: [PATCH 2/2] SITL: integrate SlungPayload --- libraries/SITL/SIM_Aircraft.cpp | 18 ++++++++++++++++++ libraries/SITL/SIM_Aircraft.h | 5 +++++ libraries/SITL/SIM_Multicopter.cpp | 5 +++++ libraries/SITL/SIM_config.h | 4 ++++ libraries/SITL/SITL.cpp | 6 ++++++ libraries/SITL/SITL.h | 4 ++++ 6 files changed, 42 insertions(+) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index a6baf91eb9da2..05493e9506157 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -796,6 +796,11 @@ 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); +#endif + // allow for changes in physics step adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1)); } @@ -1227,6 +1232,19 @@ void Aircraft::add_twist_forces(Vector3f &rot_accel) } } +#if AP_SIM_SLUNGPAYLOAD_ENABLED +// add body-frame force due to slung payload +void Aircraft::add_slungpayload_forces(Vector3f &body_accel) +{ + Vector3f forces_ef; + sitl->models.slung_payload_sim.get_forces_on_vehicle(forces_ef); + + // convert ef forces to body-frame accelerations (acceleration = force / mass) + const Vector3f accel_bf = dcm.transposed() * forces_ef / mass; + body_accel += accel_bf; +} +#endif + /* get position relative to home */ diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 90f07089ea60b..e84a078ac8faf 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -322,6 +322,11 @@ class Aircraft { void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel); void add_twist_forces(Vector3f &rot_accel); +#if AP_SIM_SLUNGPAYLOAD_ENABLED + // add body-frame force due to slung payload + void add_slungpayload_forces(Vector3f &body_accel); +#endif + // get local thermal updraft float get_local_updraft(const Vector3d ¤tPos); diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index a866762593e05..4e019dde9b4de 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -48,6 +48,11 @@ void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot add_shove_forces(rot_accel, body_accel); add_twist_forces(rot_accel); + +#if AP_SIM_SLUNGPAYLOAD_ENABLED + // add forces from slung payload + add_slungpayload_forces(body_accel); +#endif } /* diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index c83e83d44767d..8ec2d7ff42c48 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -51,6 +51,10 @@ #define AP_SIM_SHIP_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif +#ifndef AP_SIM_SLUNGPAYLOAD_ENABLED +#define AP_SIM_SLUNGPAYLOAD_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + #ifndef AP_SIM_TSYS03_ENABLED #define AP_SIM_TSYS03_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index a21e744c349b5..681d5d3dbf440 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -1249,6 +1249,12 @@ const AP_Param::GroupInfo SIM::ModelParm::var_info[] = { AP_SUBGROUPPTR(glider_ptr, "GLD_", 3, SIM::ModelParm, Glider), #endif +#if AP_SIM_SLUNGPAYLOAD_ENABLED + // @Group: SLUP_ + // @Path: ./SIM_SlungPayload.cpp + AP_SUBGROUPINFO(slung_payload_sim, "SLUP_", 4, SIM::ModelParm, SlungPayloadSim), +#endif + AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 3b197fe8469e5..d3ad9ebbd962a 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -27,6 +27,7 @@ #include "SIM_FETtecOneWireESC.h" #include "SIM_IntelligentEnergy24.h" #include "SIM_Ship.h" +#include "SIM_SlungPayload.h" #include "SIM_GPS.h" #include "SIM_DroneCANDevice.h" #include "SIM_ADSB_Sagetech_MXS.h" @@ -322,6 +323,9 @@ class SIM { #endif #if AP_SIM_GLIDER_ENABLED Glider *glider_ptr; +#endif +#if AP_SIM_SLUNGPAYLOAD_ENABLED + SlungPayloadSim slung_payload_sim; #endif }; ModelParm models;