From b8005c5510295ed52f5ddbf2918b0720127301de Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Thu, 9 Nov 2023 12:23:16 -0800 Subject: [PATCH] Plane: announce throttle limited by Battery Watt Limit --- ArduPlane/Plane.h | 1 + ArduPlane/servos.cpp | 24 ++++++++++++++++++++---- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index d2fd912d4d293d..97a52136b0c578 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -590,6 +590,7 @@ class Plane : public AP_Vehicle { int8_t throttle_watt_limit_max; int8_t throttle_watt_limit_min; // for reverse thrust uint32_t throttle_watt_limit_timer_ms; + uint32_t throttle_watt_limit_last_gcs_ms; AP_FixedWing::FlightStage flight_stage = AP_FixedWing::FlightStage::NORMAL; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index b3b61961b8cde4..dd3a96e84f5a64 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -443,15 +443,19 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) } else if (throttle_watt_limit_min == 0 && throttle_watt_limit_max == 0) { // no limiting happening, nothing to do + if (throttle_watt_limit_last_gcs_ms > 0) { + throttle_watt_limit_last_gcs_ms = 0; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Throttle no longer limited"); + } return; } // check to recover the throttle limit since we're not overloading - const int8_t most_limited_of_min_and_max = MAX(throttle_watt_limit_min, throttle_watt_limit_max); - const int8_t most_throttle_of_min_and_max = MAX(-min_throttle, max_throttle); + int8_t most_limited_of_min_and_max = MAX(throttle_watt_limit_min, throttle_watt_limit_max); + int8_t most_throttle_of_min_and_max = (most_limited_of_min_and_max == throttle_watt_limit_min) ? -min_throttle : max_throttle; - // if we're limiting all the way (min or max == 0) then the delay is larger - const uint32_t recovery_delay_ms = most_limited_of_min_and_max >= most_throttle_of_min_and_max ? 5000 : 1000; + // if we're limiting all the way (demand - min/max == 0) then the delay is larger + const uint32_t recovery_delay_ms = most_limited_of_min_and_max == most_throttle_of_min_and_max ? 5000 : 1000; if (now_ms - throttle_watt_limit_timer_ms >= recovery_delay_ms) { // it has been 1 second since last over-current, or 5s if throttle is pushed all the way down to 0%, check if we can resume higher throttle. @@ -470,6 +474,18 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) } } + // announce to user so they are aware of this event happening every 5 seconds + if (now_ms - throttle_watt_limit_last_gcs_ms >= 5000) { + if (throttle_watt_limit_last_gcs_ms == 0) { + // initial message is handled differently because it will always show max-1 which isn't very useful. + // Need to give it a few seconds to see how much we're actually limiting the throttle + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Throttle limited by battery Watt limit!"); + } else { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Throttle limited to %d%%", most_limited_of_min_and_max); + } + throttle_watt_limit_last_gcs_ms = now_ms; + } + max_throttle = constrain_int16(max_throttle, 0, max_throttle - throttle_watt_limit_max); if (min_throttle < 0) { min_throttle = constrain_int16(min_throttle, min_throttle + throttle_watt_limit_min, 0);