diff --git a/msg/TecsStatus.msg b/msg/TecsStatus.msg index 8cd1a1e1b9..c553ec32a2 100644 --- a/msg/TecsStatus.msg +++ b/msg/TecsStatus.msg @@ -18,21 +18,11 @@ float32 true_airspeed_filtered float32 true_airspeed_derivative_sp float32 true_airspeed_derivative float32 true_airspeed_derivative_raw -float32 true_airspeed_innovation -float32 total_energy_error -float32 energy_distribution_error -float32 total_energy_rate_error -float32 energy_distribution_rate_error - -float32 total_energy float32 total_energy_rate -float32 total_energy_balance float32 total_energy_balance_rate -float32 total_energy_sp float32 total_energy_rate_sp -float32 total_energy_balance_sp float32 total_energy_balance_rate_sp float32 throttle_integ diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 84adfa813c..6c044b8d77 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -42,9 +42,13 @@ #include +#include "matrix/Matrix.hpp" +#include "matrix/Vector2.hpp" + using math::constrain; using math::max; using math::min; +using namespace time_literals; static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);} @@ -54,7 +58,6 @@ 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, @@ -85,49 +88,51 @@ void TECSAirspeedFilter::update(const float dt, const Input &input, const Param airspeed_derivative = 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); + /* Filter airspeed and rate using a constant airspeed rate model in a steady state Kalman Filter. + We use the gains of the continuous Kalman filter Kc and approximate the discrete version Kalman gain Kd =dt*Kc, + since the continuous algebraic Riccatti equation is easier to solve. + */ - if (PX4_ISFINITE(input.equivalent_airspeed_rate) && airspeed_sensor_available) { - _airspeed_rate_filter.update(airspeed_derivative); + matrix::Vector2f new_state_predicted; - } else { - _airspeed_rate_filter.reset(0.0f); - } + new_state_predicted(0) = _airspeed_state.speed + dt * _airspeed_state.speed_rate; + new_state_predicted(1) = _airspeed_state.speed_rate; - AirspeedFilterState new_airspeed_state; - // Update TAS rate state - const float airspeed_innovation = airspeed - _airspeed_state.speed; - const 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; + const float airspeed_noise_inv{1.0f / param.airspeed_measurement_std_dev}; + const float airspeed_rate_noise_inv{1.0f / param.airspeed_rate_measurement_std_dev}; + const float airspeed_rate_noise_inv_squared_process_noise{airspeed_rate_noise_inv *airspeed_rate_noise_inv * param.airspeed_rate_noise_std_dev}; + const float denom{airspeed_noise_inv + airspeed_rate_noise_inv_squared_process_noise}; + const float common_nom{std::sqrt(param.airspeed_rate_noise_std_dev * (2.0f * airspeed_noise_inv + airspeed_rate_noise_inv_squared_process_noise))}; - // Update TAS state - const float airspeed_state_input = _airspeed_state.speed_rate + airspeed_innovation * param.airspeed_estimate_freq * 1.4142f; - new_airspeed_state.speed = _airspeed_state.speed + airspeed_state_input * dt; + matrix::Matrix kalman_gain; + kalman_gain(0, 0) = airspeed_noise_inv * common_nom / denom; + kalman_gain(0, 1) = airspeed_rate_noise_inv_squared_process_noise / denom; + kalman_gain(1, 0) = airspeed_noise_inv * airspeed_noise_inv * param.airspeed_rate_noise_std_dev / denom; + kalman_gain(1, 1) = airspeed_rate_noise_inv_squared_process_noise * common_nom / denom; - // Clip tas at zero - if (new_airspeed_state.speed < 0.0f) { - // clip TAS at zero, calculate input that would result in zero speed. - float desired_airspeed_state_input = -_airspeed_state.speed / dt; - float desired_airspeed_innovation = (desired_airspeed_state_input - _airspeed_state.speed_rate) / - (param.airspeed_estimate_freq * 1.4142f); - new_airspeed_state.speed_rate = _airspeed_state.speed_rate + (desired_airspeed_innovation * - param.airspeed_estimate_freq * param.airspeed_estimate_freq * dt); - new_airspeed_state.speed = 0.0f; + const matrix::Vector2f innovation{(airspeed - new_state_predicted(0)), (airspeed_derivative - new_state_predicted(1))}; + matrix::Vector2f new_state; + new_state = new_state_predicted + dt * (kalman_gain * (innovation)); + + // Clip airspeed at zero + if (new_state(0) < FLT_EPSILON) { + new_state(0) = 0.0f; + // calculate input that would result in zero speed. + const float desired_airspeed_innovation = (-new_state_predicted(0) / dt - kalman_gain(0, + 1) * innovation(1)) / kalman_gain(0, + 0); + new_state(1) = new_state_predicted(1) + dt * (kalman_gain(1, 0) * desired_airspeed_innovation + kalman_gain(1, + 1) * innovation(1)); } // Update states - _airspeed_state = new_airspeed_state; + _airspeed_state.speed = new_state(0); + _airspeed_state.speed_rate = new_state(1); } TECSAirspeedFilter::AirspeedFilterState TECSAirspeedFilter::getState() const { - AirspeedFilterState filter_state{ - .speed = _airspeed_state.speed, - .speed_rate = _airspeed_rate_filter.getState() - }; - - return filter_state; + return _airspeed_state; } void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &setpoint, float altitude, @@ -144,6 +149,7 @@ void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &se altitude = 0.0f; } + // Consider the altitude rate setpoint already smooth. No need to filter further, simply hold the value for the altitude rate reference. if (PX4_ISFINITE(setpoint.alt_rate)) { _alt_rate_ref = setpoint.alt_rate; @@ -195,23 +201,59 @@ float TECSReferenceModel::getAltitudeRateReference() const void TECSReferenceModel::initialize(const AltitudeReferenceState &state) { - AltitudeReferenceState init_state{state}; + float init_state_alt{state.alt}; + _alt_rate_ref = state.alt_rate; if (!PX4_ISFINITE(state.alt)) { - init_state.alt = 0.0f; + init_state_alt = 0.0f; } if (!PX4_ISFINITE(state.alt_rate)) { - init_state.alt_rate = 0.0f; + _alt_rate_ref = 0.0f; } - _alt_control_traj_generator.reset(0.0f, init_state.alt_rate, init_state.alt); + _alt_control_traj_generator.reset(0.0f, _alt_rate_ref, init_state_alt); } -void TECSControl::initialize() +void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param ¶m, const Flag &flag) { - _ste_rate_error_filter.reset(0.0f); resetIntegrals(); + + AltitudePitchControl control_setpoint; + + control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag); + + control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param); + + SpecificEnergyRates specific_energy_rate{_calcSpecificEnergyRates(control_setpoint, input)}; + + _detectUnderspeed(input, param, flag); + + const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)}; + ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rate)}; + + _pitch_setpoint = _calcPitchControlOutput(input, seb_rate, param, flag); + + const STERateLimit limit{_calculateTotalEnergyRateLimit(param)}; + + _ste_rate_estimate_filter.reset(specific_energy_rate.spe_rate.estimate + specific_energy_rate.ske_rate.estimate); + + ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rate, param)}; + + _throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag); + + _ste_rate = ste_rate.estimate; + + // Debug output + _debug_output.total_energy_rate_estimate = ste_rate.estimate; + _debug_output.total_energy_rate_sp = ste_rate.setpoint; + _debug_output.throttle_integrator = _throttle_integ_state; + _debug_output.energy_balance_rate_estimate = seb_rate.estimate; + _debug_output.energy_balance_rate_sp = seb_rate.setpoint; + _debug_output.pitch_integrator = _pitch_integ_state; + + _debug_output.altitude_rate_control = control_setpoint.altitude_rate_setpoint; + _debug_output.true_airspeed_derivative_control = control_setpoint.tas_rate_setpoint; } void TECSControl::update(const float dt, const Setpoint &setpoint, const Input &input, Param ¶m, const Flag &flag) @@ -225,25 +267,27 @@ void TECSControl::update(const float dt, const Setpoint &setpoint, const Input & AltitudePitchControl control_setpoint; - control_setpoint.tas_rate_setpoint = _airspeedControl(setpoint, input, param, flag); + control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag); - control_setpoint.altitude_rate_setpoint = _altitudeControl(setpoint, input, param); + control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param); - SpecificEnergy se{_updateEnergyBalance(control_setpoint, input)}; + SpecificEnergyRates specific_energy_rate{_calcSpecificEnergyRates(control_setpoint, input)}; _detectUnderspeed(input, param, flag); - _updatePitchSetpoint(dt, input, se, param, flag); + _calcPitchControl(dt, input, specific_energy_rate, param, flag); - _updateThrottleSetpoint(dt, se, param, flag); + _calcThrottleControl(dt, specific_energy_rate, param, flag); _debug_output.altitude_rate_control = control_setpoint.altitude_rate_setpoint; _debug_output.true_airspeed_derivative_control = control_setpoint.tas_rate_setpoint; + _debug_output.pitch_integrator = _pitch_integ_state; + _debug_output.throttle_integrator = _throttle_integ_state; } -TECSControl::STELimit TECSControl::_calculateTotalEnergyRateLimit(const Param ¶m) const +TECSControl::STERateLimit TECSControl::_calculateTotalEnergyRateLimit(const Param ¶m) const { - TECSControl::STELimit limit; + TECSControl::STERateLimit 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; @@ -251,12 +295,12 @@ TECSControl::STELimit TECSControl::_calculateTotalEnergyRateLimit(const Param &p return limit; } -float TECSControl::_airspeedControl(const Setpoint &setpoint, const Input &input, const Param ¶m, - const Flag &flag) const +float TECSControl::_calcAirspeedControlOutput(const Setpoint &setpoint, const Input &input, const Param ¶m, + const Flag &flag) const { float airspeed_rate_output{0.0f}; - const STELimit limit{_calculateTotalEnergyRateLimit(param)}; + const STERateLimit 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 @@ -272,7 +316,7 @@ float TECSControl::_airspeedControl(const Setpoint &setpoint, const Input &input return airspeed_rate_output; } -float TECSControl::_altitudeControl(const Setpoint &setpoint, const Input &input, const Param ¶m) const +float TECSControl::_calcAltitudeControlOutput(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 + @@ -282,29 +326,27 @@ float TECSControl::_altitudeControl(const Setpoint &setpoint, const Input &input return altitude_rate_output; } -TECSControl::SpecificEnergy TECSControl::_updateEnergyBalance(const AltitudePitchControl &control_setpoint, +TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint, const Input &input) const { - SpecificEnergy se; + SpecificEnergyRates specific_energy_rates; // 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 + specific_energy_rates.spe_rate.setpoint = control_setpoint.altitude_rate_setpoint * + CONSTANTS_ONE_G; // potential energy rate of change + specific_energy_rates.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 + specific_energy_rates.spe_rate.estimate = input.altitude_rate * CONSTANTS_ONE_G; // potential energy rate of change + specific_energy_rates.ske_rate.estimate = 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; + return specific_energy_rates; } void TECSControl::_detectUnderspeed(const Input &input, const Param ¶m, const Flag &flag) { if (!flag.detect_underspeed_enabled) { - _percent_undersped = 0.0f; + _ratio_undersped = 0.0f; return; } @@ -320,8 +362,8 @@ void TECSControl::_detectUnderspeed(const Input &input, const Param ¶m, cons 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); + _ratio_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) @@ -334,8 +376,8 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co 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 (_ratio_undersped > FLT_EPSILON && flag.airspeed_enabled) { + pitch_speed_weight = 2.0f * _ratio_undersped + (1.0f - _ratio_undersped) * pitch_speed_weight; } else if (!flag.airspeed_enabled) { pitch_speed_weight = 0.0f; @@ -350,13 +392,35 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co return weight; } -void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const SpecificEnergy &se, Param ¶m, - const Flag &flag) +void TECSControl::_calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rates, + const Param ¶m, + const Flag &flag) { const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)}; + ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)}; + + _calcPitchControlUpdate(dt, 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 + // NOTE: at zero airspeed, the pitch increment is unbounded + const float pitch_increment = dt * param.vert_accel_limit / math::max(input.tas, FLT_EPSILON); + _pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment, + _pitch_setpoint + pitch_increment); + + //Debug Output + _debug_output.energy_balance_rate_estimate = seb_rate.estimate; + _debug_output.energy_balance_rate_sp = seb_rate.setpoint; + _debug_output.pitch_integrator = _pitch_integ_state; +} + +TECSControl::ControlValues TECSControl::_calcPitchControlSebRate(const SpecificEnergyWeighting &weight, + const SpecificEnergyRates &specific_energy_rates) const +{ + ControlValues seb_rate; /* - * 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 + * The SKE_weighting variable controls how speed and altitude control are prioritized by the pitch demand calculation. + * A weighting of 1 gives 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. @@ -364,18 +428,21 @@ void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const Speci * 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 - const float seb_rate_setpoint = se.spe.rate_setpoint * weight.spe_weighting - se.ske.rate_setpoint * weight.ske_weighting; + seb_rate.setpoint = specific_energy_rates.spe_rate.setpoint * weight.spe_weighting - + specific_energy_rates.ske_rate.setpoint * + weight.ske_weighting; - // Calculate the specific energy balance rate error - const float seb_rate_error = (se.spe.rate_error * weight.spe_weighting) - (se.ske.rate_error * weight.ske_weighting); + seb_rate.estimate = (specific_energy_rates.spe_rate.estimate * weight.spe_weighting) - + (specific_energy_rates.ske_rate.estimate * weight.ske_weighting); - _debug_output.energy_balance_rate_error = seb_rate_error; - _debug_output.energy_balance_rate_sp = seb_rate_setpoint; + return seb_rate; +} - if (param.integrator_gain_pitch > 0.0f) { +void TECSControl::_calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param ¶m) +{ + if (param.integrator_gain_pitch > FLT_EPSILON) { // Calculate pitch integrator input term - float pitch_integ_input = seb_rate_error * param.integrator_gain_pitch; + float pitch_integ_input = _getControlError(seb_rate) * param.integrator_gain_pitch; // Prevent the integrator changing in a direction that will increase pitch demand saturation if (_pitch_setpoint >= param.pitch_max) { @@ -391,10 +458,16 @@ void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const Speci } else { _pitch_integ_state = 0.0f; } +} +float TECSControl::_calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param ¶m, + const Flag &flag) const +{ + // Pitch setpoint // 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; + 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; @@ -412,47 +485,70 @@ void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const Speci // 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 = 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); + return constrain(pitch_setpoint_unc, param.pitch_min, param.pitch_max); } -void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, const Param ¶m, const Flag &flag) +void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &specific_energy_rates, const Param ¶m, + const Flag &flag) { - const STELimit limit{_calculateTotalEnergyRateLimit(param)}; + const STERateLimit limit{_calculateTotalEnergyRateLimit(param)}; - float STE_rate_setpoint = se.spe.rate_setpoint + se.ske.rate_setpoint; + // Update STE rate estimate LP filter + const float STE_rate_estimate_raw = specific_energy_rates.spe_rate.estimate + specific_energy_rates.ske_rate.estimate; + _ste_rate_estimate_filter.setParameters(dt, param.ste_rate_time_const); + _ste_rate_estimate_filter.update(STE_rate_estimate_raw); + + ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)}; + _calcThrottleControlUpdate(dt, limit, ste_rate, param, flag); + float throttle_setpoint{_calcThrottleControlOutput(limit, ste_rate, param, flag)}; + + // Rate limit the throttle demand + if (fabsf(param.throttle_slewrate) > FLT_EPSILON) { + 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); + _ste_rate = ste_rate.estimate; + + // Debug output + _debug_output.total_energy_rate_estimate = ste_rate.estimate; + _debug_output.total_energy_rate_sp = ste_rate.setpoint; + _debug_output.throttle_integrator = _throttle_integ_state; +} + +TECSControl::ControlValues TECSControl::_calcThrottleControlSteRate(const STERateLimit &limit, + const SpecificEnergyRates &specific_energy_rates, + const Param ¶m) const +{ + // Output ste rate values + ControlValues ste_rate; + ste_rate.setpoint = specific_energy_rates.spe_rate.setpoint + specific_energy_rates.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 += 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.setpoint = constrain(ste_rate.setpoint, limit.STE_rate_min, limit.STE_rate_max); + ste_rate.estimate = _ste_rate_estimate_filter.getState(); - _ste_rate = se.spe.rate + se.ske.rate; - - const float STE_rate_error_raw = se.spe.rate_error + se.ske.rate_error; - _ste_rate_error_filter.setParameters(dt, param.ste_rate_time_const); - _ste_rate_error_filter.update(STE_rate_error_raw); - const 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; + return ste_rate; +} +void TECSControl::_calcThrottleControlUpdate(float dt, const STERateLimit &limit, const ControlValues &ste_rate, + const Param ¶m, const Flag &flag) +{ // 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); // Integral handling if (flag.airspeed_enabled) { - if (param.integrator_gain_throttle > 0.0f) { + if (param.integrator_gain_throttle > FLT_EPSILON) { // 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); + float throttle_integ_input = (_getControlError(ste_rate) * param.integrator_gain_throttle) * dt * + STE_rate_to_throttle * (1.0f - _ratio_undersped); // only allow integrator propagation into direction which unsaturates throttle if (_throttle_setpoint >= param.throttle_max) { @@ -477,6 +573,14 @@ void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, co } } +} + +float TECSControl::_calcThrottleControlOutput(const STERateLimit &limit, const ControlValues &ste_rate, + const Param ¶m, + const Flag &flag) const +{ + // 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); // Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle // as the starting point. Assume: @@ -485,20 +589,21 @@ void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, co // 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) { + if (ste_rate.setpoint >= FLT_EPSILON) { // throttle is between trim and maximum - throttle_predicted = param.throttle_trim + STE_rate_setpoint / limit.STE_rate_max * + 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 * + throttle_predicted = param.throttle_trim + ste_rate.setpoint / limit.STE_rate_min * (param.throttle_min - param.throttle_trim); } // 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; + float throttle_setpoint = (_getControlError(ste_rate) * param.throttle_damping_gain) * STE_rate_to_throttle + + throttle_predicted; if (flag.airspeed_enabled) { // Add the integrator feedback during closed loop operation with an airspeed sensor @@ -507,16 +612,9 @@ void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, co } // ramp in max throttle setting with underspeediness value - throttle_setpoint = _percent_undersped * param.throttle_max + (1.0f - _percent_undersped) * throttle_setpoint; + throttle_setpoint = _ratio_undersped * param.throttle_max + (1.0f - _ratio_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); + return constrain(throttle_setpoint, param.throttle_min, param.throttle_max); } void TECSControl::resetIntegrals() @@ -528,7 +626,7 @@ void TECSControl::resetIntegrals() 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}; - const float percent_undersped = _control.getPercentUndersped(); + const float percent_undersped = _control.getRatioUndersped(); // Set the TAS demand to the minimum value if an underspeed or // or a uncontrolled descent condition exists to maximise climb rate @@ -571,7 +669,7 @@ void TECS::_detect_uncommanded_descent(float throttle_setpoint_max, float altitu const float SKE_error = SKE_setpoint - SKE_estimate; const float STE_error = SPE_error + SKE_error; - const bool underspeed_detected = _control.getPercentUndersped() > FLT_EPSILON; + const bool underspeed_detected = _control.getRatioUndersped() > 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) @@ -590,14 +688,38 @@ void TECS::_detect_uncommanded_descent(float throttle_setpoint_max, float altitu } } -void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed) +void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed, + const float eas_to_tas) { // Init subclasses TECSReferenceModel::AltitudeReferenceState current_state{ .alt = altitude, .alt_rate = altitude_rate}; _reference_model.initialize(current_state); _airspeed_filter.initialize(equivalent_airspeed); - _control.initialize(); + + TECSControl::Setpoint control_setpoint; + control_setpoint.altitude_reference = _reference_model.getAltitudeReference(); + control_setpoint.altitude_rate_setpoint = _reference_model.getAltitudeRateReference(); + control_setpoint.tas_setpoint = equivalent_airspeed * eas_to_tas; + + const TECSControl::Input control_input{ .altitude = altitude, + .altitude_rate = altitude_rate, + .tas = eas_to_tas * equivalent_airspeed, + .tas_rate = 0.0f}; + + _control.initialize(control_setpoint, control_input, _control_param, _control_flag); + + _debug_status.tecs_mode = _tecs_mode; + _debug_status.control = _control.getDebugOutput(); + const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState(); + _debug_status.true_airspeed_filtered = eas_to_tas * eas.speed; + _debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate; + const TECSReferenceModel::AltitudeReferenceState ref_alt{_reference_model.getAltitudeReference()}; + _debug_status.altitude_sp = ref_alt.alt; + _debug_status.altitude_rate_alt_ref = ref_alt.alt_rate; + _debug_status.altitude_rate_feedforward = _reference_model.getAltitudeRateReference(); + + _update_timestamp = hrt_absolute_time(); } void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed, @@ -605,9 +727,23 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set 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) - const uint64_t now(hrt_absolute_time()); - const float dt = (now - _update_timestamp) * 1e-6f; + const hrt_abstime now(hrt_absolute_time()); + const float dt = static_cast((now - _update_timestamp)) / 1_s; + + // Update parameters from input + // Reference model + _reference_param.target_climbrate = target_climbrate; + _reference_param.target_sinkrate = target_sinkrate; + // Control + _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; + _control_flag.climbout_mode_active = climb_out_setpoint; if (dt < DT_MIN) { // Update intervall too small, do not update. Assume constant states/output in this case. @@ -616,23 +752,22 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set 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); + initialize(altitude, hgt_rate, equivalent_airspeed, eas_to_tas); } else { // Update airspeedfilter submodule const 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); + + _airspeed_filter.update(dt, airspeed_input, _airspeed_filter_param, _control_flag.airspeed_enabled); const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState(); // Update Reference model submodule const TECSReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint, - .alt_rate = hgt_rate_sp - }; - _reference_param.target_climbrate = target_climbrate; - _reference_param.target_sinkrate = target_sinkrate; + .alt_rate = hgt_rate_sp}; + _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(); @@ -643,59 +778,42 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set eas_to_tas * _equivalent_airspeed_max, EAS_setpoint * eas_to_tas, eas_to_tas * eas.speed); const 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; - const 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); + .altitude_rate = hgt_rate, + .tas = eas_to_tas * eas.speed, + .tas_rate = eas_to_tas * eas.speed_rate}; + + _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; + // Update time stamps + _update_timestamp = now; + + + // Set TECS mode for next frame + if (_control.getRatioUndersped() > 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; + } + + _debug_status.tecs_mode = _tecs_mode; + _debug_status.control = _control.getDebugOutput(); _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; + _debug_status.altitude_rate_alt_ref = control_setpoint.altitude_reference.alt_rate; + _debug_status.altitude_rate_feedforward = 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; - } - - _debug_status.tecs_mode = _tecs_mode; } diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 78b49ed06b..a00b605e91 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include @@ -66,9 +67,10 @@ public: * */ 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]. + float equivalent_airspeed_trim; ///< the trim value of the equivalent airspeed [m/s]. + float airspeed_measurement_std_dev; ///< airspeed measurement standard deviation in [m/s]. + float airspeed_rate_measurement_std_dev;///< airspeed rate measurement standard deviation in [m/s²]. + float airspeed_rate_noise_std_dev; ///< standard deviation on the airspeed rate deviation in the model in [m/s²]. }; /** @@ -108,7 +110,6 @@ public: private: // States - AlphaFilter _airspeed_rate_filter; ///< Alpha filter for airspeed rate AirspeedFilterState _airspeed_state{.speed = 0.0f, .speed_rate = 0.0f}; ///< Complimentary filter state }; @@ -176,8 +177,6 @@ private: // State VelocitySmoothing _alt_control_traj_generator; ///< Generates altitude rate and altitude setpoint trajectory when altitude is commanded. - - // Output float _alt_rate_ref; ///< Altitude rate reference in [m/s]. }; @@ -235,10 +234,12 @@ public: 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_estimate; ///< Total energy rate estimate [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_estimate; ///< Energy balance rate estimate [m²/s³]. float energy_balance_rate_sp; ///< Energy balance rate setpoint [m²/s³]. + float pitch_integrator; ///< Pitch control integrator state [-]. + float throttle_integrator; ///< Throttle control integrator state [-]. }; /** @@ -278,7 +279,7 @@ public: * @brief Initialization of the state. * */ - void initialize(); + void initialize(const Setpoint &setpoint, const Input &input, Param ¶m, const Flag &flag); /** * @brief Update state and output. * @@ -297,9 +298,9 @@ public: /** * @brief Get the percent of the undersped. * - * @return Percentage of detected undersped. + * @return Ratio of detected undersped [0,1]. */ - float getPercentUndersped() const {return _percent_undersped;}; + float getRatioUndersped() const {return _ratio_undersped;}; /** * @brief Get the throttle setpoint. * @@ -323,33 +324,35 @@ public: * * @return the debug outpus struct. */ - DebugOutput getDebugOutput() const {return _debug_output;}; + const DebugOutput &getDebugOutput() const { return _debug_output; } private: /** - * @brief Specific total energy limit. + * @brief Specific total energy rate limit. * */ - struct STELimit { + struct STERateLimit { 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. + * @brief Control values. + * setpoint and current state estimate value as input to a controller. * */ - 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. + struct ControlValues { + float setpoint; ///< Control setpoint. + float estimate; ///< Control estimate of current state value. + }; + + /** + * @brief Calculated specific energy rates. + * + */ + struct SpecificEnergyRates { + ControlValues ske_rate; ///< Specific kinetic energy rate [m²/s³]. + ControlValues spe_rate; ///< Specific potential energy rate [m²/s³]. }; /** @@ -371,15 +374,22 @@ private: }; private: + /** + * @brief Get control error from etpoint and estimate + * + * @param val is the current control setpoint and estimate. + * @return error value + */ + static inline constexpr float _getControlError(TECSControl::ControlValues val) {return (val.setpoint - val.estimate);}; /** * @brief Calculate specific total energy rate limits. * * @param[in] param are the control parametes. - * @return Specific total energy rate limits. + * @return Specific total energy rate limits in [m²/s³]. */ - STELimit _calculateTotalEnergyRateLimit(const Param ¶m) const; + STERateLimit _calculateTotalEnergyRateLimit(const Param ¶m) const; /** - * @brief Airspeed control loop. + * @brief calculate airspeed control proportional output. * * @param setpoint is the control setpoints. * @param input is the current input measurment of the UAS. @@ -387,24 +397,25 @@ private: * @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; + float _calcAirspeedControlOutput(const Setpoint &setpoint, const Input &input, const Param ¶m, + const Flag &flag) const; /** - * @brief Altitude control loop. + * @brief calculate altitude control proportional output. * * @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; + float _calcAltitudeControlOutput(const Setpoint &setpoint, const Input &input, const Param ¶m) const; /** - * @brief Update energy balance. + * @brief Calculate specific energy rates. * * @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. + * @return Specific energy rates in [m²/s³]. */ - SpecificEnergy _updateEnergyBalance(const AltitudePitchControl &control_setpoint, const Input &input) const; + SpecificEnergyRates _calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint, const Input &input) const; /** * @brief Detect underspeed. * @@ -422,37 +433,108 @@ private: */ SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Param ¶m, const Flag &flag); /** - * @brief Update controlled pitch setpoint. + * @brief Calculate pitch control. * * @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 input is the current input measurement of the UAS. + * @param specific_energy_rate 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); + void _calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rate, + const Param ¶m, + const Flag &flag); + + /** + * @brief Calculate pitch control specific energy balance rates. + * + * @param weight is the weighting use of the potential and kinetic energy. + * @param specific_energy_rate is the specific energy rates in [m²/s³]. + * @return specific energy balance rate values in [m²/s³]. + */ + ControlValues _calcPitchControlSebRate(const SpecificEnergyWeighting &weight, + const SpecificEnergyRates &specific_energy_rate) const; + + /** + * @brief Calculate the pitch control update function. + * Update the states of the pitch control + * + * @param dt is the update time intervall in [s]. + * @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); + + /** + * @brief Calculate the pitch control output function. + * + * @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. + * @param flag is the control flags. + * @return pitch setpoint angle [rad]. + */ + float _calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param ¶m, + const Flag &flag) const; + /** * @brief Update controlled throttle setpoint. * * @param dt is the update time intervall in [s]. - * @param se is the calculated specific energy. + * @param specific_energy_rate is the calculated specific energy. + * @param flag is the control flags. + */ + void _calcThrottleControl(float dt, const SpecificEnergyRates &specific_energy_rate, const Param ¶m, + const Flag &flag); + + /** + * @brief Calculate throttle control specific total energy + * + * @param limit is the specific total energy rate limits in [m²/s³]. + * @param specific_energy_rate is the specific energy rates in [m²/s³]. + * @param param is the control parameters. + * @return specific total energy rate values in [m²/s³] + */ + ControlValues _calcThrottleControlSteRate(const STERateLimit &limit, const SpecificEnergyRates &specific_energy_rate, + const Param ¶m) const; + + /** + * @brief Calculate the throttle control update function. + * Update the throttle control states. + * + * @param dt is the update time intervall in [s]. + * @param limit is the specific total energy rate limits in [m²/s³]. + * @param ste_rate is the specific total energy rates in [m²/s³]. * @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); + void _calcThrottleControlUpdate(float dt, const STERateLimit &limit, const ControlValues &ste_rate, const Param ¶m, + const Flag &flag); + + /** + * @brief Calculate the throttle control output function. + * + * @param limit is the specific total energy rate limits in [m²/s³]. + * @param ste_rate is the specific total energy rates in [m²/s³]. + * @param param is the control parameters. + * @param flag is the control flags. + * @return throttle setpoin in [0,1]. + */ + float _calcThrottleControlOutput(const STERateLimit &limit, const ControlValues &ste_rate, const Param ¶m, + const Flag &flag) const; private: // State - AlphaFilter _ste_rate_error_filter; ///< Low pass filter for the specific total energy rate. + 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. // Output - DebugOutput _debug_output; + DebugOutput _debug_output; ///< 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 _throttle_setpoint{0.0f}; ///< Controlled throttle setpoint [0,1]. + float _ratio_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³]. }; @@ -466,12 +548,13 @@ public: ECL_TECS_MODE_CLIMBOUT }; - struct DebugOutput : TECSControl::DebugOutput { + struct DebugOutput { + TECSControl::DebugOutput control; float true_airspeed_filtered; float true_airspeed_derivative; float altitude_sp; - float altitude_rate; - float altitude_rate_setpoint; + float altitude_rate_alt_ref; + float altitude_rate_feedforward; enum ECL_TECS_MODE tecs_mode; }; public: @@ -484,19 +567,19 @@ public: TECS(TECS &&) = delete; TECS &operator=(TECS &&) = delete; - DebugOutput getStatus() const {return _debug_status;}; + const 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; } + bool airspeed_sensor_enabled() { return _control_flag.airspeed_enabled; } /** * Set the airspeed enable state */ - void enable_airspeed(bool enabled) { _airspeed_enabled = enabled; } + void enable_airspeed(bool enabled) { _control_flag.airspeed_enabled = enabled; } /** * @brief Update the control loop calculations @@ -505,22 +588,26 @@ public: 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); + 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 initialize(float altitude, float altitude_rate, float equivalent_airspeed, float eas_to_tas); void resetIntegrals() { _control.resetIntegrals(); } - void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; }; + void set_detect_underspeed_enabled(bool enabled) { _control_flag.detect_underspeed_enabled = enabled; }; // // 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;}; + 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; }; @@ -532,12 +619,11 @@ public: 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_equivalent_airspeed_trim(float airspeed) { _control_param.equivalent_airspeed_trim = airspeed; _airspeed_filter_param.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); }; @@ -547,7 +633,6 @@ public: 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; }; @@ -574,8 +659,6 @@ public: 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. @@ -583,38 +666,32 @@ private: 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. + hrt_abstime _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, + TECSAirspeedFilter::Param _airspeed_filter_param{ + .equivalent_airspeed_trim = 15.0f, + .airspeed_measurement_std_dev = 0.2f, + .airspeed_rate_measurement_std_dev = 0.05f, + .airspeed_rate_noise_std_dev = 0.02f }; /// Reference model parameters. TECSReferenceModel::Param _reference_param{ .target_climbrate = 2.0f, .target_sinkrate = 2.0f, - .jerk_max = _jerk_max, + .jerk_max = 1000.0f, .vert_accel_limit = 0.0f, .max_climb_rate = 2.0f, .max_sink_rate = 2.0f, @@ -633,7 +710,7 @@ private: .throttle_min = 0.1f, .altitude_error_gain = 0.2f, .altitude_setpoint_gain_ff = 0.0f, - .tas_error_percentage = _tas_error_percentage, + .tas_error_percentage = 0.15f, .airspeed_error_gain = 0.1f, .ste_rate_time_const = 0.1f, .seb_rate_ff = 1.0f, @@ -647,6 +724,12 @@ private: .load_factor = 1.0f, }; + TECSControl::Flag _control_flag{ + .airspeed_enabled = false, + .climbout_mode_active = false, + .detect_underspeed_enabled = false, + }; + /** * Update the desired airspeed */ diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index c23ee3bddf..769efed070 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -134,15 +134,16 @@ FixedwingPositionControl::parameters_update() _tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get()); _tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get()); _tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get()); - _tecs.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get()); _tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get()); _tecs.set_pitch_damping(_param_fw_t_ptch_damp.get()); _tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get()); _tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get()); _tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get()); _tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get()); - _tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get()); _tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get()); + _tecs.set_airspeed_measurement_std_dev(_param_speed_standard_dev.get()); + _tecs.set_airspeed_rate_measurement_std_dev(_param_speed_rate_standard_dev.get()); + _tecs.set_airspeed_filter_process_std_dev(_param_process_noise_standard_dev.get()); int check_ret = PX4_OK; @@ -457,11 +458,12 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, } void -FixedwingPositionControl::tecs_status_publish() +FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, + float true_airspeed_derivative_raw, float throttle_trim) { tecs_status_s t{}; - TECS::DebugOutput debug_output{_tecs.getStatus()}; + const TECS::DebugOutput &debug_output{_tecs.getStatus()}; switch (_tecs.tecs_mode()) { case TECS::ECL_TECS_MODE_NORMAL: @@ -481,25 +483,25 @@ FixedwingPositionControl::tecs_status_publish() break; } + t.altitude_sp = alt_sp; t.altitude_filtered = debug_output.altitude_sp; - + t.height_rate_setpoint = debug_output.control.altitude_rate_control; + t.height_rate = -_local_pos.vz; + t.equivalent_airspeed_sp = equivalent_airspeed_sp; + t.true_airspeed_sp = _eas2tas * equivalent_airspeed_sp; t.true_airspeed_filtered = debug_output.true_airspeed_filtered; - - t.height_rate_setpoint = debug_output.altitude_rate_setpoint; - t.height_rate = debug_output.altitude_rate; - - t.true_airspeed_derivative_sp = debug_output.true_airspeed_derivative_control; + t.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control; t.true_airspeed_derivative = debug_output.true_airspeed_derivative; - - t.total_energy_rate_error = debug_output.total_energy_rate_error; - - t.energy_distribution_rate_error = debug_output.energy_balance_rate_error; - - t.total_energy_rate_sp = debug_output.total_energy_rate_sp; - t.total_energy_balance_rate_sp = debug_output.energy_balance_rate_sp; - + t.true_airspeed_derivative_raw = true_airspeed_derivative_raw; + t.total_energy_rate = debug_output.control.total_energy_rate_estimate; + t.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate; + t.total_energy_rate_sp = debug_output.control.total_energy_rate_sp; + t.total_energy_balance_rate_sp = debug_output.control.energy_balance_rate_sp; + t.throttle_integ = debug_output.control.throttle_integrator; + t.pitch_integ = debug_output.control.pitch_integrator; t.throttle_sp = _tecs.get_throttle_setpoint(); t.pitch_sp_rad = _tecs.get_pitch_setpoint(); + t.throttle_trim = throttle_trim; t.timestamp = hrt_absolute_time(); @@ -826,7 +828,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now) _was_in_air = true; _time_went_in_air = now; - _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed); + _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas); } /* reset flag when airplane landed */ @@ -834,7 +836,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now) _was_in_air = false; _completed_manual_takeoff = false; - _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed); + _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas); } } @@ -2314,7 +2316,7 @@ FixedwingPositionControl::Run() _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; _att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF; - _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed); + _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas); break; } @@ -2491,12 +2493,12 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva } // We need an altitude lock to calculate the TECS control - if (!(_local_pos.timestamp > 0)) { + if (_local_pos.timestamp == 0) { _reinitialize_tecs = true; } if (_reinitialize_tecs) { - _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed); + _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas); _reinitialize_tecs = false; } @@ -2504,7 +2506,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva _tecs.set_detect_underspeed_enabled(!disable_underspeed_detection); if (_landed) { - _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed); + _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas); } /* update TECS vehicle state estimates */ @@ -2530,7 +2532,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva -_local_pos.vz, hgt_rate_sp); - tecs_status_publish(); + tecs_status_publish(alt_sp, airspeed_sp, -_local_pos.vz, throttle_trim_comp); } float diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 0afe48ebc7..70f0bb55b7 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -437,7 +437,8 @@ private: void status_publish(); void landing_status_publish(); - void tecs_status_publish(); + void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw, + float throttle_trim); void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint); /** @@ -822,16 +823,17 @@ private: (ParamFloat) _param_fw_t_rll2thr, (ParamFloat) _param_fw_t_sink_max, (ParamFloat) _param_fw_t_sink_min, - (ParamFloat) _param_fw_t_spd_omega, (ParamFloat) _param_fw_t_spdweight, (ParamFloat) _param_fw_t_tas_error_tc, (ParamFloat) _param_fw_t_thr_damp, (ParamFloat) _param_fw_t_vert_acc, (ParamFloat) _param_ste_rate_time_const, - (ParamFloat) _param_tas_rate_time_const, (ParamFloat) _param_seb_rate_ff, (ParamFloat) _param_climbrate_target, (ParamFloat) _param_sinkrate_target, + (ParamFloat) _param_speed_standard_dev, + (ParamFloat) _param_speed_rate_standard_dev, + (ParamFloat) _param_process_noise_standard_dev, (ParamFloat) _param_fw_thr_trim, (ParamFloat) _param_fw_thr_idle, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 98c0339fc5..ea92e3b0f3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -709,22 +709,50 @@ PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f); PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); /** - * Complementary filter "omega" parameter for speed + * Airspeed measurement standard deviation for airspeed filter. * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse longitudinal acceleration and airspeed to obtain an - * improved airspeed estimate. Increasing this frequency weights the solution - * more towards use of the airspeed sensor, whilst reducing it weights the - * solution more towards use of the accelerometer data. + * This is the measurement standard deviation for the airspeed used in the airspeed filter in TECS. * - * @unit rad/s - * @min 1.0 + * @unit m/s + * @min 0.01 * @max 10.0 - * @decimal 1 - * @increment 0.5 + * @decimal 2 + * @increment 0.1 * @group FW TECS */ -PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); +PARAM_DEFINE_FLOAT(FW_T_SPD_STD, 0.2f); + +/** + * Airspeed rate measurement standard deviation for airspeed filter. + * + * This is the measurement standard deviation for the airspeed rate used in the airspeed filter in TECS. + * + * @unit m/s^2 + * @min 0.01 + * @max 10.0 + * @decimal 2 + * @increment 0.1 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SPD_DEV_STD, 0.05f); + +/** + * Process noise standard deviation for the airspeed rate in the airspeed filter. + * + * This is the process noise standard deviation in the airspeed filter filter defining the noise in the + * airspeed rate for the constant airspeed rate model. This is used to define how much the airspeed and + * the airspeed rate are filtered. The smaller the value the more the measurements are smoothed with the + * drawback for delays. + * + * @unit m/s^2 + * @min 0.01 + * @max 10.0 + * @decimal 2 + * @increment 0.1 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SPD_PRC_STD, 0.2f); + /** * Roll -> Throttle feedforward @@ -856,21 +884,6 @@ PARAM_DEFINE_INT32(FW_POS_STK_CONF, 2); */ PARAM_DEFINE_FLOAT(FW_T_STE_R_TC, 0.4f); - -/** - * True airspeed rate first order filter time constant. - * - * This filter is applied to the true airspeed rate. - * - * @min 0.0 - * @max 2 - * @decimal 2 - * @increment 0.01 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_TAS_R_TC, 0.2f); - - /** * Specific total energy balance rate feedforward gain. *