fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 13:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

TECS: rename variables and methods to make clear which are EAS and which TAS

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-01-19 08:57:15 +01:00
committed by Daniel Agar
parent 4922659ef4
commit 5e32e9be5a
3 changed files with 29 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+17 -18
View File
@@ -55,7 +55,8 @@ static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a fil
* inertial nav data is not available. It also calculates a true airspeed derivative
* which is used by the airspeed complimentary filter.
*/
void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deriv_forward, bool altitude_lock,
void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward,
bool altitude_lock,
bool in_air,
float altitude, float vz)
{
@@ -79,7 +80,7 @@ void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deri
}
_state_update_timestamp = now;
_EAS = airspeed;
_EAS = equivalent_airspeed;
_in_air = in_air;
@@ -88,7 +89,7 @@ void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deri
_vert_pos_state = altitude;
// Update and average speed rate of change if airspeed is being measured
if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) {
if (PX4_ISFINITE(equivalent_airspeed) && airspeed_sensor_enabled()) {
// Apply some noise filtering
_TAS_rate_filter.update(speed_deriv_forward);
_speed_derivative = _TAS_rate_filter.getState();
@@ -103,25 +104,24 @@ void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deri
}
void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspeed, float EAS2TAS)
void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equivalent_airspeed, float EAS2TAS)
{
// Calculate the time in seconds since the last update and use the default time step value if out of bounds
uint64_t now = hrt_absolute_time();
const float dt = constrain((now - _speed_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX);
// Convert equivalent airspeed quantities to true airspeed
_EAS_setpoint = airspeed_setpoint;
_EAS_setpoint = equivalent_airspeed_setpoint;
_TAS_setpoint = _EAS_setpoint * EAS2TAS;
_TAS_max = _indicated_airspeed_max * EAS2TAS;
_TAS_min = _indicated_airspeed_min * EAS2TAS;
_TAS_max = _equivalent_airspeed_max * EAS2TAS;
_TAS_min = _equivalent_airspeed_min * EAS2TAS;
// If airspeed measurements are not being used, fix the airspeed estimate to halfway between
// min and max limits
if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) {
_EAS = 0.5f * (_indicated_airspeed_min + _indicated_airspeed_max);
// If airspeed measurements are not being used, fix the EAS estimate to halfway between min and max limits
if (!PX4_ISFINITE(equivalent_airspeed) || !airspeed_sensor_enabled()) {
_EAS = 0.5f * (_equivalent_airspeed_min + _equivalent_airspeed_max);
} else {
_EAS = indicated_airspeed;
_EAS = equivalent_airspeed;
}
// If first time through or not flying, reset airspeed states
@@ -130,7 +130,7 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee
_tas_state = (_EAS * EAS2TAS);
}
// Obtain a smoothed airspeed estimate using a second order complementary filter
// Obtain a smoothed TAS estimate using a second order complementary filter
// Update TAS rate state
float tas_error = (_EAS * EAS2TAS) - _tas_state;
@@ -139,7 +139,6 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee
// limit integrator input to prevent windup
if (_tas_state < 3.1f) {
tas_rate_state_input = max(tas_rate_state_input, 0.0f);
}
// Update TAS state
@@ -147,7 +146,7 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee
float tas_state_input = _tas_rate_state + _speed_derivative + tas_error * _tas_estimate_freq * 1.4142f;
_tas_state = _tas_state + tas_state_input * dt;
// Limit the airspeed state to a minimum of 3 m/s
// Limit the TAS state to a minimum of 3 m/s
_tas_state = max(_tas_state, 3.0f);
_speed_update_timestamp = now;
@@ -155,7 +154,7 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee
void TECS::_update_speed_setpoint()
{
// Set the airspeed demand to the minimum value if an underspeed or
// Set the TAS demand to the minimum value if an underspeed or
// or a uncontrolled descent condition exists to maximise climb rate
if ((_uncommanded_descent_recovery) || (_underspeed_detected)) {
_TAS_setpoint = _TAS_min;
@@ -535,7 +534,7 @@ void TECS::_update_STE_rate_lim()
}
void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint,
float EAS_setpoint, float indicated_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)
{
// Calculate the time since last update (seconds)
@@ -558,7 +557,7 @@ void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float
}
// Update the true airspeed state estimate
_update_speed_states(EAS_setpoint, indicated_airspeed, eas_to_tas);
_update_speed_states(EAS_setpoint, equivalent_airspeed, eas_to_tas);
// Calculate rate limits for specific total energy
_update_STE_rate_lim();
src/lib/tecs/TECS.hpp
+10 -9
View File
@@ -74,14 +74,15 @@ public:
* Must be called prior to udating tecs control loops
* Must be called at 50Hz or greater
*/
void update_vehicle_state_estimates(float airspeed, const float speed_deriv_forward, bool altitude_lock, bool in_air,
void update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward, bool altitude_lock,
bool in_air,
float altitude, float vz);
/**
* Update the control loop calculations
*/
void update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint,
float EAS_setpoint, float indicated_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_setpoint_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max);
@@ -111,8 +112,8 @@ public:
void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; }
void set_height_error_time_constant(float time_const) { _height_error_gain = 1.0f / math::max(time_const, 0.1f); }
void set_indicated_airspeed_max(float airspeed) { _indicated_airspeed_max = airspeed; }
void set_indicated_airspeed_min(float airspeed) { _indicated_airspeed_min = airspeed; }
void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
void set_pitch_damping(float damping) { _pitch_damping_gain = damping; }
void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; }
@@ -214,12 +215,12 @@ private:
float _pitch_speed_weight{1.0f}; ///< speed control weighting used by pitch demand calculation
float _height_error_gain{0.2f}; ///< height error inverse time constant [1/s]
float _height_setpoint_gain_ff{0.0f}; ///< gain from height demand derivative to demanded climb rate
float _airspeed_error_gain{0.1f}; ///< airspeed error inverse time constant [1/s]
float _indicated_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec)
float _indicated_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec)
float _airspeed_error_gain{0.1f}; ///< airspeed error inverse time constant [1/s]
float _equivalent_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec)
float _equivalent_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec)
float _throttle_slewrate{0.0f}; ///< throttle demand slew rate limit (1/sec)
float _STE_rate_time_const{0.1f}; ///< filter time constant for specific total energy rate (damping path) (s)
float _speed_derivative_time_const{0.01f}; ///< speed derivative filter time constant (s)
float _speed_derivative_time_const{0.01f}; ///< speed derivative filter time constant (s)
// complimentary filter states
float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec)
@@ -297,7 +298,7 @@ private:
/**
* Update the airspeed internal state using a second order complementary filter
*/
void _update_speed_states(float airspeed_setpoint, float indicated_airspeed, float eas_to_tas);
void _update_speed_states(float airspeed_setpoint, float equivalent_airspeed, float cas_to_tas);
/**
* Update the desired airspeed
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+2 -2
View File
@@ -96,8 +96,8 @@ FixedwingPositionControl::parameters_update()
_tecs.set_max_climb_rate(_param_fw_t_clmb_max.get());
_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
_tecs.set_indicated_airspeed_min(_param_fw_airspd_min.get());
_tecs.set_indicated_airspeed_max(_param_fw_airspd_max.get());
_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get());
_tecs.set_equivalent_airspeed_max(_param_fw_airspd_max.get());
_tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
_tecs.set_throttle_damp(_param_fw_t_thr_damp.get());
_tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get());