Skip to content

Commit

Permalink
AP_TECS: Fixes to reset state
Browse files Browse the repository at this point in the history 
These concern the TAKEOFF flight stage and address #27147.

* Logging metadata fixes
* Disabled continuous TECS reset during takeoff
* Fixed bug in reached_speed_takeoff calculation
* Limited SPDWEIGHT=2 to only first part of takeoff climb
* _post_TO_hgt_offset set to constant
* Takeoff I-gain is now defaults to 0 and is used
* Use at least TRIM_THROTTLE during the climb
* Updated description of TECS_TKOFF_IGAIN with new behaviour
* Forced max throttle while below TKOFF_LVL_ALT

Additionally, set_min_throttle() has been created, that allows one to
set the minimum TECS throttle for the next update_pitch_throttle() loop.
  • Loading branch information
@Georacer
Georacer committed Jul 3, 2024
1 parent 238b7fa commit b84cf20
Show file tree
Hide file tree
Showing 5 changed files with 94 additions and 34 deletions.
1 change: 1 addition & 0 deletions ArduPlane/Plane.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1094,6 +1094,7 @@ class Plane : public AP_Vehicle {
bool auto_takeoff_check(void);
void takeoff_calc_roll(void);
void takeoff_calc_pitch(void);
void takeoff_calc_throttle(void);
int8_t takeoff_tail_hold(void);
int16_t get_takeoff_pitch_min_cd(void);
void landing_gear_update(void);
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/mode_takeoff.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -148,9 +148,9 @@ void ModeTakeoff::update()

if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) {
//below TAKOFF_LVL_ALT
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100.0);
plane.takeoff_calc_roll();
plane.takeoff_calc_pitch();
plane.takeoff_calc_throttle();
} else {
if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT ,or above and loitering
#if AP_FENCE_ENABLED
Expand Down
15 changes: 15 additions & 0 deletions ArduPlane/takeoff.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -218,6 +218,21 @@ void Plane::takeoff_calc_pitch(void)
}
}

/*
* Set minimum throttle to run at during a takeoff.
* This is typically invoked by TAKEOFF mode.
*/
void Plane::takeoff_calc_throttle() {
float max_throttle = aparm.throttle_max * 0.01f;
if (aparm.takeoff_throttle_max != 0) {
max_throttle = aparm.takeoff_throttle_max * 0.01f;
}
// This setting will take effect at the next run of TECS::update_pitch_throttle().
TECS_controller.set_throttle_min(max_throttle);

calc_throttle();
}

