Skip to content

Commit 880ebbc

Browse files
Georacertridge
Georacer
authored and
tridge
committed
AP_TECS: Takeoff improvements
- Refactor and split set_pitch_max_limit method. - New _update_pitch_limits to encapsulate all relevant functionality. - Automatically reset if pitch and throttle are overriden. - nullified TAKEOFF alt_dem offset on external throttle. - Simplify use of TKOFF_THR_MIN. - Prevent takeoff altitude overshoot by capping the altitude setpoint offset. - Move pitch limits after vertical acceleration limitation.
1 parent 5ea787a commit 880ebbc

File tree

2 files changed

+160
-100
lines changed

2 files changed

+160
-100
lines changed

‎libraries/AP_TECS/AP_TECS.cpp

Lines changed: 140 additions & 92 deletions
Original file line numberDiff line numberDiff line change
@@ -1054,9 +1054,6 @@ void AP_TECS::_update_pitch(void)
10541054
_pitch_dem_unc += (_TAS_dem_adj - _pitch_ff_v0) * _pitch_ff_k;
10551055
}
10561056

1057-
// Constrain pitch demand
1058-
_pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
1059-
10601057
// Rate limit the pitch demand to comply with specified vertical
10611058
// acceleration limit
10621059
float ptchRateIncr = _DT * _vertAccLim / _TAS_state;
@@ -1067,6 +1064,9 @@ void AP_TECS::_update_pitch(void)
10671064
_pitch_dem = _last_pitch_dem - ptchRateIncr;
10681065
}
10691066

1067+
// Constrain pitch demand
1068+
_pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
1069+
10701070
_last_pitch_dem = _pitch_dem;
10711071

10721072
#if HAL_LOGGING_ENABLED
@@ -1110,11 +1110,13 @@ void AP_TECS::_update_pitch(void)
11101110
#endif
11111111
}
11121112

