diff --git a/src/lib/tecs/CMakeLists.txt b/src/lib/tecs/CMakeLists.txt index 612f89b7f1..128bb6eed5 100644 --- a/src/lib/tecs/CMakeLists.txt +++ b/src/lib/tecs/CMakeLists.txt @@ -36,10 +36,4 @@ px4_add_library(tecs TECS.hpp ) -px4_add_library(tecsnew - TECSnew.cpp - TECSnew.hpp -) - target_link_libraries(tecs PRIVATE geo) -target_link_libraries(tecsnew PRIVATE geo) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 4b58207822..f576231a0c 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -30,6 +30,11 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ +/** + * @file TECS.cpp + * + * @author Paul Riseborough + */ #include "TECS.hpp" @@ -41,242 +46,423 @@ using math::constrain; using math::max; using math::min; -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) - -/** - * @file TECS.cpp - * - * @author Paul Riseborough - */ - -/* - * This function implements a complementary filter to estimate the climb rate when - * inertial nav data is not available. It also calculates a true airspeed derivative - * which is used by the airspeed complimentary filter. - */ -void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward, - bool altitude_lock, float altitude, float vz) +// TODO there seems to be an inconsistent definition of IAS/CAS/EAS/TAS +// TODO Recheck the timing. +void TECSAirspeedFilter::initialize(const float equivalent_airspeed) { - // calculate the time lapsed since the last update - uint64_t now = hrt_absolute_time(); - float dt = fmaxf((now - _state_update_timestamp) * 1e-6f, DT_MIN); - bool reset_altitude = false; - if (_state_update_timestamp == 0 || dt > DT_MAX) { - dt = DT_DEFAULT; - reset_altitude = true; - } - - if (!altitude_lock) { - reset_altitude = true; - } - - if (reset_altitude) { - _states_initialized = false; - } - - _state_update_timestamp = now; - _EAS = equivalent_airspeed; - - // Set the velocity and position state to the the INS data - _vert_vel_state = -vz; - _vert_pos_state = altitude; - - // Update and average speed rate of change if airspeed is being measured - if (PX4_ISFINITE(equivalent_airspeed) && airspeed_sensor_enabled()) { - _tas_rate_raw = speed_deriv_forward; - // Apply some noise filtering - _TAS_rate_filter.update(speed_deriv_forward); - _tas_rate_filtered = _TAS_rate_filter.getState(); - - } else { - _tas_rate_raw = 0.0f; - _tas_rate_filtered = 0.0f; - } + _airspeed_state.speed = equivalent_airspeed; + _airspeed_state.speed_rate = 0.0f; + _airspeed_rate_filter.reset(0.0f); } -void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equivalent_airspeed, float EAS2TAS) +void TECSAirspeedFilter::update(const float dt, const Input &input, const Param ¶m, + const bool airspeed_sensor_available) { - // Calculate the time in seconds since the last update and use the default time step value if out of bounds - uint64_t now = hrt_absolute_time(); - const float dt = constrain((now - _speed_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX); + // Input checking + if (!(PX4_ISFINITE(dt) && dt > FLT_EPSILON)) { + // Do not update the states. + PX4_WARN("Time intervall is not valid."); + return; + } - // Convert equivalent airspeed quantities to true airspeed - _EAS_setpoint = PX4_ISFINITE(equivalent_airspeed_setpoint) ? equivalent_airspeed_setpoint : _equivalent_airspeed_trim; - _TAS_setpoint = _EAS_setpoint * EAS2TAS; - _TAS_max = _equivalent_airspeed_max * EAS2TAS; - _TAS_min = _equivalent_airspeed_min * EAS2TAS; + float airspeed; - // If airspeed measurements are not being used, fix the airspeed estimate to the nominal trim airspeed - if (!PX4_ISFINITE(equivalent_airspeed) || !airspeed_sensor_enabled()) { - _EAS = _equivalent_airspeed_trim; + if (PX4_ISFINITE(input.equivalent_airspeed) && airspeed_sensor_available) { + airspeed = input.equivalent_airspeed; } else { - _EAS = equivalent_airspeed; + airspeed = param.equivalent_airspeed_trim; } - // If first time through reset airspeed states - if (_speed_update_timestamp == 0) { - _tas_rate_state = 0.0f; - _tas_state = (_EAS * EAS2TAS); + float airspeed_derivative; + + if (PX4_ISFINITE(input.equivalent_airspeed_rate) && airspeed_sensor_available) { + airspeed_derivative = input.equivalent_airspeed_rate; + + } else { + airspeed_derivative = 0.0f; } - // Obtain a smoothed TAS estimate using a second order complementary filter + // 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 - _tas_innov = (_EAS * EAS2TAS) - _tas_state; - float tas_rate_state_input = _tas_innov * _tas_estimate_freq * _tas_estimate_freq; + 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 - _tas_rate_state = _tas_rate_state + tas_rate_state_input * dt; - float tas_state_input = _tas_rate_state + _tas_rate_raw + _tas_innov * _tas_estimate_freq * 1.4142f; - const float new_tas_state = _tas_state + tas_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; - if (new_tas_state < 0.0f) { - // clip TAS at zero, back calculate rate - tas_state_input = -_tas_state / dt; - _tas_rate_state = tas_state_input - _tas_rate_raw - _tas_innov * _tas_estimate_freq * 1.4142f; - _tas_state = 0.0f; - - } else { - _tas_state = new_tas_state; + // 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; } - _speed_update_timestamp = now; - + // Update states + _airspeed_state = new_airspeed_state; } -void TECS::_update_speed_setpoint() +TECSAirspeedFilter::AirspeedFilterState TECSAirspeedFilter::getState() const { - // 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) { - _TAS_setpoint = _TAS_min; + AirspeedFilterState filter_state{ + .speed = _airspeed_state.speed, + .speed_rate = _airspeed_rate_filter.getState() + }; - } 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. - _TAS_setpoint = _TAS_min * _percent_undersped + (1.0f - _percent_undersped) * _TAS_setpoint; + 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; } - _TAS_setpoint = constrain(_TAS_setpoint, _TAS_min, _TAS_max); + if (!PX4_ISFINITE(altitude)) { + altitude = 0.0f; + } - // 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 * _STE_rate_max / math::max(_tas_state, FLT_EPSILON); - const float min_tas_rate_sp = 0.5f * _STE_rate_min / math::max(_tas_state, FLT_EPSILON); + // TODO rearrange handling of altitude rate and altitude. alt_rate should rather be a feedforward term. + float virtual_altitude_setpoint{setpoint.alt}; - _TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max); + // 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 (airspeed_sensor_enabled()) { - _TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, min_tas_rate_sp, - max_tas_rate_sp); - - } else { - _TAS_rate_setpoint = 0.0f; + 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; } -void TECS::runAltitudeControllerSmoothVelocity(float alt_sp_amsl_m, float target_climbrate_m_s, - float target_sinkrate_m_s, - float alt_amsl) +float TECSControl::_altitudeControl(const Setpoint &setpoint, const Input &input, const Param ¶m) const { - target_climbrate_m_s = math::min(target_climbrate_m_s, _max_climb_rate); - target_sinkrate_m_s = math::min(target_sinkrate_m_s, _max_sink_rate); + 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); - const float delta_trajectory_to_target_m = alt_sp_amsl_m - _alt_control_traj_generator.getCurrentPosition(); - - float height_rate_target = math::signNoZero(delta_trajectory_to_target_m) * - math::trajectory::computeMaxSpeedFromDistance( - _jerk_max, _vert_accel_limit, fabsf(delta_trajectory_to_target_m), 0.0f); - - height_rate_target = math::constrain(height_rate_target, -target_sinkrate_m_s, target_climbrate_m_s); - - _alt_control_traj_generator.updateDurations(height_rate_target); - _alt_control_traj_generator.updateTraj(_dt); - - _hgt_setpoint = _alt_control_traj_generator.getCurrentPosition(); - _hgt_rate_from_hgt_ref = _alt_control_traj_generator.getCurrentVelocity(); - _hgt_rate_setpoint = (_hgt_setpoint - alt_amsl) * _height_error_gain + _height_setpoint_gain_ff * - _alt_control_traj_generator.getCurrentVelocity(); - _hgt_rate_setpoint = math::constrain(_hgt_rate_setpoint, -_max_sink_rate, _max_climb_rate); + return altitude_rate_output; } -void TECS::_detect_underspeed() +TECSControl::SpecificEnergy TECSControl::_updateEnergyBalance(const AltitudePitchControl &control_setpoint, + const Input &input) const { - if (!_detect_underspeed_enabled) { + 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 = kTASErrorPercentage * _equivalent_airspeed_trim; + 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 = kTASErrorPercentage * _equivalent_airspeed_trim; + const float tas_underspeed_soft_bound = param.tas_error_percentage * param.equivalent_airspeed_trim; - const float tas_fully_undersped = math::max(_TAS_min - tas_error_bound - tas_underspeed_soft_bound, 0.0f); - const float tas_starting_to_underspeed = math::max(_TAS_min - tas_error_bound, tas_fully_undersped); + 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((_tas_state - 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); } -void TECS::_update_energy_estimates() +TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Param ¶m, const Flag &flag) { - // Calculate specific energy demands in units of (m**2/sec**2) - _SPE_setpoint = _hgt_setpoint * CONSTANTS_ONE_G; // potential energy - _SKE_setpoint = 0.5f * _TAS_setpoint_adj * _TAS_setpoint_adj; // kinetic energy - // Calculate total energy error - _STE_error = _SPE_setpoint - _SPE_estimate + _SKE_setpoint - _SKE_estimate; + 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); - // Calculate the specific energy balance demand which specifies how the available total - // energy should be allocated to speed (kinetic energy) and height (potential energy) - // Calculate the specific energy balance error - _SEB_error = SEB_setpoint() - (_SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting); + if (flag.climbout_mode_active && flag.airspeed_enabled) { + pitch_speed_weight = 2.0f; - // Calculate specific energy rate demands in units of (m**2/sec**3) - _SPE_rate_setpoint = _hgt_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change - _SKE_rate_setpoint = _tas_state * _TAS_rate_setpoint; // kinetic energy rate of change + } else if (_percent_undersped > FLT_EPSILON && flag.airspeed_enabled) { + pitch_speed_weight = 2.0f * _percent_undersped + (1.0f - _percent_undersped) * pitch_speed_weight; - // Calculate specific energies in units of (m**2/sec**2) - _SPE_estimate = _vert_pos_state * CONSTANTS_ONE_G; // potential energy - _SKE_estimate = 0.5f * _tas_state * _tas_state; // kinetic energy + } else if (!flag.airspeed_enabled) { + pitch_speed_weight = 0.0f; - // Calculate specific energy rates in units of (m**2/sec**3) - _SPE_rate = _vert_vel_state * CONSTANTS_ONE_G; // potential energy rate of change - _SKE_rate = _tas_state * _tas_rate_filtered;// kinetic energy rate of change + } + + // 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 TECS::_update_throttle_setpoint() +void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const SpecificEnergy &se, Param ¶m, + const Flag &flag) { - // Calculate demanded rate of change of total energy, respecting vehicle limits. - // We will constrain the value below. - _STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint; + 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 total energy rate error, applying a first order IIR filter - // to reduce the effect of accelerometer noise - _STE_rate_error_filter.update(-_SPE_rate - _SKE_rate + _SPE_rate_setpoint + _SKE_rate_setpoint); - _STE_rate_error = _STE_rate_error_filter.getState(); + // 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 += _load_factor_correction * (_load_factor - 1.f); + STE_rate_setpoint += param.load_factor_correction * (param.load_factor - 1.f); - _STE_rate_setpoint = constrain(_STE_rate_setpoint, _STE_rate_min, _STE_rate_max); + 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: @@ -285,31 +471,33 @@ void TECS::_update_throttle_setpoint() // 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 >= 0) { // throttle is between trim and maximum - throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max - _throttle_trim); + 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 = _throttle_trim + _STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min - _throttle_trim); + 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 / (_STE_rate_max - _STE_rate_min); + 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 * _throttle_damping_gain) * STE_rate_to_throttle + throttle_predicted; - throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max); + 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 (airspeed_sensor_enabled()) { - if (_integrator_gain_throttle > 0.0f) { - float integ_state_max = _throttle_setpoint_max - throttle_setpoint; - float integ_state_min = _throttle_setpoint_min - throttle_setpoint; + 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 * _integrator_gain_throttle) * _dt * + 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 @@ -324,7 +512,7 @@ void TECS::_update_throttle_setpoint() // 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 (_climbout_mode_active) { + 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; @@ -336,7 +524,7 @@ void TECS::_update_throttle_setpoint() } - if (airspeed_sensor_enabled()) { + if (flag.airspeed_enabled) { // Add the integrator feedback during closed loop operation with an airspeed sensor throttle_setpoint += _throttle_integ_state; @@ -347,38 +535,81 @@ void TECS::_update_throttle_setpoint() } // ramp in max throttle setting with underspeediness value - throttle_setpoint = _percent_undersped * _throttle_setpoint_max + (1.0f - _percent_undersped) * throttle_setpoint; + throttle_setpoint = _percent_undersped * param.throttle_max + (1.0f - _percent_undersped) * throttle_setpoint; // Rate limit the throttle demand - if (fabsf(_throttle_slewrate) > 0.01f) { - const float throttle_increment_limit = _dt * (_throttle_setpoint_max - _throttle_setpoint_min) * _throttle_slewrate; - throttle_setpoint = constrain(throttle_setpoint, _last_throttle_setpoint - throttle_increment_limit, - _last_throttle_setpoint + throttle_increment_limit); + 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); } - _last_throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max); + _throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max); } -void TECS::_detect_uncommanded_descent() +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 height + * aircraft can achieve in level flight. When this occurs, the vehicle will continue to reduce altitude * while attempting to maintain speed. */ - // Calculate rate of change of total specific energy - const float STE_rate = _SPE_rate + _SKE_rate; + // 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 - const bool underspeed_detected = _percent_undersped > FLT_EPSILON; + // 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) - && (STE_rate < 0.0f) - && (_last_throttle_setpoint >= _throttle_setpoint_max * 0.9f); + 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)); + const bool exit_mode = _uncommanded_descent_recovery && (underspeed_detected || (STE_error < 0.0f)); if (enter_mode) { _uncommanded_descent_recovery = true; @@ -389,243 +620,108 @@ void TECS::_detect_uncommanded_descent() } } -void TECS::_update_pitch_setpoint() +void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed) { - /* - * The SKE_weighting variable controls how speed and height control are prioritised by the pitch demand calculation. - * A weighting of 1 givea equal speed and height priority - * A weighting of 0 gives 100% priority to height 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 height 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 height accuracy requirements. - */ - - // Calculate the specific energy balance rate demand - const float SEB_rate_setpoint = _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting; - - // Calculate the specific energy balance rate error - _SEB_rate_error = SEB_rate_setpoint - (_SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting); - - // Calculate derivative from change in climb angle to rate of change of specific energy balance - const float climb_angle_to_SEB_rate = _tas_state * CONSTANTS_ONE_G; - - if (_integrator_gain_pitch > 0.0f) { - // Calculate pitch integrator input term - float pitch_integ_input = _SEB_rate_error * _integrator_gain_pitch; - - // Prevent the integrator changing in a direction that will increase pitch demand saturation - if (_pitch_setpoint_unc > _pitch_setpoint_max) { - pitch_integ_input = min(pitch_integ_input, 0.f); - - } else if (_pitch_setpoint_unc < _pitch_setpoint_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 * _pitch_damping_gain + _pitch_integ_state + _SEB_rate_ff * - SEB_rate_setpoint; - - // 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 (_climbout_mode_active) { - SEB_rate_correction += _pitch_setpoint_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. - _pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate; - - float pitch_setpoint = constrain(_pitch_setpoint_unc, _pitch_setpoint_min, _pitch_setpoint_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 * _vert_accel_limit / _tas_state; - _last_pitch_setpoint = constrain(pitch_setpoint, _last_pitch_setpoint - pitch_increment, - _last_pitch_setpoint + pitch_increment); + // 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::_updateTrajectoryGenerationConstraints() -{ - _alt_control_traj_generator.setMaxJerk(_jerk_max); - _alt_control_traj_generator.setMaxAccel(_vert_accel_limit); - _alt_control_traj_generator.setMaxVel(fmax(_max_climb_rate, _max_sink_rate)); - - _velocity_control_traj_generator.setMaxJerk(_jerk_max); - _velocity_control_traj_generator.setMaxAccelUp(_vert_accel_limit); - _velocity_control_traj_generator.setMaxAccelDown(_vert_accel_limit); - _velocity_control_traj_generator.setMaxVelUp(_max_sink_rate); // different convention for FW than for MC - _velocity_control_traj_generator.setMaxVelDown(_max_climb_rate); // different convention for FW than for MC -} - -void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate, - float target_sinkrate, float altitude_amsl) -{ - bool control_altitude = true; - const bool input_is_height_rate = PX4_ISFINITE(height_rate_sp); - - _velocity_control_traj_generator.setVelSpFeedback(_hgt_rate_setpoint); - - if (input_is_height_rate) { - _velocity_control_traj_generator.setCurrentPositionEstimate(altitude_amsl); - _velocity_control_traj_generator.update(_dt, height_rate_sp); - _hgt_rate_setpoint = _velocity_control_traj_generator.getCurrentVelocity(); - _smooth_hgt_rate_setpoint = _hgt_rate_setpoint; - altitude_sp_amsl = _velocity_control_traj_generator.getCurrentPosition(); - control_altitude = PX4_ISFINITE(altitude_sp_amsl); - - } else { - _velocity_control_traj_generator.reset(0, _hgt_rate_setpoint, _hgt_setpoint); - } - - - if (control_altitude) { - runAltitudeControllerSmoothVelocity(altitude_sp_amsl, target_climbrate, target_sinkrate, altitude_amsl); - - } else { - _alt_control_traj_generator.setCurrentVelocity(_hgt_rate_setpoint); - _alt_control_traj_generator.setCurrentPosition(altitude_amsl); - _hgt_setpoint = altitude_amsl; - } -} - -void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altitude, float pitch_min_climbout, - float EAS2TAS) -{ - if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_states_initialized) { - // On first time through or when not using TECS of if there has been a large time slip, - // states must be reset to allow filters to a clean start - _vert_vel_state = 0.0f; - _vert_pos_state = baro_altitude; - _tas_rate_state = 0.0f; - _tas_state = _EAS * EAS2TAS; - _last_throttle_setpoint = throttle_trim; - _last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max); - _pitch_setpoint_unc = _last_pitch_setpoint; - _TAS_setpoint_last = _EAS * EAS2TAS; - _TAS_setpoint_adj = _TAS_setpoint_last; - _uncommanded_descent_recovery = false; - _STE_rate_error = 0.0f; - _hgt_setpoint = baro_altitude; - - resetIntegrals(); - - if (_dt > DT_MAX || _dt < DT_MIN) { - _dt = DT_DEFAULT; - } - - resetTrajectoryGenerators(baro_altitude); - - } else if (_climbout_mode_active) { - // During climbout use the lower pitch angle limit specified by the - // calling controller - _pitch_setpoint_min = pitch_min_climbout; - - // throttle lower limit is set to a value that prevents throttle reduction - _throttle_setpoint_min = _throttle_setpoint_max - 0.01f; - - // airspeed demand states are set to track the measured airspeed - _TAS_setpoint_last = _EAS * EAS2TAS; - _TAS_setpoint_adj = _EAS * EAS2TAS; - - _hgt_setpoint = baro_altitude; - - _uncommanded_descent_recovery = false; - } - - // filter specific energy rate error using first order filter with 0.5 second time constant - _STE_rate_error_filter.setParameters(DT_DEFAULT, _STE_rate_time_const); - _STE_rate_error_filter.reset(0.0f); - - // filter true airspeed rate using first order filter with 0.5 second time constant - _TAS_rate_filter.setParameters(DT_DEFAULT, _speed_derivative_time_const); - _TAS_rate_filter.reset(0.0f); - - _states_initialized = true; -} - -void TECS::_update_STE_rate_lim() -{ - // Calculate the specific total energy upper rate limits from the max throttle climb rate - _STE_rate_max = math::max(_max_climb_rate, FLT_EPSILON) * CONSTANTS_ONE_G; - - // Calculate the specific total energy lower rate limits from the min throttle sink rate - _STE_rate_min = - math::max(_min_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G; -} - -void TECS::update_pitch_throttle(float pitch, float baro_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_max, float throttle_trim, - float pitch_limit_min, float pitch_limit_max, - float target_climbrate, float target_sinkrate, float hgt_rate_sp) +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(); - _dt = fmaxf((now - _pitch_update_timestamp) * 1e-6f, DT_MIN); + float dt = (now - _update_timestamp) * 1e-6f; - // Set class variables from inputs - _throttle_setpoint_max = throttle_max; - _throttle_setpoint_min = throttle_min; - _pitch_setpoint_max = pitch_limit_max; - _pitch_setpoint_min = pitch_limit_min; - _climbout_mode_active = climb_out_setpoint; - _throttle_trim = throttle_trim; + if (dt < DT_MIN) { + // Update intervall too small, do not update. Assume constant states/output in this case. + return; + } - // Initialize selected states and variables as required - _initialize_states(pitch, throttle_trim, baro_altitude, pitch_min_climbout, eas_to_tas); + 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); - _updateTrajectoryGenerationConstraints(); + } 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 the true airspeed state estimate - _update_speed_states(EAS_setpoint, equivalent_airspeed, eas_to_tas); + // 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 rate limits for specific total energy - _update_STE_rate_lim(); + // 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); - // Detect an underspeed condition - _detect_underspeed(); + 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); - _update_speed_height_weights(); + // 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); - // Detect an uncommanded descent caused by an unachievable airspeed demand - _detect_uncommanded_descent(); + 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; + } - // Calculate the demanded true airspeed - _update_speed_setpoint(); - _calculateHeightRateSetpoint(hgt_setpoint, hgt_rate_sp, target_climbrate, target_sinkrate, baro_altitude); - - // Calculate the specific energy values required by the control loop - _update_energy_estimates(); - - // Calculate the throttle demand - _update_throttle_setpoint(); - - // Calculate the pitch demand - _update_pitch_setpoint(); // Update time stamps - _pitch_update_timestamp = now; + _update_timestamp = now; + // Set TECS mode for next frame - if (_percent_undersped > FLT_EPSILON) { + 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 (_climbout_mode_active) { + } else if (climb_out_setpoint) { _tecs_mode = ECL_TECS_MODE_CLIMBOUT; } else { @@ -633,26 +729,6 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set _tecs_mode = ECL_TECS_MODE_NORMAL; } + _debug_status.tecs_mode = _tecs_mode; } -void TECS::_update_speed_height_weights() -{ - // Calculate the weight applied to control of specific kinetic energy error - float pitch_speed_weight = constrain(_pitch_speed_weight, 0.0f, 2.0f); - - if (_climbout_mode_active && airspeed_sensor_enabled()) { - pitch_speed_weight = 2.0f; - - } else if (_percent_undersped > FLT_EPSILON && airspeed_sensor_enabled()) { - pitch_speed_weight = 2.0f * _percent_undersped + (1.0f - _percent_undersped) * pitch_speed_weight; - - } else if (!airspeed_sensor_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 - _SPE_weighting = constrain(2.0f - pitch_speed_weight, 0.f, 1.f); - _SKE_weighting = constrain(pitch_speed_weight, 0.f, 1.f); -} diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 3a07c0e31f..cc159f4749 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -49,8 +49,431 @@ #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: + enum ECL_TECS_MODE { + ECL_TECS_MODE_NORMAL = 0, + ECL_TECS_MODE_UNDERSPEED, + ECL_TECS_MODE_BAD_DESCENT, + ECL_TECS_MODE_CLIMBOUT + }; + + struct DebugOutput : TECSControl::DebugOutput { + float true_airspeed_filtered; + float true_airspeed_derivative; + float altitude_sp; + float altitude_rate; + float altitude_rate_setpoint; + enum ECL_TECS_MODE tecs_mode; + }; public: TECS() = default; ~TECS() = default; @@ -61,6 +484,8 @@ public: TECS(TECS &&) = delete; TECS &operator=(TECS &&) = delete; + DebugOutput getStatus() const {return _debug_status;}; + /** * Get the current airspeed status * @@ -74,138 +499,58 @@ public: void enable_airspeed(bool enabled) { _airspeed_enabled = enabled; } /** - * Updates the following vehicle kineamtic state estimates: - * Vertical position, velocity and acceleration. - * Speed derivative - * Must be called prior to udating tecs control loops - * Must be called at 50Hz or greater + * @brief Update the control loop calculations + * */ - void update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward, bool altitude_lock, - float altitude, float vz); + 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); /** - * Update the control loop calculations + * @brief Initialize the control loop + * */ - void update_pitch_throttle(float pitch, float baro_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, float hgt_rate_sp = NAN); - - float get_throttle_setpoint() { return _last_throttle_setpoint; } - float get_pitch_setpoint() { return _last_pitch_setpoint; } - float get_speed_weight() { return _pitch_speed_weight; } - float get_throttle_trim() { return _throttle_trim; } - - void reset_state() { _states_initialized = false; } + void initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed); void resetIntegrals() { - _throttle_integ_state = 0.0f; - _pitch_integ_state = 0.0f; + _control.resetIntegrals(); } - /** - * @brief Resets the altitude and height rate control trajectory generators to the input altitude - * - * @param altitude Vehicle altitude (AMSL) [m] - */ - void resetTrajectoryGenerators(const float altitude) - { - _alt_control_traj_generator.reset(0, 0, altitude); - _velocity_control_traj_generator.reset(0.0f, 0.0f, altitude); - } + void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; }; - enum ECL_TECS_MODE { - ECL_TECS_MODE_NORMAL = 0, - ECL_TECS_MODE_UNDERSPEED, - ECL_TECS_MODE_BAD_DESCENT, - ECL_TECS_MODE_CLIMBOUT - }; + // // 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_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; } + 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; }; - // setters for controller parameters - void set_integrator_gain_throttle(float gain) { _integrator_gain_throttle = gain; } - void set_integrator_gain_pitch(float gain) { _integrator_gain_pitch = gain; } - - void set_min_sink_rate(float rate) { _min_sink_rate = rate; } - void set_max_sink_rate(float sink_rate) { _max_sink_rate = sink_rate; } - void set_max_climb_rate(float climb_rate) { _max_climb_rate = climb_rate; } - - void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; } - void set_height_error_time_constant(float time_const) { _height_error_gain = 1.0f / math::max(time_const, 0.1f); } + void set_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) { _pitch_damping_gain = damping; } - void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; } + 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) { _tas_estimate_freq = omega; } - void set_speed_weight(float weight) { _pitch_speed_weight = weight; } - void set_airspeed_error_time_constant(float time_const) { _airspeed_error_gain = 1.0f / math::max(time_const, 0.1f); } + 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) { _throttle_damping_gain = throttle_damp; } - void set_throttle_slewrate(float slewrate) { _throttle_slewrate = slewrate; } + 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) { _load_factor_correction = compensation; } - void set_load_factor(float load_factor) { _load_factor = load_factor; } + 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_ste_rate_time_const(float time_const) { _STE_rate_time_const = time_const; } - void set_speed_derivative_time_constant(float time_const) { _speed_derivative_time_const = time_const; } - - void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; } - - // TECS status - uint64_t timestamp() { return _pitch_update_timestamp; } - ECL_TECS_MODE tecs_mode() { return _tecs_mode; } - - float hgt_setpoint() { return _hgt_setpoint; } - float hgt_rate_from_hgt_setpoint() { return _hgt_rate_from_hgt_ref;}; - float smooth_hgt_rate_setpoint() {return _smooth_hgt_rate_setpoint;}; - float vert_pos_state() { return _vert_pos_state; } - - float TAS_setpoint_adj() { return _TAS_setpoint_adj; } - float tas_state() { return _tas_state; } - float getTASInnovation() { return _tas_innov; } - - float hgt_rate_setpoint() { return _hgt_rate_setpoint; } - float vert_vel_state() { return _vert_vel_state; } - - float get_EAS_setpoint() { return _EAS_setpoint; }; - float TAS_rate_setpoint() { return _TAS_rate_setpoint; } - float speed_derivative() { return _tas_rate_filtered; } - float speed_derivative_raw() { return _tas_rate_raw; } - - float STE_error() { return _STE_error; } - float STE_rate_error() { return _STE_rate_error; } - - float SEB_error() { return _SEB_error; } - float SEB_rate_error() { return _SEB_rate_error; } - - float SPE_rate() {return _SPE_rate;}; - float SKE_rate() {return _SKE_rate;}; - - float throttle_integ_state() { return _throttle_integ_state; } - float pitch_integ_state() { return _pitch_integ_state; } - - float STE() { return _SPE_estimate + _SKE_estimate; } - - float STE_setpoint() { return _SPE_setpoint + _SKE_setpoint; } - - float STE_rate() { return _SPE_rate + _SKE_rate; } - - float STE_rate_setpoint() { return _STE_rate_setpoint; } - - float SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; } - - float SEB_setpoint() { return _SPE_setpoint * _SPE_weighting - _SKE_setpoint * _SKE_weighting; } - - float SEB_rate() { return _SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting; } - - float SEB_rate_setpoint() { return _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting; } + 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; }; /** * Handle the altitude reset @@ -213,194 +558,104 @@ public: * If the estimation system resets the height in one discrete step this * will gracefully even out the reset over time. */ - void handle_alt_step(float delta_alt, float altitude) + void handle_alt_step(float altitude, float altitude_rate) { - _hgt_setpoint += delta_alt; + TECSReferenceModel::AltitudeReferenceState init_state{ .alt = altitude, + .alt_rate = altitude_rate}; - // reset height states - _vert_pos_state = altitude; - _vert_vel_state = 0.0f; + // reset altitude reference model. + _reference_model.initialize(init_state); } + 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. - // [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors - static constexpr float kTASErrorPercentage = 0.15; + enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; ///< Current activated mode. - static constexpr float _jerk_max = - 1000.0f; + uint64_t _update_timestamp{0}; ///< last timestamp of the update function call. - enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; - - // timestamps - uint64_t _state_update_timestamp{0}; ///< last timestamp of the 50 Hz function call - uint64_t _speed_update_timestamp{0}; ///< last timestamp of the speed function call - uint64_t _pitch_update_timestamp{0}; ///< last timestamp of the pitch function call - - // controller parameters - float _tas_estimate_freq{0.0f}; ///< cross-over frequency of the true airspeed complementary filter (rad/sec) - float _max_climb_rate{2.0f}; ///< climb rate produced by max allowed throttle (m/sec) - float _min_sink_rate{1.0f}; ///< sink rate produced by min allowed throttle (m/sec) - float _max_sink_rate{2.0f}; ///< maximum safe sink rate (m/sec) - float _pitch_damping_gain{0.0f}; ///< damping gain of the pitch demand calculation (sec) - float _throttle_damping_gain{0.0f}; ///< damping gain of the throttle demand calculation (sec) - float _integrator_gain_throttle{0.0f}; ///< integrator gain used by the throttle demand calculation - float _integrator_gain_pitch{0.0f}; ///< integrator gain used by the pitch demand calculation - float _vert_accel_limit{0.0f}; ///< magnitude of the maximum vertical acceleration allowed (m/sec**2) - float _load_factor{1.0f}; ///< additional normal load factor - float _load_factor_correction{0.0f}; ///< gain from normal load factor increase to total energy rate demand (m**2/sec**3) - float _pitch_speed_weight{1.0f}; ///< speed control weighting used by pitch demand calculation - float _height_error_gain{0.2f}; ///< height error inverse time constant [1/s] - float _height_setpoint_gain_ff{0.0f}; ///< gain from height demand derivative to demanded climb rate - float _airspeed_error_gain{0.1f}; ///< airspeed error inverse time constant [1/s] float _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) - float _throttle_slewrate{0.0f}; ///< throttle demand slew rate limit (1/sec) - float _STE_rate_time_const{0.1f}; ///< filter time constant for specific total energy rate (damping path) (s) - float _speed_derivative_time_const{0.01f}; ///< speed derivative filter time constant (s) - float _SEB_rate_ff{1.0f}; - - // complimentary filter states - float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec) - float _vert_pos_state{0.0f}; ///< complimentary filter state - height (m) - float _tas_rate_state{0.0f}; ///< complimentary filter state - true airspeed first derivative (m/sec**2) - float _tas_state{0.0f}; ///< complimentary filter state - true airspeed (m/sec) - float _tas_innov{0.0f}; ///< complimentary filter true airspeed innovation (m/sec) - - // controller states - float _throttle_integ_state{0.0f}; ///< throttle integrator state - float _pitch_integ_state{0.0f}; ///< pitch integrator state (rad) - float _last_throttle_setpoint{0.0f}; ///< throttle demand rate limiter state (1/sec) - float _last_pitch_setpoint{0.0f}; ///< pitch demand rate limiter state (rad/sec) - float _tas_rate_filtered{0.0f}; ///< low pass filtered rate of change of speed along X axis (m/sec**2) - - // speed demand calculations - float _EAS{0.0f}; ///< equivalent airspeed (m/sec) - float _TAS_max{30.0f}; ///< true airpeed demand upper limit (m/sec) - float _TAS_min{3.0f}; ///< true airpeed demand lower limit (m/sec) - float _TAS_setpoint{0.0f}; ///< current airpeed demand (m/sec) - float _TAS_setpoint_last{0.0f}; ///< previous true airpeed demand (m/sec) - float _EAS_setpoint{0.0f}; ///< Equivalent airspeed demand (m/sec) - float _TAS_setpoint_adj{0.0f}; ///< true airspeed demand tracked by the TECS algorithm (m/sec) - float _TAS_rate_setpoint{0.0f}; ///< true airspeed rate demand tracked by the TECS algorithm (m/sec**2) - float _tas_rate_raw{0.0f}; ///< true airspeed rate, calculated as inertial acceleration in body X direction - - // height demand calculations - float _hgt_setpoint{0.0f}; ///< demanded height tracked by the TECS algorithm (m) - float _hgt_rate_from_hgt_ref{0.0f}; - float _smooth_hgt_rate_setpoint{0.0f}; - float _hgt_rate_setpoint{0.0f}; ///< demanded climb rate tracked by the TECS algorithm - - // vehicle physical limits - float _pitch_setpoint_unc{0.0f}; ///< pitch demand before limiting (rad) - float _STE_rate_max{FLT_EPSILON}; ///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3) - float _STE_rate_min{-FLT_EPSILON}; ///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3) - float _throttle_setpoint_max{0.0f}; ///< normalised throttle upper limit - float _throttle_setpoint_min{0.0f}; ///< normalised throttle lower limit - float _throttle_trim{0.0f}; ///< throttle required to fly level at _EAS_setpoint, compensated for air density and vehicle weight - float _pitch_setpoint_max{0.5f}; ///< pitch demand upper limit (rad) - float _pitch_setpoint_min{-0.5f}; ///< pitch demand lower limit (rad) - - // specific energy quantities - float _SPE_setpoint{0.0f}; ///< specific potential energy demand (m**2/sec**2) - float _SKE_setpoint{0.0f}; ///< specific kinetic energy demand (m**2/sec**2) - float _SPE_rate_setpoint{0.0f}; ///< specific potential energy rate demand (m**2/sec**3) - float _SKE_rate_setpoint{0.0f}; ///< specific kinetic energy rate demand (m**2/sec**3) - float _STE_rate_setpoint{0.0f}; ///< specific total energy rate demand (m**s/sec**3) - float _SPE_estimate{0.0f}; ///< specific potential energy estimate (m**2/sec**2) - float _SKE_estimate{0.0f}; ///< specific kinetic energy estimate (m**2/sec**2) - float _SPE_rate{0.0f}; ///< specific potential energy rate estimate (m**2/sec**3) - float _SKE_rate{0.0f}; ///< specific kinetic energy rate estimate (m**2/sec**3) - - // specific energy error quantities - float _STE_error{0.0f}; ///< specific total energy error (m**2/sec**2) - float _STE_rate_error{0.0f}; ///< specific total energy rate error (m**2/sec**3) - float _SEB_error{0.0f}; ///< specific energy balance error (m**2/sec**2) - float _SEB_rate_error{0.0f}; ///< specific energy balance rate error (m**2/sec**3) - - // speed height weighting - float _SPE_weighting{1.0f}; - float _SKE_weighting{1.0f}; - - // time steps (non-fixed) - float _dt{DT_DEFAULT}; ///< Time since last update of main TECS loop (sec) - static constexpr float DT_DEFAULT = 0.02f; ///< default value for _dt (sec) + float _equivalent_airspeed_trim{15.0f}; ///< equivalent cruise airspeed for airspeed less mode (m/sec) // controller mode logic - float _percent_undersped{0.0f}; ///< a continuous representation of how "undersped" the TAS is [0,1] - bool _detect_underspeed_enabled{true}; ///< true when underspeed detection is enabled bool _uncommanded_descent_recovery{false}; ///< true when a continuous descent caused by an unachievable airspeed demand has been detected - bool _climbout_mode_active{false}; ///< true when in climbout mode bool _airspeed_enabled{false}; ///< true when airspeed use has been enabled - bool _states_initialized{false}; ///< true when TECS states have been iniitalized + bool _detect_underspeed_enabled{false}; ///< true when underspeed detection is enabled - /** - * Update the airspeed internal state using a second order complementary filter - */ - void _update_speed_states(float airspeed_setpoint, float equivalent_airspeed, float cas_to_tas); + 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 */ - void _update_speed_setpoint(); - - /** - * Calculate desired height rate from altitude demand - */ - void runAltitudeControllerSmoothVelocity(float alt_sp_amsl_m, float target_climbrate_m_s, float target_sinkrate_m_s, - float alt_amsl); - - /** - * Detect how undersped the aircraft is - */ - void _detect_underspeed(); - - /** - * Update specific energy - */ - void _update_energy_estimates(); - - /** - * Update throttle setpoint - */ - void _update_throttle_setpoint(); + 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(); - - /** - * Update the pitch setpoint - */ - void _update_pitch_setpoint(); - - void _updateTrajectoryGenerationConstraints(); - - void _calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate, - float target_sinkrate, float altitude_amsl); - - /** - * Initialize the controller - */ - void _initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout, - float eas_to_tas); - - /** - * Calculate specific total energy rate limits - */ - void _update_STE_rate_lim(); - - void _update_speed_height_weights(); - - AlphaFilter _STE_rate_error_filter; - - AlphaFilter _TAS_rate_filter; - - VelocitySmoothing - _alt_control_traj_generator; // generates height rate and altitude setpoint trajectory when altitude is commanded - ManualVelocitySmoothingZ - _velocity_control_traj_generator; // generates height rate and altitude setpoint trajectory when height rate is commanded - + void _detect_uncommanded_descent(float throttle_setpoint_max, float altitude, float altitude_setpoint, float tas, + float tas_setpoint); }; + diff --git a/src/lib/tecs/TECSnew.cpp b/src/lib/tecs/TECSnew.cpp deleted file mode 100644 index d3ac70eccf..0000000000 --- a/src/lib/tecs/TECSnew.cpp +++ /dev/null @@ -1,717 +0,0 @@ -/**************************************************************************** - * - * 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. -namespace newTECS -{ -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 deleted file mode 100644 index 4cda1493bd..0000000000 --- a/src/lib/tecs/TECSnew.hpp +++ /dev/null @@ -1,651 +0,0 @@ -/**************************************************************************** - * - * 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 - -namespace newTECS -{ -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); -}; -} - diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index 80b84d2757..d7deaa9870 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -47,6 +47,5 @@ px4_add_module( runway_takeoff SlewRate tecs - tecsnew motion_planning ) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index a752fecc1e..c23ee3bddf 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -129,7 +129,6 @@ FixedwingPositionControl::parameters_update() _tecs.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get()); _tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); _tecs.set_equivalent_airspeed_max(_param_fw_airspd_max.get()); - _tecs.set_min_sink_rate(_param_fw_t_sink_min.get()); _tecs.set_throttle_damp(_param_fw_t_thr_damp.get()); _tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get()); _tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get()); @@ -138,34 +137,13 @@ FixedwingPositionControl::parameters_update() _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_height_error_time_constant(_param_fw_t_h_error_tc.get()); - _tecs.set_heightrate_ff(_param_fw_t_hrate_ff.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()); - _tecsnew.set_max_climb_rate(_param_fw_t_clmb_max.get()); - _tecsnew.set_max_sink_rate(_param_fw_t_sink_max.get()); - _tecsnew.set_speed_weight(_param_fw_t_spdweight.get()); - _tecsnew.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get()); - _tecsnew.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); - _tecsnew.set_equivalent_airspeed_max(_param_fw_airspd_max.get()); - _tecsnew.set_throttle_damp(_param_fw_t_thr_damp.get()); - _tecsnew.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get()); - _tecsnew.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get()); - _tecsnew.set_throttle_slewrate(_param_fw_thr_slew_max.get()); - _tecsnew.set_vertical_accel_limit(_param_fw_t_vert_acc.get()); - _tecsnew.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get()); - _tecsnew.set_roll_throttle_compensation(_param_fw_t_rll2thr.get()); - _tecsnew.set_pitch_damping(_param_fw_t_ptch_damp.get()); - _tecsnew.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get()); - _tecsnew.set_altitude_rate_ff(_param_fw_t_hrate_ff.get()); - _tecsnew.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get()); - _tecsnew.set_ste_rate_time_const(_param_ste_rate_time_const.get()); - _tecsnew.set_speed_derivative_time_constant(_param_tas_rate_time_const.get()); - _tecsnew.set_seb_rate_ff_gain(_param_seb_rate_ff.get()); - int check_ret = PX4_OK; // sanity check parameters @@ -306,7 +284,6 @@ FixedwingPositionControl::airspeed_poll() // update TECS if validity changed if (airspeed_valid != _airspeed_valid) { _tecs.enable_airspeed(airspeed_valid); - _tecsnew.enable_airspeed(airspeed_valid); _airspeed_valid = airspeed_valid; } } @@ -391,7 +368,6 @@ FixedwingPositionControl::vehicle_attitude_poll() // load factor due to banking const float load_factor = 1.f / cosf(euler_angles(0)); _tecs.set_load_factor(load_factor); - _tecsnew.set_load_factor(load_factor); } } @@ -485,6 +461,8 @@ FixedwingPositionControl::tecs_status_publish() { tecs_status_s t{}; + TECS::DebugOutput debug_output{_tecs.getStatus()}; + switch (_tecs.tecs_mode()) { case TECS::ECL_TECS_MODE_NORMAL: t.mode = tecs_status_s::TECS_MODE_NORMAL; @@ -503,43 +481,25 @@ FixedwingPositionControl::tecs_status_publish() break; } - t.altitude_sp = _tecs.hgt_setpoint(); - t.altitude_filtered = _tecs.vert_pos_state(); + t.altitude_filtered = debug_output.altitude_sp; - t.true_airspeed_sp = _tecs.TAS_setpoint_adj(); - t.true_airspeed_filtered = _tecs.tas_state(); + t.true_airspeed_filtered = debug_output.true_airspeed_filtered; - t.height_rate_setpoint = _tecs.hgt_rate_setpoint(); - t.height_rate = _tecs.vert_vel_state(); + t.height_rate_setpoint = debug_output.altitude_rate_setpoint; + t.height_rate = debug_output.altitude_rate; - t.equivalent_airspeed_sp = _tecs.get_EAS_setpoint(); - t.true_airspeed_derivative_sp = _tecs.TAS_rate_setpoint(); - t.true_airspeed_derivative = _tecs.speed_derivative(); - t.true_airspeed_derivative_raw = _tecs.speed_derivative_raw(); - t.true_airspeed_innovation = _tecs.getTASInnovation(); + t.true_airspeed_derivative_sp = debug_output.true_airspeed_derivative_control; + t.true_airspeed_derivative = debug_output.true_airspeed_derivative; - t.total_energy_error = _tecs.STE_error(); - t.total_energy_rate_error = _tecs.STE_rate_error(); + t.total_energy_rate_error = debug_output.total_energy_rate_error; - t.energy_distribution_error = _tecs.SEB_error(); - t.energy_distribution_rate_error = _tecs.SEB_rate_error(); + t.energy_distribution_rate_error = debug_output.energy_balance_rate_error; - t.total_energy = _tecs.STE(); - t.total_energy_rate = _tecs.STE_rate(); - t.total_energy_balance = _tecs.SEB(); - t.total_energy_balance_rate = _tecs.SEB_rate(); - - t.total_energy_sp = _tecs.STE_setpoint(); - t.total_energy_rate_sp = _tecs.STE_rate_setpoint(); - t.total_energy_balance_sp = _tecs.SEB_setpoint(); - t.total_energy_balance_rate_sp = _tecs.SEB_rate_setpoint(); - - t.throttle_integ = _tecs.throttle_integ_state(); - t.pitch_integ = _tecs.pitch_integ_state(); + t.total_energy_rate_sp = debug_output.total_energy_rate_sp; + t.total_energy_balance_rate_sp = debug_output.energy_balance_rate_sp; t.throttle_sp = _tecs.get_throttle_setpoint(); t.pitch_sp_rad = _tecs.get_pitch_setpoint(); - t.throttle_trim = _tecs.get_throttle_trim(); t.timestamp = hrt_absolute_time(); @@ -866,10 +826,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now) _was_in_air = true; _time_went_in_air = now; - _tecs.resetIntegrals(); - _tecs.resetTrajectoryGenerators(_current_altitude); - - _tecsnew.initialize(_current_altitude, -_local_pos.vz, _airspeed); + _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed); } /* reset flag when airplane landed */ @@ -877,9 +834,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now) _was_in_air = false; _completed_manual_takeoff = false; - _tecs.resetIntegrals(); - _tecs.resetTrajectoryGenerators(_current_altitude); - _tecsnew.initialize(_current_altitude, -_local_pos.vz, _airspeed); + _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed); } } @@ -1124,7 +1079,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co if (pos_sp_curr.gliding_enabled) { /* enable gliding with this waypoint */ _tecs.set_speed_weight(2.0f); - _tecsnew.set_speed_weight(2.0f); tecs_fw_thr_min = 0.0; tecs_fw_thr_max = 0.0; @@ -1227,7 +1181,6 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co if (pos_sp_curr.gliding_enabled) { /* enable gliding with this waypoint */ _tecs.set_speed_weight(2.0f); - _tecsnew.set_speed_weight(2.0f); tecs_fw_thr_min = 0.0; tecs_fw_thr_max = 0.0; @@ -1315,7 +1268,6 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons if (pos_sp_curr.gliding_enabled) { /* enable gliding with this waypoint */ _tecs.set_speed_weight(2.0f); - _tecsnew.set_speed_weight(2.0f); tecs_fw_thr_min = 0.0; tecs_fw_thr_max = 0.0; @@ -1338,8 +1290,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons // We're in a loiter directly before a landing WP. Enable our landing configuration (flaps, // landing airspeed and potentially tighter altitude control) already such that we don't // have to do this switch (which can cause significant altitude errors) close to the ground. - _tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); - _tecsnew.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); + _tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); airspeed_sp = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : _param_fw_airspd_min.get(); _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND; _att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND; @@ -1385,8 +1336,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons _att_sp.roll_body = 0.0f; } - _tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); - _tecsnew.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); + _tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); } tecs_update_pitch_throttle(control_interval, @@ -1522,7 +1472,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo // throttle is open loop anyway during ground roll, no need to wind up the integrator _tecs.resetIntegrals(); - _tecsnew.resetIntegrals(); } tecs_update_pitch_throttle(control_interval, @@ -1538,7 +1487,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_t_clmb_max.get()); _tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation - _tecsnew.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation _att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust()); @@ -1661,8 +1609,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { // Enable tighter altitude control for landings - _tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); - _tecsnew.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); + _tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); const Vector2f local_position{_local_pos.x, _local_pos.y}; Vector2f local_land_point = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); @@ -1680,7 +1627,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo if (airspeed_land < _param_fw_airspd_min.get()) { // adjust underspeed detection bounds for landing airspeed _tecs.set_equivalent_airspeed_min(airspeed_land); - _tecsnew.set_equivalent_airspeed_min(airspeed_land); adjusted_min_airspeed = airspeed_land; } @@ -1726,7 +1672,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo if (!_flare_states.flaring) { _flare_states.flaring = true; _flare_states.start_time = now; - _flare_states.initial_height_rate_setpoint = _tecs.hgt_rate_setpoint(); + _flare_states.initial_height_rate_setpoint = -_local_pos.vz; _flare_states.initial_throttle_setpoint = _att_sp.thrust_body[0]; events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info, "Landing, flaring"); @@ -1904,7 +1850,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo } _tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation - _tecsnew.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); @@ -2156,7 +2101,7 @@ FixedwingPositionControl::Run() if (_control_mode.flag_control_manual_enabled) { if (_control_mode.flag_control_altitude_enabled && _local_pos.vz_reset_counter != _alt_reset_counter) { // make TECS accept step in altitude and demanded altitude - _tecs.handle_alt_step(-_local_pos.delta_z, _current_altitude); + _tecs.handle_alt_step(_current_altitude, -_local_pos.vz); } // adjust navigation waypoints in position control mode @@ -2303,9 +2248,7 @@ FixedwingPositionControl::Run() // restore nominal TECS parameters in case changed intermittently (e.g. in landing handling) _tecs.set_speed_weight(_param_fw_t_spdweight.get()); - _tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get()); - _tecsnew.set_speed_weight(_param_fw_t_spdweight.get()); - _tecsnew.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get()); + _tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get()); // restore lateral-directional guidance parameters (changed in takeoff mode) _npfg.setPeriod(_param_npfg_period.get()); @@ -2371,7 +2314,7 @@ FixedwingPositionControl::Run() _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; _att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF; - _tecs.reset_state(); + _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed); break; } @@ -2548,14 +2491,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.reset_state(); - _tecsnew.initialize(_current_altitude, -_local_pos.vz, _airspeed); + _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed); _reinitialize_tecs = false; } @@ -2563,82 +2504,31 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva _tecs.set_detect_underspeed_enabled(!disable_underspeed_detection); if (_landed) { - _tecs.resetIntegrals(); - _tecs.resetTrajectoryGenerators(_current_altitude); - _tecsnew.initialize(_current_altitude, -_local_pos.vz, _airspeed); + _tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed); } /* update TECS vehicle state estimates */ - _tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), _current_altitude, - _local_pos.vz); - const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min, throttle_max); - _tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()), - _current_altitude, - alt_sp, - airspeed_sp, - _airspeed, - _eas2tas, - climbout_mode, - climbout_pitch_min_rad - radians(_param_fw_psp_off.get()), - throttle_min, - throttle_max, - throttle_trim_comp, - pitch_min_rad - radians(_param_fw_psp_off.get()), - pitch_max_rad - radians(_param_fw_psp_off.get()), - desired_max_climbrate, - desired_max_sinkrate, - hgt_rate_sp); - - _tecsnew.update(_pitch - radians(_param_fw_psp_off.get()), - _current_altitude, - alt_sp, - airspeed_sp, - _airspeed, - _eas2tas, - climbout_mode, - climbout_pitch_min_rad - radians(_param_fw_psp_off.get()), - throttle_min, - throttle_max, - throttle_trim_comp, - pitch_min_rad - radians(_param_fw_psp_off.get()), - pitch_max_rad - radians(_param_fw_psp_off.get()), - desired_max_climbrate, - desired_max_sinkrate, - _body_acceleration(0), - -_local_pos.vz, - hgt_rate_sp); - - newTECS::TECS::DebugOutput new_status{_tecsnew.getStatus()}; - - printf("Compare Airspeed Filter: \n "); - printf("\t\t raw\t old \t new \n"); - printf("Airspeed: \t %.2f \t %.2f \t %.2f \n",(double)(_airspeed*_eas2tas), (double)_tecs.tas_state(), (double)new_status.true_airspeed_filtered); - printf("Airspeed_deriv:\t %.2f \t %.2f \t %.2f \n", (double)_body_acceleration(0), (double)_tecs.speed_derivative(), (double)new_status.true_airspeed_derivative); - - printf("Compare Altitude reference filter: \n "); - printf("\t\t\t raw\t\t old \t\t new \n"); - printf("Altitude Ref:\t\t %.2f \t %.2f \t %.2f \n",(double)alt_sp, (double)_tecs.hgt_setpoint(), (double)new_status.altitude_sp); - printf("Alt rate from Alt:\t %.2f \t\t %.2f \t\t %.2f \n",0.0, (double)_tecs.hgt_rate_from_hgt_setpoint(), (double)new_status.altitude_rate); - printf("Altitude Rate Ref:\t %.2f \t\t %.2f \t\t %.2f \n",(double)hgt_rate_sp, (double)_tecs.smooth_hgt_rate_setpoint(), (double)new_status.altitude_rate_setpoint); - - printf("Compare Tecs control: \n "); - printf("\t\t old \t\t new \n"); - printf("alt_rate_ref:\t %.2f \t %.2f \n", (double)_tecs.hgt_rate_setpoint(), (double)new_status.altitude_rate_control); - printf("speed_rate_ref:\t %.2f \t %.2f \n", (double)_tecs.TAS_rate_setpoint(), (double)new_status.true_airspeed_derivative_control); - printf("ste_rate_error:\t %.2f \t %.2f \n", (double)_tecs.STE_rate_error(), (double)new_status.total_energy_rate_error); - printf("ste_rate_sp:\t %.2f \t %.2f \n", (double)_tecs.STE_rate_setpoint(), (double)new_status.total_energy_rate_sp); - printf("seb_rate_error:\t %.2f \t %.2f \n", (double)_tecs.SEB_rate_error(), (double)new_status.energy_balance_rate_error); - printf("seb_rate_sp:\t %.2f \t %.2f \n", (double)_tecs.SEB_rate_setpoint(), (double)new_status.energy_balance_rate_sp); - - printf("Compare Tecs output: \n "); - printf("\t\t old \t\t new \n"); - printf("pitch control:\t %.2f \t %.2f \n", (double)_tecs.get_pitch_setpoint(), (double)_tecsnew.get_pitch_setpoint()); - printf("throttle control:\t %.2f \t %.2f \n", (double)_tecs.get_throttle_setpoint(), (double)_tecsnew.get_throttle_setpoint()); - - + _tecs.update(_pitch - radians(_param_fw_psp_off.get()), + _current_altitude, + alt_sp, + airspeed_sp, + _airspeed, + _eas2tas, + climbout_mode, + climbout_pitch_min_rad - radians(_param_fw_psp_off.get()), + throttle_min, + throttle_max, + throttle_trim_comp, + pitch_min_rad - radians(_param_fw_psp_off.get()), + pitch_max_rad - radians(_param_fw_psp_off.get()), + desired_max_climbrate, + desired_max_sinkrate, + _body_acceleration(0), + -_local_pos.vz, + hgt_rate_sp); tecs_status_publish(); } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index fe5dfbc8a3..0afe48ebc7 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -58,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -389,7 +388,6 @@ private: // total energy control system - airspeed / altitude control TECS _tecs; - newTECS::TECS _tecsnew; bool _reinitialize_tecs{true}; bool _tecs_is_running{false};