/*
* get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off
*/
Expand Down
88 changes: 57 additions & 31 deletions libraries/AP_TECS/AP_TECS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -222,7 +222,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = {

// @Param: TKOFF_IGAIN
// @DisplayName: Controller integrator during takeoff
// @Description: This is the integrator gain on the control loop during takeoff. When set to 0 then TECS_INTEG_GAIN is used. Increase to increase the rate at which speed and height offsets are trimmed out. Typically values higher than TECS_INTEG_GAIN work best
// @Description: This is the integrator gain on the control loop during takeoff. Increase to increase the rate at which speed and height offsets are trimmed out.
// @Range: 0.0 0.5
// @Increment: 0.02
// @User: Advanced
Expand Down Expand Up @@ -574,7 +574,7 @@ void AP_TECS::_update_height_demand(void)
if (_using_airspeed_for_throttle) {
// large height errors will result in the throttle saturating
max_climb_condition |= (_thr_clip_status == clipStatus::MAX) &&
!((_flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || (_flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING));
!(_in_takeoff_first_stage() || (_flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING));
max_descent_condition |= (_thr_clip_status == clipStatus::MIN) && !_landing.is_flaring();
}
const float hgt_dem_alpha = _DT / MAX(_DT + _hgt_dem_tconst, _DT);
Expand Down Expand Up @@ -770,12 +770,9 @@ void AP_TECS::_update_throttle_with_airspeed(void)
// Calculate integrator state, constraining state
// Set integrator to a max throttle value during climbout
_integTHR_state = _integTHR_state + (_STE_error * _get_i_gain()) * _DT * K_STE2Thr;
if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
if (!_flags.reached_speed_takeoff) {
// ensure we run at full throttle until we reach the target airspeed
_throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state);
}
_integTHR_state = integ_max;
if (_in_takeoff_first_stage() || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
// ensure we run at full throttle until we reach the target airspeed
_throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state);
} else {
_integTHR_state = constrain_float(_integTHR_state, integ_min, integ_max);
}
Expand Down Expand Up @@ -838,10 +835,8 @@ void AP_TECS::_update_throttle_with_airspeed(void)
float AP_TECS::_get_i_gain(void)
{
float i_gain = _integGain;
if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
if (!is_zero(_integGain_takeoff)) {
i_gain = _integGain_takeoff;
}
if (_in_takeoff_first_stage() || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
i_gain = _integGain_takeoff;
} else if (_flags.is_doing_auto_land) {
if (!is_zero(_integGain_land)) {
i_gain = _integGain_land;
Expand Down Expand Up @@ -955,7 +950,7 @@ void AP_TECS::_update_pitch(void)
// height. This is needed as the usual relationship of speed
// and height is broken by the VTOL motors
_SKE_weighting = 0.0f;
} else if ( _flags.underspeed || _flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING || _flags.is_gliding) {
} else if ( _flags.underspeed || _in_takeoff_first_stage() || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING || _flags.is_gliding) {
_SKE_weighting = 2.0f;
} else if (_flags.is_doing_auto_land) {
if (_spdWeightLand < 0) {
Expand Down Expand Up @@ -1013,7 +1008,7 @@ void AP_TECS::_update_pitch(void)
// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
if (_in_takeoff_first_stage() || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
SEBdot_dem_total += _PITCHminf * gainInv;
}

Expand Down Expand Up @@ -1084,9 +1079,9 @@ void AP_TECS::_update_pitch(void)
// @Field: PEW: Potential energy weighting
// @Field: KEW: Kinetic energy weighting
// @Field: EBD: Energy balance demand
// @Field: EBE: Energy balance error
// @Field: EBE: Energy balance estimate
// @Field: EBDD: Energy balance rate demand
// @Field: EBDE: Energy balance rate error
// @Field: EBDE: Energy balance rate estimate
// @Field: EBDDT: Energy balance rate demand + Energy balance rate error*pitch_damping
// @Field: Imin: Minimum integrator value
// @Field: Imax: Maximum integrator value
Expand Down Expand Up @@ -1114,7 +1109,7 @@ void AP_TECS::_update_pitch(void)
#endif
}

void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
void AP_TECS::_initialise_states(float hgt_afe)
{
_flags.reset = false;

Expand Down Expand Up @@ -1160,30 +1155,36 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_pitch_measured_lpf.reset(_ahrs.get_pitch());

} else if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
_PITCHminf = CentiDegreesToRadians(ptchMinCO_cd);
_PITCHminf = CentiDegreesToRadians(_ptchMinCO_cd);
_hgt_afe = hgt_afe;
_hgt_dem_lpf = hgt_afe;
_hgt_dem_rate_ltd = hgt_afe;
_hgt_dem_prev = hgt_afe;
_hgt_dem = hgt_afe;
_hgt_dem_in_prev = hgt_afe;
_hgt_dem_in_raw = hgt_afe;
_TAS_dem_adj = _TAS_dem;
_flags.reset = true;
_post_TO_hgt_offset = _climb_rate * _hgt_dem_tconst;
_flags.underspeed = false;
_flags.badDescent = false;

_max_climb_scaler = 1.0f;
_max_sink_scaler = 1.0f;
if (_in_takeoff_first_stage()) {
_post_TO_hgt_offset = _climb_rate_limit * _hgt_dem_tconst; // Replacement prevents oscillating hgt_rate_dem.
_TAS_dem_adj = _TAS_dem;
_max_climb_scaler = 1.0f;
_max_sink_scaler = 1.0f;
_pitch_demand_lpf.reset(_ahrs.get_pitch());
_pitch_measured_lpf.reset(_ahrs.get_pitch());
}

_pitch_demand_lpf.reset(_ahrs.get_pitch());
_pitch_measured_lpf.reset(_ahrs.get_pitch());
if (!_flag_have_reset_after_takeoff) {
_flags.reset = true;
_flag_have_reset_after_takeoff = true;
}
}

if (_flight_stage != AP_FixedWing::FlightStage::TAKEOFF && _flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) {
// reset takeoff speed flag when not in takeoff
_flags.reached_speed_takeoff = false;
_flag_have_reset_after_takeoff = false;
}
}

Expand All @@ -1202,7 +1203,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
int32_t EAS_dem_cm,
enum AP_FixedWing::FlightStage flight_stage,
float distance_beyond_land_wp,
int32_t ptchMinCO_cd,
int32_t ptchMinC0_cd,
int16_t throttle_nudge,
float hgt_afe,
float load_factor,
Expand Down Expand Up @@ -1238,7 +1239,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
// Don't allow height deamnd to continue changing in a direction that saturates vehicle manoeuvre limits
// if vehicle is unable to follow the demanded climb or descent.
const bool max_climb_condition = (_pitch_dem_unc > _PITCHmaxf || _thr_clip_status == clipStatus::MAX) &&
!(_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);
!(_in_takeoff_first_stage() || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);
const bool max_descent_condition = _pitch_dem_unc < _PITCHminf || _thr_clip_status == clipStatus::MIN;
if (max_climb_condition && _hgt_dem_in_raw > _hgt_dem_in_prev) {
_hgt_dem_in = _hgt_dem_in_prev;
Expand All @@ -1249,12 +1250,28 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
}

if (aparm.takeoff_throttle_max != 0 &&
(_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) {
(_in_takeoff_first_stage() || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) {
_THRmaxf = aparm.takeoff_throttle_max * 0.01f;
} else {
_THRmaxf = aparm.throttle_max * 0.01f;
}
_THRminf = aparm.throttle_min * 0.01f;

// Apply at least TKOFF throttle for the climbout phase.
if (_in_takeoff_first_stage()) {
_THRminf = _THRmaxf;
}
// Apply at least trim throttle during the whole takeoff climb.
else if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF ) {
_THRminf = aparm.throttle_cruise * 0.01f;
} else { // Otherwise, during normal situations let regular limit.
_THRminf = aparm.throttle_min * 0.01f;
}
// Allow other pieces of code to override minimum throttle (e.g. TAKEOFF mode).
if (_THRminOverride > 0) {
_THRminf = _THRminOverride;
// Erase the override, it's meant to last only one cycle.
_THRminOverride = 0;
}

// min of 1% throttle range to prevent a numerical error
_THRmaxf = MAX(_THRmaxf, _THRminf+0.01);
Expand Down Expand Up @@ -1335,7 +1352,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
}

if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) {
if (!_flags.reached_speed_takeoff && _TAS_state >= _TASmin && _TASmin > 0) {
// we have reached our target speed in takeoff, allow for
// normal throttle control
_flags.reached_speed_takeoff = true;
Expand All @@ -1350,7 +1367,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
_PITCHmaxf = MAX(_PITCHmaxf, _PITCHminf);

// initialise selected states and variables if DT > 1 second or in climbout
_initialise_states(ptchMinCO_cd, hgt_afe);
_initialise_states(hgt_afe);

// Calculate Specific Total Energy Rate Limits
_update_STE_rate_lim();
Expand Down Expand Up @@ -1436,3 +1453,12 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
}
#endif
}

