diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index af411ccb9b2ae2..2fce40636ab38e 100644 --- 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 index 71c8da60234f62..75d0184104b241 100644 --- 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;