mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
AP_TECS: Add option for speed to increase up to FBW max during descent
This commit is contained in:
parent
844907013b
commit
07501f634c
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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;
|
||||||
|
Loading…
Reference in New Issue
Block a user