1113-
void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
1113+
void AP_TECS::_initialise_states(float hgt_afe)
11141114
{
1115+
// Initialise states and variables if DT > 0.2 second or TECS is getting overriden or in climbout.
11151116
_flags.reset = false;
11161117

1117-
// Initialise states and variables if DT > 0.2 second or in climbout
1118+
_need_reset = _need_reset || (_flag_pitch_forced && _flag_throttle_forced);
1119+
11181120
if (_DT > 0.2f || _need_reset) {
11191121
_SKE_weighting = 1.0f;
11201122
_integTHR_state = 0.0f;
@@ -1156,7 +1158,16 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
11561158
_pitch_measured_lpf.reset(_ahrs.get_pitch());
11571159

11581160
} else if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
1159-
_PITCHminf = CentiDegreesToRadians(ptchMinCO_cd);
1161+
1162+
if (!_flag_throttle_forced) {
1163+
// Calculate the takeoff target height offset before _hgt_dem_in_raw gets reset below.
1164+
// Prevent the offset from becoming negative.
1165+
_post_TO_hgt_offset = MAX(MIN(_climb_rate_limit * _hgt_dem_tconst, _hgt_dem_in_raw - hgt_afe), 0);
1166+
} else {
1167+
// If throttle is externally forced, this mechanism of adding energy is unnecessary.
1168+
_post_TO_hgt_offset = 0;
1169+
}
1170+
11601171
_hgt_afe = hgt_afe;
11611172
_hgt_dem_lpf = hgt_afe;
11621173
_hgt_dem_rate_ltd = hgt_afe;
@@ -1166,12 +1177,12 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
11661177
_hgt_dem_in_raw = hgt_afe;
11671178
_flags.underspeed = false;
11681179
_flags.badDescent = false;
1169-
_post_TO_hgt_offset = _climb_rate_limit * _hgt_dem_tconst; // Replacement prevents oscillating hgt_rate_dem.
11701180
_TAS_dem_adj = _TAS_dem;
11711181
_max_climb_scaler = 1.0f;
11721182
_max_sink_scaler = 1.0f;
11731183
_pitch_demand_lpf.reset(_ahrs.get_pitch());
11741184
_pitch_measured_lpf.reset(_ahrs.get_pitch());
1185+
11751186

11761187
if (!_flag_have_reset_after_takeoff) {
11771188
_flags.reset = true;
@@ -1250,77 +1261,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
12501261
// Update the throttle limits.
12511262
_update_throttle_limits();
12521263

1253-
// work out the maximum and minimum pitch
1254-
// if TECS_PITCH_{MAX,MIN} isn't set then use
1255-
// LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be
1256-
// larger than LIM_PITCH_{MAX,MIN}
1257-
if (_pitch_max == 0) {
1258-
_PITCHmaxf = aparm.pitch_limit_max;
1259-
} else {
1260-
_PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max);
1261-
}
1262-
1263-
if (_pitch_min >= 0) {
1264-
_PITCHminf = aparm.pitch_limit_min;
1265-
} else {
1266-
_PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min);
1267-
}
1268-
1269-
// apply temporary pitch limit and clear
1270-
if (_pitch_max_limit < 90) {
1271-
_PITCHmaxf = constrain_float(_PITCHmaxf, -90, _pitch_max_limit);
1272-
_PITCHminf = constrain_float(_PITCHminf, -_pitch_max_limit, _PITCHmaxf);
1273-
_pitch_max_limit = 90;
1274-
}
1275-
1276-
if (!_landing.is_on_approach()) {
1277-
// reset land pitch min when not landing
1278-
_land_pitch_min = _PITCHminf;
1279-
}
1280-
1281-
// calculate the expected pitch angle from the demanded climb rate and airspeed for use during approach and flare
1282-
if (_landing.is_flaring()) {
1283-
// smoothly move the min pitch to the required minimum at touchdown
1284-
float p; // 0 at start of flare, 1 at finish
1285-
if (!_flare_initialised) {
1286-
p = 0.0f;
1287-
} else if (_hgt_at_start_of_flare > _flare_holdoff_hgt) {
1288-
p = constrain_float((_hgt_at_start_of_flare - _hgt_afe) / _hgt_at_start_of_flare, 0.0f, 1.0f);
1289-
} else {
1290-
p = 1.0f;
1291-
}
1292-
const float pitch_limit_deg = (1.0f - p) * _pitch_min_at_flare_entry + p * 0.01f * _landing.get_pitch_cd();
1293-
1294-
// in flare use min pitch from LAND_PITCH_DEG
1295-
_PITCHminf = MAX(_PITCHminf, pitch_limit_deg);
1296-
1297-
// and use max pitch from TECS_LAND_PMAX
1298-
if (_land_pitch_max != 0) {
1299-
// note that this allows a flare pitch outside the normal TECS auto limits
1300-
_PITCHmaxf = _land_pitch_max;
1301-
}
1302-
} else if (_landing.is_on_approach()) {
1303-
_PITCHminf = MAX(_PITCHminf, aparm.pitch_limit_min);
1304-
_pitch_min_at_flare_entry = _PITCHminf;
1305-
_flare_initialised = false;
1306-
} else {
1307-
_flare_initialised = false;
1308-
}
1309-
1310-
if (_landing.is_on_approach()) {
1311-
// don't allow the lower bound of pitch to decrease, nor allow
1312-
// it to increase rapidly. This prevents oscillation of pitch
1313-
// demand while in landing approach based on rapidly changing
1314-
// time to flare estimate
1315-
if (_land_pitch_min <= -90) {
1316-
_land_pitch_min = _PITCHminf;
1317-
}
1318-
const float flare_pitch_range = 20;
1319-
const float delta_per_loop = (flare_pitch_range/_landTimeConst) * _DT;
1320-
_PITCHminf = MIN(_PITCHminf, _land_pitch_min+delta_per_loop);
1321-
_land_pitch_min = MAX(_land_pitch_min, _PITCHminf);
1322-
_PITCHminf = MAX(_land_pitch_min, _PITCHminf);
1323-
}
1264+
_update_pitch_limits(ptchMinCO_cd);
13241265

13251266
if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
13261267
if (!_flags.reached_speed_takeoff && _TAS_state >= _TASmin && _TASmin > 0) {
@@ -1330,15 +1271,8 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
13301271
}
13311272
}
13321273

1333-
// convert to radians
1334-
_PITCHmaxf = radians(_PITCHmaxf);
1335-
_PITCHminf = radians(_PITCHminf);
1336-
1337-
// don't allow max pitch to go below min pitch
1338-
_PITCHmaxf = MAX(_PITCHmaxf, _PITCHminf);
1339-
13401274
// initialise selected states and variables if DT > 1 second or in climbout
1341-
_initialise_states(ptchMinCO_cd, hgt_afe);
1275+
_initialise_states(hgt_afe);
13421276

