Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Paul Riseborough 2024-02-19 20:17:42 +11:00 committed by Andrew Tridgell
parent 844907013b
commit 07501f634c
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 // @Param: OPTIONS
// @DisplayName: Extra TECS options // @DisplayName: Extra TECS options
// @Description: This allows the enabling of special features in the speed/height controller. // @Description: This allows the enabling of special features in the speed/height controller.
// @Bitmask: 0:GliderOnly // @Bitmask: 0:GliderOnly,1:AllowDescentSpeedup
// @User: Advanced // @User: Advanced
AP_GROUPINFO("OPTIONS", 28, AP_TECS, _options, 0), 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) 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 // Set the airspeed demand to the minimum value if an underspeed condition exists
// or a bad descent condition exists // or a bad descent condition exists
// This will minimise the rate of descent resulting from an engine failure, // 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 // Limit height rate of change
if ((hgt_dem - _hgt_dem_rate_ltd) > (_climb_rate_limit * _DT)) { if ((hgt_dem - _hgt_dem_rate_ltd) > (_climb_rate_limit * _DT)) {
_hgt_dem_rate_ltd = _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)) { } else if ((hgt_dem - _hgt_dem_rate_ltd) < (-_sink_rate_limit * _DT)) {
_hgt_dem_rate_ltd = _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 { } 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; _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; AP_Float _hgt_dem_tconst;
enum { enum {
OPTION_GLIDER_ONLY=(1<<0) OPTION_GLIDER_ONLY=(1<<0),
OPTION_DESCENT_SPEEDUP=(1<<1)
}; };
AP_Float _pitch_ff_v0; 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 // true when a reset of airspeed and height states to current is performed on this frame
bool reset:1; 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 { union {
struct flags _flags; struct flags _flags;
@ -389,6 +393,7 @@ private:
// used to scale max climb and sink limits to match vehicle ability // used to scale max climb and sink limits to match vehicle ability
float _max_climb_scaler; float _max_climb_scaler;
float _max_sink_scaler; float _max_sink_scaler;
float _sink_fraction;
// Specific energy error quantities // Specific energy error quantities
float _STE_error; float _STE_error;