From c64e111d8e55506fcf24bba8343444049e9d3370 Mon Sep 17 00:00:00 2001 From: Konrad Date: Tue, 18 Oct 2022 14:05:00 +0200 Subject: [PATCH] TECS: Rearranged the TECS library into submodules. --- src/lib/tecs/TECSnew.cpp | 713 +++++++++++++++++++++++++++++++++++++++ src/lib/tecs/TECSnew.hpp | 648 +++++++++++++++++++++++++++++++++++ 2 files changed, 1361 insertions(+) create mode 100644 src/lib/tecs/TECSnew.cpp create mode 100644 src/lib/tecs/TECSnew.hpp diff --git a/src/lib/tecs/TECSnew.cpp b/src/lib/tecs/TECSnew.cpp new file mode 100644 index 0000000000..6dea986a8d --- /dev/null +++ b/src/lib/tecs/TECSnew.cpp @@ -0,0 +1,713 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file TECS.cpp + * + * @author Paul Riseborough + */ + +#include "TECSnew.hpp" + +#include + +#include + +using math::constrain; +using math::max; +using math::min; + +// TODO there seems to be an inconsistent definition of IAS/CAS/EAS/TAS +// TODO Recheck the timing. +void TECSAirspeedFilter::initialize(const float equivalent_airspeed) +{ + + + _airspeed_state.speed= equivalent_airspeed; + _airspeed_state.speed_rate = 0.0f; + _airspeed_rate_filter.reset(0.0f); +} + +void TECSAirspeedFilter::update(const float dt, const Input &input, const Param ¶m, const bool airspeed_sensor_available) +{ + // Input checking + if(!(PX4_ISFINITE(dt) && dt > FLT_EPSILON)) + { + // Do not update the states. + PX4_WARN("Time intervall is not valid."); + return; + } + + float airspeed; + if (PX4_ISFINITE(input.equivalent_airspeed) && airspeed_sensor_available) { + airspeed = input.equivalent_airspeed; + } + else { + airspeed = param.equivalent_airspeed_trim; + } + + float airspeed_derivative; + if (PX4_ISFINITE(input.equivalent_airspeed_rate) && airspeed_sensor_available) { + airspeed_derivative = input.equivalent_airspeed_rate; + } + else { + airspeed_derivative = 0.0f; + } + + // TODO remove. Only here for compatibility check with old TECS. + // filter true airspeed rate using first order filter with 0.5 second time constant + _airspeed_rate_filter.setParameters(TECS::DT_DEFAULT, param.speed_derivative_time_const); + _airspeed_rate_filter.reset(0.0f); + + // Alpha filtering done in the TECS module. TODO merge with the second order complementary filter. + //_airspeed_rate_filter.setParameters(dt, param.speed_derivative_time_const); + if (PX4_ISFINITE(input.equivalent_airspeed_rate) && airspeed_sensor_available) { + _airspeed_rate_filter.update(airspeed_derivative); + } + else { + _airspeed_rate_filter.reset(0.0f); + } + + AirspeedFilterState new_airspeed_state; + // Update TAS rate state + float airspeed_innovation = airspeed - _airspeed_state.speed; + float airspeed_rate_state_input = airspeed_innovation * param.airspeed_estimate_freq * param.airspeed_estimate_freq; + new_airspeed_state.speed_rate = _airspeed_state.speed_rate + airspeed_rate_state_input * dt; + + // Update TAS state // TODO the airspeed rate is applied twice. + float airspeed_state_input = _airspeed_state.speed_rate + airspeed_derivative + airspeed_innovation * param.airspeed_estimate_freq * 1.4142f; + new_airspeed_state.speed = _airspeed_state.speed + airspeed_state_input * dt; + + // Clip tas at zero + if (new_airspeed_state.speed < 0.0f) { + // clip TAS at zero, back calculate rate // TODO Redo + airspeed_state_input = -_airspeed_state.speed / dt; + new_airspeed_state.speed_rate = airspeed_state_input - airspeed_derivative - airspeed_innovation * param.airspeed_estimate_freq * 1.4142f; + new_airspeed_state.speed = 0.0f; + } + + // Update states + _airspeed_state = new_airspeed_state; +} + +TECSAirspeedFilter::AirspeedFilterState TECSAirspeedFilter::getState() const +{ + AirspeedFilterState filter_state{ + .speed = _airspeed_state.speed, + .speed_rate = _airspeed_rate_filter.getState() + }; + + return filter_state; +} + +void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &setpoint, float altitude, const Param ¶m) +{ + // Input checks + if(!(PX4_ISFINITE(dt) && dt > FLT_EPSILON)) + { + // Do not update the states. + PX4_WARN("Time intervall is not valid."); + return; + } + + if (!PX4_ISFINITE(altitude)) { + altitude = 0.0f; + } + // TODO rearrange handling of altitude rate and altitude. alt_rate should rather be a feedforward term. + float virtual_altitude_setpoint{setpoint.alt}; + + // Velocity setpoint reference + const bool input_is_altitude_rate = PX4_ISFINITE(setpoint.alt_rate); + + _velocity_control_traj_generator.setMaxJerk(param.jerk_max); + _velocity_control_traj_generator.setMaxAccelUp(param.vert_accel_limit); + _velocity_control_traj_generator.setMaxAccelDown(param.vert_accel_limit); + _velocity_control_traj_generator.setMaxVelUp(param.max_sink_rate); + _velocity_control_traj_generator.setMaxVelDown(param.max_climb_rate); + + if (input_is_altitude_rate) { + _velocity_control_traj_generator.setVelSpFeedback(setpoint.alt_rate); + _velocity_control_traj_generator.setCurrentPositionEstimate(altitude); + _velocity_control_traj_generator.update(dt, setpoint.alt_rate); + virtual_altitude_setpoint = _velocity_control_traj_generator.getCurrentPosition(); + } else { + _velocity_control_traj_generator.reset(0.0f, 0.0f, altitude); + } + + // Altitude setpoint reference + bool altitude_control_enable{PX4_ISFINITE(virtual_altitude_setpoint)}; + _alt_control_traj_generator.setMaxJerk(param.jerk_max); + _alt_control_traj_generator.setMaxAccel(param.vert_accel_limit); + _alt_control_traj_generator.setMaxVel(fmax(param.max_climb_rate, param.max_sink_rate)); + + if (altitude_control_enable) + { + float target_climbrate = math::min(param.target_climbrate, param.max_climb_rate); + float target_sinkrate = math::min(param.target_sinkrate, param.max_sink_rate); + + const float delta_trajectory_to_target_m = setpoint.alt - _alt_control_traj_generator.getCurrentPosition(); + + float altitude_rate_target = math::signNoZero(delta_trajectory_to_target_m) * + math::trajectory::computeMaxSpeedFromDistance( + param.jerk_max, param.vert_accel_limit, fabsf(delta_trajectory_to_target_m), 0.0f); + + altitude_rate_target = math::constrain(altitude_rate_target, -target_sinkrate, target_climbrate); + + _alt_control_traj_generator.updateDurations(altitude_rate_target); + _alt_control_traj_generator.updateTraj(dt); + } + else + { + _alt_control_traj_generator.reset(0.0f, 0.0f, altitude); + } +} + +TECSReferenceModel::AltitudeReferenceState TECSReferenceModel::getAltitudeReference() const { + TECSReferenceModel::AltitudeReferenceState ref{ + .alt = _alt_control_traj_generator.getCurrentPosition(), + .alt_rate = _alt_control_traj_generator.getCurrentVelocity(), + }; + + return ref; +} + +float TECSReferenceModel::getAltitudeRateReference() const { + return _velocity_control_traj_generator.getCurrentVelocity(); +} + +void TECSReferenceModel::initialize(const AltitudeReferenceState &state) +{ + AltitudeReferenceState init_state{state}; + if (!PX4_ISFINITE(state.alt)) + { + init_state.alt = 0.0f; + } + if (!PX4_ISFINITE(state.alt_rate)) + { + init_state.alt_rate = 0.0f; + } + + _alt_control_traj_generator.reset(0.0f, init_state.alt_rate, init_state.alt); + _velocity_control_traj_generator.reset(0.0f,init_state.alt_rate,init_state.alt); +} + +void TECSControl::initialize() +{ + _ste_rate_error_filter.reset(0.0f); + resetIntegrals(); +} + +void TECSControl::update(const float dt, const Setpoint &setpoint, const Input &input, Param ¶m, const Flag &flag) +{ + // Input checking + if(!(PX4_ISFINITE(dt) && dt > FLT_EPSILON)) + { + // Do not update the states and output. + PX4_WARN("Time intervall is not valid."); + return; + } + + AltitudePitchControl control_setpoint; + + control_setpoint.tas_rate_setpoint = _airspeedControl(setpoint, input, param, flag); + + control_setpoint.altitude_rate_setpoint = _altitudeControl(setpoint, input, param); + + SpecificEnergy se{_updateEnergyBalance(control_setpoint, input)}; + + _detectUnderspeed(input, param, flag); + + _updatePitchSetpoint(dt, input, se, param, flag); + + _updateThrottleSetpoint(dt, se, param, flag); + + _debug_output.altitude_rate_control = control_setpoint.altitude_rate_setpoint; + _debug_output.true_airspeed_derivative_control = control_setpoint.tas_rate_setpoint; +} + +TECSControl::STELimit TECSControl::_calculateTotalEnergyRateLimit(const Param ¶m) const { + TECSControl::STELimit limit; + // Calculate the specific total energy rate limits from the max throttle limits + limit.STE_rate_max = math::max(param.max_climb_rate, FLT_EPSILON) * CONSTANTS_ONE_G; + limit.STE_rate_min = - math::max(param.max_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G; + + return limit; +} + +float TECSControl::_airspeedControl(const Setpoint &setpoint, const Input &input, const Param ¶m, const Flag &flag) const +{ + float airspeed_rate_output{0.0f}; + + STELimit limit{_calculateTotalEnergyRateLimit(param)}; + + // calculate the demanded true airspeed rate of change based on first order response of true airspeed error + // if airspeed measurement is not enabled then always set the rate setpoint to zero in order to avoid constant rate setpoints + if (flag.airspeed_enabled) { + // Calculate limits for the demanded rate of change of speed based on physical performance limits + // with a 50% margin to allow the total energy controller to correct for errors. + const float max_tas_rate_sp = 0.5f * limit.STE_rate_max / math::max(input.tas, FLT_EPSILON); + const float min_tas_rate_sp = 0.5f * limit.STE_rate_min / math::max(input.tas, FLT_EPSILON); + airspeed_rate_output = constrain((setpoint.tas_setpoint - input.tas) * param.airspeed_error_gain, min_tas_rate_sp, + max_tas_rate_sp); + } + + return airspeed_rate_output; +} + +float TECSControl::_altitudeControl(const Setpoint &setpoint, const Input &input, const Param ¶m) const +{ + float altitude_rate_output; + altitude_rate_output = (setpoint.altitude_reference.alt - input.altitude) * param.altitude_error_gain + param.altitude_setpoint_gain_ff * setpoint.altitude_reference.alt_rate; + altitude_rate_output = math::constrain(altitude_rate_output, -param.max_sink_rate, param.max_climb_rate); + + return altitude_rate_output; +} + +TECSControl::SpecificEnergy TECSControl::_updateEnergyBalance(const AltitudePitchControl &control_setpoint, const Input &input) const +{ + SpecificEnergy se; + // Calculate specific energy rate demands in units of (m**2/sec**3) + se.spe.rate_setpoint = control_setpoint.altitude_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change + se.ske.rate_setpoint = input.tas * control_setpoint.tas_rate_setpoint; // kinetic energy rate of change + + // Calculate specific energy rates in units of (m**2/sec**3) + se.spe.rate = input.altitude_rate * CONSTANTS_ONE_G; // potential energy rate of change + se.ske.rate = input.tas * input.tas_rate;// kinetic energy rate of change + + // Calculate energy rate error + se.spe.rate_error = se.spe.rate_setpoint - se.spe.rate; + se.ske.rate_error = se.ske.rate_setpoint - se.ske.rate; + + return se; +} + +void TECSControl::_detectUnderspeed(const Input &input, const Param ¶m, const Flag &flag) +{ + if (!flag.detect_underspeed_enabled) { + _percent_undersped = 0.0f; + return; + } + + // this is the expected (something like standard) deviation from the airspeed setpoint that we allow the airspeed + // to vary in before ramping in underspeed mitigation + const float tas_error_bound = param.tas_error_percentage * param.equivalent_airspeed_trim; + + // this is the soft boundary where underspeed mitigation is ramped in + // NOTE: it's currently the same as the error bound, but separated here to indicate these values do not in general + // need to be the same + const float tas_underspeed_soft_bound = param.tas_error_percentage * param.equivalent_airspeed_trim; + + const float tas_fully_undersped = math::max(param.tas_min - tas_error_bound - tas_underspeed_soft_bound, 0.0f); + const float tas_starting_to_underspeed = math::max(param.tas_min - tas_error_bound, tas_fully_undersped); + + _percent_undersped = 1.0f - math::constrain((input.tas - tas_fully_undersped) / + math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f); +} + +TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Param ¶m, const Flag &flag) { + + SpecificEnergyWeighting weight; + // Calculate the weight applied to control of specific kinetic energy error + float pitch_speed_weight = constrain(param.pitch_speed_weight, 0.0f, 2.0f); + + if (flag.climbout_mode_active && flag.airspeed_enabled) { + pitch_speed_weight = 2.0f; + + } else if (_percent_undersped > FLT_EPSILON && flag.airspeed_enabled) { + pitch_speed_weight = 2.0f * _percent_undersped + (1.0f - _percent_undersped) * pitch_speed_weight; + + } else if (!flag.airspeed_enabled) { + pitch_speed_weight = 0.0f; + + } + + // don't allow any weight to be larger than one, as it has the same effect as reducing the control + // loop time constant and therefore can lead to a destabilization of that control loop + weight.spe_weighting = constrain(2.0f - pitch_speed_weight, 0.f, 1.f); + weight.ske_weighting = constrain(pitch_speed_weight, 0.f, 1.f); + + return weight; +} + +void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const SpecificEnergy &se, Param ¶m, const Flag &flag) +{ + SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)}; + /* + * The SKE_weighting variable controls how speed and altitude control are prioritised by the pitch demand calculation. + * A weighting of 1 givea equal speed and altitude priority + * A weighting of 0 gives 100% priority to altitude control and must be used when no airspeed measurement is available. + * A weighting of 2 provides 100% priority to speed control and is used when: + * a) an underspeed condition is detected. + * b) during climbout where a minimum pitch angle has been set to ensure altitude is gained. If the airspeed + * 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 altitude accuracy requirements. + */ + // Calculate the specific energy balance rate demand + float seb_rate_setpoint = se.spe.rate_setpoint * weight.spe_weighting - se.ske.rate_setpoint * weight.ske_weighting; + + // Calculate the specific energy balance rate error + float seb_rate_error = (se.spe.rate_error * weight.spe_weighting) - (se.ske.rate_error * weight.ske_weighting); + + _debug_output.energy_balance_rate_error = seb_rate_error; + _debug_output.energy_balance_rate_sp = seb_rate_setpoint; + + if (param.integrator_gain_pitch > 0.0f) { + // Calculate pitch integrator input term + float pitch_integ_input = seb_rate_error * param.integrator_gain_pitch; + + // Prevent the integrator changing in a direction that will increase pitch demand saturation + if (_pitch_setpoint > param.pitch_max) { + pitch_integ_input = min(pitch_integ_input, 0.f); + + } else if (_pitch_setpoint < param.pitch_min) { + pitch_integ_input = max(pitch_integ_input, 0.f); + } + + // Update the pitch integrator state. + _pitch_integ_state = _pitch_integ_state + pitch_integ_input * dt; + + } else { + _pitch_integ_state = 0.0f; + } + + // Calculate a specific energy correction that doesn't include the integrator contribution + float SEB_rate_correction = seb_rate_error * 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; + + // 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 + // having to catch up before the nose can be raised to reduce excess speed during climbout. + if (flag.climbout_mode_active) { + SEB_rate_correction += param.pitch_min * climb_angle_to_SEB_rate; + } + + // 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. + float pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate; + + float pitch_setpoint = constrain(pitch_setpoint_unc, param.pitch_min, param.pitch_max); + + // Comply with the specified vertical acceleration limit by applying a pitch rate limit + // NOTE: at zero airspeed, the pitch increment is unbounded + const float pitch_increment = dt * param.vert_accel_limit / input.tas; + _pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment, + _pitch_setpoint + pitch_increment); +} + +void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, const Param ¶m, const Flag &flag) +{ + STELimit limit{_calculateTotalEnergyRateLimit(param)}; + + float STE_rate_setpoint = se.spe.rate_setpoint + se.ske.rate_setpoint; + + // 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) + STE_rate_setpoint += param.load_factor_correction * (param.load_factor - 1.f); + + STE_rate_setpoint = constrain(STE_rate_setpoint, limit.STE_rate_min, limit.STE_rate_max); + + _ste_rate = se.spe.rate + se.ske.rate; + + float STE_rate_error_raw = se.spe.rate_error + se.ske.rate_error; + // TODO rmeove reset and add correct time intervall + _ste_rate_error_filter.setParameters(TECS::DT_DEFAULT, param.ste_rate_time_const); + _ste_rate_error_filter.reset(0.0f); + _ste_rate_error_filter.update(STE_rate_error_raw); + float STE_rate_error{_ste_rate_error_filter.getState()}; + + _debug_output.total_energy_rate_error = STE_rate_error; + _debug_output.total_energy_rate_sp = STE_rate_setpoint; + + // Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle + // 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 = 0 at cruise throttle + // Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min + float throttle_predicted = 0.0f; + + if (STE_rate_setpoint >= 0) { + // throttle is between trim and maximum + throttle_predicted = param.throttle_trim + STE_rate_setpoint / limit.STE_rate_max * (param.throttle_max - param.throttle_trim); + + } else { + // throttle is between trim and minimum + throttle_predicted = param.throttle_trim + STE_rate_setpoint / limit.STE_rate_min * (param.throttle_min - param.throttle_trim); + + } + + // Calculate gain scaler from specific energy rate error to throttle + const float STE_rate_to_throttle = 1.0f / (limit.STE_rate_max - limit.STE_rate_min); + + // Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits + float throttle_setpoint = (STE_rate_error * param.throttle_damping_gain) * STE_rate_to_throttle + throttle_predicted; + throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max); + + // Integral handling + if (flag.airspeed_enabled) { + if (param.integrator_gain_throttle > 0.0f) { + float integ_state_max = param.throttle_max - throttle_setpoint; + float integ_state_min = param.throttle_min - throttle_setpoint; + + // underspeed conditions zero out integration + float throttle_integ_input = (STE_rate_error * param.integrator_gain_throttle) * dt * + STE_rate_to_throttle * (1.0f - _percent_undersped); + + // only allow integrator propagation into direction which unsaturates throttle + if (_throttle_integ_state > integ_state_max) { + throttle_integ_input = math::min(0.f, throttle_integ_input); + + } else if (_throttle_integ_state < integ_state_min) { + throttle_integ_input = math::max(0.f, throttle_integ_input); + } + + // Calculate a throttle demand from the integrated total energy rate error + // This will be added to the total throttle demand to compensate for steady state errors + _throttle_integ_state = _throttle_integ_state + throttle_integ_input; + + if (flag.climbout_mode_active) { + // During climbout, set the integrator to maximum throttle to prevent transient throttle drop + // at end of climbout when we transition to closed loop throttle control + _throttle_integ_state = integ_state_max; + } + + } else { + _throttle_integ_state = 0.0f; + } + + } + + if (flag.airspeed_enabled) { + // Add the integrator feedback during closed loop operation with an airspeed sensor + throttle_setpoint += _throttle_integ_state; + + } else { + // when flying without an airspeed sensor, use the predicted throttle only + throttle_setpoint = throttle_predicted; + + } + + // ramp in max throttle setting with underspeediness value + throttle_setpoint = _percent_undersped * param.throttle_max + (1.0f - _percent_undersped) * throttle_setpoint; + + // Rate limit the throttle demand + if (fabsf(param.throttle_slewrate) > 0.01f) { + const float throttle_increment_limit = dt * (param.throttle_max - param.throttle_min) * param.throttle_slewrate; + throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint - throttle_increment_limit, + _throttle_setpoint + throttle_increment_limit); + } + + _throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max); +} + +void TECSControl::resetIntegrals() +{ + _pitch_integ_state = 0.0f; + _throttle_integ_state = 0.0f; +} + +float TECS::_update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas) +{ + float new_setpoint{tas_setpoint}; + float percent_undersped = _control.getPercentUndersped(); + // 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) { + new_setpoint = tas_min; + + } else if (percent_undersped > FLT_EPSILON) { + // TAS setpoint is reset from external setpoint every time tecs is called, so the interpolation is still + // between current setpoint and mininimum airspeed here (it's not feeding the newly adjusted setpoint + // from this line back into this method each time). + // TODO: WOULD BE GOOD to "functionalize" this library a bit and remove many of these internal states to + // avoid the fear of side effects in simple operations like this. + new_setpoint = tas_min * percent_undersped + (1.0f - percent_undersped) * tas_setpoint; + } + + new_setpoint = constrain(new_setpoint, tas_min, tas_max); + + return new_setpoint; +} + +void TECS::_detect_uncommanded_descent(float throttle_setpoint_max, float altitude, float altitude_setpoint, float tas, float tas_setpoint) +{ + /* + * This function detects a condition that can occur when the demanded airspeed is greater than the + * aircraft can achieve in level flight. When this occurs, the vehicle will continue to reduce altitude + * while attempting to maintain speed. + */ + + // Calculate specific energy demands in units of (m**2/sec**2) + float SPE_setpoint = altitude_setpoint * CONSTANTS_ONE_G; // potential energy + float SKE_setpoint = 0.5f * altitude_setpoint * altitude_setpoint; // kinetic energy + + // Calculate specific energies in units of (m**2/sec**2) + float SPE_estimate = altitude * CONSTANTS_ONE_G; // potential energy + float SKE_estimate = 0.5f * tas * tas; // kinetic energy + + // Calculate total energy error + float SPE_error = SPE_setpoint - SPE_estimate; + float SKE_error = SKE_setpoint - SKE_estimate; + float STE_error = SPE_error + SKE_error; + + + + const bool underspeed_detected = _control.getPercentUndersped() > FLT_EPSILON; + + // 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 + const bool enter_mode = !_uncommanded_descent_recovery && !underspeed_detected && (STE_error > 200.0f) + && (_control.getSteRate() < 0.0f) + && (_control.getThrottleSetpoint() >= throttle_setpoint_max * 0.9f); + + // If we enter an underspeed condition or recover the required total energy, then exit uncommanded descent recovery mode + const bool exit_mode = _uncommanded_descent_recovery && (underspeed_detected || (STE_error < 0.0f)); + + if (enter_mode) { + _uncommanded_descent_recovery = true; + + } else if (exit_mode) { + _uncommanded_descent_recovery = false; + + } +} + +void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed) +{ + // Init subclasses + TECSReferenceModel::AltitudeReferenceState current_state{ .alt=altitude, + .alt_rate = altitude_rate}; + _reference_model.initialize(current_state); + _airspeed_filter.initialize(equivalent_airspeed); + _control.initialize(); +} + +void TECS::update(float pitch, float altitude, float hgt_setpoint, 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_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp) +{ + // Calculate the time since last update (seconds) + uint64_t now = hrt_absolute_time(); + float dt = (now - _update_timestamp) * 1e-6f; + if (dt < DT_MIN) + { + // Update intervall too small, do not update. Assume constant states/output in this case. + return; + } + + if (dt > DT_MAX || _update_timestamp == 0UL) + { + // Update time intervall too large, can't guarantee sanity of state updates anymore. reset the control loop. + initialize(altitude, hgt_rate, equivalent_airspeed); + } + else + { + // Update airspeedfilter submodule + TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed=equivalent_airspeed, + .equivalent_airspeed_rate = speed_deriv_forward/eas_to_tas}; + _airspeed_param.equivalent_airspeed_trim = _equivalent_airspeed_trim; + _airspeed_filter.update(dt, airspeed_input,_airspeed_param, _airspeed_enabled); + TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState(); + + // Update Reference model submodule + TECSReferenceModel::AltitudeReferenceState setpoint{ .alt=hgt_setpoint, + .alt_rate=hgt_rate_sp + }; + _reference_param.target_climbrate = target_climbrate; + _reference_param.target_sinkrate = target_sinkrate; + _reference_model.update(dt, setpoint, altitude, _reference_param); + TECSControl::Setpoint control_setpoint; + control_setpoint.altitude_reference = _reference_model.getAltitudeReference(); + control_setpoint.altitude_rate_setpoint = _reference_model.getAltitudeRateReference(); + + // Calculate the demanded true airspeed + // TODO this function should not be in the module. Only give feedback that the airspeed can't be achieved. + control_setpoint.tas_setpoint =_update_speed_setpoint(eas_to_tas*_equivalent_airspeed_min,eas_to_tas*_equivalent_airspeed_max, EAS_setpoint*eas_to_tas, eas_to_tas*eas.speed); + + TECSControl::Input control_input{ .altitude =altitude, + .altitude_rate = hgt_rate, + .tas = eas_to_tas*eas.speed, + .tas_rate = eas_to_tas*eas.speed_rate + }; + _control_param.equivalent_airspeed_trim = _equivalent_airspeed_trim; + _control_param.tas_min = eas_to_tas*_equivalent_airspeed_min; + _control_param.pitch_max = pitch_limit_max; + _control_param.pitch_min = pitch_limit_min; + _control_param.throttle_trim = throttle_trim; + _control_param.throttle_max = throttle_setpoint_max; + _control_param.throttle_min = throttle_min; + TECSControl::Flag control_flag{ .airspeed_enabled = _airspeed_enabled, + .climbout_mode_active = climb_out_setpoint, + .detect_underspeed_enabled = _detect_underspeed_enabled + }; + _control.update(dt, control_setpoint, control_input, _control_param, control_flag); + + // Detect an uncommanded descent caused by an unachievable airspeed demand + _detect_uncommanded_descent(throttle_setpoint_max, altitude, hgt_setpoint, equivalent_airspeed*eas_to_tas, control_setpoint.tas_setpoint); + + TECSControl::DebugOutput control_status = _control.getDebugOutput(); + _debug_status.altitude_rate_control = control_status.altitude_rate_control; + _debug_status.energy_balance_rate_error = control_status.energy_balance_rate_error; + _debug_status.energy_balance_rate_sp = control_status.energy_balance_rate_sp; + _debug_status.total_energy_rate_error = control_status.total_energy_rate_error; + _debug_status.total_energy_rate_sp = control_status.total_energy_rate_sp; + _debug_status.true_airspeed_derivative_control = control_status.true_airspeed_derivative_control; + _debug_status.true_airspeed_filtered = eas_to_tas*eas.speed; + _debug_status.true_airspeed_derivative = eas_to_tas*eas.speed_rate; + _debug_status.altitude_sp = control_setpoint.altitude_reference.alt; + _debug_status.altitude_rate = control_setpoint.altitude_reference.alt_rate; + _debug_status.altitude_rate_setpoint = control_setpoint.altitude_rate_setpoint; + } + + + + // Update time stamps + _update_timestamp = now; + + + // Set TECS mode for next frame + if (_control.getPercentUndersped() > FLT_EPSILON) { + _tecs_mode = ECL_TECS_MODE_UNDERSPEED; + + } else if (_uncommanded_descent_recovery) { + _tecs_mode = ECL_TECS_MODE_BAD_DESCENT; + + } else if (climb_out_setpoint) { + _tecs_mode = ECL_TECS_MODE_CLIMBOUT; + + } else { + // This is the default operation mode + _tecs_mode = ECL_TECS_MODE_NORMAL; + } +} diff --git a/src/lib/tecs/TECSnew.hpp b/src/lib/tecs/TECSnew.hpp new file mode 100644 index 0000000000..0f247aed97 --- /dev/null +++ b/src/lib/tecs/TECSnew.hpp @@ -0,0 +1,648 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tecs.cpp + * + * @author Paul Riseborough + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +class TECSAirspeedFilter { +public: + /** + * @brief State of the equivalent airspeed filter. + * + */ + struct AirspeedFilterState + { + float speed; ///< speed of the air in EAS [m/s] + float speed_rate; ///< rate of speed of the air [m/s²] + }; + + /** + * @brief Parameters of the airspeed filter. + * + */ + struct Param + { + float equivalent_airspeed_trim; ///< the trim value of the equivalent airspeed om [m/s]. + float airspeed_estimate_freq; ///< cross-over frequency of the equivalent airspeed complementary filter [rad/sec]. + float speed_derivative_time_const; ///< speed derivative filter time constant [s]. + }; + + /** + * @brief Input, which will be filtered. + * + */ + struct Input + { + float equivalent_airspeed; ///< the measured equivalent airspeed in [m/s]. + float equivalent_airspeed_rate; ///< the measured rate of equivalent airspeed in [m/s²]. + }; +public: + TECSAirspeedFilter() = default; + ~TECSAirspeedFilter() = default; + /** + * @brief Initialize filter + * + * @param[in] equivalent_airspeed is the equivalent airspeed in [m/s]. + */ + void initialize(float equivalent_airspeed); + + /** + * @brief Update filter + * + * @param[in] dt is the timestep in [s]. + * @param[in] input are the raw measured values. + * @param[in] param are the filter parameters. + * @param[in] airspeed_sensor_available boolean if the airspeed sensor is available. + */ + void update(float dt, const Input &input, const Param ¶m, const bool airspeed_sensor_available); + + /** + * @brief Get the filtered airspeed states. + * + * @return Current state of the airspeed filter. + */ + AirspeedFilterState getState() const; + +private: + // States + AlphaFilter _airspeed_rate_filter; ///< Alpha filter for airspeed rate + AirspeedFilterState _airspeed_state{.speed=0.0f, .speed_rate=0.0f}; ///< Complimentary filter state +}; + +class TECSReferenceModel { +public: + /** + * @brief Altitude reference state. + * + */ + struct AltitudeReferenceState { + float alt; ///< Reference altitude amsl in [m]. + float alt_rate; ///< Reference altitude rate in [m/s]. + }; + + /** + * @brief Parameters for the reference model. + * + */ + struct Param { + float target_climbrate; ///< The target climbrate in [m/s]. + float target_sinkrate; ///< The target sinkrate in [m/s]. + float jerk_max; ///< Magnitude of the maximum jerk allowed [m/s³]. + float vert_accel_limit; ///< Magnitude of the maximum vertical acceleration allowed [m/s²]. + float max_climb_rate; ///< Climb rate produced by max allowed throttle [m/s]. + float max_sink_rate; ///< Maximum safe sink rate [m/s]. + }; + +public: + TECSReferenceModel() = default; + ~TECSReferenceModel() = default; + + /** + * @brief Initialize reference models. + * + * @param[in] state is the current altitude state of the vehicle. + */ + void initialize(const AltitudeReferenceState &state); + + /** + * @brief Update reference models. + * + * @param[in] dt is the update interval in [s]. + * @param[in] setpoint are the desired setpoints. + * @param[in] altitude is the altitude amsl in [m]. + * @param[in] param are the reference model parameters. + */ + void update(float dt, const AltitudeReferenceState &setpoint, float altitude, const Param ¶m); + + /** + * @brief Get the current altitude reference of altitude reference model. + * + * @return Altitude reference state. + */ + AltitudeReferenceState getAltitudeReference() const; + + /** + * @brief Get the altitude rate reference of the altitude rate reference model. + * + * @return Current altitude rate reference point. + */ + float getAltitudeRateReference() const; + +private: + // State + VelocitySmoothing + _alt_control_traj_generator; ///< Generates altitude rate and altitude setpoint trajectory when altitude is commanded. + + ManualVelocitySmoothingZ + _velocity_control_traj_generator; ///< Generates altitude rate setpoint when altitude rate is commanded. +}; + +class TECSControl { +public: + /** + * @brief The control parameters. + * + */ + struct Param + { + // Vehicle specific params + float max_sink_rate; ///< Maximum safe sink rate [m/s]. + float max_climb_rate; ///< Climb rate produced by max allowed throttle [m/s]. + float vert_accel_limit; ///< Magnitude of the maximum vertical acceleration allowed [m/s²]. + float equivalent_airspeed_trim; ///< Equivalent cruise airspeed for airspeed less mode [m/s]. + float tas_min; ///< True airpeed demand lower limit [m/s]. + float pitch_max; ///< Maximum pitch angle allowed in [rad]. + float pitch_min; ///< Minimal pitch angle allowed in [rad]. + float throttle_trim; ///< Normalized throttle required to fly level at given eas. + float throttle_max; ///< Normalized throttle upper limit. + float throttle_min; ///< Normalized throttle lower limit. + + // Altitude control param + float altitude_error_gain; ///< Altitude error inverse time constant [1/s]. + float altitude_setpoint_gain_ff; ///< Gain from altitude demand derivative to demanded climb rate. + + // Airspeed control param + /// [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors + float tas_error_percentage; + float airspeed_error_gain; ///< Airspeed error inverse time constant [1/s]. + + // Energy control param + float ste_rate_time_const; ///< Filter time constant for specific total energy rate (damping path) [s]. + float seb_rate_ff; ///< Specific energy balance rate feedforward gain. + + // Pitch control param + float pitch_speed_weight; ///< Speed control weighting used by pitch demand calculation. + float integrator_gain_pitch; ///< Integrator gain used by the pitch demand calculation. + float pitch_damping_gain; ///< Damping gain of the pitch demand calculation [s]. + + // Throttle control param + float integrator_gain_throttle; ///< Integrator gain used by the throttle demand calculation. + float throttle_damping_gain; ///< Damping gain of the throttle demand calculation [s]. + float throttle_slewrate; ///< Throttle demand slew rate limit [1/s]. + + float load_factor_correction; ///< Gain from normal load factor increase to total energy rate demand [m²/s³]. + float load_factor; ///< Additional normal load factor. + }; + + /** + * @brief The debug output + * + */ + struct DebugOutput + { + float altitude_rate_control; ///< Altitude rate setpoint from altitude control loop [m/s]. + float true_airspeed_derivative_control; ///< Airspeed rate setpoint from airspeed control loop [m/s²]. + float total_energy_rate_error; ///< Total energy rate error [m²/s³]. + float total_energy_rate_sp; ///< Total energy rate setpoint [m²/s³]. + float energy_balance_rate_error; ///< Energy balance rate error [m²/s³]. + float energy_balance_rate_sp; ///< Energy balance rate setpoint [m²/s³]. + }; + + /** + * @brief Given setpoint to control. + * + */ + struct Setpoint + { + TECSReferenceModel::AltitudeReferenceState altitude_reference; ///< Altitude reference from reference model. + float altitude_rate_setpoint; ///< Altitude rate setpoint. + float tas_setpoint; ///< True airspeed setpoint. + }; + + /** + * @brief Givent current measurement from the UAS. + * + */ + struct Input + { + float altitude; ///< Current altitude 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²]. + }; + + /** + * @brief Control flags. + * + */ + struct Flag + { + bool airspeed_enabled; ///< Flag if the airspeed sensor is enabled. + bool climbout_mode_active; ///< Flag if climbout mode is activated. + bool detect_underspeed_enabled; ///< Flag if underspeed detection is enabled. + }; +public: + TECSControl() = default; + ~TECSControl() = default; + /** + * @brief Initialization of the state. + * + */ + void initialize(); + /** + * @brief Update state and output. + * + * @param[in] dt is the update time intervall in [s]. + * @param[in] setpoint is the current setpoint struct. + * @param[in] input is the current input measurements. + * @param[in] param is the current parameter set. + * @param[in] flag is the current activated flags. + */ + void update(float dt, const Setpoint &setpoint, const Input &input, Param ¶m, const Flag &flag); + /** + * @brief Reset the control loop integrals. + * + */ + void resetIntegrals(); + /** + * @brief Get the percent of the undersped. + * + * @return Percentage of detected undersped. + */ + float getPercentUndersped() const {return _percent_undersped;}; + /** + * @brief Get the throttle setpoint. + * + * @return throttle setpoint. + */ + float getThrottleSetpoint() const {return _throttle_setpoint;}; + /** + * @brief Get the pitch setpoint. + * + * @return THe commanded pitch angle in [rad]. + */ + float getPitchSetpoint() const {return _pitch_setpoint;}; + /** + * @brief Get specific total energy rate. + * + * @return the total specific energy rate in [m²/s³]. + */ + float getSteRate() const {return _ste_rate;}; + /** + * @brief Get the Debug Output + * + * @return the debug outpus struct. + */ + DebugOutput getDebugOutput() const {return _debug_output;}; + +private: + /** + * @brief Specific total energy limit. + * + */ + struct STELimit + { + float STE_rate_max; ///< Maximum specific total energy rate limit [m²/s³]. + float STE_rate_min; ///< Minimal specific total energy rate limit [m²/s³]. + }; + + /** + * @brief Calculated specific energy. + * + */ + struct SpecificEnergy + { + struct { + float rate; ///< Specific kinetic energy rate in [m²/s³]. + float rate_setpoint; ///< Specific kinetic energy setpoint rate in [m²/s³]. + float rate_error; ///< Specific kinetic energy rate error in [m²/s³]. + } ske; ///< Specific kinetic energy. + struct { + float rate; ///< Specific potential energy rate in [m²/s³]. + float rate_setpoint; ///< Specific potential energy setpoint rate in [m²/s³]. + float rate_error; ///< Specific potential energy rate error in [m²/s³]. + } spe; ///< Specific potential energy rate. + }; + + /** + * @brief Controlled altitude and pitch setpoints. + * + */ + struct AltitudePitchControl { + float altitude_rate_setpoint; ///< Controlled altitude rate setpoint [m/s]. + float tas_rate_setpoint; ///< Controlled true airspeed rate setpoint [m/s²]. + }; + + /** + * @brief Weight factors for specific energy. + * + */ + struct SpecificEnergyWeighting { + float spe_weighting; ///< Specific potential energy weight. + float ske_weighting; ///< Specific kinetic energy weight. + }; + +private: + /** + * @brief Calculate specific total energy rate limits. + * + * @param[in] param are the control parametes. + * @return Specific total energy rate limits. + */ + STELimit _calculateTotalEnergyRateLimit(const Param ¶m) const; + /** + * @brief Airspeed control loop. + * + * @param setpoint is the control setpoints. + * @param input is the current input measurment of the UAS. + * @param param is the control parameters. + * @param flag is the control flags. + * @return controlled airspeed rate setpoint in [m/s²]. + */ + float _airspeedControl(const Setpoint &setpoint, const Input &input, const Param ¶m, const Flag &flag) const; + /** + * @brief Altitude control loop. + * + * @param setpoint is the control setpoints. + * @param input is the current input measurment of the UAS. + * @param param is the control parameters. + * @return controlled altitude rate setpoint in [m/s]. + */ + float _altitudeControl(const Setpoint &setpoint, const Input &input, const Param ¶m) const; + /** + * @brief Update energy balance. + * + * @param control_setpoint is the controlles altitude and airspeed rate setpoints. + * @param input is the current input measurment of the UAS. + * @return Specific energy rates. + */ + SpecificEnergy _updateEnergyBalance(const AltitudePitchControl &control_setpoint, const Input &input) const; + /** + * @brief Detect underspeed. + * + * @param input is the current input measurment of the UAS. + * @param param is the control parameters. + * @param flag is the control flags. + */ + void _detectUnderspeed(const Input &input, const Param ¶m, const Flag &flag); + /** + * @brief Update specific energy balance weights. + * + * @param param is the control parameters. + * @param flag is the control flags. + * @return Weights used for the specific energy balance. + */ + SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Param ¶m, const Flag &flag); + /** + * @brief Update controlled pitch setpoint. + * + * @param dt is the update time intervall in [s]. + * @param input is the current input measurment of the UAS. + * @param se is the calculated specific energy. + * @param param is the control parameters. + * @param flag is the control flags. + */ + void _updatePitchSetpoint(float dt, const Input &input, const SpecificEnergy &se, Param ¶m, const Flag &flag); + /** + * @brief Update controlled throttle setpoint. + * + * @param dt is the update time intervall in [s]. + * @param se is the calculated specific energy. + * @param param is the control parameters. + * @param flag is the control flags. + */ + void _updateThrottleSetpoint(float dt, const SpecificEnergy &se, const Param ¶m, const Flag &flag); + +private: + // State + AlphaFilter _ste_rate_error_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. + + + // Output + DebugOutput _debug_output; + float _pitch_setpoint{0.0f}; ///< Controlled pitch setpoint [rad]. + float _throttle_setpoint{0.0f}; ///< Controlled throttle setpoint. + float _percent_undersped{0.0f}; ///< A continuous representation of how "undersped" the TAS is [0,1] + float _ste_rate{0.0f}; ///< Specific total energy rate [m²/s³]. +}; + +class TECS +{ +public: + struct DebugOutput : TECSControl::DebugOutput + { + float true_airspeed_filtered; + float true_airspeed_derivative; + float altitude_sp; + float altitude_rate; + float altitude_rate_setpoint; + }; +public: + TECS() = default; + ~TECS() = default; + + // no copy, assignment, move, move assignment + TECS(const TECS &) = delete; + TECS &operator=(const TECS &) = delete; + TECS(TECS &&) = delete; + TECS &operator=(TECS &&) = delete; + + DebugOutput getStatus() const {return _debug_status;}; + + /** + * Get the current airspeed status + * + * @return true if airspeed is enabled for control + */ + bool airspeed_sensor_enabled() { return _airspeed_enabled; } + + /** + * Set the airspeed enable state + */ + void enable_airspeed(bool enabled) { _airspeed_enabled = enabled; } + + /** + * @brief Update the control loop calculations + * + */ + void update(float pitch, float altitude, float hgt_setpoint, 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_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN); + + /** + * @brief Initialize the control loop + * + */ + void initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed); + + void resetIntegrals() + { + _control.resetIntegrals(); + } + + enum ECL_TECS_MODE { + ECL_TECS_MODE_NORMAL = 0, + ECL_TECS_MODE_UNDERSPEED, + ECL_TECS_MODE_BAD_DESCENT, + ECL_TECS_MODE_CLIMBOUT + }; + + void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled=enabled; }; + + // // setters for parameters + void set_integrator_gain_throttle(float gain) { _control_param.integrator_gain_throttle=gain;}; + void set_integrator_gain_pitch(float gain) { _control_param.integrator_gain_pitch=gain; }; + + void set_max_sink_rate(float sink_rate) { _control_param.max_sink_rate=sink_rate; _reference_param.max_sink_rate=sink_rate; }; + void set_max_climb_rate(float climb_rate) { _control_param.max_climb_rate=climb_rate; _reference_param.max_climb_rate=climb_rate; }; + + void set_altitude_rate_ff(float altitude_rate_ff) { _control_param.altitude_setpoint_gain_ff=altitude_rate_ff; }; + void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain= 1.0f / math::max(time_const, 0.1f);; }; + + void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; } + void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; } + void set_equivalent_airspeed_trim(float airspeed) { _equivalent_airspeed_trim = airspeed; } + + void set_pitch_damping(float damping) { _control_param.pitch_damping_gain=damping; } + void set_vertical_accel_limit(float limit) { _reference_param.vert_accel_limit=limit; _control_param.vert_accel_limit=limit; }; + + void set_speed_comp_filter_omega(float omega) { _airspeed_param.airspeed_estimate_freq=omega; }; + void set_speed_weight(float weight) { _control_param.pitch_speed_weight=weight; }; + void set_airspeed_error_time_constant(float time_const) { _control_param.airspeed_error_gain = 1.0f / math::max(time_const, 0.1f); }; + + void set_throttle_damp(float throttle_damp) { _control_param.throttle_damping_gain=throttle_damp; }; + void set_throttle_slewrate(float slewrate) { _control_param.throttle_slewrate=slewrate; }; + + void set_roll_throttle_compensation(float compensation) { _control_param.load_factor_correction=compensation; }; + void set_load_factor(float load_factor) { _control_param.load_factor=load_factor; }; + + void set_speed_derivative_time_constant(float time_const) { _airspeed_param.speed_derivative_time_const=time_const; }; + void set_ste_rate_time_const(float time_const) { _control_param.ste_rate_time_const=time_const; }; + + void set_seb_rate_ff_gain(float ff_gain) { _control_param.seb_rate_ff=ff_gain; }; + + 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; } + + static constexpr float DT_DEFAULT = 0.02f; + +private: + TECSControl _control; ///< Control submodule. + TECSAirspeedFilter _airspeed_filter; ///< Airspeed filter submodule. + TECSReferenceModel _reference_model; ///< Setpoint reference model submodule. + + enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; ///< Current activated mode. + + uint64_t _update_timestamp{0}; ///< last timestamp of the update function call. + + 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 _equivalent_airspeed_trim{15.0f}; ///< equivalent cruise airspeed for airspeed less mode (m/sec) + + // controller mode logic + bool _uncommanded_descent_recovery{false}; ///< true when a continuous descent caused by an unachievable airspeed demand has been detected + bool _airspeed_enabled{false}; ///< true when airspeed use has been enabled + bool _detect_underspeed_enabled{false}; ///< true when underspeed detection is enabled + + static constexpr float DT_MIN = 0.001f; ///< minimum allowed value of _dt (sec) + static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a filter state reset is performed (sec) + + static constexpr float _jerk_max = 1000.0f; ///< Magnitude of the maximum jerk allowed [m/s³]. + static constexpr float _tas_error_percentage = 0.15f; ///< [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors + + DebugOutput _debug_status{}; + + // Params + /// Airspeed filter parameters. + TECSAirspeedFilter::Param _airspeed_param{ + .equivalent_airspeed_trim=0.0f, + .airspeed_estimate_freq= 0.0f, + .speed_derivative_time_const= 0.01f, + }; + /// Reference model parameters. + TECSReferenceModel::Param _reference_param{ + .target_climbrate=2.0f, + .target_sinkrate=2.0f, + .jerk_max=_jerk_max, + .vert_accel_limit=0.0f, + .max_climb_rate=2.0f, + .max_sink_rate=2.0f, + }; + /// Control parameters. + TECSControl::Param _control_param{ + .max_sink_rate=2.0f, + .max_climb_rate=2.0f, + .vert_accel_limit=0.0f, + .equivalent_airspeed_trim=15.0f, + .tas_min=3.0f, + .pitch_max=5.0f, + .pitch_min=-5.0f, + .throttle_trim=0.0f, + .throttle_max=1.0f, + .throttle_min=0.1f, + .altitude_error_gain=0.2f, + .altitude_setpoint_gain_ff=0.0f, + .tas_error_percentage=_tas_error_percentage, + .airspeed_error_gain=0.1f, + .ste_rate_time_const=0.1f, + .seb_rate_ff=1.0f, + .pitch_speed_weight=1.0f, + .integrator_gain_pitch=0.0f, + .pitch_damping_gain=0.0f, + .integrator_gain_throttle=0.0f, + .throttle_damping_gain=0.0f, + .throttle_slewrate=0.0f, + .load_factor_correction=0.0f, + .load_factor=1.0f, + }; + + /** + * Update the desired airspeed + */ + float _update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas); + + /** + * Detect an uncommanded descent + */ + void _detect_uncommanded_descent(float throttle_setpoint_max, float altitude, float altitude_setpoint, float tas, float tas_setpoint); +}; +