SITL: add simulated parachute

Doesn't actually change aerodynamics at the moment

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
+    }
 }

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

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  simple parachute simulator class
+*/
+
+#include "SIM_Parachute.h"
+#include "AP_HAL/AP_HAL.h"
+#include <AP_Math/AP_Math.h>
+#include <GCS_MAVLink/GCS.h>
+
+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;
+}
+

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  simple parachute simulation class
+*/
+
+#pragma once
+
+#include "stdint.h"
+#include <AP_Param/AP_Param.h>
+#include "SITL_Input.h"
+#include <AP_Math/AP_Math.h>
+
+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<bool>(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;
+};
+
+}

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
 };
     

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