zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
release/1.12
Silvan Fuhrer 4 years ago committed by Daniel Agar
parent
4922659ef4
commit
5e32e9be5a
3 changed files with 29 additions and 29 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 35
      src/lib/tecs/TECS.cpp
  2. 19
      src/lib/tecs/TECS.hpp
  3. 4
      src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

35
src/lib/tecs/TECS.cpp
Unescape 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 * inertial nav data is not available. It also calculates a true airspeed derivative
* which is used by the airspeed complimentary filter. * 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, bool in_air,
float altitude, float vz) float altitude, float vz)
{ {
@ -79,7 +80,7 @@ void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deri
} }
_state_update_timestamp = now; _state_update_timestamp = now;
_EAS = airspeed; _EAS = equivalent_airspeed;
_in_air = in_air; _in_air = in_air;
@ -88,7 +89,7 @@ void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deri
_vert_pos_state = altitude; _vert_pos_state = altitude;
// Update and average speed rate of change if airspeed is being measured // 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 // Apply some noise filtering
_TAS_rate_filter.update(speed_deriv_forward); _TAS_rate_filter.update(speed_deriv_forward);
_speed_derivative = _TAS_rate_filter.getState(); _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 // 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(); uint64_t now = hrt_absolute_time();
const float dt = constrain((now - _speed_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX); const float dt = constrain((now - _speed_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX);
// Convert equivalent airspeed quantities to true airspeed // Convert equivalent airspeed quantities to true airspeed
_EAS_setpoint = airspeed_setpoint; _EAS_setpoint = equivalent_airspeed_setpoint;
_TAS_setpoint = _EAS_setpoint * EAS2TAS; _TAS_setpoint = _EAS_setpoint * EAS2TAS;
_TAS_max = _indicated_airspeed_max * EAS2TAS; _TAS_max = _equivalent_airspeed_max * EAS2TAS;
_TAS_min = _indicated_airspeed_min * EAS2TAS; _TAS_min = _equivalent_airspeed_min * EAS2TAS;
// If airspeed measurements are not being used, fix the airspeed estimate to halfway between // If airspeed measurements are not being used, fix the EAS estimate to halfway between min and max limits
// min and max limits if (!PX4_ISFINITE(equivalent_airspeed) || !airspeed_sensor_enabled()) {
if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) { _EAS = 0.5f * (_equivalent_airspeed_min + _equivalent_airspeed_max);
_EAS = 0.5f * (_indicated_airspeed_min + _indicated_airspeed_max);
} else { } else {
_EAS = indicated_airspeed; _EAS = equivalent_airspeed;
} }
// If first time through or not flying, reset airspeed states // 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); _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 // Update TAS rate state
float tas_error = (_EAS * EAS2TAS) - _tas_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 // limit integrator input to prevent windup
if (_tas_state < 3.1f) { if (_tas_state < 3.1f) {
tas_rate_state_input = max(tas_rate_state_input, 0.0f); tas_rate_state_input = max(tas_rate_state_input, 0.0f);
} }
// Update TAS state // 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; float tas_state_input = _tas_rate_state + _speed_derivative + tas_error * _tas_estimate_freq * 1.4142f;
_tas_state = _tas_state + tas_state_input * dt; _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); _tas_state = max(_tas_state, 3.0f);
_speed_update_timestamp = now; _speed_update_timestamp = now;
@ -155,7 +154,7 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee
void TECS::_update_speed_setpoint() 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 // or a uncontrolled descent condition exists to maximise climb rate
if ((_uncommanded_descent_recovery) || (_underspeed_detected)) { if ((_uncommanded_descent_recovery) || (_underspeed_detected)) {
_TAS_setpoint = _TAS_min; _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, 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) float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max)
{ {
// Calculate the time since last update (seconds) // 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 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 // Calculate rate limits for specific total energy
_update_STE_rate_lim(); _update_STE_rate_lim();

19
src/lib/tecs/TECS.hpp
Unescape View File

@ -74,14 +74,15 @@ public:
* Must be called prior to udating tecs control loops * Must be called prior to udating tecs control loops
* Must be called at 50Hz or greater * 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); float altitude, float vz);
/** /**
* Update the control loop calculations * Update the control loop calculations
*/ */
void update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint, 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 throttle_min, float throttle_setpoint_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max); 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_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_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_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
void set_indicated_airspeed_min(float airspeed) { _indicated_airspeed_min = airspeed; } void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
void set_pitch_damping(float damping) { _pitch_damping_gain = damping; } void set_pitch_damping(float damping) { _pitch_damping_gain = damping; }
void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; } 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 _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_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 _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 _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 _equivalent_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 _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 _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 _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 // complimentary filter states
float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec) 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 * 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 * Update the desired airspeed

4
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
Unescape View File

@ -96,8 +96,8 @@ FixedwingPositionControl::parameters_update()
_tecs.set_max_climb_rate(_param_fw_t_clmb_max.get()); _tecs.set_max_climb_rate(_param_fw_t_clmb_max.get());
_tecs.set_max_sink_rate(_param_fw_t_sink_max.get()); _tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
_tecs.set_speed_weight(_param_fw_t_spdweight.get()); _tecs.set_speed_weight(_param_fw_t_spdweight.get());
_tecs.set_indicated_airspeed_min(_param_fw_airspd_min.get()); _tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get());
_tecs.set_indicated_airspeed_max(_param_fw_airspd_max.get()); _tecs.set_equivalent_airspeed_max(_param_fw_airspd_max.get());
_tecs.set_min_sink_rate(_param_fw_t_sink_min.get()); _tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
_tecs.set_throttle_damp(_param_fw_t_thr_damp.get()); _tecs.set_throttle_damp(_param_fw_t_thr_damp.get());
_tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get()); _tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get());

Write Preview
Loading…
Cancel
Save