AP_TECS: Throttle limits refactor

ArduPilot · Aug 30, 2024 · 1c8de0f · 1c8de0f
1 parent 6152ba8
commit 1c8de0f
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 48 deletions.
diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
@@ -328,7 +328,6 @@ void AP_TECS::update_50hz(void)
         _height_filter.dd_height = 0.0f;
         DT = 0.02f; // when first starting TECS, use most likely time constant
         _vdot_filter.reset();
-        _takeoff_start_ms = 0;
     }
     _update_50hz_last_usec = now;
 
@@ -1134,7 +1133,6 @@ void AP_TECS::_initialise_states(float hgt_afe)
         _DT                   = 0.02f; // when first starting TECS, use the most likely time constant
         _lag_comp_hgt_offset  = 0.0f;
         _post_TO_hgt_offset   = 0.0f;
-        _takeoff_start_ms = 0;
         _use_synthetic_airspeed_once = false;
 
         _flags.underspeed            = false;
@@ -1379,49 +1377,10 @@ void AP_TECS::set_throttle_max(const float thr_max) {
 
 void AP_TECS::_update_throttle_limits() {
 
-    // Configure max throttle.
-
-    // Read the maximum throttle limit.
-    _THRmaxf  = aparm.throttle_max * 0.01f;
-    // If more max throttle is allowed during takeoff, use it.
-    if (aparm.takeoff_throttle_max*0.01f > _THRmaxf
-        && (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)
-        ) {
-        _THRmaxf  = aparm.takeoff_throttle_max * 0.01f;
-    }
-    // In any case, constrain to the external safety limits.
-    _THRmaxf = MIN(_THRmaxf, _THRmaxf_ext);
-
-    // Configure min throttle.
-
-    // Use takeoff min throttle, if applicable.
-    bool use_takeoff_throttle = _flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING;
-    use_takeoff_throttle = use_takeoff_throttle && (aparm.takeoff_throttle_min != 0);
-    if ( use_takeoff_throttle ) { 
-        _THRminf = MIN(_THRminf, aparm.takeoff_throttle_min * 0.01f);
-    }
-    else { // Otherwise, during normal situations let regular limit.
-        _THRminf = aparm.throttle_min * 0.01f;
-    }
-    // Raise min to force max throttle for TKOFF_THR_MAX_T after a takeoff.
-    if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF) {
-        const uint32_t now = AP_HAL::millis();
-        if (_takeoff_start_ms == 0) {
-            _takeoff_start_ms = now;
-        }
-        const uint32_t dt = now - _takeoff_start_ms;
-        if (dt*0.001 < aparm.takeoff_throttle_max_t) {
-            _THRminf = _THRmaxf;
-        }
-    } else {
-        _takeoff_start_ms = 0;
-    }
-    // If we are flaring, allow the throttle to go to 0.
-    if (_landing.is_flaring()) {
-        _THRminf = 0;
-    }
-    // In any case, constrain to the external safety limits.
-    _THRminf = MAX(_THRminf, _THRminf_ext);
+    // Configure max throttle; constrain to the external safety limits.
+    _THRmaxf = MIN(1.0f, _THRmaxf_ext);
+    // Configure min throttle; constrain to the external safety limits.
+    _THRminf = MAX(-1.0f, _THRminf_ext);
 
     // Allow a minimum of 1% throttle range, primarily to prevent TECS numerical errors.
     const float thr_eps = 0.01;
@@ -1437,6 +1396,7 @@ void AP_TECS::_update_throttle_limits() {
     }
 
     // Reset the external throttle limits.
+    // Caller will have to reset them in the next iteration.
     _THRminf_ext = -1.0f;
     _THRmaxf_ext = 1.0f;
 }

diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
@@ -316,9 +316,6 @@ class AP_TECS {
     // Total energy rate filter state
     float _STEdotErrLast;
 
-    // time we started a takeoff
-    uint32_t _takeoff_start_ms;
-
     struct flags {
         // Underspeed condition
         bool underspeed:1;