fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 21:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

TECS: Rearranged the TECS library into submodules.

Browse Source
This commit is contained in:
Konrad
2022-10-18 14:05:00 +02:00
committed by Silvan Fuhrer
parent 54a32eb2f7
commit c64e111d8e
2 changed files with 1361 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECSnew.cpp
+713
View File
@@ -0,0 +1,713 @@
/****************************************************************************
*
* Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file TECS.cpp
*
* @author Paul Riseborough
*/
#include "TECSnew.hpp"
#include <lib/geo/geo.h>
#include <px4_platform_common/defines.h>
using math::constrain;
using math::max;
using math::min;
// TODO there seems to be an inconsistent definition of IAS/CAS/EAS/TAS
// TODO Recheck the timing.
void TECSAirspeedFilter::initialize(const float equivalent_airspeed)
{
_airspeed_state.speed= equivalent_airspeed;
_airspeed_state.speed_rate = 0.0f;
_airspeed_rate_filter.reset(0.0f);
}
void TECSAirspeedFilter::update(const float dt, const Input &input, const Param &param, 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 &param)
{
// 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<float>(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 &param, 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 &param) 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 &param, 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 &param) 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 &param, 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 &param, 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 &param, 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 &param, 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;
}
}
src/lib/tecs/TECSnew.hpp
+648
View File
@@ -0,0 +1,648 @@
/****************************************************************************
*
* Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file tecs.cpp
*
* @author Paul Riseborough
*/
#pragma once
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/tecs_status.h>
#include <uORB/uORB.h>
#include <motion_planning/VelocitySmoothing.hpp>
#include <motion_planning/ManualVelocitySmoothingZ.hpp>
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 &param, const bool airspeed_sensor_available);
/**
* @brief Get the filtered airspeed states.
*
* @return Current state of the airspeed filter.
*/
AirspeedFilterState getState() const;
private:
// States
AlphaFilter<float> _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 &param);
/**
* @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 &param, 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 &param) 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 &param, 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 &param) 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 &param, 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 &param, 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 &param, 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 &param, const Flag &flag);
private:
// State
AlphaFilter<float> _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);
};