bool AP_TECS::_in_takeoff_first_stage(void)
{
// We are in the first stage of FlightStage::TAKEOFF if we are in TAKEOFF...
bool result = _flight_stage == AP_FixedWing::FlightStage::TAKEOFF;
// ... and we haven't yet reached the minimum speed.
result &= !_flags.reached_speed_takeoff;
return result;
}
22 changes: 20 additions & 2 deletions libraries/AP_TECS/AP_TECS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ class AP_TECS {
int32_t EAS_dem_cm,
enum AP_FixedWing::FlightStage flight_stage,
float distance_beyond_land_wp,
int32_t ptchMinCO_cd,
int32_t ptchMinC0_cd,
int16_t throttle_nudge,
float hgt_afe,
float load_factor,
Expand Down Expand Up @@ -134,6 +134,12 @@ class AP_TECS {
_pitch_max_limit = pitch_limit;
}

// set minimum throttle override, 0-1 range
// it is applicable for one control cycle only
void set_min_throttle(const float thr) {
_THRminOverride = thr;
}

// force use of synthetic airspeed for one loop
void use_synthetic_airspeed(void) {
_use_synthetic_airspeed_once = true;
Expand Down Expand Up @@ -360,6 +366,8 @@ class AP_TECS {
// Maximum and minimum floating point throttle limits
float _THRmaxf;
float _THRminf;
// External override for minimum throttle.
float _THRminOverride;

// Maximum and minimum floating point pitch limits
float _PITCHmaxf;
Expand Down Expand Up @@ -419,6 +427,12 @@ class AP_TECS {
// need to reset on next loop
bool _need_reset;

// minimum pitch angle to hold during takeoff stage, in centidegrees
int32_t _ptchMinCO_cd ;

// Checks if we reset at the beginning of takeoff.
bool _flag_have_reset_after_takeoff;

float _SKE_weighting;

AP_Int8 _use_synthetic_airspeed;
Expand Down Expand Up @@ -468,7 +482,7 @@ class AP_TECS {
void _update_pitch(void);

// Initialise states and variables
void _initialise_states(int32_t ptchMinCO_cd, float hgt_afe);
void _initialise_states(float hgt_afe);

// Calculate specific total energy rate limits
void _update_STE_rate_lim(void);
Expand All @@ -478,4 +492,8 @@ class AP_TECS {

// current time constant
float timeConstant(void) const;

// Reply if we are in the first stage of a takeoff
// Corresponds to the initial full-throttle segment
bool _in_takeoff_first_stage(void);
};

0 comments on commit b84cf20

Please sign in to comment.