diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp index e792571f14..b9d250a98a 100644 --- a/libraries/AP_Parachute/AP_Parachute.cpp +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -62,6 +62,16 @@ const AP_Param::GroupInfo AP_Parachute::var_info[] = { // @User: Standard AP_GROUPINFO("DELAY_MS", 5, AP_Parachute, _delay_ms, AP_PARACHUTE_RELEASE_DELAY_MS), + // @Param: CRT_SINK + // @DisplayName: Critical sink speed rate in m/s to trigger emergency parachute + // @Description: Release parachute when critical sink rate is reached + // @Range: 0 15 + // @Units: m/s + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("CRT_SINK", 6, AP_Parachute, _critical_sink, AP_PARACHUTE_CRITICAL_SINK_DEFAULT), + + AP_GROUPEND }; @@ -105,7 +115,10 @@ void AP_Parachute::update() if (_enabled <= 0) { return; } - + // check if the plane is sinking too fast and release parachute + if((_critical_sink > 0) && (_sink_rate > _critical_sink) && !_release_initiated && _is_flying) { + release(); + } // calc time since release uint32_t time_diff = AP_HAL::millis() - _release_time; uint32_t delay_ms = _delay_ms<=0 ? 0: (uint32_t)_delay_ms; diff --git a/libraries/AP_Parachute/AP_Parachute.h b/libraries/AP_Parachute/AP_Parachute.h index 7d4378112f..fde7171f43 100644 --- a/libraries/AP_Parachute/AP_Parachute.h +++ b/libraries/AP_Parachute/AP_Parachute.h @@ -20,6 +20,8 @@ #define AP_PARACHUTE_ALT_MIN_DEFAULT 10 // default min altitude the vehicle should have before parachute is released +#define AP_PARACHUTE_CRITICAL_SINK_DEFAULT 0 // default critical sink speed in m/s to trigger emergency parachute + /// @class AP_Parachute /// @brief Class managing the release of a parachute class AP_Parachute { @@ -63,11 +65,20 @@ public: /// update - shuts off the trigger should be called at about 10hz void update(); + + /// critical_sink - returns the configured maximum sink rate to trigger emergency release + float critical_sink() const { return _critical_sink; } /// alt_min - returns the min altitude above home the vehicle should have before parachute is released /// 0 = altitude check disabled int16_t alt_min() const { return _alt_min; } + /// set_is_flying - accessor to the is_flying flag + void set_is_flying(const bool is_flying) { _is_flying = is_flying; } + + // set_sink_rate - set vehicle sink rate + void set_sink_rate(float sink_rate) { _sink_rate = sink_rate; } + static const struct AP_Param::GroupInfo var_info[]; // get singleton instance @@ -82,6 +93,7 @@ private: AP_Int16 _servo_off_pwm; // PWM value to move servo to when shutter is deactivated AP_Int16 _alt_min; // min altitude the vehicle should have before parachute is released AP_Int16 _delay_ms; // delay before chute release for motors to stop + AP_Float _critical_sink; // critical sink rate to trigger emergency parachute // internal variables AP_Relay &_relay; // pointer to relay object from the base class Relay. @@ -89,6 +101,8 @@ private: bool _release_initiated:1; // true if the parachute release initiated (may still be waiting for engine to be suppressed etc.) bool _release_in_progress:1; // true if the parachute release is in progress bool _released:1; // true if the parachute has been released + bool _is_flying:1; // true if the vehicle is flying + float _sink_rate; // vehicle sink rate in m/s }; namespace AP {