Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: Add option for speed to increase up to FBW max during descent

This commit is contained in:
Paul Riseborough 2024-02-19 20:17:42 +11:00 committed by Andrew Tridgell
parent bf1d364201
commit 846eaecc0b
2 changed files with 19 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_TECS/AP_TECS.cpp
View File

@ -246,7 +246,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = {
// @Param: OPTIONS
// @DisplayName: Extra TECS options
// @Description: This allows the enabling of special features in the speed/height controller.
// @Bitmask: 0:GliderOnly
// @Bitmask: 0:GliderOnly,1:AllowDescentSpeedup
// @User: Advanced
AP_GROUPINFO("OPTIONS", 28, AP_TECS, _options, 0),
@ -458,6 +458,11 @@ void AP_TECS::_update_speed(float DT)
void AP_TECS::_update_speed_demand(void)
{
if (_options & OPTION_DESCENT_SPEEDUP) {
// Allow demanded speed to go to maximum when descending at maximum descent rate
_TAS_dem = _TAS_dem + (_TASmax - _TAS_dem) * _sink_fraction;
}
// Set the airspeed demand to the minimum value if an underspeed condition exists
// or a bad descent condition exists
// This will minimise the rate of descent resulting from an engine failure,
@ -532,9 +537,16 @@ void AP_TECS::_update_height_demand(void)
// Limit height rate of change
if ((hgt_dem - _hgt_dem_rate_ltd) > (_climb_rate_limit * _DT)) {
_hgt_dem_rate_ltd = _hgt_dem_rate_ltd + _climb_rate_limit * _DT;
_sink_fraction = 0.0f;
} else if ((hgt_dem - _hgt_dem_rate_ltd) < (-_sink_rate_limit * _DT)) {
_hgt_dem_rate_ltd = _hgt_dem_rate_ltd - _sink_rate_limit * _DT;
_sink_fraction = 1.0f;
} else {
if (is_negative(hgt_dem - _hgt_dem_rate_ltd)) {
_sink_fraction = (hgt_dem - _hgt_dem_rate_ltd) / (-_sink_rate_limit * _DT);
} else {
_sink_fraction = 0.0f;
}
_hgt_dem_rate_ltd = hgt_dem;
}

7
libraries/AP_TECS/AP_TECS.h
View File

@ -197,7 +197,8 @@ private:
AP_Float _hgt_dem_tconst;
enum {
OPTION_GLIDER_ONLY=(1<<0)
OPTION_GLIDER_ONLY=(1<<0),
OPTION_DESCENT_SPEEDUP=(1<<1)
};
AP_Float _pitch_ff_v0;
@ -334,6 +335,9 @@ private:
// true when a reset of airspeed and height states to current is performed on this frame
bool reset:1;
// true when we are allowing the plane to speed up on descent to maintain the target descent rate
bool speedup:1;
};
union {
struct flags _flags;
@ -389,6 +393,7 @@ private:
// used to scale max climb and sink limits to match vehicle ability
float _max_climb_scaler;
float _max_sink_scaler;
float _sink_fraction;
// Specific energy error quantities
float _STE_error;