From 454a57000b20914fd6417658283f9cd9f25aa9c7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 2 Oct 2023 14:02:40 +0900 Subject: [PATCH] AP_Winch: Daiwa gets stuck protection --- libraries/AP_Winch/AP_Winch.cpp | 4 +- libraries/AP_Winch/AP_Winch.h | 1 + libraries/AP_Winch/AP_Winch_Daiwa.cpp | 82 ++++++++++++++++++++++++++- libraries/AP_Winch/AP_Winch_Daiwa.h | 13 +++++ 4 files changed, 97 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Winch/AP_Winch.cpp b/libraries/AP_Winch/AP_Winch.cpp index 9cfc4f1e84..5a5a7bc9b9 100644 --- a/libraries/AP_Winch/AP_Winch.cpp +++ b/libraries/AP_Winch/AP_Winch.cpp @@ -36,9 +36,9 @@ const AP_Param::GroupInfo AP_Winch::var_info[] = { // @Param: _OPTIONS // @DisplayName: Winch options // @Description: Winch options - // @Bitmask: 0:Spin freely on startup, 1:Verbose output + // @Bitmask: 0:Spin freely on startup, 1:Verbose output, 2:Retry if stuck (Daiwa only) // @User: Standard - AP_GROUPINFO("_OPTIONS", 4, AP_Winch, config.options, 3.0f), + AP_GROUPINFO("_OPTIONS", 4, AP_Winch, config.options, 7.0f), // 4 was _RATE_PID diff --git a/libraries/AP_Winch/AP_Winch.h b/libraries/AP_Winch/AP_Winch.h index 57f5276641..df1ecf9c89 100644 --- a/libraries/AP_Winch/AP_Winch.h +++ b/libraries/AP_Winch/AP_Winch.h @@ -86,6 +86,7 @@ private: enum class Options : int16_t { SpinFreelyOnStartup = (1U << 0), // winch allows line to be manually pulled out soon after startup VerboseOutput = (1U << 1), // verbose output of winch state sent to GCS + RetryIfStuck = (1U << 2), // retries to raise or lower if winch stops }; // winch states diff --git a/libraries/AP_Winch/AP_Winch_Daiwa.cpp b/libraries/AP_Winch/AP_Winch_Daiwa.cpp index 97e5f99e9a..ab1f5ac8e0 100644 --- a/libraries/AP_Winch/AP_Winch_Daiwa.cpp +++ b/libraries/AP_Winch/AP_Winch_Daiwa.cpp @@ -6,6 +6,11 @@ #include #include +#define AP_WINCH_DAIWA_STUCK_TIMEOUT_MS 1000 // winch is considered stuck if unmoving for this many milliseconds +#define AP_WINCH_DAIWA_STUCK_CENTER_MS 1000 // stuck protection outputs zero rate for this many milliseconds +#define AP_WINCH_DAIWA_STUCK_LENGTH_MIN 0.1 // stuck protection active when line length is more than this many meters +#define AP_WINCH_DAIWA_STUCK_RATE_MIN 0.2 // stuck protection active when desired rate is at least this value (+ve or -ve) + extern const AP_HAL::HAL& hal; const char* AP_Winch_Daiwa::send_text_prefix = "Winch:"; @@ -217,7 +222,10 @@ void AP_Winch_Daiwa::control_winch() } // apply acceleration limited to rate - const float rate_limited = get_rate_limited_by_accel(config.rate_desired, dt); + float rate_limited = get_rate_limited_by_accel(config.rate_desired, dt); + + // apply stuck protection to rate + rate_limited = get_stuck_protected_rate(now_ms, rate_limited); // use linear interpolation to calculate output to move winch at desired rate float scaled_output = 0; @@ -227,6 +235,78 @@ void AP_Winch_Daiwa::control_winch() SRV_Channels::set_output_scaled(SRV_Channel::k_winch, scaled_output); } +// returns the rate which may be modified to unstick the winch +// if the winch stops, the rate is temporarily set to zero +// now_ms should be set to the current system time +// rate should be the rate used to calculate the final PWM output to the winch +float AP_Winch_Daiwa::get_stuck_protected_rate(uint32_t now_ms, float rate) +{ + // exit immediately if stuck protection disabled + if (!option_enabled(AP_Winch::Options::RetryIfStuck)) { + return rate; + } + + // check for timeout + bool timeout = (now_ms - stuck_protection.last_update_ms) > 1000; + stuck_protection.last_update_ms = now_ms; + + // check if winch is nearly fully pulled in + const bool near_thread_start = (latest.line_length < AP_WINCH_DAIWA_STUCK_LENGTH_MIN) && is_negative(rate); + + // check if rate is near zero (winch may not move with very low desired rates) + const bool rate_near_zero = fabsf(rate) < AP_WINCH_DAIWA_STUCK_RATE_MIN; + + // return rate unchanged if this protection has not been called recently or winch is unhealthy + // or if winch is moving, desired rate is near zero or winch has stopped at thread start or thread end + if (timeout || !healthy() || latest.moving || rate_near_zero || near_thread_start || latest.thread_end) { + // notify user when winch becomes unstuck + if (option_enabled(AP_Winch::Options::VerboseOutput) && (stuck_protection.stuck_start_ms != 0) && (stuck_protection.user_notified)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s unstuck", send_text_prefix); + } + // reset stuck protection state + stuck_protection.stuck_start_ms = 0; + return rate; + } + + // winch is healthy, with non-zero requested rate but not moving + // record when winch became stuck + if (stuck_protection.stuck_start_ms == 0) { + stuck_protection.stuck_start_ms = now_ms; + stuck_protection.user_notified = false; + } + + // if stuck for between 1 to 2 seconds return zero rate + const uint32_t stuck_time_ms = (now_ms - stuck_protection.stuck_start_ms); + if (stuck_time_ms > AP_WINCH_DAIWA_STUCK_TIMEOUT_MS) { + // notify user + if (!stuck_protection.user_notified) { + stuck_protection.user_notified = true; + if (option_enabled(AP_Winch::Options::VerboseOutput)) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s stuck", send_text_prefix); + } + } + + // return zero rate for 1 second + if (stuck_time_ms <= (AP_WINCH_DAIWA_STUCK_TIMEOUT_MS+AP_WINCH_DAIWA_STUCK_CENTER_MS)) { + return 0; + } + + // rate has been set to zero for 1 sec so release and restart stuck detection + stuck_protection.stuck_start_ms = 0; + + // rate used for acceleration limiting also reset to zero + set_previous_rate(0.0f); + + // update user + if (option_enabled(AP_Winch::Options::VerboseOutput)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s retrying", send_text_prefix); + } + } + + // give winch more time to start moving + return rate; +} + // update user with changes to winch state via send text messages void AP_Winch_Daiwa::update_user() { diff --git a/libraries/AP_Winch/AP_Winch_Daiwa.h b/libraries/AP_Winch/AP_Winch_Daiwa.h index 2abf08682d..301a73d9e9 100644 --- a/libraries/AP_Winch/AP_Winch_Daiwa.h +++ b/libraries/AP_Winch/AP_Winch_Daiwa.h @@ -63,6 +63,12 @@ private: // update pwm outputs to control winch void control_winch(); + // returns the rate which may be modified to unstick the winch + // if the winch stops, the rate is temporarily set to zero + // now_ms should be set to the current system time + // rate should be the rate used to calculate the final PWM output to the winch + float get_stuck_protected_rate(uint32_t now_ms, float rate); + static const uint8_t buff_len_max = 20; // buffer maximum length static const int16_t output_dz = 100; // output deadzone in scale of -1000 to +1000 const float line_length_correction_factor = 0.003333f; // convert winch counter to meters @@ -118,6 +124,13 @@ private: uint8_t moving; // 0:stopped, 1:retracting line, 2:extending line, 3:clutch engaged, 4:zero reset uint8_t clutch; // 0:clutch off, 1:clutch engaged weakly, 2:clutch engaged strongly, motor can spin freely } user_update; + + // stuck protection + struct { + uint32_t last_update_ms; // system time that stuck protection was last called + uint32_t stuck_start_ms; // system time that winch became stuck (0 if not stuck) + bool user_notified; // true if user has been notified that winch is stuck + } stuck_protection; }; #endif // AP_WINCH_DAIWA_ENABLED