diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 165ec676ed..ca07ab7c8b 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -765,4 +765,11 @@ void Aircraft::update_external_payload(const struct sitl_input &input) gripper_epm->update(input); external_payload_mass += gripper_epm->payload_mass(); } + + + // update parachute + if (parachute && parachute->is_enabled()) { + parachute->update(input); + // TODO: add drag to vehicle, presumably proportional to velocity + } } diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 384843808a..df9ec7e8f9 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -26,6 +26,7 @@ #include "SIM_Sprayer.h" #include "SIM_Gripper_Servo.h" #include "SIM_Gripper_EPM.h" +#include "SIM_Parachute.h" namespace SITL { @@ -109,6 +110,7 @@ public: } void set_sprayer(Sprayer *_sprayer) { sprayer = _sprayer; } + void set_parachute(Parachute *_parachute) { parachute = _parachute; } void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; } void set_gripper_epm(Gripper_EPM *_gripper_epm) { gripper_epm = _gripper_epm; } @@ -258,6 +260,7 @@ private: Sprayer *sprayer; Gripper_Servo *gripper; Gripper_EPM *gripper_epm; + Parachute *parachute; }; } // namespace SITL diff --git a/libraries/SITL/SIM_Parachute.cpp b/libraries/SITL/SIM_Parachute.cpp new file mode 100644 index 0000000000..f1d89d2e42 --- /dev/null +++ b/libraries/SITL/SIM_Parachute.cpp @@ -0,0 +1,71 @@ +/* + 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 . + */ +/* + simple parachute simulator class +*/ + +#include "SIM_Parachute.h" +#include "AP_HAL/AP_HAL.h" +#include +#include + +using namespace SITL; + +// table of user settable parameters +const AP_Param::GroupInfo Parachute::var_info[] = { + + // @Param: ENABLE + // @DisplayName: Gripper servo Sim enable/disable + // @Description: Allows you to enable (1) or disable (0) the gripper servo simulation + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("ENABLE", 0, Parachute, parachute_enable, 0), + + // @Param: PIN + // @DisplayName: Parachute pin + // @Description: The pin number that the Parachute pyrotechnics are connected to. (start at 1) + // @Range: 0 15 + // @User: Advanced + AP_GROUPINFO("PIN", 1, Parachute, parachute_pin, -1), + + AP_GROUPEND +}; + +/* + update parachute state + */ +void Parachute::update(const struct sitl_input &input) +{ + const int16_t pwm = parachute_pin >= 1 ? input.servos[parachute_pin-1] : -1; + const uint64_t now = AP_HAL::micros64(); + // const float dt = (now - last_update_us) * 1.0e-6f; + if (pwm >= 1250) { + if (!deployed_ms) { + deployed_ms = AP_HAL::millis(); + gcs().send_text(MAV_SEVERITY_WARNING, "BANG! Parachute deployed"); + } + } + last_update_us = now; +} + +bool Parachute::should_report() +{ + if (AP_HAL::micros64() - last_report_us < report_interval) { + return false; + } + + return false; +} + diff --git a/libraries/SITL/SIM_Parachute.h b/libraries/SITL/SIM_Parachute.h new file mode 100644 index 0000000000..7fdc97fad4 --- /dev/null +++ b/libraries/SITL/SIM_Parachute.h @@ -0,0 +1,58 @@ +/* + 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 . + */ +/* + simple parachute simulation class +*/ + +#pragma once + +#include "stdint.h" +#include +#include "SITL_Input.h" +#include + +namespace SITL { + +class Parachute { +public: + Parachute() { + AP_Param::setup_object_defaults(this, var_info); + }; + + // update parachute state + void update(const struct sitl_input &input); + + Vector3f drag() const; + + static const struct AP_Param::GroupInfo var_info[]; + bool is_enabled() const {return static_cast(parachute_enable);} + + private: + + AP_Int8 parachute_enable; // enable parachute sim + AP_Int8 parachute_pin; // pin with pyrotechnics on + + const uint32_t report_interval = 1000000; // microseconds + uint64_t last_report_us; + + uint32_t deployed_ms; // time parachute was deployed + + uint64_t last_update_us; + + bool should_report(); + bool zero_report_done = false; +}; + +} diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 9c801693a1..460b4f4dcd 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -137,7 +137,10 @@ const AP_Param::GroupInfo SITL::var_info2[] = { // vibration frequencies on each axis AP_GROUPINFO("VIB_FREQ", 26, SITL, vibe_freq, 0), - + + // @Path: ./SIM_Parachute.cpp + AP_SUBGROUPINFO(parachute_sim, "PARA_", 27, SITL, Parachute), + AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 1453df0365..ad2e148d7d 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -6,6 +6,7 @@ #include "SIM_Sprayer.h" #include "SIM_Gripper_Servo.h" #include "SIM_Gripper_EPM.h" +#include "SIM_Parachute.h" class DataFlash_Class; @@ -238,6 +239,8 @@ public: Gripper_Servo gripper_sim; Gripper_EPM gripper_epm_sim; + + Parachute parachute_sim; }; } // namespace SITL