13431277
// Calculate Specific Total Energy Rate Limits
13441278
_update_STE_rate_lim();
@@ -1460,10 +1394,9 @@ void AP_TECS::_update_throttle_limits() {
14601394

14611395
// Configure min throttle.
14621396

1463-
// If less min throttle is allowed during takeoff, use it.
1397+
// Use takeoff min throttle, if applicable.
14641398
bool use_takeoff_throttle = _flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING;
1465-
const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE);
1466-
use_takeoff_throttle = use_takeoff_throttle && (use_throttle_range == 1) && (aparm.takeoff_throttle_min != 0);
1399+
use_takeoff_throttle = use_takeoff_throttle && (aparm.takeoff_throttle_min != 0);
14671400
if ( use_takeoff_throttle ) {
14681401
_THRminf = MIN(_THRminf, aparm.takeoff_throttle_min * 0.01f);
14691402
}
@@ -1491,13 +1424,128 @@ void AP_TECS::_update_throttle_limits() {
14911424
_THRminf = MAX(_THRminf, _THRminf_ext);
14921425

14931426
// Allow a minimum of 1% throttle range, primarily to prevent TECS numerical errors.
1494-
if (_THRmaxf < 1) {
1495-
_THRmaxf = MAX(_THRmaxf, _THRminf + 0.01f);
1427+
const float thr_eps = 0.01;
1428+
if (fabsf(_THRminf-_THRmaxf) < thr_eps) {
1429+
_flag_throttle_forced = true;
1430+
if (_THRmaxf < 1) {
1431+
_THRmaxf = MAX(_THRmaxf, _THRminf + 0.01f);
1432+
} else {
1433+
_THRminf = MIN(_THRminf, _THRmaxf - 0.01f);
1434+
}
14961435
} else {
1497-
_THRminf = MIN(_THRminf, _THRmaxf - 0.01f);
1436+
_flag_throttle_forced = false;
14981437
}
14991438

15001439
// Reset the external throttle limits.
15011440
_THRminf_ext = -1.0f;
15021441
_THRmaxf_ext = 1.0f;
1442+
}
1443+
1444+
void AP_TECS::set_pitch_min(const float pitch_min) {
1445+
// Don't change the limit if it is already covered.
1446+
if (pitch_min > _PITCHminf_ext) {
1447+
_PITCHminf_ext = pitch_min;
1448+
}
1449+
}
1450+
1451+
void AP_TECS::set_pitch_max(const float pitch_max) {
1452+
// Don't change the limit if it is already covered.
1453+
if (pitch_max < _PITCHmaxf_ext) {
1454+
_PITCHmaxf_ext = pitch_max;
1455+
}
1456+
}
1457+
1458+
void AP_TECS::_update_pitch_limits(const int32_t ptchMinCO_cd) {
1459+
// If TECS_PITCH_{MAX,MIN} isn't set then use LIM_PITCH_{MAX,MIN}.
1460+
// Don't allow TECS_PITCH_{MAX,MIN} to be larger than LIM_PITCH_{MAX,MIN}.
1461+
if (_pitch_max == 0) {
1462+
_PITCHmaxf = aparm.pitch_limit_max;
1463+
} else {
1464+
_PITCHmaxf = _pitch_max;
1465+
}
1466+
1467+
if (_pitch_min == 0) {
1468+
_PITCHminf = aparm.pitch_limit_min;
1469+
} else {
1470+
_PITCHminf = _pitch_min;
1471+
}
1472+
1473+
if (!_landing.is_on_approach()) {
1474+
// reset land pitch min when not landing
1475+
_land_pitch_min = _PITCHminf;
1476+
}
1477+
1478+
// calculate the expected pitch angle from the demanded climb rate and airspeed for use during approach and flare
1479+
if (_landing.is_flaring()) {
1480+
// smoothly move the min pitch to the required minimum at touchdown
1481+
float p; // 0 at start of flare, 1 at finish
1482+
if (!_flare_initialised) {
1483+
p = 0.0f;
1484+
} else if (_hgt_at_start_of_flare > _flare_holdoff_hgt) {
1485+
p = constrain_float((_hgt_at_start_of_flare - _hgt_afe) / _hgt_at_start_of_flare, 0.0f, 1.0f);
1486+
} else {
1487+
p = 1.0f;
1488+
}
1489+
const float pitch_limit_deg = (1.0f - p) * _pitch_min_at_flare_entry + p * 0.01f * _landing.get_pitch_cd();
1490+
1491+
// in flare use min pitch from LAND_PITCH_DEG
1492+
_PITCHminf = MAX(_PITCHminf, pitch_limit_deg);
1493+
1494+
// and use max pitch from TECS_LAND_PMAX
1495+
if (_land_pitch_max != 0) {
1496+
// note that this allows a flare pitch outside the normal TECS auto limits
1497+
_PITCHmaxf = _land_pitch_max;
1498+
}
1499+
} else if (_landing.is_on_approach()) {
1500+
_PITCHminf = MAX(_PITCHminf, aparm.pitch_limit_min);
1501+
_pitch_min_at_flare_entry = _PITCHminf;
1502+
_flare_initialised = false;
1503+
} else {
1504+
_flare_initialised = false;
1505+
}
1506+
1507+
if (_landing.is_on_approach()) {
1508+
// don't allow the lower bound of pitch to decrease, nor allow
1509+
// it to increase rapidly. This prevents oscillation of pitch
1510+
// demand while in landing approach based on rapidly changing
1511+
// time to flare estimate
1512+
if (_land_pitch_min <= -90) {
1513+
_land_pitch_min = _PITCHminf;
1514+
}
1515+
const float flare_pitch_range = 20;
1516+
const float delta_per_loop = (flare_pitch_range/_landTimeConst) * _DT;
1517+
_PITCHminf = MIN(_PITCHminf, _land_pitch_min+delta_per_loop);
1518+
_land_pitch_min = MAX(_land_pitch_min, _PITCHminf);
1519+
_PITCHminf = MAX(_land_pitch_min, _PITCHminf);
1520+
}
1521+
1522+
// Apply TAKEOFF minimum pitch
1523+
if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF
1524+
|| _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)
1525+
{
1526+
_PITCHminf = CentiDegreesToRadians(ptchMinCO_cd);
1527+
}
1528+
1529+
// Apply external limits.
1530+
_PITCHmaxf = MIN(_PITCHmaxf, _PITCHmaxf_ext);
1531+
_PITCHminf = MAX(_PITCHminf, _PITCHminf_ext);
1532+
1533+
// Reset the external pitch limits.
1534+
_PITCHminf_ext = -90.0f;
1535+
_PITCHmaxf_ext = 90.0f;
1536+
1537+
// convert to radians
1538+
_PITCHmaxf = radians(_PITCHmaxf);
1539+
_PITCHminf = radians(_PITCHminf);
1540+
1541+
// don't allow max pitch to go below min pitch
1542+
_PITCHmaxf = MAX(_PITCHmaxf, _PITCHminf);
1543+
1544+
// Test if someone is forcing pitch to a specific value.
1545+
const float pitch_eps = DEG_TO_RAD*1;
1546+
if (fabsf(_PITCHmaxf-_PITCHminf)<pitch_eps) {
1547+
_flag_pitch_forced = true;
1548+
} else {
1549+
_flag_pitch_forced = false;
1550+
}
15031551
}

