From 6293780acee1dea7d32026c1522be41c58a9b792 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C4=99drzej=20Stasik?= Date: Tue, 9 Jul 2024 15:52:14 +0200 Subject: [PATCH 1/5] AP_Parachute: add CHUTE_TIMEOUT parameter --- ArduCopter/crash_check.cpp | 7 +++---- libraries/AP_Parachute/AP_Parachute.cpp | 8 ++++++++ libraries/AP_Parachute/AP_Parachute.h | 8 ++++++-- 3 files changed, 17 insertions(+), 6 deletions(-) diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 6383636f3ccec..d1e2d67675334 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -232,7 +232,6 @@ void Copter::yaw_imbalance_check() #if PARACHUTE == ENABLED // Code to detect a crash main ArduCopter code -#define PARACHUTE_CHECK_TRIGGER_SEC 1 // 1 second of loss of control triggers the parachute #define PARACHUTE_CHECK_ANGLE_DEVIATION_DEG 30.0f // 30 degrees off from target indicates a loss of control // parachute_check - disarms motors and triggers the parachute if serious loss of control has been detected @@ -302,7 +301,7 @@ void Copter::parachute_check() } // increment counter - if (control_loss_count < (PARACHUTE_CHECK_TRIGGER_SEC*scheduler.get_loop_rate_hz())) { + if (control_loss_count < (parachute.get_chute_timeout()*scheduler.get_loop_rate_hz())) { control_loss_count++; } @@ -317,8 +316,8 @@ void Copter::parachute_check() // To-Do: add check that the vehicle is actually falling - // check if loss of control for at least 1 second - } else if (control_loss_count >= (PARACHUTE_CHECK_TRIGGER_SEC*scheduler.get_loop_rate_hz())) { + // check if loss of control for at least time defined in CHUTE_TIMEOUT parameter + } else if (control_loss_count >= (parachute.get_chute_timeout()*scheduler.get_loop_rate_hz())) { // reset control loss counter control_loss_count = 0; LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_LOSS_OF_CONTROL); diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp index ad45fed687042..c08e1a500dad1 100644 --- a/libraries/AP_Parachute/AP_Parachute.cpp +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -83,6 +83,14 @@ const AP_Param::GroupInfo AP_Parachute::var_info[] = { // @User: Standard AP_GROUPINFO("OPTIONS", 7, AP_Parachute, _options, AP_PARACHUTE_OPTIONS_DEFAULT), + // @Param: TIMEOUT + // @DisplayName: Parachute timeout + // @Description: Triggers the parachute if the loss of control lasts for the time specified by this parameter + // @Units: s + // @Range: 0.5 5.0 + // @User: Advanced + AP_GROUPINFO("TIMEOUT", 8, AP_Parachute, _timeout, AP_PARACHUTE_TIMEOUT_DEFAULT), + AP_GROUPEND }; diff --git a/libraries/AP_Parachute/AP_Parachute.h b/libraries/AP_Parachute/AP_Parachute.h index b37f20051a97a..eba5cc3bff5cf 100644 --- a/libraries/AP_Parachute/AP_Parachute.h +++ b/libraries/AP_Parachute/AP_Parachute.h @@ -21,6 +21,7 @@ #define AP_PARACHUTE_CRITICAL_SINK_DEFAULT 0 // default critical sink speed in m/s to trigger emergency parachute #define AP_PARACHUTE_OPTIONS_DEFAULT 0 // default parachute options: enabled disarm after parachute release +#define AP_PARACHUTE_TIMEOUT_DEFAULT 1.0f // default parachute control loss timeout #ifndef HAL_PARACHUTE_ENABLED // default to parachute enabled to match previous configs @@ -90,6 +91,9 @@ class AP_Parachute { // Return the relay index that would be used for param conversion to relay functions bool get_legacy_relay_index(int8_t &index) const; + // Return the CHUTE_TIMEOUT parameter value + float get_chute_timeout() const { return _timeout; } + static const struct AP_Param::GroupInfo var_info[]; // get singleton instance @@ -105,6 +109,8 @@ class AP_Parachute { 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 + AP_Int32 _options; // optional behaviour for parachute bitmask + AP_Float _timeout; // loss of control timeout value in seconds // internal variables uint32_t _release_time; // system time that parachute is ordered to be released (actual release will happen 0.5 seconds later) @@ -118,8 +124,6 @@ class AP_Parachute { HoldOpen = (1U<<0), SkipDisarmBeforeParachuteRelease = (1U<<1), }; - - AP_Int32 _options; }; namespace AP { From abd43aea59910fba518f4cbad8e7663cac6ad767 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C4=99drzej=20Stasik?= Date: Fri, 12 Jul 2024 08:50:46 +0200 Subject: [PATCH 2/5] fix: conditional CRASH_CHECK_TRIGGER_SEC value --- ArduCopter/crash_check.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index d1e2d67675334..a5a921f63e873 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -1,7 +1,13 @@ #include "Copter.h" // Code to detect a crash main ArduCopter code -#define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash +#if PARACHUTE == ENABLED + // CRASH_CHECK_TRIGGER_SEC should be grater than CHUTE_TIMEOUT, to prevent disarming without deploying parachute + #define CRASH_CHECK_TRIGGER_SEC parachute.get_chute_timeout() + 1.0f +#else + #define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash +#endif + #define CRASH_CHECK_ANGLE_DEVIATION_DEG 30.0f // 30 degrees beyond target angle is signal we are out of control #define CRASH_CHECK_ANGLE_MIN_DEG 15.0f // vehicle must be leaning at least 15deg to trigger crash check #define CRASH_CHECK_SPEED_MAX 10.0f // vehicle must be moving at less than 10m/s to trigger crash check @@ -86,7 +92,7 @@ void Copter::crash_check() // we may be crashing crash_counter++; - // check if crashing for 2 seconds + // check if crashing for time defined by CRASH_CHECK_TRIGGER_SEC seconds if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) { LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH); // send message to gcs From dc2e04dc851a3cb029835114046a1ee4bdd7474d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C4=99drzej=20Stasik?= Date: Fri, 12 Jul 2024 08:51:14 +0200 Subject: [PATCH 3/5] fix: make CHUTE_TIMEOUT only for Copter --- libraries/AP_Parachute/AP_Parachute.cpp | 2 ++ libraries/AP_Parachute/AP_Parachute.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp index c08e1a500dad1..d9291459944fb 100644 --- a/libraries/AP_Parachute/AP_Parachute.cpp +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -83,6 +83,7 @@ const AP_Param::GroupInfo AP_Parachute::var_info[] = { // @User: Standard AP_GROUPINFO("OPTIONS", 7, AP_Parachute, _options, AP_PARACHUTE_OPTIONS_DEFAULT), +#if APM_BUILD_COPTER_OR_HELI // @Param: TIMEOUT // @DisplayName: Parachute timeout // @Description: Triggers the parachute if the loss of control lasts for the time specified by this parameter @@ -90,6 +91,7 @@ const AP_Param::GroupInfo AP_Parachute::var_info[] = { // @Range: 0.5 5.0 // @User: Advanced AP_GROUPINFO("TIMEOUT", 8, AP_Parachute, _timeout, AP_PARACHUTE_TIMEOUT_DEFAULT), +#endif AP_GROUPEND }; diff --git a/libraries/AP_Parachute/AP_Parachute.h b/libraries/AP_Parachute/AP_Parachute.h index eba5cc3bff5cf..8a96105111381 100644 --- a/libraries/AP_Parachute/AP_Parachute.h +++ b/libraries/AP_Parachute/AP_Parachute.h @@ -91,8 +91,10 @@ class AP_Parachute { // Return the relay index that would be used for param conversion to relay functions bool get_legacy_relay_index(int8_t &index) const; +#if APM_BUILD_COPTER_OR_HELI // Return the CHUTE_TIMEOUT parameter value float get_chute_timeout() const { return _timeout; } +#endif static const struct AP_Param::GroupInfo var_info[]; From ab2df433646397bab7261c4035e11fe562554f40 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Aug 2024 07:26:26 +1000 Subject: [PATCH 4/5] Copter: fixed issue with crash check define used in a multiplication, needs parantheses --- ArduCopter/crash_check.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index a5a921f63e873..b63e91f52bea2 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -3,7 +3,7 @@ // Code to detect a crash main ArduCopter code #if PARACHUTE == ENABLED // CRASH_CHECK_TRIGGER_SEC should be grater than CHUTE_TIMEOUT, to prevent disarming without deploying parachute - #define CRASH_CHECK_TRIGGER_SEC parachute.get_chute_timeout() + 1.0f + #define CRASH_CHECK_TRIGGER_SEC (parachute.get_chute_timeout() + 1.0) #else #define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash #endif From d8a189fb313008401e819a5977ab21e4092777c5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Aug 2024 07:26:39 +1000 Subject: [PATCH 5/5] AP_Parachute: fixed build --- libraries/AP_Parachute/AP_Parachute.cpp | 4 +--- libraries/AP_Parachute/AP_Parachute.h | 2 -- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp index d9291459944fb..cb1ab1b520f04 100644 --- a/libraries/AP_Parachute/AP_Parachute.cpp +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -83,15 +83,13 @@ const AP_Param::GroupInfo AP_Parachute::var_info[] = { // @User: Standard AP_GROUPINFO("OPTIONS", 7, AP_Parachute, _options, AP_PARACHUTE_OPTIONS_DEFAULT), -#if APM_BUILD_COPTER_OR_HELI // @Param: TIMEOUT // @DisplayName: Parachute timeout // @Description: Triggers the parachute if the loss of control lasts for the time specified by this parameter // @Units: s // @Range: 0.5 5.0 // @User: Advanced - AP_GROUPINFO("TIMEOUT", 8, AP_Parachute, _timeout, AP_PARACHUTE_TIMEOUT_DEFAULT), -#endif + AP_GROUPINFO_FRAME("TIMEOUT", 8, AP_Parachute, _timeout, AP_PARACHUTE_TIMEOUT_DEFAULT, AP_PARAM_FRAME_COPTER), AP_GROUPEND }; diff --git a/libraries/AP_Parachute/AP_Parachute.h b/libraries/AP_Parachute/AP_Parachute.h index 8a96105111381..eba5cc3bff5cf 100644 --- a/libraries/AP_Parachute/AP_Parachute.h +++ b/libraries/AP_Parachute/AP_Parachute.h @@ -91,10 +91,8 @@ class AP_Parachute { // Return the relay index that would be used for param conversion to relay functions bool get_legacy_relay_index(int8_t &index) const; -#if APM_BUILD_COPTER_OR_HELI // Return the CHUTE_TIMEOUT parameter value float get_chute_timeout() const { return _timeout; } -#endif static const struct AP_Param::GroupInfo var_info[];