fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 21:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

TECS: rename airspeed/throttle "cruise" to "trim"

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2022-07-12 09:07:20 +02:00
committed by Martina Rivizzigno
parent b57d3c6d74
commit 92cdff6798
2 changed files with 23 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+22 -18
View File
@@ -116,9 +116,9 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
_TAS_max = _equivalent_airspeed_max * EAS2TAS; _TAS_max = _equivalent_airspeed_max * EAS2TAS;
_TAS_min = _equivalent_airspeed_min * EAS2TAS; _TAS_min = _equivalent_airspeed_min * EAS2TAS;
// If airspeed measurements are not being used, fix the airspeed estimate to the nominal cruise airspeed // If airspeed measurements are not being used, fix the airspeed estimate to the nominal trim airspeed
if (!PX4_ISFINITE(equivalent_airspeed) || !airspeed_sensor_enabled()) { if (!PX4_ISFINITE(equivalent_airspeed) || !airspeed_sensor_enabled()) {
_EAS = _equivalent_airspeed_cruise; _EAS = _equivalent_airspeed_trim;
} else { } else {
_EAS = equivalent_airspeed; _EAS = equivalent_airspeed;
@@ -248,11 +248,11 @@ void TECS::_update_energy_estimates()
_SKE_rate = _tas_state * _tas_rate_filtered;// kinetic energy rate of change _SKE_rate = _tas_state * _tas_rate_filtered;// kinetic energy rate of change
} }
void TECS::_update_throttle_setpoint(const float throttle_cruise) void TECS::_update_throttle_setpoint()
{ {
// Calculate demanded rate of change of total energy, respecting vehicle limits. // Calculate demanded rate of change of total energy, respecting vehicle limits.
// We will constrain the value below. // We will constrain the value below.
float STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint; _STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint;
// Calculate the total energy rate error, applying a first order IIR filter // Calculate the total energy rate error, applying a first order IIR filter
// to reduce the effect of accelerometer noise // to reduce the effect of accelerometer noise
@@ -270,24 +270,26 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise)
// Adjust the demanded total energy rate to compensate for induced drag rise in turns. // Adjust the demanded total energy rate to compensate for induced drag rise in turns.
// Assume induced drag scales linearly with normal load factor. // Assume induced drag scales linearly with normal load factor.
// The additional normal load factor is given by (1/cos(bank angle) - 1) // The additional normal load factor is given by (1/cos(bank angle) - 1)
STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (_load_factor - 1.f); _STE_rate_setpoint += _load_factor_correction * (_load_factor - 1.f);
STE_rate_setpoint = constrain(STE_rate_setpoint, _STE_rate_min, _STE_rate_max); _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 // Calculate a predicted throttle from the demanded rate of change of energy, using the trim throttle
// as the starting point. Assume: // as the starting point. Assume:
// Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max // Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max
// Specific total energy rate = 0 at cruise throttle // Specific total energy rate = 0 at trim throttle
// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min // Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
float throttle_predicted = 0.0f; float throttle_predicted = 0.0f;
if (STE_rate_setpoint >= 0) { if (_STE_rate_setpoint >= 0) {
// throttle is between cruise and maximum // throttle is between trim and maximum
throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max - throttle_cruise); throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max -
_throttle_trim);
} else { } else {
// throttle is between cruise and minimum // throttle is between trim and minimum
throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min - throttle_cruise); throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min -
_throttle_trim);
} }
@@ -488,7 +490,7 @@ void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rat
} }
} }
void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout, void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altitude, float pitch_min_climbout,
float EAS2TAS) float EAS2TAS)
{ {
if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initialized) { if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initialized) {
@@ -500,7 +502,7 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt
_tas_state = _EAS * EAS2TAS; _tas_state = _EAS * EAS2TAS;
_throttle_integ_state = 0.0f; _throttle_integ_state = 0.0f;
_pitch_integ_state = 0.0f; _pitch_integ_state = 0.0f;
_last_throttle_setpoint = (_in_air ? throttle_cruise : 0.0f);; _last_throttle_setpoint = (_in_air ? throttle_trim : 0.0f);;
_last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max); _last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max);
_pitch_setpoint_unc = _last_pitch_setpoint; _pitch_setpoint_unc = _last_pitch_setpoint;
_TAS_setpoint_last = _EAS * EAS2TAS; _TAS_setpoint_last = _EAS * EAS2TAS;
@@ -559,7 +561,8 @@ void TECS::_update_STE_rate_lim()
void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint, void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max, float throttle_min, float throttle_max, float throttle_trim,
float pitch_limit_min, float pitch_limit_max,
float target_climbrate, float target_sinkrate, float hgt_rate_sp) float target_climbrate, float target_sinkrate, float hgt_rate_sp)
{ {
// Calculate the time since last update (seconds) // Calculate the time since last update (seconds)
@@ -572,9 +575,10 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
_pitch_setpoint_max = pitch_limit_max; _pitch_setpoint_max = pitch_limit_max;
_pitch_setpoint_min = pitch_limit_min; _pitch_setpoint_min = pitch_limit_min;
_climbout_mode_active = climb_out_setpoint; _climbout_mode_active = climb_out_setpoint;
_throttle_trim = throttle_trim;
// Initialize selected states and variables as required // Initialize selected states and variables as required
_initialize_states(pitch, throttle_cruise, baro_altitude, pitch_min_climbout, eas_to_tas); _initialize_states(pitch, throttle_trim, baro_altitude, pitch_min_climbout, eas_to_tas);
// Don't run TECS control algorithms when not in flight // Don't run TECS control algorithms when not in flight
if (!_in_air) { if (!_in_air) {
@@ -606,7 +610,7 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
_update_energy_estimates(); _update_energy_estimates();
// Calculate the throttle demand // Calculate the throttle demand
_update_throttle_setpoint(throttle_cruise); _update_throttle_setpoint();
// Calculate the pitch demand // Calculate the pitch demand
_update_pitch_setpoint(); _update_pitch_setpoint();
src/lib/tecs/TECS.hpp
+1 -1
View File
@@ -95,6 +95,7 @@ public:
float get_throttle_setpoint() { return _last_throttle_setpoint; } float get_throttle_setpoint() { return _last_throttle_setpoint; }
float get_pitch_setpoint() { return _last_pitch_setpoint; } float get_pitch_setpoint() { return _last_pitch_setpoint; }
float get_speed_weight() { return _pitch_speed_weight; } float get_speed_weight() { return _pitch_speed_weight; }
float get_throttle_trim() { return _throttle_trim; }
void reset_state() { _states_initialized = false; } void reset_state() { _states_initialized = false; }
@@ -140,7 +141,6 @@ public:
void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; } void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; }
// TECS status // TECS status
uint64_t timestamp() { return _pitch_update_timestamp; } uint64_t timestamp() { return _pitch_update_timestamp; }
ECL_TECS_MODE tecs_mode() { return _tecs_mode; } ECL_TECS_MODE tecs_mode() { return _tecs_mode; }