diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 7602abc6a4..702bf3206e 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -394,7 +394,7 @@ void TECSControl::_calcPitchControl(float dt, const Input &input, const Specific const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)}; ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)}; - _calcPitchControlUpdate(dt, seb_rate, param); + _calcPitchControlUpdate(dt, input, seb_rate, param); const float pitch_setpoint{_calcPitchControlOutput(input, seb_rate, param, flag)}; // Comply with the specified vertical acceleration limit by applying a pitch rate limit @@ -433,11 +433,16 @@ TECSControl::ControlValues TECSControl::_calcPitchControlSebRate(const SpecificE return seb_rate; } -void TECSControl::_calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param ¶m) +void TECSControl::_calcPitchControlUpdate(float dt, const Input &input, const ControlValues &seb_rate, + const Param ¶m) { if (param.integrator_gain_pitch > FLT_EPSILON) { + + // Calculate derivative from change in climb angle to rate of change of specific energy balance + const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G; + // Calculate pitch integrator input term - float pitch_integ_input = _getControlError(seb_rate) * param.integrator_gain_pitch; + float pitch_integ_input = _getControlError(seb_rate) * param.integrator_gain_pitch / climb_angle_to_SEB_rate; // Prevent the integrator changing in a direction that will increase pitch demand saturation if (_pitch_setpoint >= param.pitch_max) { @@ -458,19 +463,19 @@ void TECSControl::_calcPitchControlUpdate(float dt, const ControlValues &seb_rat float TECSControl::_calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param ¶m, const Flag &flag) const { - // Calculate a specific energy correction that doesn't include the integrator contribution - float SEB_rate_correction = _getControlError(seb_rate) * param.pitch_damping_gain + _pitch_integ_state + - param.seb_rate_ff * - seb_rate.setpoint; - // Calculate derivative from change in climb angle to rate of change of specific energy balance const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G; + // Calculate a specific energy correction that doesn't include the integrator contribution + float SEB_rate_correction = _getControlError(seb_rate) * param.pitch_damping_gain + + param.seb_rate_ff * + seb_rate.setpoint; + // Convert the specific energy balance rate 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. - const float pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate; + const float pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate + _pitch_integ_state; return constrain(pitch_setpoint_unc, param.pitch_min, param.pitch_max); } diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index fa0a73e740..4f98ae3d31 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -257,7 +257,7 @@ public: * */ struct Input { - float altitude; ///< Current altitude of the UAS [m]. + float altitude; ///< Current altitude amsl of the UAS [m]. float altitude_rate; ///< Current altitude rate of the UAS [m/s]. float tas; ///< Current true airspeed of the UAS [m/s]. float tas_rate; ///< Current true airspeed rate of the UAS [m/s²]. @@ -456,13 +456,14 @@ private: /** * @brief Calculate the pitch control update function. - * Update the states of the pitch control + * Update the states of the pitch control (pitch integrator). * * @param dt is the update time intervall in [s]. + * @param input is the current input measurement of the UAS. * @param seb_rate is the specific energy balance rate in [m²/s³]. * @param param is the control parameters. */ - void _calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param ¶m); + void _calcPitchControlUpdate(float dt, const Input &input, const ControlValues &seb_rate, const Param ¶m); /** * @brief Calculate the pitch control output function. @@ -499,7 +500,7 @@ private: /** * @brief Calculate the throttle control update function. - * Update the throttle control states. + * Update the throttle control states (throttle integrator). * * @param dt is the update time intervall in [s]. * @param limit is the specific total energy rate limits in [m²/s³]. @@ -526,8 +527,7 @@ private: // State AlphaFilter _ste_rate_estimate_filter; ///< Low pass filter for the specific total energy rate. float _pitch_integ_state{0.0f}; ///< Pitch integrator state [rad]. - float _throttle_integ_state{0.0f}; ///< Throttle integrator state. - + float _throttle_integ_state{0.0f}; ///< Throttle integrator state [-]. // Output DebugOutput _debug_output; ///< Debug output. @@ -602,7 +602,7 @@ public: void set_detect_underspeed_enabled(bool enabled) { _control_flag.detect_underspeed_enabled = enabled; }; - // // setters for parameters + // setters for parameters void set_airspeed_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_measurement_std_dev = std_dev;}; void set_airspeed_rate_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_measurement_std_dev = std_dev;}; void set_airspeed_filter_process_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_noise_std_dev = std_dev;}; @@ -654,7 +654,6 @@ public: float get_pitch_setpoint() {return _control.getPitchSetpoint();} float get_throttle_setpoint() {return _control.getThrottleSetpoint();} - // // TECS status uint64_t timestamp() { return _update_timestamp; } ECL_TECS_MODE tecs_mode() { return _tecs_mode; }