‎libraries/AP_TECS/AP_TECS.h

Lines changed: 20 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -128,20 +128,22 @@ class AP_TECS {
128128
_flags.propulsion_failed = propulsion_failed;
129129
}
130130

131-
132-
// set pitch max limit in degrees
133-
void set_pitch_max_limit(int8_t pitch_limit) {
134-
_pitch_max_limit = pitch_limit;
135-
}
136-
137131
// set minimum throttle override, [-1, -1] range
138132
// it is applicable for one control cycle only
139133
void set_throttle_min(const float thr_min);
140134

141-
// set minimum throttle override, [0, -1] range
135+
// set maximum throttle override, [0, -1] range
142136
// it is applicable for one control cycle only
143137
void set_throttle_max(const float thr_max);
144138

139+
// set minimum pitch override, in degrees.
140+
// it is applicable for one control cycle only
141+
void set_pitch_min(const float pitch_min);
142+
143+
// set maximum pitch override, in degrees.
144+
// it is applicable for one control cycle only
145+
void set_pitch_max(const float pitch_max);
146+
145147
// force use of synthetic airspeed for one loop
146148
void use_synthetic_airspeed(void) {
147149
_use_synthetic_airspeed_once = true;
@@ -371,6 +373,9 @@ class AP_TECS {
371373
// Maximum and minimum throttle safety limits, set externally, typically by servos.cpp:apply_throttle_limits()
372374
float _THRmaxf_ext = 1.0f;
373375
float _THRminf_ext = -1.0f;
376+
// Maximum and minimum pitch limits, set externally, typically by the takeoff logic.
377+
float _PITCHmaxf_ext = 90.0f;
378+
float _PITCHminf_ext = -90.0f;
374379

375380
// Maximum and minimum floating point pitch limits
376381
float _PITCHmaxf;
@@ -429,6 +434,10 @@ class AP_TECS {
429434

430435
// need to reset on next loop
431436
bool _need_reset;
437+
// Flag if someone else drives pitch externally.
438+
bool _flag_pitch_forced;
439+
// Flag if someone else drives throttle externally.
440+
bool _flag_throttle_forced;
432441

433442
// Checks if we reset at the beginning of takeoff.
434443
bool _flag_have_reset_after_takeoff;
@@ -485,7 +494,7 @@ class AP_TECS {
485494
void _update_pitch(void);
486495

487496
// Initialise states and variables
488-
void _initialise_states(int32_t ptchMinCO_cd, float hgt_afe);
497+
void _initialise_states(float hgt_afe);
489498

490499
// Calculate specific total energy rate limits
491500
void _update_STE_rate_lim(void);
@@ -498,4 +507,7 @@ class AP_TECS {
498507

499508
// Update the allowable throttle range.
500509
void _update_throttle_limits();
510+
511+
// Update the allowable pitch range.
512+
void _update_pitch_limits(const int32_t ptchMinCO_cd);
501513
};

0 commit comments

Comments
 (0)