fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 04:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

TECS: move to new control loop architecture

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2021-01-13 21:21:07 +03:00
committed by Roman Bapst
parent 447e14906c
commit 01f891618b
5 changed files with 89 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+67 -66
View File
@@ -174,9 +174,8 @@ void TECS::_update_speed_setpoint()
_TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max);
// calculate the demanded rate of change of speed proportional to speed error
// and apply performance limits
_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _speed_error_gain, velRateMin, velRateMax);
// calculate the demanded true airspeed rate of change based on first order response of true airspeed error
_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, velRateMin, velRateMax);
}
@@ -211,10 +210,11 @@ void TECS::_update_height_setpoint(float desired, float state)
// Apply a first order noise filter
_hgt_setpoint_adj = 0.1f * _hgt_setpoint + 0.9f * _hgt_setpoint_adj_prev;
// Calculate the demanded climb rate proportional to height error plus a feedforward term to provide
// tight tracking during steady climb and descent manoeuvres.
// Use a first order system to calculate a height rate setpoint from the current height error.
// Additionally, allow to add feedforward from heigh setpoint change
_hgt_rate_setpoint = (_hgt_setpoint_adj - state) * _height_error_gain + _height_setpoint_gain_ff *
(_hgt_setpoint_adj - _hgt_setpoint_adj_prev) / _dt;
_hgt_setpoint_adj_prev = _hgt_setpoint_adj;
// Limit the rate of change of height demand to respect vehicle performance limits
@@ -233,7 +233,7 @@ void TECS::_detect_underspeed()
return;
}
if (((_tas_state < _TAS_min * 0.9f) && (_throttle_setpoint >= _throttle_setpoint_max * 0.95f))
if (((_tas_state < _TAS_min * 0.9f) && (_last_throttle_setpoint >= _throttle_setpoint_max * 0.95f))
|| ((_vert_pos_state < _hgt_setpoint_adj) && _underspeed_detected)) {
_underspeed_detected = true;
@@ -249,6 +249,21 @@ void TECS::_update_energy_estimates()
_SPE_setpoint = _hgt_setpoint_adj * CONSTANTS_ONE_G; // potential energy
_SKE_setpoint = 0.5f * _TAS_setpoint_adj * _TAS_setpoint_adj; // kinetic energy
// Calculate total energy error
_STE_error = _SPE_setpoint - _SPE_estimate + _SKE_setpoint - _SKE_estimate;
const float SKE_weighting = get_SKE_weighting();
// Calculate the weighting applied to control of specific potential energy error
const float SPE_weighting = 2.0f - SKE_weighting;
// Calculate the specific energy balance demand which specifies how the available total
// energy should be allocated to speed (kinetic energy) and height (potential energy)
float SEB_setpoint = _SPE_setpoint * SPE_weighting - _SKE_setpoint * SKE_weighting;
// Calculate the specific energy balance error
_SEB_error = SEB_setpoint - (_SPE_estimate * SPE_weighting - _SKE_estimate * SKE_weighting);
// Calculate specific energy rate demands in units of (m**2/sec**3)
_SPE_rate_setpoint = _hgt_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change
_SKE_rate_setpoint = _tas_state * _TAS_rate_setpoint; // kinetic energy rate of change
@@ -264,28 +279,30 @@ void TECS::_update_energy_estimates()
void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::Dcmf &rotMat)
{
// Calculate total energy error
_STE_error = _SPE_setpoint - _SPE_estimate + _SKE_setpoint - _SKE_estimate;
// Calculate demanded rate of change of total energy, respecting vehicle limits
float STE_rate_setpoint = constrain((_SPE_rate_setpoint + _SKE_rate_setpoint), _STE_rate_min, _STE_rate_max);
// Calculate demanded rate of change of total energy, respecting vehicle limits.
// We will constrain the value below.
float STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint;
// Calculate the total energy rate error, applying a first order IIR filter
// to reduce the effect of accelerometer noise
_STE_rate_error_filter.update(-_SPE_rate - _SKE_rate);
_STE_rate_error_filter.update(-_SPE_rate - _SKE_rate + _SPE_rate_setpoint + _SKE_rate_setpoint);
_STE_rate_error = _STE_rate_error_filter.getState();
float throttle_setpoint;
// Calculate the throttle demand
if (_underspeed_detected) {
// always use full throttle to recover from an underspeed condition
_throttle_setpoint = 1.0f;
throttle_setpoint = _throttle_setpoint_max;
} else {
// Adjust the demanded total energy rate to compensate for induced drag rise in turns.
// Assume induced drag scales linearly with normal load factor.
// The additional normal load factor is given by (1/cos(bank angle) - 1)
float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (1.0f / constrain(cosPhi, 0.1f, 1.0f) - 1.0f);
const float cosPhi = constrain(sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1))), 0.1f, 1.0f);
STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (1.0f / cosPhi - 1.0f);
STE_rate_setpoint = constrain(STE_rate_setpoint, _STE_rate_min, _STE_rate_max);
// Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle
// as the starting point. Assume:
@@ -304,31 +321,22 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::
}
// Calculate gain scaler from specific energy error to throttle
float STE_to_throttle = 1.0f / (_throttle_time_constant * (_STE_rate_max - _STE_rate_min));
// Calculate gain scaler from specific energy rate error to throttle
const float STE_rate_to_throttle = 1.0f / (_STE_rate_max - _STE_rate_min);
// Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits
_throttle_setpoint = (_STE_error + _STE_rate_error * _throttle_damping_gain) * STE_to_throttle + throttle_predicted;
_throttle_setpoint = constrain(_throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
// Rate limit the throttle demand
if (fabsf(_throttle_slewrate) > 0.01f) {
float throttle_increment_limit = _dt * (_throttle_setpoint_max - _throttle_setpoint_min) * _throttle_slewrate;
_throttle_setpoint = constrain(_throttle_setpoint, _last_throttle_setpoint - throttle_increment_limit,
_last_throttle_setpoint + throttle_increment_limit);
}
_last_throttle_setpoint = _throttle_setpoint;
throttle_setpoint = (_STE_rate_error * _throttle_damping_gain) * STE_rate_to_throttle + throttle_predicted;
throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
if (_integrator_gain > 0.0f) {
// Calculate throttle integrator state upper and lower limits with allowance for
// 10% throttle saturation to accommodate noise on the demand.
float integ_state_max = _throttle_setpoint_max - _throttle_setpoint + 0.1f;
float integ_state_min = _throttle_setpoint_min - _throttle_setpoint - 0.1f;
float integ_state_max = _throttle_setpoint_max - throttle_setpoint + 0.1f;
float integ_state_min = _throttle_setpoint_min - throttle_setpoint - 0.1f;
// Calculate a throttle demand from the integrated total energy error
// This will be added to the total throttle demand to compensate for steady state errors
_throttle_integ_state = _throttle_integ_state + (_STE_error * _integrator_gain) * _dt * STE_to_throttle;
_throttle_integ_state = _throttle_integ_state + (_STE_rate_error * _integrator_gain) * _dt * STE_rate_to_throttle;
if (_climbout_mode_active) {
// During climbout, set the integrator to maximum throttle to prevent transient throttle drop
@@ -346,16 +354,23 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::
if (airspeed_sensor_enabled()) {
// Add the integrator feedback during closed loop operation with an airspeed sensor
_throttle_setpoint = _throttle_setpoint + _throttle_integ_state;
throttle_setpoint += _throttle_integ_state;
} else {
// when flying without an airspeed sensor, use the predicted throttle only
_throttle_setpoint = throttle_predicted;
throttle_setpoint = throttle_predicted;
}
_throttle_setpoint = constrain(_throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
}
// Rate limit the throttle demand
if (fabsf(_throttle_slewrate) > 0.01f) {
const float throttle_increment_limit = _dt * (_throttle_setpoint_max - _throttle_setpoint_min) * _throttle_slewrate;
throttle_setpoint = constrain(throttle_setpoint, _last_throttle_setpoint - throttle_increment_limit,
_last_throttle_setpoint + throttle_increment_limit);
}
_last_throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
}
void TECS::_detect_uncommanded_descent()
@@ -367,14 +382,15 @@ void TECS::_detect_uncommanded_descent()
*/
// Calculate rate of change of total specific energy
float STE_rate = _SPE_rate + _SKE_rate;
const float STE_rate = _SPE_rate + _SKE_rate;
// If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode
bool enter_mode = !_uncommanded_descent_recovery && !_underspeed_detected && (_STE_error > 200.0f) && (STE_rate < 0.0f)
&& (_throttle_setpoint >= _throttle_setpoint_max * 0.9f);
const bool enter_mode = !_uncommanded_descent_recovery && !_underspeed_detected && (_STE_error > 200.0f)
&& (STE_rate < 0.0f)
&& (_last_throttle_setpoint >= _throttle_setpoint_max * 0.9f);
// If we enter an underspeed condition or recover the required total energy, then exit uncommanded descent recovery mode
bool exit_mode = _uncommanded_descent_recovery && (_underspeed_detected || (_STE_error < 0.0f));
const bool exit_mode = _uncommanded_descent_recovery && (_underspeed_detected || (_STE_error < 0.0f));
if (enter_mode) {
_uncommanded_descent_recovery = true;
@@ -397,39 +413,31 @@ void TECS::_update_pitch_setpoint()
* rises above the demanded value, the pitch angle demand is increased by the TECS controller to prevent the vehicle overspeeding.
* The weighting can be adjusted between 0 and 2 depending on speed and height accuracy requirements.
*/
const float SKE_weighting = get_SKE_weighting();
// Calculate the weighting applied to control of specific potential energy error
const float SPE_weighting = 2.0f - SKE_weighting;
// Calculate the specific energy balance demand which specifies how the available total
// energy should be allocated to speed (kinetic energy) and height (potential energy)
float SEB_setpoint = _SPE_setpoint * SPE_weighting - _SKE_setpoint * SKE_weighting;
// Calculate the specific energy balance rate demand
float SEB_rate_setpoint = _SPE_rate_setpoint * SPE_weighting - _SKE_rate_setpoint * SKE_weighting;
const float SEB_rate_setpoint = _SPE_rate_setpoint * SPE_weighting - _SKE_rate_setpoint * SKE_weighting;
// Calculate the specific energy balance and balance rate error
_SEB_error = SEB_setpoint - (_SPE_estimate * SPE_weighting - _SKE_estimate * SKE_weighting);
// Calculate the specific energy balance rate error
_SEB_rate_error = SEB_rate_setpoint - (_SPE_rate * SPE_weighting - _SKE_rate * SKE_weighting);
// Calculate derivative from change in climb angle to rate of change of specific energy balance
float climb_angle_to_SEB_rate = _tas_state * _pitch_time_constant * CONSTANTS_ONE_G;
const float climb_angle_to_SEB_rate = _tas_state * CONSTANTS_ONE_G;
if (_integrator_gain > 0.0f) {
// Calculate pitch integrator input term
float pitch_integ_input = _SEB_error * _integrator_gain;
float pitch_integ_input = _SEB_rate_error * _integrator_gain;
// Prevent the integrator changing in a direction that will increase pitch demand saturation
// Decay the integrator at the control loop time constant if the pitch demand from the previous time step is saturated
// Decay the integrator using a 1 second time constant if the pitch demand from the previous time step is saturated
if (_pitch_setpoint_unc > _pitch_setpoint_max) {
pitch_integ_input = min(pitch_integ_input,
min((_pitch_setpoint_max - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f));
pitch_integ_input = min(pitch_integ_input, (_pitch_setpoint_max - _pitch_setpoint_unc) * climb_angle_to_SEB_rate);
} else if (_pitch_setpoint_unc < _pitch_setpoint_min) {
pitch_integ_input = max(pitch_integ_input,
max((_pitch_setpoint_min - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f));
pitch_integ_input = max(pitch_integ_input, (_pitch_setpoint_min - _pitch_setpoint_unc) * climb_angle_to_SEB_rate);
}
// Update the pitch integrator state.
@@ -440,7 +448,7 @@ void TECS::_update_pitch_setpoint()
}
// Calculate a specific energy correction that doesn't include the integrator contribution
float SEB_correction = _SEB_error + _SEB_rate_error * _pitch_damping_gain + SEB_rate_setpoint * _pitch_time_constant;
float SEB_correction = _SEB_rate_error * _pitch_damping_gain + _pitch_integ_state + SEB_rate_setpoint;
// During climbout, bias the demanded pitch angle so that a zero speed error produces a pitch angle
// demand equal to the minimum pitch angle set by the mission plan. This prevents the integrator
@@ -449,23 +457,16 @@ void TECS::_update_pitch_setpoint()
SEB_correction += _pitch_setpoint_min * climb_angle_to_SEB_rate;
}
// Sum the correction terms and convert to a pitch angle demand. This calculation assumes:
// Convert the specific energy balance correction to a target pitch angle. This calculation assumes:
// a) The climb angle follows pitch angle with a lag that is small enough not to destabilise the control loop.
// b) The offset between climb angle and pitch angle (angle of attack) is constant, excluding the effect of
// pitch transients due to control action or turbulence.
_pitch_setpoint_unc = (SEB_correction + _pitch_integ_state) / climb_angle_to_SEB_rate;
_pitch_setpoint_unc = SEB_correction / climb_angle_to_SEB_rate;
_pitch_setpoint = constrain(_pitch_setpoint_unc, _pitch_setpoint_min, _pitch_setpoint_max);
// Comply with the specified vertical acceleration limit by applying a pitch rate limit
float ptchRateIncr = _dt * _vert_accel_limit / _tas_state;
if ((_pitch_setpoint - _last_pitch_setpoint) > ptchRateIncr) {
_pitch_setpoint = _last_pitch_setpoint + ptchRateIncr;
} else if ((_pitch_setpoint - _last_pitch_setpoint) < -ptchRateIncr) {
_pitch_setpoint = _last_pitch_setpoint - ptchRateIncr;
}
const float ptchRateIncr = _dt * _vert_accel_limit / _tas_state;
_pitch_setpoint = constrain(_pitch_setpoint, _last_pitch_setpoint - ptchRateIncr, _last_pitch_setpoint + ptchRateIncr);
_last_pitch_setpoint = _pitch_setpoint;
}