Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: Throttle limits refactor

Browse Source
This commit is contained in:
George Zogopoulos 2024-08-16 18:04:49 +02:00 committed by Andrew Tridgell
parent 36991de2b8
commit 121adf5f6d
2 changed files with 5 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
libraries/AP_TECS/AP_TECS.cpp
View File

@ -328,7 +328,6 @@ void AP_TECS::update_50hz(void)
_height_filter.dd_height = 0.0f; _height_filter.dd_height = 0.0f;
DT = 0.02f; // when first starting TECS, use most likely time constant DT = 0.02f; // when first starting TECS, use most likely time constant
_vdot_filter.reset(); _vdot_filter.reset();
_takeoff_start_ms = 0;
} }
_update_50hz_last_usec = now; _update_50hz_last_usec = now;
@ -1115,8 +1114,6 @@ void AP_TECS::_initialise_states(float hgt_afe)
// Initialise states and variables if DT > 0.2 second or TECS is getting overriden or in climbout. // Initialise states and variables if DT > 0.2 second or TECS is getting overriden or in climbout.
_flags.reset = false; _flags.reset = false;
_need_reset = _need_reset || (_flag_pitch_forced && _flag_throttle_forced);
if (_DT > 0.2f || _need_reset) { if (_DT > 0.2f || _need_reset) {
_SKE_weighting = 1.0f; _SKE_weighting = 1.0f;
_integTHR_state = 0.0f; _integTHR_state = 0.0f;
@ -1134,7 +1131,6 @@ void AP_TECS::_initialise_states(float hgt_afe)
_DT = 0.02f; // when first starting TECS, use the most likely time constant _DT = 0.02f; // when first starting TECS, use the most likely time constant
_lag_comp_hgt_offset = 0.0f; _lag_comp_hgt_offset = 0.0f;
_post_TO_hgt_offset = 0.0f; _post_TO_hgt_offset = 0.0f;
_takeoff_start_ms = 0;
_use_synthetic_airspeed_once = false; _use_synthetic_airspeed_once = false;
_flags.underspeed = false; _flags.underspeed = false;
@ -1379,49 +1375,10 @@ void AP_TECS::set_throttle_max(const float thr_max) {
void AP_TECS::_update_throttle_limits() { void AP_TECS::_update_throttle_limits() {
// Configure max throttle. // Configure max throttle; constrain to the external safety limits.
_THRmaxf = MIN(1.0f, _THRmaxf_ext);
// Read the maximum throttle limit. // Configure min throttle; constrain to the external safety limits.
_THRmaxf = aparm.throttle_max * 0.01f; _THRminf = MAX(-1.0f, _THRminf_ext);
// 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);
// Allow a minimum of 1% throttle range, primarily to prevent TECS numerical errors. // Allow a minimum of 1% throttle range, primarily to prevent TECS numerical errors.
const float thr_eps = 0.01; const float thr_eps = 0.01;
@ -1437,6 +1394,7 @@ void AP_TECS::_update_throttle_limits() {
} }
// Reset the external throttle limits. // Reset the external throttle limits.
// Caller will have to reset them in the next iteration.
_THRminf_ext = -1.0f; _THRminf_ext = -1.0f;
_THRmaxf_ext = 1.0f; _THRmaxf_ext = 1.0f;
} }
@ -1540,12 +1498,4 @@ void AP_TECS::_update_pitch_limits(const int32_t ptchMinCO_cd) {
// don't allow max pitch to go below min pitch // don't allow max pitch to go below min pitch
_PITCHmaxf = MAX(_PITCHmaxf, _PITCHminf); _PITCHmaxf = MAX(_PITCHmaxf, _PITCHminf);
// Test if someone is forcing pitch to a specific value.
const float pitch_eps = DEG_TO_RAD*1;
if (fabsf(_PITCHmaxf-_PITCHminf)<pitch_eps) {
_flag_pitch_forced = true;
} else {
_flag_pitch_forced = false;
}
} }

5
libraries/AP_TECS/AP_TECS.h
View File

@ -316,9 +316,6 @@ private:
// Total energy rate filter state // Total energy rate filter state
float _STEdotErrLast; float _STEdotErrLast;
// time we started a takeoff
uint32_t _takeoff_start_ms;
struct flags { struct flags {
// Underspeed condition // Underspeed condition
bool underspeed:1; bool underspeed:1;
@ -434,8 +431,6 @@ private:
// need to reset on next loop // need to reset on next loop
bool _need_reset; bool _need_reset;
// Flag if someone else drives pitch externally.
bool _flag_pitch_forced;
// Flag if someone else drives throttle externally. // Flag if someone else drives throttle externally.
bool _flag_throttle_forced; bool _flag_throttle_forced;