forked from Archive/PX4-Autopilot
TECS: Combine both airspeed and airspeed derivative filters in TECS into one MIMO filter using a steady state Kalman filter.
This commit is contained in:
Konrad
Silvan Fuhrer
parent
08c36612b3
commit
f5524fa605
|
|
@ -18,21 +18,11 @@ float32 true_airspeed_filtered
|
|||
float32 true_airspeed_derivative_sp
|
||||
float32 true_airspeed_derivative
|
||||
float32 true_airspeed_derivative_raw
|
||||
float32 true_airspeed_innovation
|
||||
|
||||
float32 total_energy_error
|
||||
float32 energy_distribution_error
|
||||
float32 total_energy_rate_error
|
||||
float32 energy_distribution_rate_error
|
||||
|
||||
float32 total_energy
|
||||
float32 total_energy_rate
|
||||
float32 total_energy_balance
|
||||
float32 total_energy_balance_rate
|
||||
|
||||
float32 total_energy_sp
|
||||
float32 total_energy_rate_sp
|
||||
float32 total_energy_balance_sp
|
||||
float32 total_energy_balance_rate_sp
|
||||
|
||||
float32 throttle_integ
|
||||
|
|
|
|||
|
|
@ -42,9 +42,13 @@
|
|||
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
#include "matrix/Matrix.hpp"
|
||||
#include "matrix/Vector2.hpp"
|
||||
|
||||
using math::constrain;
|
||||
using math::max;
|
||||
using math::min;
|
||||
using namespace time_literals;
|
||||
|
||||
static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);}
|
||||
|
||||
|
|
@ -54,7 +58,6 @@ void TECSAirspeedFilter::initialize(const float equivalent_airspeed)
|
|||
|
||||
_airspeed_state.speed = equivalent_airspeed;
|
||||
_airspeed_state.speed_rate = 0.0f;
|
||||
_airspeed_rate_filter.reset(0.0f);
|
||||
}
|
||||
|
||||
void TECSAirspeedFilter::update(const float dt, const Input &input, const Param ¶m,
|
||||
|
|
@ -85,49 +88,51 @@ void TECSAirspeedFilter::update(const float dt, const Input &input, const Param
|
|||
airspeed_derivative = 0.0f;
|
||||
}
|
||||
|
||||
// Alpha filtering done in the TECS module. TODO merge with the second order complementary filter.
|
||||
_airspeed_rate_filter.setParameters(dt, param.speed_derivative_time_const);
|
||||
/* Filter airspeed and rate using a constant airspeed rate model in a steady state Kalman Filter.
|
||||
We use the gains of the continuous Kalman filter Kc and approximate the discrete version Kalman gain Kd =dt*Kc,
|
||||
since the continuous algebraic Riccatti equation is easier to solve.
|
||||
*/
|
||||
|
||||
if (PX4_ISFINITE(input.equivalent_airspeed_rate) && airspeed_sensor_available) {
|
||||
_airspeed_rate_filter.update(airspeed_derivative);
|
||||
matrix::Vector2f new_state_predicted;
|
||||
|
||||
} else {
|
||||
_airspeed_rate_filter.reset(0.0f);
|
||||
}
|
||||
new_state_predicted(0) = _airspeed_state.speed + dt * _airspeed_state.speed_rate;
|
||||
new_state_predicted(1) = _airspeed_state.speed_rate;
|
||||
|
||||
AirspeedFilterState new_airspeed_state;
|
||||
// Update TAS rate state
|
||||
const float airspeed_innovation = airspeed - _airspeed_state.speed;
|
||||
const float airspeed_rate_state_input = airspeed_innovation * param.airspeed_estimate_freq * param.airspeed_estimate_freq;
|
||||
new_airspeed_state.speed_rate = _airspeed_state.speed_rate + airspeed_rate_state_input * dt;
|
||||
const float airspeed_noise_inv{1.0f / param.airspeed_measurement_std_dev};
|
||||
const float airspeed_rate_noise_inv{1.0f / param.airspeed_rate_measurement_std_dev};
|
||||
const float airspeed_rate_noise_inv_squared_process_noise{airspeed_rate_noise_inv *airspeed_rate_noise_inv * param.airspeed_rate_noise_std_dev};
|
||||
const float denom{airspeed_noise_inv + airspeed_rate_noise_inv_squared_process_noise};
|
||||
const float common_nom{std::sqrt(param.airspeed_rate_noise_std_dev * (2.0f * airspeed_noise_inv + airspeed_rate_noise_inv_squared_process_noise))};
|
||||
|
||||
// Update TAS state
|
||||
const float airspeed_state_input = _airspeed_state.speed_rate + airspeed_innovation * param.airspeed_estimate_freq * 1.4142f;
|
||||
new_airspeed_state.speed = _airspeed_state.speed + airspeed_state_input * dt;
|
||||
matrix::Matrix<float, 2, 2> kalman_gain;
|
||||
kalman_gain(0, 0) = airspeed_noise_inv * common_nom / denom;
|
||||
kalman_gain(0, 1) = airspeed_rate_noise_inv_squared_process_noise / denom;
|
||||
kalman_gain(1, 0) = airspeed_noise_inv * airspeed_noise_inv * param.airspeed_rate_noise_std_dev / denom;
|
||||
kalman_gain(1, 1) = airspeed_rate_noise_inv_squared_process_noise * common_nom / denom;
|
||||
|
||||
// Clip tas at zero
|
||||
if (new_airspeed_state.speed < 0.0f) {
|
||||
// clip TAS at zero, calculate input that would result in zero speed.
|
||||
float desired_airspeed_state_input = -_airspeed_state.speed / dt;
|
||||
float desired_airspeed_innovation = (desired_airspeed_state_input - _airspeed_state.speed_rate) /
|
||||
(param.airspeed_estimate_freq * 1.4142f);
|
||||
new_airspeed_state.speed_rate = _airspeed_state.speed_rate + (desired_airspeed_innovation *
|
||||
param.airspeed_estimate_freq * param.airspeed_estimate_freq * dt);
|
||||
new_airspeed_state.speed = 0.0f;
|
||||
const matrix::Vector2f innovation{(airspeed - new_state_predicted(0)), (airspeed_derivative - new_state_predicted(1))};
|
||||
matrix::Vector2f new_state;
|
||||
new_state = new_state_predicted + dt * (kalman_gain * (innovation));
|
||||
|
||||
// Clip airspeed at zero
|
||||
if (new_state(0) < FLT_EPSILON) {
|
||||
new_state(0) = 0.0f;
|
||||
// calculate input that would result in zero speed.
|
||||
const float desired_airspeed_innovation = (-new_state_predicted(0) / dt - kalman_gain(0,
|
||||
1) * innovation(1)) / kalman_gain(0,
|
||||
0);
|
||||
new_state(1) = new_state_predicted(1) + dt * (kalman_gain(1, 0) * desired_airspeed_innovation + kalman_gain(1,
|
||||
1) * innovation(1));
|
||||
}
|
||||
|
||||
// Update states
|
||||
_airspeed_state = new_airspeed_state;
|
||||
_airspeed_state.speed = new_state(0);
|
||||
_airspeed_state.speed_rate = new_state(1);
|
||||
}
|
||||
|
||||
TECSAirspeedFilter::AirspeedFilterState TECSAirspeedFilter::getState() const
|
||||
{
|
||||
AirspeedFilterState filter_state{
|
||||
.speed = _airspeed_state.speed,
|
||||
.speed_rate = _airspeed_rate_filter.getState()
|
||||
};
|
||||
|
||||
return filter_state;
|
||||
return _airspeed_state;
|
||||
}
|
||||
|
||||
void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &setpoint, float altitude,
|
||||
|
|
@ -144,6 +149,7 @@ void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &se
|
|||
altitude = 0.0f;
|
||||
}
|
||||
|
||||
// Consider the altitude rate setpoint already smooth. No need to filter further, simply hold the value for the altitude rate reference.
|
||||
if (PX4_ISFINITE(setpoint.alt_rate)) {
|
||||
_alt_rate_ref = setpoint.alt_rate;
|
||||
|
||||
|
|
@ -195,23 +201,59 @@ float TECSReferenceModel::getAltitudeRateReference() const
|
|||
|
||||
void TECSReferenceModel::initialize(const AltitudeReferenceState &state)
|
||||
{
|
||||
AltitudeReferenceState init_state{state};
|
||||
float init_state_alt{state.alt};
|
||||
_alt_rate_ref = state.alt_rate;
|
||||
|
||||
if (!PX4_ISFINITE(state.alt)) {
|
||||
init_state.alt = 0.0f;
|
||||
init_state_alt = 0.0f;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(state.alt_rate)) {
|
||||
init_state.alt_rate = 0.0f;
|
||||
_alt_rate_ref = 0.0f;
|
||||
}
|
||||
|
||||
_alt_control_traj_generator.reset(0.0f, init_state.alt_rate, init_state.alt);
|
||||
_alt_control_traj_generator.reset(0.0f, _alt_rate_ref, init_state_alt);
|
||||
}
|
||||
|
||||
void TECSControl::initialize()
|
||||
void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param ¶m, const Flag &flag)
|
||||
{
|
||||
_ste_rate_error_filter.reset(0.0f);
|
||||
resetIntegrals();
|
||||
|
||||
AltitudePitchControl control_setpoint;
|
||||
|
||||
control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);
|
||||
|
||||
control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param);
|
||||
|
||||
SpecificEnergyRates specific_energy_rate{_calcSpecificEnergyRates(control_setpoint, input)};
|
||||
|
||||
_detectUnderspeed(input, param, flag);
|
||||
|
||||
const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
|
||||
ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rate)};
|
||||
|
||||
_pitch_setpoint = _calcPitchControlOutput(input, seb_rate, param, flag);
|
||||
|
||||
const STERateLimit limit{_calculateTotalEnergyRateLimit(param)};
|
||||
|
||||
_ste_rate_estimate_filter.reset(specific_energy_rate.spe_rate.estimate + specific_energy_rate.ske_rate.estimate);
|
||||
|
||||
ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rate, param)};
|
||||
|
||||
_throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag);
|
||||
|
||||
_ste_rate = ste_rate.estimate;
|
||||
|
||||
// Debug output
|
||||
_debug_output.total_energy_rate_estimate = ste_rate.estimate;
|
||||
_debug_output.total_energy_rate_sp = ste_rate.setpoint;
|
||||
_debug_output.throttle_integrator = _throttle_integ_state;
|
||||
_debug_output.energy_balance_rate_estimate = seb_rate.estimate;
|
||||
_debug_output.energy_balance_rate_sp = seb_rate.setpoint;
|
||||
_debug_output.pitch_integrator = _pitch_integ_state;
|
||||
|
||||
_debug_output.altitude_rate_control = control_setpoint.altitude_rate_setpoint;
|
||||
_debug_output.true_airspeed_derivative_control = control_setpoint.tas_rate_setpoint;
|
||||
}
|
||||
|
||||
void TECSControl::update(const float dt, const Setpoint &setpoint, const Input &input, Param ¶m, const Flag &flag)
|
||||
|
|
@ -225,25 +267,27 @@ void TECSControl::update(const float dt, const Setpoint &setpoint, const Input &
|
|||
|
||||
AltitudePitchControl control_setpoint;
|
||||
|
||||
control_setpoint.tas_rate_setpoint = _airspeedControl(setpoint, input, param, flag);
|
||||
control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);
|
||||
|
||||
control_setpoint.altitude_rate_setpoint = _altitudeControl(setpoint, input, param);
|
||||
control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param);
|
||||
|
||||
SpecificEnergy se{_updateEnergyBalance(control_setpoint, input)};
|
||||
SpecificEnergyRates specific_energy_rate{_calcSpecificEnergyRates(control_setpoint, input)};
|
||||
|
||||
_detectUnderspeed(input, param, flag);
|
||||
|
||||
_updatePitchSetpoint(dt, input, se, param, flag);
|
||||
_calcPitchControl(dt, input, specific_energy_rate, param, flag);
|
||||
|
||||
_updateThrottleSetpoint(dt, se, param, flag);
|
||||
_calcThrottleControl(dt, specific_energy_rate, param, flag);
|
||||
|
||||
_debug_output.altitude_rate_control = control_setpoint.altitude_rate_setpoint;
|
||||
_debug_output.true_airspeed_derivative_control = control_setpoint.tas_rate_setpoint;
|
||||
_debug_output.pitch_integrator = _pitch_integ_state;
|
||||
_debug_output.throttle_integrator = _throttle_integ_state;
|
||||
}
|
||||
|
||||
TECSControl::STELimit TECSControl::_calculateTotalEnergyRateLimit(const Param ¶m) const
|
||||
TECSControl::STERateLimit TECSControl::_calculateTotalEnergyRateLimit(const Param ¶m) const
|
||||
{
|
||||
TECSControl::STELimit limit;
|
||||
TECSControl::STERateLimit limit;
|
||||
// Calculate the specific total energy rate limits from the max throttle limits
|
||||
limit.STE_rate_max = math::max(param.max_climb_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
|
||||
limit.STE_rate_min = - math::max(param.max_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
|
||||
|
|
@ -251,12 +295,12 @@ TECSControl::STELimit TECSControl::_calculateTotalEnergyRateLimit(const Param &p
|
|||
return limit;
|
||||
}
|
||||
|
||||
float TECSControl::_airspeedControl(const Setpoint &setpoint, const Input &input, const Param ¶m,
|
||||
const Flag &flag) const
|
||||
float TECSControl::_calcAirspeedControlOutput(const Setpoint &setpoint, const Input &input, const Param ¶m,
|
||||
const Flag &flag) const
|
||||
{
|
||||
float airspeed_rate_output{0.0f};
|
||||
|
||||
const STELimit limit{_calculateTotalEnergyRateLimit(param)};
|
||||
const STERateLimit limit{_calculateTotalEnergyRateLimit(param)};
|
||||
|
||||
// calculate the demanded true airspeed rate of change based on first order response of true airspeed error
|
||||
// if airspeed measurement is not enabled then always set the rate setpoint to zero in order to avoid constant rate setpoints
|
||||
|
|
@ -272,7 +316,7 @@ float TECSControl::_airspeedControl(const Setpoint &setpoint, const Input &input
|
|||
return airspeed_rate_output;
|
||||
}
|
||||
|
||||
float TECSControl::_altitudeControl(const Setpoint &setpoint, const Input &input, const Param ¶m) const
|
||||
float TECSControl::_calcAltitudeControlOutput(const Setpoint &setpoint, const Input &input, const Param ¶m) const
|
||||
{
|
||||
float altitude_rate_output;
|
||||
altitude_rate_output = (setpoint.altitude_reference.alt - input.altitude) * param.altitude_error_gain +
|
||||
|
|
@ -282,29 +326,27 @@ float TECSControl::_altitudeControl(const Setpoint &setpoint, const Input &input
|
|||
return altitude_rate_output;
|
||||
}
|
||||
|
||||
TECSControl::SpecificEnergy TECSControl::_updateEnergyBalance(const AltitudePitchControl &control_setpoint,
|
||||
TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint,
|
||||
const Input &input) const
|
||||
{
|
||||
SpecificEnergy se;
|
||||
SpecificEnergyRates specific_energy_rates;
|
||||
// Calculate specific energy rate demands in units of (m**2/sec**3)
|
||||
se.spe.rate_setpoint = control_setpoint.altitude_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change
|
||||
se.ske.rate_setpoint = input.tas * control_setpoint.tas_rate_setpoint; // kinetic energy rate of change
|
||||
specific_energy_rates.spe_rate.setpoint = control_setpoint.altitude_rate_setpoint *
|
||||
CONSTANTS_ONE_G; // potential energy rate of change
|
||||
specific_energy_rates.ske_rate.setpoint = input.tas *
|
||||
control_setpoint.tas_rate_setpoint; // kinetic energy rate of change
|
||||
|
||||
// Calculate specific energy rates in units of (m**2/sec**3)
|
||||
se.spe.rate = input.altitude_rate * CONSTANTS_ONE_G; // potential energy rate of change
|
||||
se.ske.rate = input.tas * input.tas_rate;// kinetic energy rate of change
|
||||
specific_energy_rates.spe_rate.estimate = input.altitude_rate * CONSTANTS_ONE_G; // potential energy rate of change
|
||||
specific_energy_rates.ske_rate.estimate = input.tas * input.tas_rate;// kinetic energy rate of change
|
||||
|
||||
// Calculate energy rate error
|
||||
se.spe.rate_error = se.spe.rate_setpoint - se.spe.rate;
|
||||
se.ske.rate_error = se.ske.rate_setpoint - se.ske.rate;
|
||||
|
||||
return se;
|
||||
return specific_energy_rates;
|
||||
}
|
||||
|
||||
void TECSControl::_detectUnderspeed(const Input &input, const Param ¶m, const Flag &flag)
|
||||
{
|
||||
if (!flag.detect_underspeed_enabled) {
|
||||
_percent_undersped = 0.0f;
|
||||
_ratio_undersped = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -320,8 +362,8 @@ void TECSControl::_detectUnderspeed(const Input &input, const Param ¶m, cons
|
|||
const float tas_fully_undersped = math::max(param.tas_min - tas_error_bound - tas_underspeed_soft_bound, 0.0f);
|
||||
const float tas_starting_to_underspeed = math::max(param.tas_min - tas_error_bound, tas_fully_undersped);
|
||||
|
||||
_percent_undersped = 1.0f - math::constrain((input.tas - tas_fully_undersped) /
|
||||
math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f);
|
||||
_ratio_undersped = 1.0f - math::constrain((input.tas - tas_fully_undersped) /
|
||||
math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f);
|
||||
}
|
||||
|
||||
TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Param ¶m, const Flag &flag)
|
||||
|
|
@ -334,8 +376,8 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
|
|||
if (flag.climbout_mode_active && flag.airspeed_enabled) {
|
||||
pitch_speed_weight = 2.0f;
|
||||
|
||||
} else if (_percent_undersped > FLT_EPSILON && flag.airspeed_enabled) {
|
||||
pitch_speed_weight = 2.0f * _percent_undersped + (1.0f - _percent_undersped) * pitch_speed_weight;
|
||||
} else if (_ratio_undersped > FLT_EPSILON && flag.airspeed_enabled) {
|
||||
pitch_speed_weight = 2.0f * _ratio_undersped + (1.0f - _ratio_undersped) * pitch_speed_weight;
|
||||
|
||||
} else if (!flag.airspeed_enabled) {
|
||||
pitch_speed_weight = 0.0f;
|
||||
|
|
@ -350,13 +392,35 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
|
|||
return weight;
|
||||
}
|
||||
|
||||
void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const SpecificEnergy &se, Param ¶m,
|
||||
const Flag &flag)
|
||||
void TECSControl::_calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rates,
|
||||
const Param ¶m,
|
||||
const Flag &flag)
|
||||
{
|
||||
const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
|
||||
ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)};
|
||||
|
||||
_calcPitchControlUpdate(dt, seb_rate, param);
|
||||
const float pitch_setpoint{_calcPitchControlOutput(input, seb_rate, param, flag)};
|
||||
|
||||
// Comply with the specified vertical acceleration limit by applying a pitch rate limit
|
||||
// NOTE: at zero airspeed, the pitch increment is unbounded
|
||||
const float pitch_increment = dt * param.vert_accel_limit / math::max(input.tas, FLT_EPSILON);
|
||||
_pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment,
|
||||
_pitch_setpoint + pitch_increment);
|
||||
|
||||
//Debug Output
|
||||
_debug_output.energy_balance_rate_estimate = seb_rate.estimate;
|
||||
_debug_output.energy_balance_rate_sp = seb_rate.setpoint;
|
||||
_debug_output.pitch_integrator = _pitch_integ_state;
|
||||
}
|
||||
|
||||
TECSControl::ControlValues TECSControl::_calcPitchControlSebRate(const SpecificEnergyWeighting &weight,
|
||||
const SpecificEnergyRates &specific_energy_rates) const
|
||||
{
|
||||
ControlValues seb_rate;
|
||||
/*
|
||||
* The SKE_weighting variable controls how speed and altitude control are prioritised by the pitch demand calculation.
|
||||
* A weighting of 1 givea equal speed and altitude priority
|
||||
* The SKE_weighting variable controls how speed and altitude control are prioritized by the pitch demand calculation.
|
||||
* A weighting of 1 gives equal speed and altitude priority
|
||||
* A weighting of 0 gives 100% priority to altitude control and must be used when no airspeed measurement is available.
|
||||
* A weighting of 2 provides 100% priority to speed control and is used when:
|
||||
* a) an underspeed condition is detected.
|
||||
|
|
@ -364,18 +428,21 @@ void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const Speci
|
|||
* rises above the demanded value, the pitch angle demand is increased by the TECS controller to prevent the vehicle overspeeding.
|
||||
* The weighting can be adjusted between 0 and 2 depending on speed and altitude accuracy requirements.
|
||||
*/
|
||||
// Calculate the specific energy balance rate demand
|
||||
const float seb_rate_setpoint = se.spe.rate_setpoint * weight.spe_weighting - se.ske.rate_setpoint * weight.ske_weighting;
|
||||
seb_rate.setpoint = specific_energy_rates.spe_rate.setpoint * weight.spe_weighting -
|
||||
specific_energy_rates.ske_rate.setpoint *
|
||||
weight.ske_weighting;
|
||||
|
||||
// Calculate the specific energy balance rate error
|
||||
const float seb_rate_error = (se.spe.rate_error * weight.spe_weighting) - (se.ske.rate_error * weight.ske_weighting);
|
||||
seb_rate.estimate = (specific_energy_rates.spe_rate.estimate * weight.spe_weighting) -
|
||||
(specific_energy_rates.ske_rate.estimate * weight.ske_weighting);
|
||||
|
||||
_debug_output.energy_balance_rate_error = seb_rate_error;
|
||||
_debug_output.energy_balance_rate_sp = seb_rate_setpoint;
|
||||
return seb_rate;
|
||||
}
|
||||
|
||||
if (param.integrator_gain_pitch > 0.0f) {
|
||||
void TECSControl::_calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param ¶m)
|
||||
{
|
||||
if (param.integrator_gain_pitch > FLT_EPSILON) {
|
||||
// Calculate pitch integrator input term
|
||||
float pitch_integ_input = seb_rate_error * param.integrator_gain_pitch;
|
||||
float pitch_integ_input = _getControlError(seb_rate) * param.integrator_gain_pitch;
|
||||
|
||||
// Prevent the integrator changing in a direction that will increase pitch demand saturation
|
||||
if (_pitch_setpoint >= param.pitch_max) {
|
||||
|
|
@ -391,10 +458,16 @@ void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const Speci
|
|||
} else {
|
||||
_pitch_integ_state = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
float TECSControl::_calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param ¶m,
|
||||
const Flag &flag) const
|
||||
{
|
||||
// Pitch setpoint
|
||||
// Calculate a specific energy correction that doesn't include the integrator contribution
|
||||
float SEB_rate_correction = seb_rate_error * param.pitch_damping_gain + _pitch_integ_state + param.seb_rate_ff *
|
||||
seb_rate_setpoint;
|
||||
float SEB_rate_correction = _getControlError(seb_rate) * param.pitch_damping_gain + _pitch_integ_state +
|
||||
param.seb_rate_ff *
|
||||
seb_rate.setpoint;
|
||||
|
||||
// Calculate derivative from change in climb angle to rate of change of specific energy balance
|
||||
const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G;
|
||||
|
|
@ -412,47 +485,70 @@ void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const Speci
|
|||
// pitch transients due to control action or turbulence.
|
||||
const float pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate;
|
||||
|
||||
const float pitch_setpoint = constrain(pitch_setpoint_unc, param.pitch_min, param.pitch_max);
|
||||
|
||||
// Comply with the specified vertical acceleration limit by applying a pitch rate limit
|
||||
// NOTE: at zero airspeed, the pitch increment is unbounded
|
||||
const float pitch_increment = dt * param.vert_accel_limit / input.tas;
|
||||
_pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment,
|
||||
_pitch_setpoint + pitch_increment);
|
||||
return constrain(pitch_setpoint_unc, param.pitch_min, param.pitch_max);
|
||||
}
|
||||
|
||||
void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, const Param ¶m, const Flag &flag)
|
||||
void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &specific_energy_rates, const Param ¶m,
|
||||
const Flag &flag)
|
||||
{
|
||||
const STELimit limit{_calculateTotalEnergyRateLimit(param)};
|
||||
const STERateLimit limit{_calculateTotalEnergyRateLimit(param)};
|
||||
|
||||
float STE_rate_setpoint = se.spe.rate_setpoint + se.ske.rate_setpoint;
|
||||
// Update STE rate estimate LP filter
|
||||
const float STE_rate_estimate_raw = specific_energy_rates.spe_rate.estimate + specific_energy_rates.ske_rate.estimate;
|
||||
_ste_rate_estimate_filter.setParameters(dt, param.ste_rate_time_const);
|
||||
_ste_rate_estimate_filter.update(STE_rate_estimate_raw);
|
||||
|
||||
ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)};
|
||||
_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
|
||||
float throttle_setpoint{_calcThrottleControlOutput(limit, ste_rate, param, flag)};
|
||||
|
||||
// Rate limit the throttle demand
|
||||
if (fabsf(param.throttle_slewrate) > FLT_EPSILON) {
|
||||
const float throttle_increment_limit = dt * (param.throttle_max - param.throttle_min) * param.throttle_slewrate;
|
||||
throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint - throttle_increment_limit,
|
||||
_throttle_setpoint + throttle_increment_limit);
|
||||
}
|
||||
|
||||
_throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
|
||||
_ste_rate = ste_rate.estimate;
|
||||
|
||||
// Debug output
|
||||
_debug_output.total_energy_rate_estimate = ste_rate.estimate;
|
||||
_debug_output.total_energy_rate_sp = ste_rate.setpoint;
|
||||
_debug_output.throttle_integrator = _throttle_integ_state;
|
||||
}
|
||||
|
||||
TECSControl::ControlValues TECSControl::_calcThrottleControlSteRate(const STERateLimit &limit,
|
||||
const SpecificEnergyRates &specific_energy_rates,
|
||||
const Param ¶m) const
|
||||
{
|
||||
// Output ste rate values
|
||||
ControlValues ste_rate;
|
||||
ste_rate.setpoint = specific_energy_rates.spe_rate.setpoint + specific_energy_rates.ske_rate.setpoint;
|
||||
|
||||
// Adjust the demanded total energy rate to compensate for induced drag rise in turns.
|
||||
// Assume induced drag scales linearly with normal load factor.
|
||||
// The additional normal load factor is given by (1/cos(bank angle) - 1)
|
||||
STE_rate_setpoint += param.load_factor_correction * (param.load_factor - 1.f);
|
||||
ste_rate.setpoint += param.load_factor_correction * (param.load_factor - 1.f);
|
||||
|
||||
STE_rate_setpoint = constrain(STE_rate_setpoint, limit.STE_rate_min, limit.STE_rate_max);
|
||||
ste_rate.setpoint = constrain(ste_rate.setpoint, limit.STE_rate_min, limit.STE_rate_max);
|
||||
ste_rate.estimate = _ste_rate_estimate_filter.getState();
|
||||
|
||||
_ste_rate = se.spe.rate + se.ske.rate;
|
||||
|
||||
const float STE_rate_error_raw = se.spe.rate_error + se.ske.rate_error;
|
||||
_ste_rate_error_filter.setParameters(dt, param.ste_rate_time_const);
|
||||
_ste_rate_error_filter.update(STE_rate_error_raw);
|
||||
const float STE_rate_error{_ste_rate_error_filter.getState()};
|
||||
|
||||
_debug_output.total_energy_rate_error = STE_rate_error;
|
||||
_debug_output.total_energy_rate_sp = STE_rate_setpoint;
|
||||
return ste_rate;
|
||||
}
|
||||
|
||||
void TECSControl::_calcThrottleControlUpdate(float dt, const STERateLimit &limit, const ControlValues &ste_rate,
|
||||
const Param ¶m, const Flag &flag)
|
||||
{
|
||||
// Calculate gain scaler from specific energy rate error to throttle
|
||||
const float STE_rate_to_throttle = 1.0f / (limit.STE_rate_max - limit.STE_rate_min);
|
||||
|
||||
// Integral handling
|
||||
if (flag.airspeed_enabled) {
|
||||
if (param.integrator_gain_throttle > 0.0f) {
|
||||
if (param.integrator_gain_throttle > FLT_EPSILON) {
|
||||
// underspeed conditions zero out integration
|
||||
float throttle_integ_input = (STE_rate_error * param.integrator_gain_throttle) * dt *
|
||||
STE_rate_to_throttle * (1.0f - _percent_undersped);
|
||||
float throttle_integ_input = (_getControlError(ste_rate) * param.integrator_gain_throttle) * dt *
|
||||
STE_rate_to_throttle * (1.0f - _ratio_undersped);
|
||||
|
||||
// only allow integrator propagation into direction which unsaturates throttle
|
||||
if (_throttle_setpoint >= param.throttle_max) {
|
||||
|
|
@ -477,6 +573,14 @@ void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, co
|
|||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
float TECSControl::_calcThrottleControlOutput(const STERateLimit &limit, const ControlValues &ste_rate,
|
||||
const Param ¶m,
|
||||
const Flag &flag) const
|
||||
{
|
||||
// Calculate gain scaler from specific energy rate error to throttle
|
||||
const float STE_rate_to_throttle = 1.0f / (limit.STE_rate_max - limit.STE_rate_min);
|
||||
|
||||
// Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle
|
||||
// as the starting point. Assume:
|
||||
|
|
@ -485,20 +589,21 @@ void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, co
|
|||
// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
|
||||
float throttle_predicted = 0.0f;
|
||||
|
||||
if (STE_rate_setpoint >= 0) {
|
||||
if (ste_rate.setpoint >= FLT_EPSILON) {
|
||||
// throttle is between trim and maximum
|
||||
throttle_predicted = param.throttle_trim + STE_rate_setpoint / limit.STE_rate_max *
|
||||
throttle_predicted = param.throttle_trim + ste_rate.setpoint / limit.STE_rate_max *
|
||||
(param.throttle_max - param.throttle_trim);
|
||||
|
||||
} else {
|
||||
// throttle is between trim and minimum
|
||||
throttle_predicted = param.throttle_trim + STE_rate_setpoint / limit.STE_rate_min *
|
||||
throttle_predicted = param.throttle_trim + ste_rate.setpoint / limit.STE_rate_min *
|
||||
(param.throttle_min - param.throttle_trim);
|
||||
|
||||
}
|
||||
|
||||
// Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits
|
||||
float throttle_setpoint = (STE_rate_error * param.throttle_damping_gain) * STE_rate_to_throttle + throttle_predicted;
|
||||
float throttle_setpoint = (_getControlError(ste_rate) * param.throttle_damping_gain) * STE_rate_to_throttle +
|
||||
throttle_predicted;
|
||||
|
||||
if (flag.airspeed_enabled) {
|
||||
// Add the integrator feedback during closed loop operation with an airspeed sensor
|
||||
|
|
@ -507,16 +612,9 @@ void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, co
|
|||
}
|
||||
|
||||
// ramp in max throttle setting with underspeediness value
|
||||
throttle_setpoint = _percent_undersped * param.throttle_max + (1.0f - _percent_undersped) * throttle_setpoint;
|
||||
throttle_setpoint = _ratio_undersped * param.throttle_max + (1.0f - _ratio_undersped) * throttle_setpoint;
|
||||
|
||||
// Rate limit the throttle demand
|
||||
if (fabsf(param.throttle_slewrate) > 0.01f) {
|
||||
const float throttle_increment_limit = dt * (param.throttle_max - param.throttle_min) * param.throttle_slewrate;
|
||||
throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint - throttle_increment_limit,
|
||||
_throttle_setpoint + throttle_increment_limit);
|
||||
}
|
||||
|
||||
_throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
|
||||
return constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
|
||||
}
|
||||
|
||||
void TECSControl::resetIntegrals()
|
||||
|
|
@ -528,7 +626,7 @@ void TECSControl::resetIntegrals()
|
|||
float TECS::_update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas)
|
||||
{
|
||||
float new_setpoint{tas_setpoint};
|
||||
const float percent_undersped = _control.getPercentUndersped();
|
||||
const float percent_undersped = _control.getRatioUndersped();
|
||||
|
||||
// Set the TAS demand to the minimum value if an underspeed or
|
||||
// or a uncontrolled descent condition exists to maximise climb rate
|
||||
|
|
@ -571,7 +669,7 @@ void TECS::_detect_uncommanded_descent(float throttle_setpoint_max, float altitu
|
|||
const float SKE_error = SKE_setpoint - SKE_estimate;
|
||||
const float STE_error = SPE_error + SKE_error;
|
||||
|
||||
const bool underspeed_detected = _control.getPercentUndersped() > FLT_EPSILON;
|
||||
const bool underspeed_detected = _control.getRatioUndersped() > FLT_EPSILON;
|
||||
|
||||
// If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode
|
||||
const bool enter_mode = !_uncommanded_descent_recovery && !underspeed_detected && (STE_error > 200.0f)
|
||||
|
|
@ -590,14 +688,38 @@ void TECS::_detect_uncommanded_descent(float throttle_setpoint_max, float altitu
|
|||
}
|
||||
}
|
||||
|
||||
void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed)
|
||||
void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed,
|
||||
const float eas_to_tas)
|
||||
{
|
||||
// Init subclasses
|
||||
TECSReferenceModel::AltitudeReferenceState current_state{ .alt = altitude,
|
||||
.alt_rate = altitude_rate};
|
||||
_reference_model.initialize(current_state);
|
||||
_airspeed_filter.initialize(equivalent_airspeed);
|
||||
_control.initialize();
|
||||
|
||||
TECSControl::Setpoint control_setpoint;
|
||||
control_setpoint.altitude_reference = _reference_model.getAltitudeReference();
|
||||
control_setpoint.altitude_rate_setpoint = _reference_model.getAltitudeRateReference();
|
||||
control_setpoint.tas_setpoint = equivalent_airspeed * eas_to_tas;
|
||||
|
||||
const TECSControl::Input control_input{ .altitude = altitude,
|
||||
.altitude_rate = altitude_rate,
|
||||
.tas = eas_to_tas * equivalent_airspeed,
|
||||
.tas_rate = 0.0f};
|
||||
|
||||
_control.initialize(control_setpoint, control_input, _control_param, _control_flag);
|
||||
|
||||
_debug_status.tecs_mode = _tecs_mode;
|
||||
_debug_status.control = _control.getDebugOutput();
|
||||
const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState();
|
||||
_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
|
||||
_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
|
||||
const TECSReferenceModel::AltitudeReferenceState ref_alt{_reference_model.getAltitudeReference()};
|
||||
_debug_status.altitude_sp = ref_alt.alt;
|
||||
_debug_status.altitude_rate_alt_ref = ref_alt.alt_rate;
|
||||
_debug_status.altitude_rate_feedforward = _reference_model.getAltitudeRateReference();
|
||||
|
||||
_update_timestamp = hrt_absolute_time();
|
||||
}
|
||||
|
||||
void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
|
||||
|
|
@ -605,9 +727,23 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
|
|||
float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
|
||||
const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp)
|
||||
{
|
||||
|
||||
// Calculate the time since last update (seconds)
|
||||
const uint64_t now(hrt_absolute_time());
|
||||
const float dt = (now - _update_timestamp) * 1e-6f;
|
||||
const hrt_abstime now(hrt_absolute_time());
|
||||
const float dt = static_cast<float>((now - _update_timestamp)) / 1_s;
|
||||
|
||||
// Update parameters from input
|
||||
// Reference model
|
||||
_reference_param.target_climbrate = target_climbrate;
|
||||
_reference_param.target_sinkrate = target_sinkrate;
|
||||
// Control
|
||||
_control_param.tas_min = eas_to_tas * _equivalent_airspeed_min;
|
||||
_control_param.pitch_max = pitch_limit_max;
|
||||
_control_param.pitch_min = pitch_limit_min;
|
||||
_control_param.throttle_trim = throttle_trim;
|
||||
_control_param.throttle_max = throttle_setpoint_max;
|
||||
_control_param.throttle_min = throttle_min;
|
||||
_control_flag.climbout_mode_active = climb_out_setpoint;
|
||||
|
||||
if (dt < DT_MIN) {
|
||||
// Update intervall too small, do not update. Assume constant states/output in this case.
|
||||
|
|
@ -616,23 +752,22 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
|
|||
|
||||
if (dt > DT_MAX || _update_timestamp == 0UL) {
|
||||
// Update time intervall too large, can't guarantee sanity of state updates anymore. reset the control loop.
|
||||
initialize(altitude, hgt_rate, equivalent_airspeed);
|
||||
initialize(altitude, hgt_rate, equivalent_airspeed, eas_to_tas);
|
||||
|
||||
} else {
|
||||
// Update airspeedfilter submodule
|
||||
const TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed = equivalent_airspeed,
|
||||
.equivalent_airspeed_rate = speed_deriv_forward / eas_to_tas};
|
||||
_airspeed_param.equivalent_airspeed_trim = _equivalent_airspeed_trim;
|
||||
_airspeed_filter.update(dt, airspeed_input, _airspeed_param, _airspeed_enabled);
|
||||
|
||||
_airspeed_filter.update(dt, airspeed_input, _airspeed_filter_param, _control_flag.airspeed_enabled);
|
||||
const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState();
|
||||
|
||||
// Update Reference model submodule
|
||||
const TECSReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
|
||||
.alt_rate = hgt_rate_sp
|
||||
};
|
||||
_reference_param.target_climbrate = target_climbrate;
|
||||
_reference_param.target_sinkrate = target_sinkrate;
|
||||
.alt_rate = hgt_rate_sp};
|
||||
|
||||
_reference_model.update(dt, setpoint, altitude, _reference_param);
|
||||
|
||||
TECSControl::Setpoint control_setpoint;
|
||||
control_setpoint.altitude_reference = _reference_model.getAltitudeReference();
|
||||
control_setpoint.altitude_rate_setpoint = _reference_model.getAltitudeRateReference();
|
||||
|
|
@ -643,59 +778,42 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
|
|||
eas_to_tas * _equivalent_airspeed_max, EAS_setpoint * eas_to_tas, eas_to_tas * eas.speed);
|
||||
|
||||
const TECSControl::Input control_input{ .altitude = altitude,
|
||||
.altitude_rate = hgt_rate,
|
||||
.tas = eas_to_tas * eas.speed,
|
||||
.tas_rate = eas_to_tas * eas.speed_rate
|
||||
};
|
||||
_control_param.equivalent_airspeed_trim = _equivalent_airspeed_trim;
|
||||
_control_param.tas_min = eas_to_tas * _equivalent_airspeed_min;
|
||||
_control_param.pitch_max = pitch_limit_max;
|
||||
_control_param.pitch_min = pitch_limit_min;
|
||||
_control_param.throttle_trim = throttle_trim;
|
||||
_control_param.throttle_max = throttle_setpoint_max;
|
||||
_control_param.throttle_min = throttle_min;
|
||||
const TECSControl::Flag control_flag{ .airspeed_enabled = _airspeed_enabled,
|
||||
.climbout_mode_active = climb_out_setpoint,
|
||||
.detect_underspeed_enabled = _detect_underspeed_enabled
|
||||
};
|
||||
_control.update(dt, control_setpoint, control_input, _control_param, control_flag);
|
||||
.altitude_rate = hgt_rate,
|
||||
.tas = eas_to_tas * eas.speed,
|
||||
.tas_rate = eas_to_tas * eas.speed_rate};
|
||||
|
||||
_control.update(dt, control_setpoint, control_input, _control_param, _control_flag);
|
||||
|
||||
// Detect an uncommanded descent caused by an unachievable airspeed demand
|
||||
_detect_uncommanded_descent(throttle_setpoint_max, altitude, hgt_setpoint, equivalent_airspeed * eas_to_tas,
|
||||
control_setpoint.tas_setpoint);
|
||||
|
||||
TECSControl::DebugOutput control_status = _control.getDebugOutput();
|
||||
_debug_status.altitude_rate_control = control_status.altitude_rate_control;
|
||||
_debug_status.energy_balance_rate_error = control_status.energy_balance_rate_error;
|
||||
_debug_status.energy_balance_rate_sp = control_status.energy_balance_rate_sp;
|
||||
_debug_status.total_energy_rate_error = control_status.total_energy_rate_error;
|
||||
_debug_status.total_energy_rate_sp = control_status.total_energy_rate_sp;
|
||||
_debug_status.true_airspeed_derivative_control = control_status.true_airspeed_derivative_control;
|
||||
// Update time stamps
|
||||
_update_timestamp = now;
|
||||
|
||||
|
||||
// Set TECS mode for next frame
|
||||
if (_control.getRatioUndersped() > FLT_EPSILON) {
|
||||
_tecs_mode = ECL_TECS_MODE_UNDERSPEED;
|
||||
|
||||
} else if (_uncommanded_descent_recovery) {
|
||||
_tecs_mode = ECL_TECS_MODE_BAD_DESCENT;
|
||||
|
||||
} else if (climb_out_setpoint) {
|
||||
_tecs_mode = ECL_TECS_MODE_CLIMBOUT;
|
||||
|
||||
} else {
|
||||
// This is the default operation mode
|
||||
_tecs_mode = ECL_TECS_MODE_NORMAL;
|
||||
}
|
||||
|
||||
_debug_status.tecs_mode = _tecs_mode;
|
||||
_debug_status.control = _control.getDebugOutput();
|
||||
_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
|
||||
_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
|
||||
_debug_status.altitude_sp = control_setpoint.altitude_reference.alt;
|
||||
_debug_status.altitude_rate = control_setpoint.altitude_reference.alt_rate;
|
||||
_debug_status.altitude_rate_setpoint = control_setpoint.altitude_rate_setpoint;
|
||||
_debug_status.altitude_rate_alt_ref = control_setpoint.altitude_reference.alt_rate;
|
||||
_debug_status.altitude_rate_feedforward = control_setpoint.altitude_rate_setpoint;
|
||||
}
|
||||
|
||||
// Update time stamps
|
||||
_update_timestamp = now;
|
||||
|
||||
// Set TECS mode for next frame
|
||||
if (_control.getPercentUndersped() > FLT_EPSILON) {
|
||||
_tecs_mode = ECL_TECS_MODE_UNDERSPEED;
|
||||
|
||||
} else if (_uncommanded_descent_recovery) {
|
||||
_tecs_mode = ECL_TECS_MODE_BAD_DESCENT;
|
||||
|
||||
} else if (climb_out_setpoint) {
|
||||
_tecs_mode = ECL_TECS_MODE_CLIMBOUT;
|
||||
|
||||
} else {
|
||||
// This is the default operation mode
|
||||
_tecs_mode = ECL_TECS_MODE_NORMAL;
|
||||
}
|
||||
|
||||
_debug_status.tecs_mode = _tecs_mode;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -39,6 +39,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
|
||||
|
|
@ -66,9 +67,10 @@ public:
|
|||
*
|
||||
*/
|
||||
struct Param {
|
||||
float equivalent_airspeed_trim; ///< the trim value of the equivalent airspeed om [m/s].
|
||||
float airspeed_estimate_freq; ///< cross-over frequency of the equivalent airspeed complementary filter [rad/sec].
|
||||
float speed_derivative_time_const; ///< speed derivative filter time constant [s].
|
||||
float equivalent_airspeed_trim; ///< the trim value of the equivalent airspeed [m/s].
|
||||
float airspeed_measurement_std_dev; ///< airspeed measurement standard deviation in [m/s].
|
||||
float airspeed_rate_measurement_std_dev;///< airspeed rate measurement standard deviation in [m/s²].
|
||||
float airspeed_rate_noise_std_dev; ///< standard deviation on the airspeed rate deviation in the model in [m/s²].
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -108,7 +110,6 @@ public:
|
|||
|
||||
private:
|
||||
// States
|
||||
AlphaFilter<float> _airspeed_rate_filter; ///< Alpha filter for airspeed rate
|
||||
AirspeedFilterState _airspeed_state{.speed = 0.0f, .speed_rate = 0.0f}; ///< Complimentary filter state
|
||||
};
|
||||
|
||||
|
|
@ -176,8 +177,6 @@ private:
|
|||
// State
|
||||
VelocitySmoothing
|
||||
_alt_control_traj_generator; ///< Generates altitude rate and altitude setpoint trajectory when altitude is commanded.
|
||||
|
||||
// Output
|
||||
float _alt_rate_ref; ///< Altitude rate reference in [m/s].
|
||||
};
|
||||
|
||||
|
|
@ -235,10 +234,12 @@ public:
|
|||
struct DebugOutput {
|
||||
float altitude_rate_control; ///< Altitude rate setpoint from altitude control loop [m/s].
|
||||
float true_airspeed_derivative_control; ///< Airspeed rate setpoint from airspeed control loop [m/s²].
|
||||
float total_energy_rate_error; ///< Total energy rate error [m²/s³].
|
||||
float total_energy_rate_estimate; ///< Total energy rate estimate [m²/s³].
|
||||
float total_energy_rate_sp; ///< Total energy rate setpoint [m²/s³].
|
||||
float energy_balance_rate_error; ///< Energy balance rate error [m²/s³].
|
||||
float energy_balance_rate_estimate; ///< Energy balance rate estimate [m²/s³].
|
||||
float energy_balance_rate_sp; ///< Energy balance rate setpoint [m²/s³].
|
||||
float pitch_integrator; ///< Pitch control integrator state [-].
|
||||
float throttle_integrator; ///< Throttle control integrator state [-].
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -278,7 +279,7 @@ public:
|
|||
* @brief Initialization of the state.
|
||||
*
|
||||
*/
|
||||
void initialize();
|
||||
void initialize(const Setpoint &setpoint, const Input &input, Param ¶m, const Flag &flag);
|
||||
/**
|
||||
* @brief Update state and output.
|
||||
*
|
||||
|
|
@ -297,9 +298,9 @@ public:
|
|||
/**
|
||||
* @brief Get the percent of the undersped.
|
||||
*
|
||||
* @return Percentage of detected undersped.
|
||||
* @return Ratio of detected undersped [0,1].
|
||||
*/
|
||||
float getPercentUndersped() const {return _percent_undersped;};
|
||||
float getRatioUndersped() const {return _ratio_undersped;};
|
||||
/**
|
||||
* @brief Get the throttle setpoint.
|
||||
*
|
||||
|
|
@ -323,33 +324,35 @@ public:
|
|||
*
|
||||
* @return the debug outpus struct.
|
||||
*/
|
||||
DebugOutput getDebugOutput() const {return _debug_output;};
|
||||
const DebugOutput &getDebugOutput() const { return _debug_output; }
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Specific total energy limit.
|
||||
* @brief Specific total energy rate limit.
|
||||
*
|
||||
*/
|
||||
struct STELimit {
|
||||
struct STERateLimit {
|
||||
float STE_rate_max; ///< Maximum specific total energy rate limit [m²/s³].
|
||||
float STE_rate_min; ///< Minimal specific total energy rate limit [m²/s³].
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Calculated specific energy.
|
||||
* @brief Control values.
|
||||
* setpoint and current state estimate value as input to a controller.
|
||||
*
|
||||
*/
|
||||
struct SpecificEnergy {
|
||||
struct {
|
||||
float rate; ///< Specific kinetic energy rate in [m²/s³].
|
||||
float rate_setpoint; ///< Specific kinetic energy setpoint rate in [m²/s³].
|
||||
float rate_error; ///< Specific kinetic energy rate error in [m²/s³].
|
||||
} ske; ///< Specific kinetic energy.
|
||||
struct {
|
||||
float rate; ///< Specific potential energy rate in [m²/s³].
|
||||
float rate_setpoint; ///< Specific potential energy setpoint rate in [m²/s³].
|
||||
float rate_error; ///< Specific potential energy rate error in [m²/s³].
|
||||
} spe; ///< Specific potential energy rate.
|
||||
struct ControlValues {
|
||||
float setpoint; ///< Control setpoint.
|
||||
float estimate; ///< Control estimate of current state value.
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Calculated specific energy rates.
|
||||
*
|
||||
*/
|
||||
struct SpecificEnergyRates {
|
||||
ControlValues ske_rate; ///< Specific kinetic energy rate [m²/s³].
|
||||
ControlValues spe_rate; ///< Specific potential energy rate [m²/s³].
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -371,15 +374,22 @@ private:
|
|||
};
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Get control error from etpoint and estimate
|
||||
*
|
||||
* @param val is the current control setpoint and estimate.
|
||||
* @return error value
|
||||
*/
|
||||
static inline constexpr float _getControlError(TECSControl::ControlValues val) {return (val.setpoint - val.estimate);};
|
||||
/**
|
||||
* @brief Calculate specific total energy rate limits.
|
||||
*
|
||||
* @param[in] param are the control parametes.
|
||||
* @return Specific total energy rate limits.
|
||||
* @return Specific total energy rate limits in [m²/s³].
|
||||
*/
|
||||
STELimit _calculateTotalEnergyRateLimit(const Param ¶m) const;
|
||||
STERateLimit _calculateTotalEnergyRateLimit(const Param ¶m) const;
|
||||
/**
|
||||
* @brief Airspeed control loop.
|
||||
* @brief calculate airspeed control proportional output.
|
||||
*
|
||||
* @param setpoint is the control setpoints.
|
||||
* @param input is the current input measurment of the UAS.
|
||||
|
|
@ -387,24 +397,25 @@ private:
|
|||
* @param flag is the control flags.
|
||||
* @return controlled airspeed rate setpoint in [m/s²].
|
||||
*/
|
||||
float _airspeedControl(const Setpoint &setpoint, const Input &input, const Param ¶m, const Flag &flag) const;
|
||||
float _calcAirspeedControlOutput(const Setpoint &setpoint, const Input &input, const Param ¶m,
|
||||
const Flag &flag) const;
|
||||
/**
|
||||
* @brief Altitude control loop.
|
||||
* @brief calculate altitude control proportional output.
|
||||
*
|
||||
* @param setpoint is the control setpoints.
|
||||
* @param input is the current input measurment of the UAS.
|
||||
* @param param is the control parameters.
|
||||
* @return controlled altitude rate setpoint in [m/s].
|
||||
*/
|
||||
float _altitudeControl(const Setpoint &setpoint, const Input &input, const Param ¶m) const;
|
||||
float _calcAltitudeControlOutput(const Setpoint &setpoint, const Input &input, const Param ¶m) const;
|
||||
/**
|
||||
* @brief Update energy balance.
|
||||
* @brief Calculate specific energy rates.
|
||||
*
|
||||
* @param control_setpoint is the controlles altitude and airspeed rate setpoints.
|
||||
* @param input is the current input measurment of the UAS.
|
||||
* @return Specific energy rates.
|
||||
* @return Specific energy rates in [m²/s³].
|
||||
*/
|
||||
SpecificEnergy _updateEnergyBalance(const AltitudePitchControl &control_setpoint, const Input &input) const;
|
||||
SpecificEnergyRates _calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint, const Input &input) const;
|
||||
/**
|
||||
* @brief Detect underspeed.
|
||||
*
|
||||
|
|
@ -422,37 +433,108 @@ private:
|
|||
*/
|
||||
SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Param ¶m, const Flag &flag);
|
||||
/**
|
||||
* @brief Update controlled pitch setpoint.
|
||||
* @brief Calculate pitch control.
|
||||
*
|
||||
* @param dt is the update time intervall in [s].
|
||||
* @param input is the current input measurment of the UAS.
|
||||
* @param se is the calculated specific energy.
|
||||
* @param input is the current input measurement of the UAS.
|
||||
* @param specific_energy_rate is the calculated specific energy.
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
*/
|
||||
void _updatePitchSetpoint(float dt, const Input &input, const SpecificEnergy &se, Param ¶m, const Flag &flag);
|
||||
void _calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rate,
|
||||
const Param ¶m,
|
||||
const Flag &flag);
|
||||
|
||||
/**
|
||||
* @brief Calculate pitch control specific energy balance rates.
|
||||
*
|
||||
* @param weight is the weighting use of the potential and kinetic energy.
|
||||
* @param specific_energy_rate is the specific energy rates in [m²/s³].
|
||||
* @return specific energy balance rate values in [m²/s³].
|
||||
*/
|
||||
ControlValues _calcPitchControlSebRate(const SpecificEnergyWeighting &weight,
|
||||
const SpecificEnergyRates &specific_energy_rate) const;
|
||||
|
||||
/**
|
||||
* @brief Calculate the pitch control update function.
|
||||
* Update the states of the pitch control
|
||||
*
|
||||
* @param dt is the update time intervall in [s].
|
||||
* @param seb_rate is the specific energy balance rate in [m²/s³].
|
||||
* @param param is the control parameters.
|
||||
*/
|
||||
void _calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param ¶m);
|
||||
|
||||
/**
|
||||
* @brief Calculate the pitch control output function.
|
||||
*
|
||||
* @param input is the current input measurement of the UAS.
|
||||
* @param seb_rate is the specific energy balance rate in [m²/s³].
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
* @return pitch setpoint angle [rad].
|
||||
*/
|
||||
float _calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param ¶m,
|
||||
const Flag &flag) const;
|
||||
|
||||
/**
|
||||
* @brief Update controlled throttle setpoint.
|
||||
*
|
||||
* @param dt is the update time intervall in [s].
|
||||
* @param se is the calculated specific energy.
|
||||
* @param specific_energy_rate is the calculated specific energy.
|
||||
* @param flag is the control flags.
|
||||
*/
|
||||
void _calcThrottleControl(float dt, const SpecificEnergyRates &specific_energy_rate, const Param ¶m,
|
||||
const Flag &flag);
|
||||
|
||||
/**
|
||||
* @brief Calculate throttle control specific total energy
|
||||
*
|
||||
* @param limit is the specific total energy rate limits in [m²/s³].
|
||||
* @param specific_energy_rate is the specific energy rates in [m²/s³].
|
||||
* @param param is the control parameters.
|
||||
* @return specific total energy rate values in [m²/s³]
|
||||
*/
|
||||
ControlValues _calcThrottleControlSteRate(const STERateLimit &limit, const SpecificEnergyRates &specific_energy_rate,
|
||||
const Param ¶m) const;
|
||||
|
||||
/**
|
||||
* @brief Calculate the throttle control update function.
|
||||
* Update the throttle control states.
|
||||
*
|
||||
* @param dt is the update time intervall in [s].
|
||||
* @param limit is the specific total energy rate limits in [m²/s³].
|
||||
* @param ste_rate is the specific total energy rates in [m²/s³].
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
*/
|
||||
void _updateThrottleSetpoint(float dt, const SpecificEnergy &se, const Param ¶m, const Flag &flag);
|
||||
void _calcThrottleControlUpdate(float dt, const STERateLimit &limit, const ControlValues &ste_rate, const Param ¶m,
|
||||
const Flag &flag);
|
||||
|
||||
/**
|
||||
* @brief Calculate the throttle control output function.
|
||||
*
|
||||
* @param limit is the specific total energy rate limits in [m²/s³].
|
||||
* @param ste_rate is the specific total energy rates in [m²/s³].
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
* @return throttle setpoin in [0,1].
|
||||
*/
|
||||
float _calcThrottleControlOutput(const STERateLimit &limit, const ControlValues &ste_rate, const Param ¶m,
|
||||
const Flag &flag) const;
|
||||
|
||||
private:
|
||||
// State
|
||||
AlphaFilter<float> _ste_rate_error_filter; ///< Low pass filter for the specific total energy rate.
|
||||
AlphaFilter<float> _ste_rate_estimate_filter; ///< Low pass filter for the specific total energy rate.
|
||||
float _pitch_integ_state{0.0f}; ///< Pitch integrator state [rad].
|
||||
float _throttle_integ_state{0.0f}; ///< Throttle integrator state.
|
||||
|
||||
|
||||
// Output
|
||||
DebugOutput _debug_output;
|
||||
DebugOutput _debug_output; ///< Debug output.
|
||||
float _pitch_setpoint{0.0f}; ///< Controlled pitch setpoint [rad].
|
||||
float _throttle_setpoint{0.0f}; ///< Controlled throttle setpoint.
|
||||
float _percent_undersped{0.0f}; ///< A continuous representation of how "undersped" the TAS is [0,1]
|
||||
float _throttle_setpoint{0.0f}; ///< Controlled throttle setpoint [0,1].
|
||||
float _ratio_undersped{0.0f}; ///< A continuous representation of how "undersped" the TAS is [0,1]
|
||||
float _ste_rate{0.0f}; ///< Specific total energy rate [m²/s³].
|
||||
};
|
||||
|
||||
|
|
@ -466,12 +548,13 @@ public:
|
|||
ECL_TECS_MODE_CLIMBOUT
|
||||
};
|
||||
|
||||
struct DebugOutput : TECSControl::DebugOutput {
|
||||
struct DebugOutput {
|
||||
TECSControl::DebugOutput control;
|
||||
float true_airspeed_filtered;
|
||||
float true_airspeed_derivative;
|
||||
float altitude_sp;
|
||||
float altitude_rate;
|
||||
float altitude_rate_setpoint;
|
||||
float altitude_rate_alt_ref;
|
||||
float altitude_rate_feedforward;
|
||||
enum ECL_TECS_MODE tecs_mode;
|
||||
};
|
||||
public:
|
||||
|
|
@ -484,19 +567,19 @@ public:
|
|||
TECS(TECS &&) = delete;
|
||||
TECS &operator=(TECS &&) = delete;
|
||||
|
||||
DebugOutput getStatus() const {return _debug_status;};
|
||||
const DebugOutput &getStatus() const { return _debug_status; }
|
||||
|
||||
/**
|
||||
* Get the current airspeed status
|
||||
*
|
||||
* @return true if airspeed is enabled for control
|
||||
*/
|
||||
bool airspeed_sensor_enabled() { return _airspeed_enabled; }
|
||||
bool airspeed_sensor_enabled() { return _control_flag.airspeed_enabled; }
|
||||
|
||||
/**
|
||||
* Set the airspeed enable state
|
||||
*/
|
||||
void enable_airspeed(bool enabled) { _airspeed_enabled = enabled; }
|
||||
void enable_airspeed(bool enabled) { _control_flag.airspeed_enabled = enabled; }
|
||||
|
||||
/**
|
||||
* @brief Update the control loop calculations
|
||||
|
|
@ -505,22 +588,26 @@ public:
|
|||
void update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
|
||||
float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_setpoint_max,
|
||||
float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
|
||||
const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
|
||||
float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
|
||||
|
||||
/**
|
||||
* @brief Initialize the control loop
|
||||
*
|
||||
*/
|
||||
void initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed);
|
||||
void initialize(float altitude, float altitude_rate, float equivalent_airspeed, float eas_to_tas);
|
||||
|
||||
void resetIntegrals()
|
||||
{
|
||||
_control.resetIntegrals();
|
||||
}
|
||||
|
||||
void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; };
|
||||
void set_detect_underspeed_enabled(bool enabled) { _control_flag.detect_underspeed_enabled = enabled; };
|
||||
|
||||
// // setters for parameters
|
||||
void set_airspeed_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_measurement_std_dev = std_dev;};
|
||||
void set_airspeed_rate_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_measurement_std_dev = std_dev;};
|
||||
void set_airspeed_filter_process_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_noise_std_dev = std_dev;};
|
||||
|
||||
void set_integrator_gain_throttle(float gain) { _control_param.integrator_gain_throttle = gain;};
|
||||
void set_integrator_gain_pitch(float gain) { _control_param.integrator_gain_pitch = gain; };
|
||||
|
||||
|
|
@ -532,12 +619,11 @@ public:
|
|||
|
||||
void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
|
||||
void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
|
||||
void set_equivalent_airspeed_trim(float airspeed) { _equivalent_airspeed_trim = airspeed; }
|
||||
void set_equivalent_airspeed_trim(float airspeed) { _control_param.equivalent_airspeed_trim = airspeed; _airspeed_filter_param.equivalent_airspeed_trim = airspeed; }
|
||||
|
||||
void set_pitch_damping(float damping) { _control_param.pitch_damping_gain = damping; }
|
||||
void set_vertical_accel_limit(float limit) { _reference_param.vert_accel_limit = limit; _control_param.vert_accel_limit = limit; };
|
||||
|
||||
void set_speed_comp_filter_omega(float omega) { _airspeed_param.airspeed_estimate_freq = omega; };
|
||||
void set_speed_weight(float weight) { _control_param.pitch_speed_weight = weight; };
|
||||
void set_airspeed_error_time_constant(float time_const) { _control_param.airspeed_error_gain = 1.0f / math::max(time_const, 0.1f); };
|
||||
|
||||
|
|
@ -547,7 +633,6 @@ public:
|
|||
void set_roll_throttle_compensation(float compensation) { _control_param.load_factor_correction = compensation; };
|
||||
void set_load_factor(float load_factor) { _control_param.load_factor = load_factor; };
|
||||
|
||||
void set_speed_derivative_time_constant(float time_const) { _airspeed_param.speed_derivative_time_const = time_const; };
|
||||
void set_ste_rate_time_const(float time_const) { _control_param.ste_rate_time_const = time_const; };
|
||||
|
||||
void set_seb_rate_ff_gain(float ff_gain) { _control_param.seb_rate_ff = ff_gain; };
|
||||
|
|
@ -574,8 +659,6 @@ public:
|
|||
uint64_t timestamp() { return _update_timestamp; }
|
||||
ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
|
||||
|
||||
static constexpr float DT_DEFAULT = 0.02f;
|
||||
|
||||
private:
|
||||
TECSControl _control; ///< Control submodule.
|
||||
TECSAirspeedFilter _airspeed_filter; ///< Airspeed filter submodule.
|
||||
|
|
@ -583,38 +666,32 @@ private:
|
|||
|
||||
enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; ///< Current activated mode.
|
||||
|
||||
uint64_t _update_timestamp{0}; ///< last timestamp of the update function call.
|
||||
hrt_abstime _update_timestamp{0}; ///< last timestamp of the update function call.
|
||||
|
||||
float _equivalent_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec)
|
||||
float _equivalent_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec)
|
||||
float _equivalent_airspeed_trim{15.0f}; ///< equivalent cruise airspeed for airspeed less mode (m/sec)
|
||||
|
||||
// controller mode logic
|
||||
bool _uncommanded_descent_recovery{false}; ///< true when a continuous descent caused by an unachievable airspeed demand has been detected
|
||||
bool _airspeed_enabled{false}; ///< true when airspeed use has been enabled
|
||||
bool _detect_underspeed_enabled{false}; ///< true when underspeed detection is enabled
|
||||
|
||||
static constexpr float DT_MIN = 0.001f; ///< minimum allowed value of _dt (sec)
|
||||
static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a filter state reset is performed (sec)
|
||||
|
||||
static constexpr float _jerk_max = 1000.0f; ///< Magnitude of the maximum jerk allowed [m/s³].
|
||||
static constexpr float _tas_error_percentage =
|
||||
0.15f; ///< [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors
|
||||
|
||||
DebugOutput _debug_status{};
|
||||
|
||||
// Params
|
||||
/// Airspeed filter parameters.
|
||||
TECSAirspeedFilter::Param _airspeed_param{
|
||||
.equivalent_airspeed_trim = 0.0f,
|
||||
.airspeed_estimate_freq = 0.0f,
|
||||
.speed_derivative_time_const = 0.01f,
|
||||
TECSAirspeedFilter::Param _airspeed_filter_param{
|
||||
.equivalent_airspeed_trim = 15.0f,
|
||||
.airspeed_measurement_std_dev = 0.2f,
|
||||
.airspeed_rate_measurement_std_dev = 0.05f,
|
||||
.airspeed_rate_noise_std_dev = 0.02f
|
||||
};
|
||||
/// Reference model parameters.
|
||||
TECSReferenceModel::Param _reference_param{
|
||||
.target_climbrate = 2.0f,
|
||||
.target_sinkrate = 2.0f,
|
||||
.jerk_max = _jerk_max,
|
||||
.jerk_max = 1000.0f,
|
||||
.vert_accel_limit = 0.0f,
|
||||
.max_climb_rate = 2.0f,
|
||||
.max_sink_rate = 2.0f,
|
||||
|
|
@ -633,7 +710,7 @@ private:
|
|||
.throttle_min = 0.1f,
|
||||
.altitude_error_gain = 0.2f,
|
||||
.altitude_setpoint_gain_ff = 0.0f,
|
||||
.tas_error_percentage = _tas_error_percentage,
|
||||
.tas_error_percentage = 0.15f,
|
||||
.airspeed_error_gain = 0.1f,
|
||||
.ste_rate_time_const = 0.1f,
|
||||
.seb_rate_ff = 1.0f,
|
||||
|
|
@ -647,6 +724,12 @@ private:
|
|||
.load_factor = 1.0f,
|
||||
};
|
||||
|
||||
TECSControl::Flag _control_flag{
|
||||
.airspeed_enabled = false,
|
||||
.climbout_mode_active = false,
|
||||
.detect_underspeed_enabled = false,
|
||||
};
|
||||
|
||||
/**
|
||||
* Update the desired airspeed
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -134,15 +134,16 @@ FixedwingPositionControl::parameters_update()
|
|||
_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
|
||||
_tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get());
|
||||
_tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get());
|
||||
_tecs.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get());
|
||||
_tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
|
||||
_tecs.set_pitch_damping(_param_fw_t_ptch_damp.get());
|
||||
_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
|
||||
_tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get());
|
||||
_tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get());
|
||||
_tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get());
|
||||
_tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get());
|
||||
_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
|
||||
_tecs.set_airspeed_measurement_std_dev(_param_speed_standard_dev.get());
|
||||
_tecs.set_airspeed_rate_measurement_std_dev(_param_speed_rate_standard_dev.get());
|
||||
_tecs.set_airspeed_filter_process_std_dev(_param_process_noise_standard_dev.get());
|
||||
|
||||
int check_ret = PX4_OK;
|
||||
|
||||
|
|
@ -457,11 +458,12 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
|
|||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::tecs_status_publish()
|
||||
FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
|
||||
float true_airspeed_derivative_raw, float throttle_trim)
|
||||
{
|
||||
tecs_status_s t{};
|
||||
|
||||
TECS::DebugOutput debug_output{_tecs.getStatus()};
|
||||
const TECS::DebugOutput &debug_output{_tecs.getStatus()};
|
||||
|
||||
switch (_tecs.tecs_mode()) {
|
||||
case TECS::ECL_TECS_MODE_NORMAL:
|
||||
|
|
@ -481,25 +483,25 @@ FixedwingPositionControl::tecs_status_publish()
|
|||
break;
|
||||
}
|
||||
|
||||
t.altitude_sp = alt_sp;
|
||||
t.altitude_filtered = debug_output.altitude_sp;
|
||||
|
||||
t.height_rate_setpoint = debug_output.control.altitude_rate_control;
|
||||
t.height_rate = -_local_pos.vz;
|
||||
t.equivalent_airspeed_sp = equivalent_airspeed_sp;
|
||||
t.true_airspeed_sp = _eas2tas * equivalent_airspeed_sp;
|
||||
t.true_airspeed_filtered = debug_output.true_airspeed_filtered;
|
||||
|
||||
t.height_rate_setpoint = debug_output.altitude_rate_setpoint;
|
||||
t.height_rate = debug_output.altitude_rate;
|
||||
|
||||
t.true_airspeed_derivative_sp = debug_output.true_airspeed_derivative_control;
|
||||
t.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control;
|
||||
t.true_airspeed_derivative = debug_output.true_airspeed_derivative;
|
||||
|
||||
t.total_energy_rate_error = debug_output.total_energy_rate_error;
|
||||
|
||||
t.energy_distribution_rate_error = debug_output.energy_balance_rate_error;
|
||||
|
||||
t.total_energy_rate_sp = debug_output.total_energy_rate_sp;
|
||||
t.total_energy_balance_rate_sp = debug_output.energy_balance_rate_sp;
|
||||
|
||||
t.true_airspeed_derivative_raw = true_airspeed_derivative_raw;
|
||||
t.total_energy_rate = debug_output.control.total_energy_rate_estimate;
|
||||
t.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate;
|
||||
t.total_energy_rate_sp = debug_output.control.total_energy_rate_sp;
|
||||
t.total_energy_balance_rate_sp = debug_output.control.energy_balance_rate_sp;
|
||||
t.throttle_integ = debug_output.control.throttle_integrator;
|
||||
t.pitch_integ = debug_output.control.pitch_integrator;
|
||||
t.throttle_sp = _tecs.get_throttle_setpoint();
|
||||
t.pitch_sp_rad = _tecs.get_pitch_setpoint();
|
||||
t.throttle_trim = throttle_trim;
|
||||
|
||||
t.timestamp = hrt_absolute_time();
|
||||
|
||||
|
|
@ -826,7 +828,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
|
|||
_was_in_air = true;
|
||||
_time_went_in_air = now;
|
||||
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
|
||||
}
|
||||
|
||||
/* reset flag when airplane landed */
|
||||
|
|
@ -834,7 +836,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
|
|||
_was_in_air = false;
|
||||
_completed_manual_takeoff = false;
|
||||
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -2314,7 +2316,7 @@ FixedwingPositionControl::Run()
|
|||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
|
||||
|
||||
break;
|
||||
}
|
||||
|
|
@ -2491,12 +2493,12 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
|
|||
}
|
||||
|
||||
// We need an altitude lock to calculate the TECS control
|
||||
if (!(_local_pos.timestamp > 0)) {
|
||||
if (_local_pos.timestamp == 0) {
|
||||
_reinitialize_tecs = true;
|
||||
}
|
||||
|
||||
if (_reinitialize_tecs) {
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
|
||||
_reinitialize_tecs = false;
|
||||
}
|
||||
|
||||
|
|
@ -2504,7 +2506,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
|
|||
_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
|
||||
|
||||
if (_landed) {
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
|
||||
}
|
||||
|
||||
/* update TECS vehicle state estimates */
|
||||
|
|
@ -2530,7 +2532,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
|
|||
-_local_pos.vz,
|
||||
hgt_rate_sp);
|
||||
|
||||
tecs_status_publish();
|
||||
tecs_status_publish(alt_sp, airspeed_sp, -_local_pos.vz, throttle_trim_comp);
|
||||
}
|
||||
|
||||
float
|
||||
|
|
|
|||
|
|
@ -437,7 +437,8 @@ private:
|
|||
|
||||
void status_publish();
|
||||
void landing_status_publish();
|
||||
void tecs_status_publish();
|
||||
void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
|
||||
float throttle_trim);
|
||||
void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint);
|
||||
|
||||
/**
|
||||
|
|
@ -822,16 +823,17 @@ private:
|
|||
(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
|
||||
(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,
|
||||
(ParamFloat<px4::params::FW_T_SINK_MIN>) _param_fw_t_sink_min,
|
||||
(ParamFloat<px4::params::FW_T_SPD_OMEGA>) _param_fw_t_spd_omega,
|
||||
(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_fw_t_spdweight,
|
||||
(ParamFloat<px4::params::FW_T_TAS_TC>) _param_fw_t_tas_error_tc,
|
||||
(ParamFloat<px4::params::FW_T_THR_DAMP>) _param_fw_t_thr_damp,
|
||||
(ParamFloat<px4::params::FW_T_VERT_ACC>) _param_fw_t_vert_acc,
|
||||
(ParamFloat<px4::params::FW_T_STE_R_TC>) _param_ste_rate_time_const,
|
||||
(ParamFloat<px4::params::FW_T_TAS_R_TC>) _param_tas_rate_time_const,
|
||||
(ParamFloat<px4::params::FW_T_SEB_R_FF>) _param_seb_rate_ff,
|
||||
(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
|
||||
(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
|
||||
(ParamFloat<px4::params::FW_T_SPD_STD>) _param_speed_standard_dev,
|
||||
(ParamFloat<px4::params::FW_T_SPD_DEV_STD>) _param_speed_rate_standard_dev,
|
||||
(ParamFloat<px4::params::FW_T_SPD_PRC_STD>) _param_process_noise_standard_dev,
|
||||
|
||||
(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
|
||||
(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
|
||||
|
|
|
|||
|
|
@ -709,22 +709,50 @@ PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f);
|
|||
PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
|
||||
|
||||
/**
|
||||
* Complementary filter "omega" parameter for speed
|
||||
* Airspeed measurement standard deviation for airspeed filter.
|
||||
*
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse longitudinal acceleration and airspeed to obtain an
|
||||
* improved airspeed estimate. Increasing this frequency weights the solution
|
||||
* more towards use of the airspeed sensor, whilst reducing it weights the
|
||||
* solution more towards use of the accelerometer data.
|
||||
* This is the measurement standard deviation for the airspeed used in the airspeed filter in TECS.
|
||||
*
|
||||
* @unit rad/s
|
||||
* @min 1.0
|
||||
* @unit m/s
|
||||
* @min 0.01
|
||||
* @max 10.0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @decimal 2
|
||||
* @increment 0.1
|
||||
* @group FW TECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_T_SPD_STD, 0.2f);
|
||||
|
||||
/**
|
||||
* Airspeed rate measurement standard deviation for airspeed filter.
|
||||
*
|
||||
* This is the measurement standard deviation for the airspeed rate used in the airspeed filter in TECS.
|
||||
*
|
||||
* @unit m/s^2
|
||||
* @min 0.01
|
||||
* @max 10.0
|
||||
* @decimal 2
|
||||
* @increment 0.1
|
||||
* @group FW TECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_SPD_DEV_STD, 0.05f);
|
||||
|
||||
/**
|
||||
* Process noise standard deviation for the airspeed rate in the airspeed filter.
|
||||
*
|
||||
* This is the process noise standard deviation in the airspeed filter filter defining the noise in the
|
||||
* airspeed rate for the constant airspeed rate model. This is used to define how much the airspeed and
|
||||
* the airspeed rate are filtered. The smaller the value the more the measurements are smoothed with the
|
||||
* drawback for delays.
|
||||
*
|
||||
* @unit m/s^2
|
||||
* @min 0.01
|
||||
* @max 10.0
|
||||
* @decimal 2
|
||||
* @increment 0.1
|
||||
* @group FW TECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_SPD_PRC_STD, 0.2f);
|
||||
|
||||
|
||||
/**
|
||||
* Roll -> Throttle feedforward
|
||||
|
|
@ -856,21 +884,6 @@ PARAM_DEFINE_INT32(FW_POS_STK_CONF, 2);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_STE_R_TC, 0.4f);
|
||||
|
||||
|
||||
/**
|
||||
* True airspeed rate first order filter time constant.
|
||||
*
|
||||
* This filter is applied to the true airspeed rate.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 2
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW TECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_TAS_R_TC, 0.2f);
|
||||
|
||||
|
||||
/**
|
||||
* Specific total energy balance rate feedforward gain.
|
||||
*
|
||||
|
|
|
|||
Loading…
Reference in New Issue