mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
TECS: Replaced old tecs by cleaned up version.
This commit is contained in:
Konrad
Silvan Fuhrer
parent
991689d3cd
commit
7a3e0f53c2
@ -36,10 +36,4 @@ px4_add_library(tecs
|
||||
TECS.hpp
|
||||
)
|
||||
|
||||
px4_add_library(tecsnew
|
||||
TECSnew.cpp
|
||||
TECSnew.hpp
|
||||
)
|
||||
|
||||
target_link_libraries(tecs PRIVATE geo)
|
||||
target_link_libraries(tecsnew PRIVATE geo)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -49,8 +49,431 @@
|
||||
#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 ¶m, const bool airspeed_sensor_available);
|
||||
|
||||
/**
|
||||
* @brief Get the filtered airspeed states.
|
||||
*
|
||||
* @return Current state of the airspeed filter.
|
||||
*/
|
||||
AirspeedFilterState getState() const;
|
||||
|
||||
private:
|
||||
// States
|
||||
AlphaFilter<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 ¶m);
|
||||
|
||||
/**
|
||||
* @brief Get the current altitude reference of altitude reference model.
|
||||
*
|
||||
* @return Altitude reference state.
|
||||
*/
|
||||
AltitudeReferenceState getAltitudeReference() const;
|
||||
|
||||
/**
|
||||
* @brief Get the altitude rate reference of the altitude rate reference model.
|
||||
*
|
||||
* @return Current altitude rate reference point.
|
||||
*/
|
||||
float getAltitudeRateReference() const;
|
||||
|
||||
private:
|
||||
// State
|
||||
VelocitySmoothing
|
||||
_alt_control_traj_generator; ///< Generates altitude rate and altitude setpoint trajectory when altitude is commanded.
|
||||
|
||||
ManualVelocitySmoothingZ
|
||||
_velocity_control_traj_generator; ///< Generates altitude rate setpoint when altitude rate is commanded.
|
||||
};
|
||||
|
||||
class TECSControl
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief The control parameters.
|
||||
*
|
||||
*/
|
||||
struct Param {
|
||||
// Vehicle specific params
|
||||
float max_sink_rate; ///< Maximum safe sink rate [m/s].
|
||||
float max_climb_rate; ///< Climb rate produced by max allowed throttle [m/s].
|
||||
float vert_accel_limit; ///< Magnitude of the maximum vertical acceleration allowed [m/s²].
|
||||
float equivalent_airspeed_trim; ///< Equivalent cruise airspeed for airspeed less mode [m/s].
|
||||
float tas_min; ///< True airpeed demand lower limit [m/s].
|
||||
float pitch_max; ///< Maximum pitch angle allowed in [rad].
|
||||
float pitch_min; ///< Minimal pitch angle allowed in [rad].
|
||||
float throttle_trim; ///< Normalized throttle required to fly level at given eas.
|
||||
float throttle_max; ///< Normalized throttle upper limit.
|
||||
float throttle_min; ///< Normalized throttle lower limit.
|
||||
|
||||
// Altitude control param
|
||||
float altitude_error_gain; ///< Altitude error inverse time constant [1/s].
|
||||
float altitude_setpoint_gain_ff; ///< Gain from altitude demand derivative to demanded climb rate.
|
||||
|
||||
// Airspeed control param
|
||||
/// [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors
|
||||
float tas_error_percentage;
|
||||
float airspeed_error_gain; ///< Airspeed error inverse time constant [1/s].
|
||||
|
||||
// Energy control param
|
||||
float ste_rate_time_const; ///< Filter time constant for specific total energy rate (damping path) [s].
|
||||
float seb_rate_ff; ///< Specific energy balance rate feedforward gain.
|
||||
|
||||
// Pitch control param
|
||||
float pitch_speed_weight; ///< Speed control weighting used by pitch demand calculation.
|
||||
float integrator_gain_pitch; ///< Integrator gain used by the pitch demand calculation.
|
||||
float pitch_damping_gain; ///< Damping gain of the pitch demand calculation [s].
|
||||
|
||||
// Throttle control param
|
||||
float integrator_gain_throttle; ///< Integrator gain used by the throttle demand calculation.
|
||||
float throttle_damping_gain; ///< Damping gain of the throttle demand calculation [s].
|
||||
float throttle_slewrate; ///< Throttle demand slew rate limit [1/s].
|
||||
|
||||
float load_factor_correction; ///< Gain from normal load factor increase to total energy rate demand [m²/s³].
|
||||
float load_factor; ///< Additional normal load factor.
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief The debug output
|
||||
*
|
||||
*/
|
||||
struct DebugOutput {
|
||||
float altitude_rate_control; ///< Altitude rate setpoint from altitude control loop [m/s].
|
||||
float true_airspeed_derivative_control; ///< Airspeed rate setpoint from airspeed control loop [m/s²].
|
||||
float total_energy_rate_error; ///< Total energy rate error [m²/s³].
|
||||
float total_energy_rate_sp; ///< Total energy rate setpoint [m²/s³].
|
||||
float energy_balance_rate_error; ///< Energy balance rate error [m²/s³].
|
||||
float energy_balance_rate_sp; ///< Energy balance rate setpoint [m²/s³].
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Given setpoint to control.
|
||||
*
|
||||
*/
|
||||
struct Setpoint {
|
||||
TECSReferenceModel::AltitudeReferenceState altitude_reference; ///< Altitude reference from reference model.
|
||||
float altitude_rate_setpoint; ///< Altitude rate setpoint.
|
||||
float tas_setpoint; ///< True airspeed setpoint.
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Givent current measurement from the UAS.
|
||||
*
|
||||
*/
|
||||
struct Input {
|
||||
float altitude; ///< Current altitude of the UAS [m].
|
||||
float altitude_rate; ///< Current altitude rate of the UAS [m/s].
|
||||
float tas; ///< Current true airspeed of the UAS [m/s].
|
||||
float tas_rate; ///< Current true airspeed rate of the UAS [m/s²].
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Control flags.
|
||||
*
|
||||
*/
|
||||
struct Flag {
|
||||
bool airspeed_enabled; ///< Flag if the airspeed sensor is enabled.
|
||||
bool climbout_mode_active; ///< Flag if climbout mode is activated.
|
||||
bool detect_underspeed_enabled; ///< Flag if underspeed detection is enabled.
|
||||
};
|
||||
public:
|
||||
TECSControl() = default;
|
||||
~TECSControl() = default;
|
||||
/**
|
||||
* @brief Initialization of the state.
|
||||
*
|
||||
*/
|
||||
void initialize();
|
||||
/**
|
||||
* @brief Update state and output.
|
||||
*
|
||||
* @param[in] dt is the update time intervall in [s].
|
||||
* @param[in] setpoint is the current setpoint struct.
|
||||
* @param[in] input is the current input measurements.
|
||||
* @param[in] param is the current parameter set.
|
||||
* @param[in] flag is the current activated flags.
|
||||
*/
|
||||
void update(float dt, const Setpoint &setpoint, const Input &input, Param ¶m, const Flag &flag);
|
||||
/**
|
||||
* @brief Reset the control loop integrals.
|
||||
*
|
||||
*/
|
||||
void resetIntegrals();
|
||||
/**
|
||||
* @brief Get the percent of the undersped.
|
||||
*
|
||||
* @return Percentage of detected undersped.
|
||||
*/
|
||||
float getPercentUndersped() const {return _percent_undersped;};
|
||||
/**
|
||||
* @brief Get the throttle setpoint.
|
||||
*
|
||||
* @return throttle setpoint.
|
||||
*/
|
||||
float getThrottleSetpoint() const {return _throttle_setpoint;};
|
||||
/**
|
||||
* @brief Get the pitch setpoint.
|
||||
*
|
||||
* @return THe commanded pitch angle in [rad].
|
||||
*/
|
||||
float getPitchSetpoint() const {return _pitch_setpoint;};
|
||||
/**
|
||||
* @brief Get specific total energy rate.
|
||||
*
|
||||
* @return the total specific energy rate in [m²/s³].
|
||||
*/
|
||||
float getSteRate() const {return _ste_rate;};
|
||||
/**
|
||||
* @brief Get the Debug Output
|
||||
*
|
||||
* @return the debug outpus struct.
|
||||
*/
|
||||
DebugOutput getDebugOutput() const {return _debug_output;};
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Specific total energy limit.
|
||||
*
|
||||
*/
|
||||
struct STELimit {
|
||||
float STE_rate_max; ///< Maximum specific total energy rate limit [m²/s³].
|
||||
float STE_rate_min; ///< Minimal specific total energy rate limit [m²/s³].
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Calculated specific energy.
|
||||
*
|
||||
*/
|
||||
struct SpecificEnergy {
|
||||
struct {
|
||||
float rate; ///< Specific kinetic energy rate in [m²/s³].
|
||||
float rate_setpoint; ///< Specific kinetic energy setpoint rate in [m²/s³].
|
||||
float rate_error; ///< Specific kinetic energy rate error in [m²/s³].
|
||||
} ske; ///< Specific kinetic energy.
|
||||
struct {
|
||||
float rate; ///< Specific potential energy rate in [m²/s³].
|
||||
float rate_setpoint; ///< Specific potential energy setpoint rate in [m²/s³].
|
||||
float rate_error; ///< Specific potential energy rate error in [m²/s³].
|
||||
} spe; ///< Specific potential energy rate.
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Controlled altitude and pitch setpoints.
|
||||
*
|
||||
*/
|
||||
struct AltitudePitchControl {
|
||||
float altitude_rate_setpoint; ///< Controlled altitude rate setpoint [m/s].
|
||||
float tas_rate_setpoint; ///< Controlled true airspeed rate setpoint [m/s²].
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Weight factors for specific energy.
|
||||
*
|
||||
*/
|
||||
struct SpecificEnergyWeighting {
|
||||
float spe_weighting; ///< Specific potential energy weight.
|
||||
float ske_weighting; ///< Specific kinetic energy weight.
|
||||
};
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Calculate specific total energy rate limits.
|
||||
*
|
||||
* @param[in] param are the control parametes.
|
||||
* @return Specific total energy rate limits.
|
||||
*/
|
||||
STELimit _calculateTotalEnergyRateLimit(const Param ¶m) const;
|
||||
/**
|
||||
* @brief Airspeed control loop.
|
||||
*
|
||||
* @param setpoint is the control setpoints.
|
||||
* @param input is the current input measurment of the UAS.
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
* @return controlled airspeed rate setpoint in [m/s²].
|
||||
*/
|
||||
float _airspeedControl(const Setpoint &setpoint, const Input &input, const Param ¶m, const Flag &flag) const;
|
||||
/**
|
||||
* @brief Altitude control loop.
|
||||
*
|
||||
* @param setpoint is the control setpoints.
|
||||
* @param input is the current input measurment of the UAS.
|
||||
* @param param is the control parameters.
|
||||
* @return controlled altitude rate setpoint in [m/s].
|
||||
*/
|
||||
float _altitudeControl(const Setpoint &setpoint, const Input &input, const Param ¶m) const;
|
||||
/**
|
||||
* @brief Update energy balance.
|
||||
*
|
||||
* @param control_setpoint is the controlles altitude and airspeed rate setpoints.
|
||||
* @param input is the current input measurment of the UAS.
|
||||
* @return Specific energy rates.
|
||||
*/
|
||||
SpecificEnergy _updateEnergyBalance(const AltitudePitchControl &control_setpoint, const Input &input) const;
|
||||
/**
|
||||
* @brief Detect underspeed.
|
||||
*
|
||||
* @param input is the current input measurment of the UAS.
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
*/
|
||||
void _detectUnderspeed(const Input &input, const Param ¶m, const Flag &flag);
|
||||
/**
|
||||
* @brief Update specific energy balance weights.
|
||||
*
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
* @return Weights used for the specific energy balance.
|
||||
*/
|
||||
SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Param ¶m, const Flag &flag);
|
||||
/**
|
||||
* @brief Update controlled pitch setpoint.
|
||||
*
|
||||
* @param dt is the update time intervall in [s].
|
||||
* @param input is the current input measurment of the UAS.
|
||||
* @param se is the calculated specific energy.
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
*/
|
||||
void _updatePitchSetpoint(float dt, const Input &input, const SpecificEnergy &se, Param ¶m, const Flag &flag);
|
||||
/**
|
||||
* @brief Update controlled throttle setpoint.
|
||||
*
|
||||
* @param dt is the update time intervall in [s].
|
||||
* @param se is the calculated specific energy.
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
*/
|
||||
void _updateThrottleSetpoint(float dt, const SpecificEnergy &se, const Param ¶m, const Flag &flag);
|
||||
|
||||
private:
|
||||
// State
|
||||
AlphaFilter<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:
|
||||
enum ECL_TECS_MODE {
|
||||
ECL_TECS_MODE_NORMAL = 0,
|
||||
ECL_TECS_MODE_UNDERSPEED,
|
||||
ECL_TECS_MODE_BAD_DESCENT,
|
||||
ECL_TECS_MODE_CLIMBOUT
|
||||
};
|
||||
|
||||
struct DebugOutput : TECSControl::DebugOutput {
|
||||
float true_airspeed_filtered;
|
||||
float true_airspeed_derivative;
|
||||
float altitude_sp;
|
||||
float altitude_rate;
|
||||
float altitude_rate_setpoint;
|
||||
enum ECL_TECS_MODE tecs_mode;
|
||||
};
|
||||
public:
|
||||
TECS() = default;
|
||||
~TECS() = default;
|
||||
@ -61,6 +484,8 @@ public:
|
||||
TECS(TECS &&) = delete;
|
||||
TECS &operator=(TECS &&) = delete;
|
||||
|
||||
DebugOutput getStatus() const {return _debug_status;};
|
||||
|
||||
/**
|
||||
* Get the current airspeed status
|
||||
*
|
||||
@ -74,138 +499,58 @@ public:
|
||||
void enable_airspeed(bool enabled) { _airspeed_enabled = enabled; }
|
||||
|
||||
/**
|
||||
* Updates the following vehicle kineamtic state estimates:
|
||||
* Vertical position, velocity and acceleration.
|
||||
* Speed derivative
|
||||
* Must be called prior to udating tecs control loops
|
||||
* Must be called at 50Hz or greater
|
||||
* @brief Update the control loop calculations
|
||||
*
|
||||
*/
|
||||
void update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward, bool altitude_lock,
|
||||
float altitude, float vz);
|
||||
void update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
|
||||
float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_setpoint_max,
|
||||
float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
|
||||
const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
|
||||
|
||||
/**
|
||||
* Update the control loop calculations
|
||||
* @brief Initialize the control loop
|
||||
*
|
||||
*/
|
||||
void update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
|
||||
float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
|
||||
float throttle_min, float throttle_setpoint_max, float throttle_trim,
|
||||
float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN);
|
||||
|
||||
float get_throttle_setpoint() { return _last_throttle_setpoint; }
|
||||
float get_pitch_setpoint() { return _last_pitch_setpoint; }
|
||||
float get_speed_weight() { return _pitch_speed_weight; }
|
||||
float get_throttle_trim() { return _throttle_trim; }
|
||||
|
||||
void reset_state() { _states_initialized = false; }
|
||||
void initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed);
|
||||
|
||||
void resetIntegrals()
|
||||
{
|
||||
_throttle_integ_state = 0.0f;
|
||||
_pitch_integ_state = 0.0f;
|
||||
_control.resetIntegrals();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Resets the altitude and height rate control trajectory generators to the input altitude
|
||||
*
|
||||
* @param altitude Vehicle altitude (AMSL) [m]
|
||||
*/
|
||||
void resetTrajectoryGenerators(const float altitude)
|
||||
{
|
||||
_alt_control_traj_generator.reset(0, 0, altitude);
|
||||
_velocity_control_traj_generator.reset(0.0f, 0.0f, altitude);
|
||||
}
|
||||
void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; };
|
||||
|
||||
enum ECL_TECS_MODE {
|
||||
ECL_TECS_MODE_NORMAL = 0,
|
||||
ECL_TECS_MODE_UNDERSPEED,
|
||||
ECL_TECS_MODE_BAD_DESCENT,
|
||||
ECL_TECS_MODE_CLIMBOUT
|
||||
};
|
||||
// // setters for parameters
|
||||
void set_integrator_gain_throttle(float gain) { _control_param.integrator_gain_throttle = gain;};
|
||||
void set_integrator_gain_pitch(float gain) { _control_param.integrator_gain_pitch = gain; };
|
||||
|
||||
void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; }
|
||||
void set_max_sink_rate(float sink_rate) { _control_param.max_sink_rate = sink_rate; _reference_param.max_sink_rate = sink_rate; };
|
||||
void set_max_climb_rate(float climb_rate) { _control_param.max_climb_rate = climb_rate; _reference_param.max_climb_rate = climb_rate; };
|
||||
|
||||
// setters for controller parameters
|
||||
void set_integrator_gain_throttle(float gain) { _integrator_gain_throttle = gain; }
|
||||
void set_integrator_gain_pitch(float gain) { _integrator_gain_pitch = gain; }
|
||||
|
||||
void set_min_sink_rate(float rate) { _min_sink_rate = rate; }
|
||||
void set_max_sink_rate(float sink_rate) { _max_sink_rate = sink_rate; }
|
||||
void set_max_climb_rate(float climb_rate) { _max_climb_rate = climb_rate; }
|
||||
|
||||
void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; }
|
||||
void set_height_error_time_constant(float time_const) { _height_error_gain = 1.0f / math::max(time_const, 0.1f); }
|
||||
void set_altitude_rate_ff(float altitude_rate_ff) { _control_param.altitude_setpoint_gain_ff = altitude_rate_ff; };
|
||||
void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f);; };
|
||||
|
||||
void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
|
||||
void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
|
||||
void set_equivalent_airspeed_trim(float airspeed) { _equivalent_airspeed_trim = airspeed; }
|
||||
|
||||
void set_pitch_damping(float damping) { _pitch_damping_gain = damping; }
|
||||
void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; }
|
||||
void set_pitch_damping(float damping) { _control_param.pitch_damping_gain = damping; }
|
||||
void set_vertical_accel_limit(float limit) { _reference_param.vert_accel_limit = limit; _control_param.vert_accel_limit = limit; };
|
||||
|
||||
void set_speed_comp_filter_omega(float omega) { _tas_estimate_freq = omega; }
|
||||
void set_speed_weight(float weight) { _pitch_speed_weight = weight; }
|
||||
void set_airspeed_error_time_constant(float time_const) { _airspeed_error_gain = 1.0f / math::max(time_const, 0.1f); }
|
||||
void set_speed_comp_filter_omega(float omega) { _airspeed_param.airspeed_estimate_freq = omega; };
|
||||
void set_speed_weight(float weight) { _control_param.pitch_speed_weight = weight; };
|
||||
void set_airspeed_error_time_constant(float time_const) { _control_param.airspeed_error_gain = 1.0f / math::max(time_const, 0.1f); };
|
||||
|
||||
void set_throttle_damp(float throttle_damp) { _throttle_damping_gain = throttle_damp; }
|
||||
void set_throttle_slewrate(float slewrate) { _throttle_slewrate = slewrate; }
|
||||
void set_throttle_damp(float throttle_damp) { _control_param.throttle_damping_gain = throttle_damp; };
|
||||
void set_throttle_slewrate(float slewrate) { _control_param.throttle_slewrate = slewrate; };
|
||||
|
||||
void set_roll_throttle_compensation(float compensation) { _load_factor_correction = compensation; }
|
||||
void set_load_factor(float load_factor) { _load_factor = load_factor; }
|
||||
void set_roll_throttle_compensation(float compensation) { _control_param.load_factor_correction = compensation; };
|
||||
void set_load_factor(float load_factor) { _control_param.load_factor = load_factor; };
|
||||
|
||||
void set_ste_rate_time_const(float time_const) { _STE_rate_time_const = time_const; }
|
||||
void set_speed_derivative_time_constant(float time_const) { _speed_derivative_time_const = time_const; }
|
||||
|
||||
void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; }
|
||||
|
||||
// TECS status
|
||||
uint64_t timestamp() { return _pitch_update_timestamp; }
|
||||
ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
|
||||
|
||||
float hgt_setpoint() { return _hgt_setpoint; }
|
||||
float hgt_rate_from_hgt_setpoint() { return _hgt_rate_from_hgt_ref;};
|
||||
float smooth_hgt_rate_setpoint() {return _smooth_hgt_rate_setpoint;};
|
||||
float vert_pos_state() { return _vert_pos_state; }
|
||||
|
||||
float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
|
||||
float tas_state() { return _tas_state; }
|
||||
float getTASInnovation() { return _tas_innov; }
|
||||
|
||||
float hgt_rate_setpoint() { return _hgt_rate_setpoint; }
|
||||
float vert_vel_state() { return _vert_vel_state; }
|
||||
|
||||
float get_EAS_setpoint() { return _EAS_setpoint; };
|
||||
float TAS_rate_setpoint() { return _TAS_rate_setpoint; }
|
||||
float speed_derivative() { return _tas_rate_filtered; }
|
||||
float speed_derivative_raw() { return _tas_rate_raw; }
|
||||
|
||||
float STE_error() { return _STE_error; }
|
||||
float STE_rate_error() { return _STE_rate_error; }
|
||||
|
||||
float SEB_error() { return _SEB_error; }
|
||||
float SEB_rate_error() { return _SEB_rate_error; }
|
||||
|
||||
float SPE_rate() {return _SPE_rate;};
|
||||
float SKE_rate() {return _SKE_rate;};
|
||||
|
||||
float throttle_integ_state() { return _throttle_integ_state; }
|
||||
float pitch_integ_state() { return _pitch_integ_state; }
|
||||
|
||||
float STE() { return _SPE_estimate + _SKE_estimate; }
|
||||
|
||||
float STE_setpoint() { return _SPE_setpoint + _SKE_setpoint; }
|
||||
|
||||
float STE_rate() { return _SPE_rate + _SKE_rate; }
|
||||
|
||||
float STE_rate_setpoint() { return _STE_rate_setpoint; }
|
||||
|
||||
float SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; }
|
||||
|
||||
float SEB_setpoint() { return _SPE_setpoint * _SPE_weighting - _SKE_setpoint * _SKE_weighting; }
|
||||
|
||||
float SEB_rate() { return _SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting; }
|
||||
|
||||
float SEB_rate_setpoint() { return _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting; }
|
||||
void set_speed_derivative_time_constant(float time_const) { _airspeed_param.speed_derivative_time_const = time_const; };
|
||||
void set_ste_rate_time_const(float time_const) { _control_param.ste_rate_time_const = time_const; };
|
||||
|
||||
void set_seb_rate_ff_gain(float ff_gain) { _control_param.seb_rate_ff = ff_gain; };
|
||||
|
||||
/**
|
||||
* Handle the altitude reset
|
||||
@ -213,194 +558,104 @@ public:
|
||||
* If the estimation system resets the height in one discrete step this
|
||||
* will gracefully even out the reset over time.
|
||||
*/
|
||||
void handle_alt_step(float delta_alt, float altitude)
|
||||
void handle_alt_step(float altitude, float altitude_rate)
|
||||
{
|
||||
_hgt_setpoint += delta_alt;
|
||||
TECSReferenceModel::AltitudeReferenceState init_state{ .alt = altitude,
|
||||
.alt_rate = altitude_rate};
|
||||
|
||||
// reset height states
|
||||
_vert_pos_state = altitude;
|
||||
_vert_vel_state = 0.0f;
|
||||
// reset altitude reference model.
|
||||
_reference_model.initialize(init_state);
|
||||
}
|
||||
|
||||
float get_pitch_setpoint() {return _control.getPitchSetpoint();}
|
||||
float get_throttle_setpoint() {return _control.getThrottleSetpoint();}
|
||||
|
||||
// // TECS status
|
||||
uint64_t timestamp() { return _update_timestamp; }
|
||||
ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
|
||||
|
||||
static constexpr float DT_DEFAULT = 0.02f;
|
||||
|
||||
private:
|
||||
TECSControl _control; ///< Control submodule.
|
||||
TECSAirspeedFilter _airspeed_filter; ///< Airspeed filter submodule.
|
||||
TECSReferenceModel _reference_model; ///< Setpoint reference model submodule.
|
||||
|
||||
// [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors
|
||||
static constexpr float kTASErrorPercentage = 0.15;
|
||||
enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; ///< Current activated mode.
|
||||
|
||||
static constexpr float _jerk_max =
|
||||
1000.0f;
|
||||
uint64_t _update_timestamp{0}; ///< last timestamp of the update function call.
|
||||
|
||||
enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL};
|
||||
|
||||
// timestamps
|
||||
uint64_t _state_update_timestamp{0}; ///< last timestamp of the 50 Hz function call
|
||||
uint64_t _speed_update_timestamp{0}; ///< last timestamp of the speed function call
|
||||
uint64_t _pitch_update_timestamp{0}; ///< last timestamp of the pitch function call
|
||||
|
||||
// controller parameters
|
||||
float _tas_estimate_freq{0.0f}; ///< cross-over frequency of the true airspeed complementary filter (rad/sec)
|
||||
float _max_climb_rate{2.0f}; ///< climb rate produced by max allowed throttle (m/sec)
|
||||
float _min_sink_rate{1.0f}; ///< sink rate produced by min allowed throttle (m/sec)
|
||||
float _max_sink_rate{2.0f}; ///< maximum safe sink rate (m/sec)
|
||||
float _pitch_damping_gain{0.0f}; ///< damping gain of the pitch demand calculation (sec)
|
||||
float _throttle_damping_gain{0.0f}; ///< damping gain of the throttle demand calculation (sec)
|
||||
float _integrator_gain_throttle{0.0f}; ///< integrator gain used by the throttle demand calculation
|
||||
float _integrator_gain_pitch{0.0f}; ///< integrator gain used by the pitch demand calculation
|
||||
float _vert_accel_limit{0.0f}; ///< magnitude of the maximum vertical acceleration allowed (m/sec**2)
|
||||
float _load_factor{1.0f}; ///< additional normal load factor
|
||||
float _load_factor_correction{0.0f}; ///< gain from normal load factor increase to total energy rate demand (m**2/sec**3)
|
||||
float _pitch_speed_weight{1.0f}; ///< speed control weighting used by pitch demand calculation
|
||||
float _height_error_gain{0.2f}; ///< height error inverse time constant [1/s]
|
||||
float _height_setpoint_gain_ff{0.0f}; ///< gain from height demand derivative to demanded climb rate
|
||||
float _airspeed_error_gain{0.1f}; ///< airspeed error inverse time constant [1/s]
|
||||
float _equivalent_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec)
|
||||
float _equivalent_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec)
|
||||
float _equivalent_airspeed_trim{15.0f}; ///< equivalent cruise airspeed for airspeed less mode (m/sec)
|
||||
float _throttle_slewrate{0.0f}; ///< throttle demand slew rate limit (1/sec)
|
||||
float _STE_rate_time_const{0.1f}; ///< filter time constant for specific total energy rate (damping path) (s)
|
||||
float _speed_derivative_time_const{0.01f}; ///< speed derivative filter time constant (s)
|
||||
float _SEB_rate_ff{1.0f};
|
||||
|
||||
// complimentary filter states
|
||||
float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec)
|
||||
float _vert_pos_state{0.0f}; ///< complimentary filter state - height (m)
|
||||
float _tas_rate_state{0.0f}; ///< complimentary filter state - true airspeed first derivative (m/sec**2)
|
||||
float _tas_state{0.0f}; ///< complimentary filter state - true airspeed (m/sec)
|
||||
float _tas_innov{0.0f}; ///< complimentary filter true airspeed innovation (m/sec)
|
||||
|
||||
// controller states
|
||||
float _throttle_integ_state{0.0f}; ///< throttle integrator state
|
||||
float _pitch_integ_state{0.0f}; ///< pitch integrator state (rad)
|
||||
float _last_throttle_setpoint{0.0f}; ///< throttle demand rate limiter state (1/sec)
|
||||
float _last_pitch_setpoint{0.0f}; ///< pitch demand rate limiter state (rad/sec)
|
||||
float _tas_rate_filtered{0.0f}; ///< low pass filtered rate of change of speed along X axis (m/sec**2)
|
||||
|
||||
// speed demand calculations
|
||||
float _EAS{0.0f}; ///< equivalent airspeed (m/sec)
|
||||
float _TAS_max{30.0f}; ///< true airpeed demand upper limit (m/sec)
|
||||
float _TAS_min{3.0f}; ///< true airpeed demand lower limit (m/sec)
|
||||
float _TAS_setpoint{0.0f}; ///< current airpeed demand (m/sec)
|
||||
float _TAS_setpoint_last{0.0f}; ///< previous true airpeed demand (m/sec)
|
||||
float _EAS_setpoint{0.0f}; ///< Equivalent airspeed demand (m/sec)
|
||||
float _TAS_setpoint_adj{0.0f}; ///< true airspeed demand tracked by the TECS algorithm (m/sec)
|
||||
float _TAS_rate_setpoint{0.0f}; ///< true airspeed rate demand tracked by the TECS algorithm (m/sec**2)
|
||||
float _tas_rate_raw{0.0f}; ///< true airspeed rate, calculated as inertial acceleration in body X direction
|
||||
|
||||
// height demand calculations
|
||||
float _hgt_setpoint{0.0f}; ///< demanded height tracked by the TECS algorithm (m)
|
||||
float _hgt_rate_from_hgt_ref{0.0f};
|
||||
float _smooth_hgt_rate_setpoint{0.0f};
|
||||
float _hgt_rate_setpoint{0.0f}; ///< demanded climb rate tracked by the TECS algorithm
|
||||
|
||||
// vehicle physical limits
|
||||
float _pitch_setpoint_unc{0.0f}; ///< pitch demand before limiting (rad)
|
||||
float _STE_rate_max{FLT_EPSILON}; ///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3)
|
||||
float _STE_rate_min{-FLT_EPSILON}; ///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3)
|
||||
float _throttle_setpoint_max{0.0f}; ///< normalised throttle upper limit
|
||||
float _throttle_setpoint_min{0.0f}; ///< normalised throttle lower limit
|
||||
float _throttle_trim{0.0f}; ///< throttle required to fly level at _EAS_setpoint, compensated for air density and vehicle weight
|
||||
float _pitch_setpoint_max{0.5f}; ///< pitch demand upper limit (rad)
|
||||
float _pitch_setpoint_min{-0.5f}; ///< pitch demand lower limit (rad)
|
||||
|
||||
// specific energy quantities
|
||||
float _SPE_setpoint{0.0f}; ///< specific potential energy demand (m**2/sec**2)
|
||||
float _SKE_setpoint{0.0f}; ///< specific kinetic energy demand (m**2/sec**2)
|
||||
float _SPE_rate_setpoint{0.0f}; ///< specific potential energy rate demand (m**2/sec**3)
|
||||
float _SKE_rate_setpoint{0.0f}; ///< specific kinetic energy rate demand (m**2/sec**3)
|
||||
float _STE_rate_setpoint{0.0f}; ///< specific total energy rate demand (m**s/sec**3)
|
||||
float _SPE_estimate{0.0f}; ///< specific potential energy estimate (m**2/sec**2)
|
||||
float _SKE_estimate{0.0f}; ///< specific kinetic energy estimate (m**2/sec**2)
|
||||
float _SPE_rate{0.0f}; ///< specific potential energy rate estimate (m**2/sec**3)
|
||||
float _SKE_rate{0.0f}; ///< specific kinetic energy rate estimate (m**2/sec**3)
|
||||
|
||||
// specific energy error quantities
|
||||
float _STE_error{0.0f}; ///< specific total energy error (m**2/sec**2)
|
||||
float _STE_rate_error{0.0f}; ///< specific total energy rate error (m**2/sec**3)
|
||||
float _SEB_error{0.0f}; ///< specific energy balance error (m**2/sec**2)
|
||||
float _SEB_rate_error{0.0f}; ///< specific energy balance rate error (m**2/sec**3)
|
||||
|
||||
// speed height weighting
|
||||
float _SPE_weighting{1.0f};
|
||||
float _SKE_weighting{1.0f};
|
||||
|
||||
// time steps (non-fixed)
|
||||
float _dt{DT_DEFAULT}; ///< Time since last update of main TECS loop (sec)
|
||||
static constexpr float DT_DEFAULT = 0.02f; ///< default value for _dt (sec)
|
||||
float _equivalent_airspeed_trim{15.0f}; ///< equivalent cruise airspeed for airspeed less mode (m/sec)
|
||||
|
||||
// controller mode logic
|
||||
float _percent_undersped{0.0f}; ///< a continuous representation of how "undersped" the TAS is [0,1]
|
||||
bool _detect_underspeed_enabled{true}; ///< true when underspeed detection is enabled
|
||||
bool _uncommanded_descent_recovery{false}; ///< true when a continuous descent caused by an unachievable airspeed demand has been detected
|
||||
bool _climbout_mode_active{false}; ///< true when in climbout mode
|
||||
bool _airspeed_enabled{false}; ///< true when airspeed use has been enabled
|
||||
bool _states_initialized{false}; ///< true when TECS states have been iniitalized
|
||||
bool _detect_underspeed_enabled{false}; ///< true when underspeed detection is enabled
|
||||
|
||||
/**
|
||||
* Update the airspeed internal state using a second order complementary filter
|
||||
*/
|
||||
void _update_speed_states(float airspeed_setpoint, float equivalent_airspeed, float cas_to_tas);
|
||||
static constexpr float DT_MIN = 0.001f; ///< minimum allowed value of _dt (sec)
|
||||
static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a filter state reset is performed (sec)
|
||||
|
||||
static constexpr float _jerk_max = 1000.0f; ///< Magnitude of the maximum jerk allowed [m/s³].
|
||||
static constexpr float _tas_error_percentage =
|
||||
0.15f; ///< [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors
|
||||
|
||||
DebugOutput _debug_status{};
|
||||
|
||||
// Params
|
||||
/// Airspeed filter parameters.
|
||||
TECSAirspeedFilter::Param _airspeed_param{
|
||||
.equivalent_airspeed_trim = 0.0f,
|
||||
.airspeed_estimate_freq = 0.0f,
|
||||
.speed_derivative_time_const = 0.01f,
|
||||
};
|
||||
/// Reference model parameters.
|
||||
TECSReferenceModel::Param _reference_param{
|
||||
.target_climbrate = 2.0f,
|
||||
.target_sinkrate = 2.0f,
|
||||
.jerk_max = _jerk_max,
|
||||
.vert_accel_limit = 0.0f,
|
||||
.max_climb_rate = 2.0f,
|
||||
.max_sink_rate = 2.0f,
|
||||
};
|
||||
/// Control parameters.
|
||||
TECSControl::Param _control_param{
|
||||
.max_sink_rate = 2.0f,
|
||||
.max_climb_rate = 2.0f,
|
||||
.vert_accel_limit = 0.0f,
|
||||
.equivalent_airspeed_trim = 15.0f,
|
||||
.tas_min = 3.0f,
|
||||
.pitch_max = 5.0f,
|
||||
.pitch_min = -5.0f,
|
||||
.throttle_trim = 0.0f,
|
||||
.throttle_max = 1.0f,
|
||||
.throttle_min = 0.1f,
|
||||
.altitude_error_gain = 0.2f,
|
||||
.altitude_setpoint_gain_ff = 0.0f,
|
||||
.tas_error_percentage = _tas_error_percentage,
|
||||
.airspeed_error_gain = 0.1f,
|
||||
.ste_rate_time_const = 0.1f,
|
||||
.seb_rate_ff = 1.0f,
|
||||
.pitch_speed_weight = 1.0f,
|
||||
.integrator_gain_pitch = 0.0f,
|
||||
.pitch_damping_gain = 0.0f,
|
||||
.integrator_gain_throttle = 0.0f,
|
||||
.throttle_damping_gain = 0.0f,
|
||||
.throttle_slewrate = 0.0f,
|
||||
.load_factor_correction = 0.0f,
|
||||
.load_factor = 1.0f,
|
||||
};
|
||||
|
||||
/**
|
||||
* Update the desired airspeed
|
||||
*/
|
||||
void _update_speed_setpoint();
|
||||
|
||||
/**
|
||||
* Calculate desired height rate from altitude demand
|
||||
*/
|
||||
void runAltitudeControllerSmoothVelocity(float alt_sp_amsl_m, float target_climbrate_m_s, float target_sinkrate_m_s,
|
||||
float alt_amsl);
|
||||
|
||||
/**
|
||||
* Detect how undersped the aircraft is
|
||||
*/
|
||||
void _detect_underspeed();
|
||||
|
||||
/**
|
||||
* Update specific energy
|
||||
*/
|
||||
void _update_energy_estimates();
|
||||
|
||||
/**
|
||||
* Update throttle setpoint
|
||||
*/
|
||||
void _update_throttle_setpoint();
|
||||
float _update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas);
|
||||
|
||||
/**
|
||||
* Detect an uncommanded descent
|
||||
*/
|
||||
void _detect_uncommanded_descent();
|
||||
|
||||
/**
|
||||
* Update the pitch setpoint
|
||||
*/
|
||||
void _update_pitch_setpoint();
|
||||
|
||||
void _updateTrajectoryGenerationConstraints();
|
||||
|
||||
void _calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate,
|
||||
float target_sinkrate, float altitude_amsl);
|
||||
|
||||
/**
|
||||
* Initialize the controller
|
||||
*/
|
||||
void _initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout,
|
||||
float eas_to_tas);
|
||||
|
||||
/**
|
||||
* Calculate specific total energy rate limits
|
||||
*/
|
||||
void _update_STE_rate_lim();
|
||||
|
||||
void _update_speed_height_weights();
|
||||
|
||||
AlphaFilter<float> _STE_rate_error_filter;
|
||||
|
||||
AlphaFilter<float> _TAS_rate_filter;
|
||||
|
||||
VelocitySmoothing
|
||||
_alt_control_traj_generator; // generates height rate and altitude setpoint trajectory when altitude is commanded
|
||||
ManualVelocitySmoothingZ
|
||||
_velocity_control_traj_generator; // generates height rate and altitude setpoint trajectory when height rate is commanded
|
||||
|
||||
void _detect_uncommanded_descent(float throttle_setpoint_max, float altitude, float altitude_setpoint, float tas,
|
||||
float tas_setpoint);
|
||||
};
|
||||
|
||||
|
||||
@ -1,717 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
/**
|
||||
* @file TECS.cpp
|
||||
*
|
||||
* @author Paul Riseborough
|
||||
*/
|
||||
|
||||
#include "TECSnew.hpp"
|
||||
|
||||
#include <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.
|
||||
namespace newTECS
|
||||
{
|
||||
void TECSAirspeedFilter::initialize(const float equivalent_airspeed)
|
||||
{
|
||||
|
||||
|
||||
_airspeed_state.speed= equivalent_airspeed;
|
||||
_airspeed_state.speed_rate = 0.0f;
|
||||
_airspeed_rate_filter.reset(0.0f);
|
||||
}
|
||||
|
||||
void TECSAirspeedFilter::update(const float dt, const Input &input, const Param ¶m, const bool airspeed_sensor_available)
|
||||
{
|
||||
// Input checking
|
||||
if(!(PX4_ISFINITE(dt) && dt > FLT_EPSILON))
|
||||
{
|
||||
// Do not update the states.
|
||||
PX4_WARN("Time intervall is not valid.");
|
||||
return;
|
||||
}
|
||||
|
||||
float airspeed;
|
||||
if (PX4_ISFINITE(input.equivalent_airspeed) && airspeed_sensor_available) {
|
||||
airspeed = input.equivalent_airspeed;
|
||||
}
|
||||
else {
|
||||
airspeed = param.equivalent_airspeed_trim;
|
||||
}
|
||||
|
||||
float airspeed_derivative;
|
||||
if (PX4_ISFINITE(input.equivalent_airspeed_rate) && airspeed_sensor_available) {
|
||||
airspeed_derivative = input.equivalent_airspeed_rate;
|
||||
}
|
||||
else {
|
||||
airspeed_derivative = 0.0f;
|
||||
}
|
||||
|
||||
// TODO remove. Only here for compatibility check with old TECS.
|
||||
// filter true airspeed rate using first order filter with 0.5 second time constant
|
||||
_airspeed_rate_filter.setParameters(TECS::DT_DEFAULT, param.speed_derivative_time_const);
|
||||
_airspeed_rate_filter.reset(0.0f);
|
||||
|
||||
// Alpha filtering done in the TECS module. TODO merge with the second order complementary filter.
|
||||
//_airspeed_rate_filter.setParameters(dt, param.speed_derivative_time_const);
|
||||
if (PX4_ISFINITE(input.equivalent_airspeed_rate) && airspeed_sensor_available) {
|
||||
_airspeed_rate_filter.update(airspeed_derivative);
|
||||
}
|
||||
else {
|
||||
_airspeed_rate_filter.reset(0.0f);
|
||||
}
|
||||
|
||||
AirspeedFilterState new_airspeed_state;
|
||||
// Update TAS rate state
|
||||
float airspeed_innovation = airspeed - _airspeed_state.speed;
|
||||
float airspeed_rate_state_input = airspeed_innovation * param.airspeed_estimate_freq * param.airspeed_estimate_freq;
|
||||
new_airspeed_state.speed_rate = _airspeed_state.speed_rate + airspeed_rate_state_input * dt;
|
||||
|
||||
// Update TAS state // TODO the airspeed rate is applied twice.
|
||||
float airspeed_state_input = _airspeed_state.speed_rate + airspeed_derivative + airspeed_innovation * param.airspeed_estimate_freq * 1.4142f;
|
||||
new_airspeed_state.speed = _airspeed_state.speed + airspeed_state_input * dt;
|
||||
|
||||
// Clip tas at zero
|
||||
if (new_airspeed_state.speed < 0.0f) {
|
||||
// clip TAS at zero, back calculate rate // TODO Redo
|
||||
airspeed_state_input = -_airspeed_state.speed / dt;
|
||||
new_airspeed_state.speed_rate = airspeed_state_input - airspeed_derivative - airspeed_innovation * param.airspeed_estimate_freq * 1.4142f;
|
||||
new_airspeed_state.speed = 0.0f;
|
||||
}
|
||||
|
||||
// Update states
|
||||
_airspeed_state = new_airspeed_state;
|
||||
}
|
||||
|
||||
TECSAirspeedFilter::AirspeedFilterState TECSAirspeedFilter::getState() const
|
||||
{
|
||||
AirspeedFilterState filter_state{
|
||||
.speed = _airspeed_state.speed,
|
||||
.speed_rate = _airspeed_rate_filter.getState()
|
||||
};
|
||||
|
||||
return filter_state;
|
||||
}
|
||||
|
||||
void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &setpoint, float altitude, const Param ¶m)
|
||||
{
|
||||
// Input checks
|
||||
if(!(PX4_ISFINITE(dt) && dt > FLT_EPSILON))
|
||||
{
|
||||
// Do not update the states.
|
||||
PX4_WARN("Time intervall is not valid.");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(altitude)) {
|
||||
altitude = 0.0f;
|
||||
}
|
||||
// TODO rearrange handling of altitude rate and altitude. alt_rate should rather be a feedforward term.
|
||||
float virtual_altitude_setpoint{setpoint.alt};
|
||||
|
||||
// Velocity setpoint reference
|
||||
const bool input_is_altitude_rate = PX4_ISFINITE(setpoint.alt_rate);
|
||||
|
||||
_velocity_control_traj_generator.setMaxJerk(param.jerk_max);
|
||||
_velocity_control_traj_generator.setMaxAccelUp(param.vert_accel_limit);
|
||||
_velocity_control_traj_generator.setMaxAccelDown(param.vert_accel_limit);
|
||||
_velocity_control_traj_generator.setMaxVelUp(param.max_sink_rate);
|
||||
_velocity_control_traj_generator.setMaxVelDown(param.max_climb_rate);
|
||||
|
||||
if (input_is_altitude_rate) {
|
||||
_velocity_control_traj_generator.setVelSpFeedback(setpoint.alt_rate);
|
||||
_velocity_control_traj_generator.setCurrentPositionEstimate(altitude);
|
||||
_velocity_control_traj_generator.update(dt, setpoint.alt_rate);
|
||||
virtual_altitude_setpoint = _velocity_control_traj_generator.getCurrentPosition();
|
||||
} else {
|
||||
_velocity_control_traj_generator.reset(0.0f, 0.0f, altitude);
|
||||
}
|
||||
|
||||
// Altitude setpoint reference
|
||||
bool altitude_control_enable{PX4_ISFINITE(virtual_altitude_setpoint)};
|
||||
_alt_control_traj_generator.setMaxJerk(param.jerk_max);
|
||||
_alt_control_traj_generator.setMaxAccel(param.vert_accel_limit);
|
||||
_alt_control_traj_generator.setMaxVel(fmax(param.max_climb_rate, param.max_sink_rate));
|
||||
|
||||
if (altitude_control_enable)
|
||||
{
|
||||
float target_climbrate = math::min(param.target_climbrate, param.max_climb_rate);
|
||||
float target_sinkrate = math::min(param.target_sinkrate, param.max_sink_rate);
|
||||
|
||||
const float delta_trajectory_to_target_m = setpoint.alt - _alt_control_traj_generator.getCurrentPosition();
|
||||
|
||||
float altitude_rate_target = math::signNoZero<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 ¶m, const Flag &flag)
|
||||
{
|
||||
// Input checking
|
||||
if(!(PX4_ISFINITE(dt) && dt > FLT_EPSILON))
|
||||
{
|
||||
// Do not update the states and output.
|
||||
PX4_WARN("Time intervall is not valid.");
|
||||
return;
|
||||
}
|
||||
|
||||
AltitudePitchControl control_setpoint;
|
||||
|
||||
control_setpoint.tas_rate_setpoint = _airspeedControl(setpoint, input, param, flag);
|
||||
|
||||
control_setpoint.altitude_rate_setpoint = _altitudeControl(setpoint, input, param);
|
||||
|
||||
SpecificEnergy se{_updateEnergyBalance(control_setpoint, input)};
|
||||
|
||||
_detectUnderspeed(input, param, flag);
|
||||
|
||||
_updatePitchSetpoint(dt, input, se, param, flag);
|
||||
|
||||
_updateThrottleSetpoint(dt, se, param, flag);
|
||||
|
||||
_debug_output.altitude_rate_control = control_setpoint.altitude_rate_setpoint;
|
||||
_debug_output.true_airspeed_derivative_control = control_setpoint.tas_rate_setpoint;
|
||||
}
|
||||
|
||||
TECSControl::STELimit TECSControl::_calculateTotalEnergyRateLimit(const Param ¶m) const {
|
||||
TECSControl::STELimit limit;
|
||||
// Calculate the specific total energy rate limits from the max throttle limits
|
||||
limit.STE_rate_max = math::max(param.max_climb_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
|
||||
limit.STE_rate_min = - math::max(param.max_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
|
||||
|
||||
return limit;
|
||||
}
|
||||
|
||||
float TECSControl::_airspeedControl(const Setpoint &setpoint, const Input &input, const Param ¶m, const Flag &flag) const
|
||||
{
|
||||
float airspeed_rate_output{0.0f};
|
||||
|
||||
STELimit limit{_calculateTotalEnergyRateLimit(param)};
|
||||
|
||||
// calculate the demanded true airspeed rate of change based on first order response of true airspeed error
|
||||
// if airspeed measurement is not enabled then always set the rate setpoint to zero in order to avoid constant rate setpoints
|
||||
if (flag.airspeed_enabled) {
|
||||
// Calculate limits for the demanded rate of change of speed based on physical performance limits
|
||||
// with a 50% margin to allow the total energy controller to correct for errors.
|
||||
const float max_tas_rate_sp = 0.5f * limit.STE_rate_max / math::max(input.tas, FLT_EPSILON);
|
||||
const float min_tas_rate_sp = 0.5f * limit.STE_rate_min / math::max(input.tas, FLT_EPSILON);
|
||||
airspeed_rate_output = constrain((setpoint.tas_setpoint - input.tas) * param.airspeed_error_gain, min_tas_rate_sp,
|
||||
max_tas_rate_sp);
|
||||
}
|
||||
|
||||
return airspeed_rate_output;
|
||||
}
|
||||
|
||||
float TECSControl::_altitudeControl(const Setpoint &setpoint, const Input &input, const Param ¶m) const
|
||||
{
|
||||
float altitude_rate_output;
|
||||
altitude_rate_output = (setpoint.altitude_reference.alt - input.altitude) * param.altitude_error_gain + param.altitude_setpoint_gain_ff * setpoint.altitude_reference.alt_rate;
|
||||
altitude_rate_output = math::constrain(altitude_rate_output, -param.max_sink_rate, param.max_climb_rate);
|
||||
|
||||
return altitude_rate_output;
|
||||
}
|
||||
|
||||
TECSControl::SpecificEnergy TECSControl::_updateEnergyBalance(const AltitudePitchControl &control_setpoint, const Input &input) const
|
||||
{
|
||||
SpecificEnergy se;
|
||||
// Calculate specific energy rate demands in units of (m**2/sec**3)
|
||||
se.spe.rate_setpoint = control_setpoint.altitude_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change
|
||||
se.ske.rate_setpoint = input.tas * control_setpoint.tas_rate_setpoint; // kinetic energy rate of change
|
||||
|
||||
// Calculate specific energy rates in units of (m**2/sec**3)
|
||||
se.spe.rate = input.altitude_rate * CONSTANTS_ONE_G; // potential energy rate of change
|
||||
se.ske.rate = input.tas * input.tas_rate;// kinetic energy rate of change
|
||||
|
||||
// Calculate energy rate error
|
||||
se.spe.rate_error = se.spe.rate_setpoint - se.spe.rate;
|
||||
se.ske.rate_error = se.ske.rate_setpoint - se.ske.rate;
|
||||
|
||||
return se;
|
||||
}
|
||||
|
||||
void TECSControl::_detectUnderspeed(const Input &input, const Param ¶m, const Flag &flag)
|
||||
{
|
||||
if (!flag.detect_underspeed_enabled) {
|
||||
_percent_undersped = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
// this is the expected (something like standard) deviation from the airspeed setpoint that we allow the airspeed
|
||||
// to vary in before ramping in underspeed mitigation
|
||||
const float tas_error_bound = param.tas_error_percentage * param.equivalent_airspeed_trim;
|
||||
|
||||
// this is the soft boundary where underspeed mitigation is ramped in
|
||||
// NOTE: it's currently the same as the error bound, but separated here to indicate these values do not in general
|
||||
// need to be the same
|
||||
const float tas_underspeed_soft_bound = param.tas_error_percentage * param.equivalent_airspeed_trim;
|
||||
|
||||
const float tas_fully_undersped = math::max(param.tas_min - tas_error_bound - tas_underspeed_soft_bound, 0.0f);
|
||||
const float tas_starting_to_underspeed = math::max(param.tas_min - tas_error_bound, tas_fully_undersped);
|
||||
|
||||
_percent_undersped = 1.0f - math::constrain((input.tas - tas_fully_undersped) /
|
||||
math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f);
|
||||
}
|
||||
|
||||
TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Param ¶m, const Flag &flag) {
|
||||
|
||||
SpecificEnergyWeighting weight;
|
||||
// Calculate the weight applied to control of specific kinetic energy error
|
||||
float pitch_speed_weight = constrain(param.pitch_speed_weight, 0.0f, 2.0f);
|
||||
|
||||
if (flag.climbout_mode_active && flag.airspeed_enabled) {
|
||||
pitch_speed_weight = 2.0f;
|
||||
|
||||
} else if (_percent_undersped > FLT_EPSILON && flag.airspeed_enabled) {
|
||||
pitch_speed_weight = 2.0f * _percent_undersped + (1.0f - _percent_undersped) * pitch_speed_weight;
|
||||
|
||||
} else if (!flag.airspeed_enabled) {
|
||||
pitch_speed_weight = 0.0f;
|
||||
|
||||
}
|
||||
|
||||
// don't allow any weight to be larger than one, as it has the same effect as reducing the control
|
||||
// loop time constant and therefore can lead to a destabilization of that control loop
|
||||
weight.spe_weighting = constrain(2.0f - pitch_speed_weight, 0.f, 1.f);
|
||||
weight.ske_weighting = constrain(pitch_speed_weight, 0.f, 1.f);
|
||||
|
||||
return weight;
|
||||
}
|
||||
|
||||
void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const SpecificEnergy &se, Param ¶m, const Flag &flag)
|
||||
{
|
||||
SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
|
||||
/*
|
||||
* The SKE_weighting variable controls how speed and altitude control are prioritised by the pitch demand calculation.
|
||||
* A weighting of 1 givea equal speed and altitude priority
|
||||
* A weighting of 0 gives 100% priority to altitude control and must be used when no airspeed measurement is available.
|
||||
* A weighting of 2 provides 100% priority to speed control and is used when:
|
||||
* a) an underspeed condition is detected.
|
||||
* b) during climbout where a minimum pitch angle has been set to ensure altitude is gained. If the airspeed
|
||||
* rises above the demanded value, the pitch angle demand is increased by the TECS controller to prevent the vehicle overspeeding.
|
||||
* The weighting can be adjusted between 0 and 2 depending on speed and altitude accuracy requirements.
|
||||
*/
|
||||
// Calculate the specific energy balance rate demand
|
||||
float seb_rate_setpoint = se.spe.rate_setpoint * weight.spe_weighting - se.ske.rate_setpoint * weight.ske_weighting;
|
||||
|
||||
// Calculate the specific energy balance rate error
|
||||
float seb_rate_error = (se.spe.rate_error * weight.spe_weighting) - (se.ske.rate_error * weight.ske_weighting);
|
||||
|
||||
_debug_output.energy_balance_rate_error = seb_rate_error;
|
||||
_debug_output.energy_balance_rate_sp = seb_rate_setpoint;
|
||||
|
||||
if (param.integrator_gain_pitch > 0.0f) {
|
||||
// Calculate pitch integrator input term
|
||||
float pitch_integ_input = seb_rate_error * param.integrator_gain_pitch;
|
||||
|
||||
// Prevent the integrator changing in a direction that will increase pitch demand saturation
|
||||
if (_pitch_setpoint > param.pitch_max) {
|
||||
pitch_integ_input = min(pitch_integ_input, 0.f);
|
||||
|
||||
} else if (_pitch_setpoint < param.pitch_min) {
|
||||
pitch_integ_input = max(pitch_integ_input, 0.f);
|
||||
}
|
||||
|
||||
// Update the pitch integrator state.
|
||||
_pitch_integ_state = _pitch_integ_state + pitch_integ_input * dt;
|
||||
|
||||
} else {
|
||||
_pitch_integ_state = 0.0f;
|
||||
}
|
||||
|
||||
// Calculate a specific energy correction that doesn't include the integrator contribution
|
||||
float SEB_rate_correction = seb_rate_error * param.pitch_damping_gain + _pitch_integ_state + param.seb_rate_ff *
|
||||
seb_rate_setpoint;
|
||||
|
||||
// Calculate derivative from change in climb angle to rate of change of specific energy balance
|
||||
const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G;
|
||||
|
||||
// During climbout, bias the demanded pitch angle so that a zero speed error produces a pitch angle
|
||||
// demand equal to the minimum pitch angle set by the mission plan. This prevents the integrator
|
||||
// having to catch up before the nose can be raised to reduce excess speed during climbout.
|
||||
if (flag.climbout_mode_active) {
|
||||
SEB_rate_correction += param.pitch_min * climb_angle_to_SEB_rate;
|
||||
}
|
||||
|
||||
// Convert the specific energy balance rate correction to a target pitch angle. This calculation assumes:
|
||||
// a) The climb angle follows pitch angle with a lag that is small enough not to destabilise the control loop.
|
||||
// b) The offset between climb angle and pitch angle (angle of attack) is constant, excluding the effect of
|
||||
// pitch transients due to control action or turbulence.
|
||||
float pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate;
|
||||
|
||||
float pitch_setpoint = constrain(pitch_setpoint_unc, param.pitch_min, param.pitch_max);
|
||||
|
||||
// Comply with the specified vertical acceleration limit by applying a pitch rate limit
|
||||
// NOTE: at zero airspeed, the pitch increment is unbounded
|
||||
const float pitch_increment = dt * param.vert_accel_limit / input.tas;
|
||||
_pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment,
|
||||
_pitch_setpoint + pitch_increment);
|
||||
}
|
||||
|
||||
void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, const Param ¶m, const Flag &flag)
|
||||
{
|
||||
STELimit limit{_calculateTotalEnergyRateLimit(param)};
|
||||
|
||||
float STE_rate_setpoint = se.spe.rate_setpoint + se.ske.rate_setpoint;
|
||||
|
||||
// Adjust the demanded total energy rate to compensate for induced drag rise in turns.
|
||||
// Assume induced drag scales linearly with normal load factor.
|
||||
// The additional normal load factor is given by (1/cos(bank angle) - 1)
|
||||
STE_rate_setpoint += param.load_factor_correction * (param.load_factor - 1.f);
|
||||
|
||||
STE_rate_setpoint = constrain(STE_rate_setpoint, limit.STE_rate_min, limit.STE_rate_max);
|
||||
|
||||
_ste_rate = se.spe.rate + se.ske.rate;
|
||||
|
||||
float STE_rate_error_raw = se.spe.rate_error + se.ske.rate_error;
|
||||
// TODO rmeove reset and add correct time intervall
|
||||
_ste_rate_error_filter.setParameters(TECS::DT_DEFAULT, param.ste_rate_time_const);
|
||||
_ste_rate_error_filter.reset(0.0f);
|
||||
_ste_rate_error_filter.update(STE_rate_error_raw);
|
||||
float STE_rate_error{_ste_rate_error_filter.getState()};
|
||||
|
||||
_debug_output.total_energy_rate_error = STE_rate_error;
|
||||
_debug_output.total_energy_rate_sp = STE_rate_setpoint;
|
||||
|
||||
// Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle
|
||||
// as the starting point. Assume:
|
||||
// Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max
|
||||
// Specific total energy rate = 0 at cruise throttle
|
||||
// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
|
||||
float throttle_predicted = 0.0f;
|
||||
|
||||
if (STE_rate_setpoint >= 0) {
|
||||
// throttle is between trim and maximum
|
||||
throttle_predicted = param.throttle_trim + STE_rate_setpoint / limit.STE_rate_max * (param.throttle_max - param.throttle_trim);
|
||||
|
||||
} else {
|
||||
// throttle is between trim and minimum
|
||||
throttle_predicted = param.throttle_trim + STE_rate_setpoint / limit.STE_rate_min * (param.throttle_min - param.throttle_trim);
|
||||
|
||||
}
|
||||
|
||||
// Calculate gain scaler from specific energy rate error to throttle
|
||||
const float STE_rate_to_throttle = 1.0f / (limit.STE_rate_max - limit.STE_rate_min);
|
||||
|
||||
// Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits
|
||||
float throttle_setpoint = (STE_rate_error * param.throttle_damping_gain) * STE_rate_to_throttle + throttle_predicted;
|
||||
throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
|
||||
|
||||
// Integral handling
|
||||
if (flag.airspeed_enabled) {
|
||||
if (param.integrator_gain_throttle > 0.0f) {
|
||||
float integ_state_max = param.throttle_max - throttle_setpoint;
|
||||
float integ_state_min = param.throttle_min - throttle_setpoint;
|
||||
|
||||
// underspeed conditions zero out integration
|
||||
float throttle_integ_input = (STE_rate_error * param.integrator_gain_throttle) * dt *
|
||||
STE_rate_to_throttle * (1.0f - _percent_undersped);
|
||||
|
||||
// only allow integrator propagation into direction which unsaturates throttle
|
||||
if (_throttle_integ_state > integ_state_max) {
|
||||
throttle_integ_input = math::min(0.f, throttle_integ_input);
|
||||
|
||||
} else if (_throttle_integ_state < integ_state_min) {
|
||||
throttle_integ_input = math::max(0.f, throttle_integ_input);
|
||||
}
|
||||
|
||||
// Calculate a throttle demand from the integrated total energy rate error
|
||||
// This will be added to the total throttle demand to compensate for steady state errors
|
||||
_throttle_integ_state = _throttle_integ_state + throttle_integ_input;
|
||||
|
||||
if (flag.climbout_mode_active) {
|
||||
// During climbout, set the integrator to maximum throttle to prevent transient throttle drop
|
||||
// at end of climbout when we transition to closed loop throttle control
|
||||
_throttle_integ_state = integ_state_max;
|
||||
}
|
||||
|
||||
} else {
|
||||
_throttle_integ_state = 0.0f;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (flag.airspeed_enabled) {
|
||||
// Add the integrator feedback during closed loop operation with an airspeed sensor
|
||||
throttle_setpoint += _throttle_integ_state;
|
||||
|
||||
} else {
|
||||
// when flying without an airspeed sensor, use the predicted throttle only
|
||||
throttle_setpoint = throttle_predicted;
|
||||
|
||||
}
|
||||
|
||||
// ramp in max throttle setting with underspeediness value
|
||||
throttle_setpoint = _percent_undersped * param.throttle_max + (1.0f - _percent_undersped) * throttle_setpoint;
|
||||
|
||||
// Rate limit the throttle demand
|
||||
if (fabsf(param.throttle_slewrate) > 0.01f) {
|
||||
const float throttle_increment_limit = dt * (param.throttle_max - param.throttle_min) * param.throttle_slewrate;
|
||||
throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint - throttle_increment_limit,
|
||||
_throttle_setpoint + throttle_increment_limit);
|
||||
}
|
||||
|
||||
_throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
|
||||
}
|
||||
|
||||
void TECSControl::resetIntegrals()
|
||||
{
|
||||
_pitch_integ_state = 0.0f;
|
||||
_throttle_integ_state = 0.0f;
|
||||
}
|
||||
|
||||
float TECS::_update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas)
|
||||
{
|
||||
float new_setpoint{tas_setpoint};
|
||||
float percent_undersped = _control.getPercentUndersped();
|
||||
// Set the TAS demand to the minimum value if an underspeed or
|
||||
// or a uncontrolled descent condition exists to maximise climb rate
|
||||
if (_uncommanded_descent_recovery) {
|
||||
new_setpoint = tas_min;
|
||||
|
||||
} else if (percent_undersped > FLT_EPSILON) {
|
||||
// TAS setpoint is reset from external setpoint every time tecs is called, so the interpolation is still
|
||||
// between current setpoint and mininimum airspeed here (it's not feeding the newly adjusted setpoint
|
||||
// from this line back into this method each time).
|
||||
// TODO: WOULD BE GOOD to "functionalize" this library a bit and remove many of these internal states to
|
||||
// avoid the fear of side effects in simple operations like this.
|
||||
new_setpoint = tas_min * percent_undersped + (1.0f - percent_undersped) * tas_setpoint;
|
||||
}
|
||||
|
||||
new_setpoint = constrain(new_setpoint, tas_min, tas_max);
|
||||
|
||||
return new_setpoint;
|
||||
}
|
||||
|
||||
void TECS::_detect_uncommanded_descent(float throttle_setpoint_max, float altitude, float altitude_setpoint, float tas, float tas_setpoint)
|
||||
{
|
||||
/*
|
||||
* This function detects a condition that can occur when the demanded airspeed is greater than the
|
||||
* aircraft can achieve in level flight. When this occurs, the vehicle will continue to reduce altitude
|
||||
* while attempting to maintain speed.
|
||||
*/
|
||||
|
||||
// Calculate specific energy demands in units of (m**2/sec**2)
|
||||
float SPE_setpoint = altitude_setpoint * CONSTANTS_ONE_G; // potential energy
|
||||
float SKE_setpoint = 0.5f * altitude_setpoint * altitude_setpoint; // kinetic energy
|
||||
|
||||
// Calculate specific energies in units of (m**2/sec**2)
|
||||
float SPE_estimate = altitude * CONSTANTS_ONE_G; // potential energy
|
||||
float SKE_estimate = 0.5f * tas * tas; // kinetic energy
|
||||
|
||||
// Calculate total energy error
|
||||
float SPE_error = SPE_setpoint - SPE_estimate;
|
||||
float SKE_error = SKE_setpoint - SKE_estimate;
|
||||
float STE_error = SPE_error + SKE_error;
|
||||
|
||||
|
||||
|
||||
const bool underspeed_detected = _control.getPercentUndersped() > FLT_EPSILON;
|
||||
|
||||
// If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode
|
||||
const bool enter_mode = !_uncommanded_descent_recovery && !underspeed_detected && (STE_error > 200.0f)
|
||||
&& (_control.getSteRate() < 0.0f)
|
||||
&& (_control.getThrottleSetpoint() >= throttle_setpoint_max * 0.9f);
|
||||
|
||||
// If we enter an underspeed condition or recover the required total energy, then exit uncommanded descent recovery mode
|
||||
const bool exit_mode = _uncommanded_descent_recovery && (underspeed_detected || (STE_error < 0.0f));
|
||||
|
||||
if (enter_mode) {
|
||||
_uncommanded_descent_recovery = true;
|
||||
|
||||
} else if (exit_mode) {
|
||||
_uncommanded_descent_recovery = false;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed)
|
||||
{
|
||||
// Init subclasses
|
||||
TECSReferenceModel::AltitudeReferenceState current_state{ .alt=altitude,
|
||||
.alt_rate = altitude_rate};
|
||||
_reference_model.initialize(current_state);
|
||||
_airspeed_filter.initialize(equivalent_airspeed);
|
||||
_control.initialize();
|
||||
}
|
||||
|
||||
void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_setpoint_max, float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp)
|
||||
{
|
||||
// Calculate the time since last update (seconds)
|
||||
uint64_t now = hrt_absolute_time();
|
||||
float dt = (now - _update_timestamp) * 1e-6f;
|
||||
if (dt < DT_MIN)
|
||||
{
|
||||
// Update intervall too small, do not update. Assume constant states/output in this case.
|
||||
return;
|
||||
}
|
||||
|
||||
if (dt > DT_MAX || _update_timestamp == 0UL)
|
||||
{
|
||||
// Update time intervall too large, can't guarantee sanity of state updates anymore. reset the control loop.
|
||||
initialize(altitude, hgt_rate, equivalent_airspeed);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Update airspeedfilter submodule
|
||||
TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed=equivalent_airspeed,
|
||||
.equivalent_airspeed_rate = speed_deriv_forward/eas_to_tas};
|
||||
_airspeed_param.equivalent_airspeed_trim = _equivalent_airspeed_trim;
|
||||
_airspeed_filter.update(dt, airspeed_input,_airspeed_param, _airspeed_enabled);
|
||||
TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState();
|
||||
|
||||
// Update Reference model submodule
|
||||
TECSReferenceModel::AltitudeReferenceState setpoint{ .alt=hgt_setpoint,
|
||||
.alt_rate=hgt_rate_sp
|
||||
};
|
||||
_reference_param.target_climbrate = target_climbrate;
|
||||
_reference_param.target_sinkrate = target_sinkrate;
|
||||
_reference_model.update(dt, setpoint, altitude, _reference_param);
|
||||
TECSControl::Setpoint control_setpoint;
|
||||
control_setpoint.altitude_reference = _reference_model.getAltitudeReference();
|
||||
control_setpoint.altitude_rate_setpoint = _reference_model.getAltitudeRateReference();
|
||||
|
||||
// Calculate the demanded true airspeed
|
||||
// TODO this function should not be in the module. Only give feedback that the airspeed can't be achieved.
|
||||
control_setpoint.tas_setpoint =_update_speed_setpoint(eas_to_tas*_equivalent_airspeed_min,eas_to_tas*_equivalent_airspeed_max, EAS_setpoint*eas_to_tas, eas_to_tas*eas.speed);
|
||||
|
||||
TECSControl::Input control_input{ .altitude =altitude,
|
||||
.altitude_rate = hgt_rate,
|
||||
.tas = eas_to_tas*eas.speed,
|
||||
.tas_rate = eas_to_tas*eas.speed_rate
|
||||
};
|
||||
_control_param.equivalent_airspeed_trim = _equivalent_airspeed_trim;
|
||||
_control_param.tas_min = eas_to_tas*_equivalent_airspeed_min;
|
||||
_control_param.pitch_max = pitch_limit_max;
|
||||
_control_param.pitch_min = pitch_limit_min;
|
||||
_control_param.throttle_trim = throttle_trim;
|
||||
_control_param.throttle_max = throttle_setpoint_max;
|
||||
_control_param.throttle_min = throttle_min;
|
||||
TECSControl::Flag control_flag{ .airspeed_enabled = _airspeed_enabled,
|
||||
.climbout_mode_active = climb_out_setpoint,
|
||||
.detect_underspeed_enabled = _detect_underspeed_enabled
|
||||
};
|
||||
_control.update(dt, control_setpoint, control_input, _control_param, control_flag);
|
||||
|
||||
// Detect an uncommanded descent caused by an unachievable airspeed demand
|
||||
_detect_uncommanded_descent(throttle_setpoint_max, altitude, hgt_setpoint, equivalent_airspeed*eas_to_tas, control_setpoint.tas_setpoint);
|
||||
|
||||
TECSControl::DebugOutput control_status = _control.getDebugOutput();
|
||||
_debug_status.altitude_rate_control = control_status.altitude_rate_control;
|
||||
_debug_status.energy_balance_rate_error = control_status.energy_balance_rate_error;
|
||||
_debug_status.energy_balance_rate_sp = control_status.energy_balance_rate_sp;
|
||||
_debug_status.total_energy_rate_error = control_status.total_energy_rate_error;
|
||||
_debug_status.total_energy_rate_sp = control_status.total_energy_rate_sp;
|
||||
_debug_status.true_airspeed_derivative_control = control_status.true_airspeed_derivative_control;
|
||||
_debug_status.true_airspeed_filtered = eas_to_tas*eas.speed;
|
||||
_debug_status.true_airspeed_derivative = eas_to_tas*eas.speed_rate;
|
||||
_debug_status.altitude_sp = control_setpoint.altitude_reference.alt;
|
||||
_debug_status.altitude_rate = control_setpoint.altitude_reference.alt_rate;
|
||||
_debug_status.altitude_rate_setpoint = control_setpoint.altitude_rate_setpoint;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Update time stamps
|
||||
_update_timestamp = now;
|
||||
|
||||
|
||||
// Set TECS mode for next frame
|
||||
if (_control.getPercentUndersped() > FLT_EPSILON) {
|
||||
_tecs_mode = ECL_TECS_MODE_UNDERSPEED;
|
||||
|
||||
} else if (_uncommanded_descent_recovery) {
|
||||
_tecs_mode = ECL_TECS_MODE_BAD_DESCENT;
|
||||
|
||||
} else if (climb_out_setpoint) {
|
||||
_tecs_mode = ECL_TECS_MODE_CLIMBOUT;
|
||||
|
||||
} else {
|
||||
// This is the default operation mode
|
||||
_tecs_mode = ECL_TECS_MODE_NORMAL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,651 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file tecs.cpp
|
||||
*
|
||||
* @author Paul Riseborough
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <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>
|
||||
|
||||
namespace newTECS
|
||||
{
|
||||
class TECSAirspeedFilter {
|
||||
public:
|
||||
/**
|
||||
* @brief State of the equivalent airspeed filter.
|
||||
*
|
||||
*/
|
||||
struct AirspeedFilterState
|
||||
{
|
||||
float speed; ///< speed of the air in EAS [m/s]
|
||||
float speed_rate; ///< rate of speed of the air [m/s²]
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Parameters of the airspeed filter.
|
||||
*
|
||||
*/
|
||||
struct Param
|
||||
{
|
||||
float equivalent_airspeed_trim; ///< the trim value of the equivalent airspeed om [m/s].
|
||||
float airspeed_estimate_freq; ///< cross-over frequency of the equivalent airspeed complementary filter [rad/sec].
|
||||
float speed_derivative_time_const; ///< speed derivative filter time constant [s].
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Input, which will be filtered.
|
||||
*
|
||||
*/
|
||||
struct Input
|
||||
{
|
||||
float equivalent_airspeed; ///< the measured equivalent airspeed in [m/s].
|
||||
float equivalent_airspeed_rate; ///< the measured rate of equivalent airspeed in [m/s²].
|
||||
};
|
||||
public:
|
||||
TECSAirspeedFilter() = default;
|
||||
~TECSAirspeedFilter() = default;
|
||||
/**
|
||||
* @brief Initialize filter
|
||||
*
|
||||
* @param[in] equivalent_airspeed is the equivalent airspeed in [m/s].
|
||||
*/
|
||||
void initialize(float equivalent_airspeed);
|
||||
|
||||
/**
|
||||
* @brief Update filter
|
||||
*
|
||||
* @param[in] dt is the timestep in [s].
|
||||
* @param[in] input are the raw measured values.
|
||||
* @param[in] param are the filter parameters.
|
||||
* @param[in] airspeed_sensor_available boolean if the airspeed sensor is available.
|
||||
*/
|
||||
void update(float dt, const Input &input, const Param ¶m, const bool airspeed_sensor_available);
|
||||
|
||||
/**
|
||||
* @brief Get the filtered airspeed states.
|
||||
*
|
||||
* @return Current state of the airspeed filter.
|
||||
*/
|
||||
AirspeedFilterState getState() const;
|
||||
|
||||
private:
|
||||
// States
|
||||
AlphaFilter<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 ¶m);
|
||||
|
||||
/**
|
||||
* @brief Get the current altitude reference of altitude reference model.
|
||||
*
|
||||
* @return Altitude reference state.
|
||||
*/
|
||||
AltitudeReferenceState getAltitudeReference() const;
|
||||
|
||||
/**
|
||||
* @brief Get the altitude rate reference of the altitude rate reference model.
|
||||
*
|
||||
* @return Current altitude rate reference point.
|
||||
*/
|
||||
float getAltitudeRateReference() const;
|
||||
|
||||
private:
|
||||
// State
|
||||
VelocitySmoothing
|
||||
_alt_control_traj_generator; ///< Generates altitude rate and altitude setpoint trajectory when altitude is commanded.
|
||||
|
||||
ManualVelocitySmoothingZ
|
||||
_velocity_control_traj_generator; ///< Generates altitude rate setpoint when altitude rate is commanded.
|
||||
};
|
||||
|
||||
class TECSControl {
|
||||
public:
|
||||
/**
|
||||
* @brief The control parameters.
|
||||
*
|
||||
*/
|
||||
struct Param
|
||||
{
|
||||
// Vehicle specific params
|
||||
float max_sink_rate; ///< Maximum safe sink rate [m/s].
|
||||
float max_climb_rate; ///< Climb rate produced by max allowed throttle [m/s].
|
||||
float vert_accel_limit; ///< Magnitude of the maximum vertical acceleration allowed [m/s²].
|
||||
float equivalent_airspeed_trim; ///< Equivalent cruise airspeed for airspeed less mode [m/s].
|
||||
float tas_min; ///< True airpeed demand lower limit [m/s].
|
||||
float pitch_max; ///< Maximum pitch angle allowed in [rad].
|
||||
float pitch_min; ///< Minimal pitch angle allowed in [rad].
|
||||
float throttle_trim; ///< Normalized throttle required to fly level at given eas.
|
||||
float throttle_max; ///< Normalized throttle upper limit.
|
||||
float throttle_min; ///< Normalized throttle lower limit.
|
||||
|
||||
// Altitude control param
|
||||
float altitude_error_gain; ///< Altitude error inverse time constant [1/s].
|
||||
float altitude_setpoint_gain_ff; ///< Gain from altitude demand derivative to demanded climb rate.
|
||||
|
||||
// Airspeed control param
|
||||
/// [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors
|
||||
float tas_error_percentage;
|
||||
float airspeed_error_gain; ///< Airspeed error inverse time constant [1/s].
|
||||
|
||||
// Energy control param
|
||||
float ste_rate_time_const; ///< Filter time constant for specific total energy rate (damping path) [s].
|
||||
float seb_rate_ff; ///< Specific energy balance rate feedforward gain.
|
||||
|
||||
// Pitch control param
|
||||
float pitch_speed_weight; ///< Speed control weighting used by pitch demand calculation.
|
||||
float integrator_gain_pitch; ///< Integrator gain used by the pitch demand calculation.
|
||||
float pitch_damping_gain; ///< Damping gain of the pitch demand calculation [s].
|
||||
|
||||
// Throttle control param
|
||||
float integrator_gain_throttle; ///< Integrator gain used by the throttle demand calculation.
|
||||
float throttle_damping_gain; ///< Damping gain of the throttle demand calculation [s].
|
||||
float throttle_slewrate; ///< Throttle demand slew rate limit [1/s].
|
||||
|
||||
float load_factor_correction; ///< Gain from normal load factor increase to total energy rate demand [m²/s³].
|
||||
float load_factor; ///< Additional normal load factor.
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief The debug output
|
||||
*
|
||||
*/
|
||||
struct DebugOutput
|
||||
{
|
||||
float altitude_rate_control; ///< Altitude rate setpoint from altitude control loop [m/s].
|
||||
float true_airspeed_derivative_control; ///< Airspeed rate setpoint from airspeed control loop [m/s²].
|
||||
float total_energy_rate_error; ///< Total energy rate error [m²/s³].
|
||||
float total_energy_rate_sp; ///< Total energy rate setpoint [m²/s³].
|
||||
float energy_balance_rate_error; ///< Energy balance rate error [m²/s³].
|
||||
float energy_balance_rate_sp; ///< Energy balance rate setpoint [m²/s³].
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Given setpoint to control.
|
||||
*
|
||||
*/
|
||||
struct Setpoint
|
||||
{
|
||||
TECSReferenceModel::AltitudeReferenceState altitude_reference; ///< Altitude reference from reference model.
|
||||
float altitude_rate_setpoint; ///< Altitude rate setpoint.
|
||||
float tas_setpoint; ///< True airspeed setpoint.
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Givent current measurement from the UAS.
|
||||
*
|
||||
*/
|
||||
struct Input
|
||||
{
|
||||
float altitude; ///< Current altitude of the UAS [m].
|
||||
float altitude_rate; ///< Current altitude rate of the UAS [m/s].
|
||||
float tas; ///< Current true airspeed of the UAS [m/s].
|
||||
float tas_rate; ///< Current true airspeed rate of the UAS [m/s²].
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Control flags.
|
||||
*
|
||||
*/
|
||||
struct Flag
|
||||
{
|
||||
bool airspeed_enabled; ///< Flag if the airspeed sensor is enabled.
|
||||
bool climbout_mode_active; ///< Flag if climbout mode is activated.
|
||||
bool detect_underspeed_enabled; ///< Flag if underspeed detection is enabled.
|
||||
};
|
||||
public:
|
||||
TECSControl() = default;
|
||||
~TECSControl() = default;
|
||||
/**
|
||||
* @brief Initialization of the state.
|
||||
*
|
||||
*/
|
||||
void initialize();
|
||||
/**
|
||||
* @brief Update state and output.
|
||||
*
|
||||
* @param[in] dt is the update time intervall in [s].
|
||||
* @param[in] setpoint is the current setpoint struct.
|
||||
* @param[in] input is the current input measurements.
|
||||
* @param[in] param is the current parameter set.
|
||||
* @param[in] flag is the current activated flags.
|
||||
*/
|
||||
void update(float dt, const Setpoint &setpoint, const Input &input, Param ¶m, const Flag &flag);
|
||||
/**
|
||||
* @brief Reset the control loop integrals.
|
||||
*
|
||||
*/
|
||||
void resetIntegrals();
|
||||
/**
|
||||
* @brief Get the percent of the undersped.
|
||||
*
|
||||
* @return Percentage of detected undersped.
|
||||
*/
|
||||
float getPercentUndersped() const {return _percent_undersped;};
|
||||
/**
|
||||
* @brief Get the throttle setpoint.
|
||||
*
|
||||
* @return throttle setpoint.
|
||||
*/
|
||||
float getThrottleSetpoint() const {return _throttle_setpoint;};
|
||||
/**
|
||||
* @brief Get the pitch setpoint.
|
||||
*
|
||||
* @return THe commanded pitch angle in [rad].
|
||||
*/
|
||||
float getPitchSetpoint() const {return _pitch_setpoint;};
|
||||
/**
|
||||
* @brief Get specific total energy rate.
|
||||
*
|
||||
* @return the total specific energy rate in [m²/s³].
|
||||
*/
|
||||
float getSteRate() const {return _ste_rate;};
|
||||
/**
|
||||
* @brief Get the Debug Output
|
||||
*
|
||||
* @return the debug outpus struct.
|
||||
*/
|
||||
DebugOutput getDebugOutput() const {return _debug_output;};
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Specific total energy limit.
|
||||
*
|
||||
*/
|
||||
struct STELimit
|
||||
{
|
||||
float STE_rate_max; ///< Maximum specific total energy rate limit [m²/s³].
|
||||
float STE_rate_min; ///< Minimal specific total energy rate limit [m²/s³].
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Calculated specific energy.
|
||||
*
|
||||
*/
|
||||
struct SpecificEnergy
|
||||
{
|
||||
struct {
|
||||
float rate; ///< Specific kinetic energy rate in [m²/s³].
|
||||
float rate_setpoint; ///< Specific kinetic energy setpoint rate in [m²/s³].
|
||||
float rate_error; ///< Specific kinetic energy rate error in [m²/s³].
|
||||
} ske; ///< Specific kinetic energy.
|
||||
struct {
|
||||
float rate; ///< Specific potential energy rate in [m²/s³].
|
||||
float rate_setpoint; ///< Specific potential energy setpoint rate in [m²/s³].
|
||||
float rate_error; ///< Specific potential energy rate error in [m²/s³].
|
||||
} spe; ///< Specific potential energy rate.
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Controlled altitude and pitch setpoints.
|
||||
*
|
||||
*/
|
||||
struct AltitudePitchControl {
|
||||
float altitude_rate_setpoint; ///< Controlled altitude rate setpoint [m/s].
|
||||
float tas_rate_setpoint; ///< Controlled true airspeed rate setpoint [m/s²].
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Weight factors for specific energy.
|
||||
*
|
||||
*/
|
||||
struct SpecificEnergyWeighting {
|
||||
float spe_weighting; ///< Specific potential energy weight.
|
||||
float ske_weighting; ///< Specific kinetic energy weight.
|
||||
};
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Calculate specific total energy rate limits.
|
||||
*
|
||||
* @param[in] param are the control parametes.
|
||||
* @return Specific total energy rate limits.
|
||||
*/
|
||||
STELimit _calculateTotalEnergyRateLimit(const Param ¶m) const;
|
||||
/**
|
||||
* @brief Airspeed control loop.
|
||||
*
|
||||
* @param setpoint is the control setpoints.
|
||||
* @param input is the current input measurment of the UAS.
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
* @return controlled airspeed rate setpoint in [m/s²].
|
||||
*/
|
||||
float _airspeedControl(const Setpoint &setpoint, const Input &input, const Param ¶m, const Flag &flag) const;
|
||||
/**
|
||||
* @brief Altitude control loop.
|
||||
*
|
||||
* @param setpoint is the control setpoints.
|
||||
* @param input is the current input measurment of the UAS.
|
||||
* @param param is the control parameters.
|
||||
* @return controlled altitude rate setpoint in [m/s].
|
||||
*/
|
||||
float _altitudeControl(const Setpoint &setpoint, const Input &input, const Param ¶m) const;
|
||||
/**
|
||||
* @brief Update energy balance.
|
||||
*
|
||||
* @param control_setpoint is the controlles altitude and airspeed rate setpoints.
|
||||
* @param input is the current input measurment of the UAS.
|
||||
* @return Specific energy rates.
|
||||
*/
|
||||
SpecificEnergy _updateEnergyBalance(const AltitudePitchControl &control_setpoint, const Input &input) const;
|
||||
/**
|
||||
* @brief Detect underspeed.
|
||||
*
|
||||
* @param input is the current input measurment of the UAS.
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
*/
|
||||
void _detectUnderspeed(const Input &input, const Param ¶m, const Flag &flag);
|
||||
/**
|
||||
* @brief Update specific energy balance weights.
|
||||
*
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
* @return Weights used for the specific energy balance.
|
||||
*/
|
||||
SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Param ¶m, const Flag &flag);
|
||||
/**
|
||||
* @brief Update controlled pitch setpoint.
|
||||
*
|
||||
* @param dt is the update time intervall in [s].
|
||||
* @param input is the current input measurment of the UAS.
|
||||
* @param se is the calculated specific energy.
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
*/
|
||||
void _updatePitchSetpoint(float dt, const Input &input, const SpecificEnergy &se, Param ¶m, const Flag &flag);
|
||||
/**
|
||||
* @brief Update controlled throttle setpoint.
|
||||
*
|
||||
* @param dt is the update time intervall in [s].
|
||||
* @param se is the calculated specific energy.
|
||||
* @param param is the control parameters.
|
||||
* @param flag is the control flags.
|
||||
*/
|
||||
void _updateThrottleSetpoint(float dt, const SpecificEnergy &se, const Param ¶m, const Flag &flag);
|
||||
|
||||
private:
|
||||
// State
|
||||
AlphaFilter<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);
|
||||
};
|
||||
}
|
||||
|
||||
@ -47,6 +47,5 @@ px4_add_module(
|
||||
runway_takeoff
|
||||
SlewRate
|
||||
tecs
|
||||
tecsnew
|
||||
motion_planning
|
||||
)
|
||||
|
||||
@ -129,7 +129,6 @@ FixedwingPositionControl::parameters_update()
|
||||
_tecs.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get());
|
||||
_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get());
|
||||
_tecs.set_equivalent_airspeed_max(_param_fw_airspd_max.get());
|
||||
_tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
|
||||
_tecs.set_throttle_damp(_param_fw_t_thr_damp.get());
|
||||
_tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get());
|
||||
_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
|
||||
@ -138,34 +137,13 @@ FixedwingPositionControl::parameters_update()
|
||||
_tecs.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get());
|
||||
_tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
|
||||
_tecs.set_pitch_damping(_param_fw_t_ptch_damp.get());
|
||||
_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
|
||||
_tecs.set_heightrate_ff(_param_fw_t_hrate_ff.get());
|
||||
_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
|
||||
_tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get());
|
||||
_tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get());
|
||||
_tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get());
|
||||
_tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get());
|
||||
_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
|
||||
|
||||
_tecsnew.set_max_climb_rate(_param_fw_t_clmb_max.get());
|
||||
_tecsnew.set_max_sink_rate(_param_fw_t_sink_max.get());
|
||||
_tecsnew.set_speed_weight(_param_fw_t_spdweight.get());
|
||||
_tecsnew.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get());
|
||||
_tecsnew.set_equivalent_airspeed_min(_param_fw_airspd_min.get());
|
||||
_tecsnew.set_equivalent_airspeed_max(_param_fw_airspd_max.get());
|
||||
_tecsnew.set_throttle_damp(_param_fw_t_thr_damp.get());
|
||||
_tecsnew.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get());
|
||||
_tecsnew.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
|
||||
_tecsnew.set_throttle_slewrate(_param_fw_thr_slew_max.get());
|
||||
_tecsnew.set_vertical_accel_limit(_param_fw_t_vert_acc.get());
|
||||
_tecsnew.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get());
|
||||
_tecsnew.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
|
||||
_tecsnew.set_pitch_damping(_param_fw_t_ptch_damp.get());
|
||||
_tecsnew.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
|
||||
_tecsnew.set_altitude_rate_ff(_param_fw_t_hrate_ff.get());
|
||||
_tecsnew.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get());
|
||||
_tecsnew.set_ste_rate_time_const(_param_ste_rate_time_const.get());
|
||||
_tecsnew.set_speed_derivative_time_constant(_param_tas_rate_time_const.get());
|
||||
_tecsnew.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
|
||||
|
||||
int check_ret = PX4_OK;
|
||||
|
||||
// sanity check parameters
|
||||
@ -306,7 +284,6 @@ FixedwingPositionControl::airspeed_poll()
|
||||
// update TECS if validity changed
|
||||
if (airspeed_valid != _airspeed_valid) {
|
||||
_tecs.enable_airspeed(airspeed_valid);
|
||||
_tecsnew.enable_airspeed(airspeed_valid);
|
||||
_airspeed_valid = airspeed_valid;
|
||||
}
|
||||
}
|
||||
@ -391,7 +368,6 @@ FixedwingPositionControl::vehicle_attitude_poll()
|
||||
// load factor due to banking
|
||||
const float load_factor = 1.f / cosf(euler_angles(0));
|
||||
_tecs.set_load_factor(load_factor);
|
||||
_tecsnew.set_load_factor(load_factor);
|
||||
}
|
||||
}
|
||||
|
||||
@ -485,6 +461,8 @@ FixedwingPositionControl::tecs_status_publish()
|
||||
{
|
||||
tecs_status_s t{};
|
||||
|
||||
TECS::DebugOutput debug_output{_tecs.getStatus()};
|
||||
|
||||
switch (_tecs.tecs_mode()) {
|
||||
case TECS::ECL_TECS_MODE_NORMAL:
|
||||
t.mode = tecs_status_s::TECS_MODE_NORMAL;
|
||||
@ -503,43 +481,25 @@ FixedwingPositionControl::tecs_status_publish()
|
||||
break;
|
||||
}
|
||||
|
||||
t.altitude_sp = _tecs.hgt_setpoint();
|
||||
t.altitude_filtered = _tecs.vert_pos_state();
|
||||
t.altitude_filtered = debug_output.altitude_sp;
|
||||
|
||||
t.true_airspeed_sp = _tecs.TAS_setpoint_adj();
|
||||
t.true_airspeed_filtered = _tecs.tas_state();
|
||||
t.true_airspeed_filtered = debug_output.true_airspeed_filtered;
|
||||
|
||||
t.height_rate_setpoint = _tecs.hgt_rate_setpoint();
|
||||
t.height_rate = _tecs.vert_vel_state();
|
||||
t.height_rate_setpoint = debug_output.altitude_rate_setpoint;
|
||||
t.height_rate = debug_output.altitude_rate;
|
||||
|
||||
t.equivalent_airspeed_sp = _tecs.get_EAS_setpoint();
|
||||
t.true_airspeed_derivative_sp = _tecs.TAS_rate_setpoint();
|
||||
t.true_airspeed_derivative = _tecs.speed_derivative();
|
||||
t.true_airspeed_derivative_raw = _tecs.speed_derivative_raw();
|
||||
t.true_airspeed_innovation = _tecs.getTASInnovation();
|
||||
t.true_airspeed_derivative_sp = debug_output.true_airspeed_derivative_control;
|
||||
t.true_airspeed_derivative = debug_output.true_airspeed_derivative;
|
||||
|
||||
t.total_energy_error = _tecs.STE_error();
|
||||
t.total_energy_rate_error = _tecs.STE_rate_error();
|
||||
t.total_energy_rate_error = debug_output.total_energy_rate_error;
|
||||
|
||||
t.energy_distribution_error = _tecs.SEB_error();
|
||||
t.energy_distribution_rate_error = _tecs.SEB_rate_error();
|
||||
t.energy_distribution_rate_error = debug_output.energy_balance_rate_error;
|
||||
|
||||
t.total_energy = _tecs.STE();
|
||||
t.total_energy_rate = _tecs.STE_rate();
|
||||
t.total_energy_balance = _tecs.SEB();
|
||||
t.total_energy_balance_rate = _tecs.SEB_rate();
|
||||
|
||||
t.total_energy_sp = _tecs.STE_setpoint();
|
||||
t.total_energy_rate_sp = _tecs.STE_rate_setpoint();
|
||||
t.total_energy_balance_sp = _tecs.SEB_setpoint();
|
||||
t.total_energy_balance_rate_sp = _tecs.SEB_rate_setpoint();
|
||||
|
||||
t.throttle_integ = _tecs.throttle_integ_state();
|
||||
t.pitch_integ = _tecs.pitch_integ_state();
|
||||
t.total_energy_rate_sp = debug_output.total_energy_rate_sp;
|
||||
t.total_energy_balance_rate_sp = debug_output.energy_balance_rate_sp;
|
||||
|
||||
t.throttle_sp = _tecs.get_throttle_setpoint();
|
||||
t.pitch_sp_rad = _tecs.get_pitch_setpoint();
|
||||
t.throttle_trim = _tecs.get_throttle_trim();
|
||||
|
||||
t.timestamp = hrt_absolute_time();
|
||||
|
||||
@ -866,10 +826,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
|
||||
_was_in_air = true;
|
||||
_time_went_in_air = now;
|
||||
|
||||
_tecs.resetIntegrals();
|
||||
_tecs.resetTrajectoryGenerators(_current_altitude);
|
||||
|
||||
_tecsnew.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
}
|
||||
|
||||
/* reset flag when airplane landed */
|
||||
@ -877,9 +834,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
|
||||
_was_in_air = false;
|
||||
_completed_manual_takeoff = false;
|
||||
|
||||
_tecs.resetIntegrals();
|
||||
_tecs.resetTrajectoryGenerators(_current_altitude);
|
||||
_tecsnew.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1124,7 +1079,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
|
||||
if (pos_sp_curr.gliding_enabled) {
|
||||
/* enable gliding with this waypoint */
|
||||
_tecs.set_speed_weight(2.0f);
|
||||
_tecsnew.set_speed_weight(2.0f);
|
||||
tecs_fw_thr_min = 0.0;
|
||||
tecs_fw_thr_max = 0.0;
|
||||
|
||||
@ -1227,7 +1181,6 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
|
||||
if (pos_sp_curr.gliding_enabled) {
|
||||
/* enable gliding with this waypoint */
|
||||
_tecs.set_speed_weight(2.0f);
|
||||
_tecsnew.set_speed_weight(2.0f);
|
||||
tecs_fw_thr_min = 0.0;
|
||||
tecs_fw_thr_max = 0.0;
|
||||
|
||||
@ -1315,7 +1268,6 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
|
||||
if (pos_sp_curr.gliding_enabled) {
|
||||
/* enable gliding with this waypoint */
|
||||
_tecs.set_speed_weight(2.0f);
|
||||
_tecsnew.set_speed_weight(2.0f);
|
||||
tecs_fw_thr_min = 0.0;
|
||||
tecs_fw_thr_max = 0.0;
|
||||
|
||||
@ -1338,8 +1290,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
|
||||
// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
|
||||
// landing airspeed and potentially tighter altitude control) already such that we don't
|
||||
// have to do this switch (which can cause significant altitude errors) close to the ground.
|
||||
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
_tecsnew.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
airspeed_sp = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : _param_fw_airspd_min.get();
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND;
|
||||
@ -1385,8 +1336,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
|
||||
_att_sp.roll_body = 0.0f;
|
||||
}
|
||||
|
||||
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
_tecsnew.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
}
|
||||
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
@ -1522,7 +1472,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
|
||||
// throttle is open loop anyway during ground roll, no need to wind up the integrator
|
||||
_tecs.resetIntegrals();
|
||||
_tecsnew.resetIntegrals();
|
||||
}
|
||||
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
@ -1538,7 +1487,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
_param_fw_t_clmb_max.get());
|
||||
|
||||
_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
|
||||
_tecsnew.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
|
||||
|
||||
_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
|
||||
_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust());
|
||||
@ -1661,8 +1609,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
||||
{
|
||||
// Enable tighter altitude control for landings
|
||||
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
_tecsnew.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
|
||||
const Vector2f local_position{_local_pos.x, _local_pos.y};
|
||||
Vector2f local_land_point = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
|
||||
@ -1680,7 +1627,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
||||
if (airspeed_land < _param_fw_airspd_min.get()) {
|
||||
// adjust underspeed detection bounds for landing airspeed
|
||||
_tecs.set_equivalent_airspeed_min(airspeed_land);
|
||||
_tecsnew.set_equivalent_airspeed_min(airspeed_land);
|
||||
adjusted_min_airspeed = airspeed_land;
|
||||
}
|
||||
|
||||
@ -1726,7 +1672,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
||||
if (!_flare_states.flaring) {
|
||||
_flare_states.flaring = true;
|
||||
_flare_states.start_time = now;
|
||||
_flare_states.initial_height_rate_setpoint = _tecs.hgt_rate_setpoint();
|
||||
_flare_states.initial_height_rate_setpoint = -_local_pos.vz;
|
||||
_flare_states.initial_throttle_setpoint = _att_sp.thrust_body[0];
|
||||
events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info,
|
||||
"Landing, flaring");
|
||||
@ -1904,7 +1850,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
||||
}
|
||||
|
||||
_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
|
||||
_tecsnew.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
|
||||
|
||||
_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
|
||||
|
||||
@ -2156,7 +2101,7 @@ FixedwingPositionControl::Run()
|
||||
if (_control_mode.flag_control_manual_enabled) {
|
||||
if (_control_mode.flag_control_altitude_enabled && _local_pos.vz_reset_counter != _alt_reset_counter) {
|
||||
// make TECS accept step in altitude and demanded altitude
|
||||
_tecs.handle_alt_step(-_local_pos.delta_z, _current_altitude);
|
||||
_tecs.handle_alt_step(_current_altitude, -_local_pos.vz);
|
||||
}
|
||||
|
||||
// adjust navigation waypoints in position control mode
|
||||
@ -2303,9 +2248,7 @@ FixedwingPositionControl::Run()
|
||||
|
||||
// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
|
||||
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
|
||||
_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
|
||||
_tecsnew.set_speed_weight(_param_fw_t_spdweight.get());
|
||||
_tecsnew.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
|
||||
_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
|
||||
|
||||
// restore lateral-directional guidance parameters (changed in takeoff mode)
|
||||
_npfg.setPeriod(_param_npfg_period.get());
|
||||
@ -2371,7 +2314,7 @@ FixedwingPositionControl::Run()
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
|
||||
_tecs.reset_state();
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
|
||||
break;
|
||||
}
|
||||
@ -2548,14 +2491,12 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
|
||||
}
|
||||
|
||||
// We need an altitude lock to calculate the TECS control
|
||||
if (!(_local_pos.timestamp > 0))
|
||||
{
|
||||
if (!(_local_pos.timestamp > 0)) {
|
||||
_reinitialize_tecs = true;
|
||||
}
|
||||
|
||||
if (_reinitialize_tecs) {
|
||||
_tecs.reset_state();
|
||||
_tecsnew.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
_reinitialize_tecs = false;
|
||||
}
|
||||
|
||||
@ -2563,82 +2504,31 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
|
||||
_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
|
||||
|
||||
if (_landed) {
|
||||
_tecs.resetIntegrals();
|
||||
_tecs.resetTrajectoryGenerators(_current_altitude);
|
||||
_tecsnew.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
|
||||
}
|
||||
|
||||
/* update TECS vehicle state estimates */
|
||||
_tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), _current_altitude,
|
||||
_local_pos.vz);
|
||||
|
||||
const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min,
|
||||
throttle_max);
|
||||
|
||||
_tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()),
|
||||
_current_altitude,
|
||||
alt_sp,
|
||||
airspeed_sp,
|
||||
_airspeed,
|
||||
_eas2tas,
|
||||
climbout_mode,
|
||||
climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
|
||||
throttle_min,
|
||||
throttle_max,
|
||||
throttle_trim_comp,
|
||||
pitch_min_rad - radians(_param_fw_psp_off.get()),
|
||||
pitch_max_rad - radians(_param_fw_psp_off.get()),
|
||||
desired_max_climbrate,
|
||||
desired_max_sinkrate,
|
||||
hgt_rate_sp);
|
||||
|
||||
_tecsnew.update(_pitch - radians(_param_fw_psp_off.get()),
|
||||
_current_altitude,
|
||||
alt_sp,
|
||||
airspeed_sp,
|
||||
_airspeed,
|
||||
_eas2tas,
|
||||
climbout_mode,
|
||||
climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
|
||||
throttle_min,
|
||||
throttle_max,
|
||||
throttle_trim_comp,
|
||||
pitch_min_rad - radians(_param_fw_psp_off.get()),
|
||||
pitch_max_rad - radians(_param_fw_psp_off.get()),
|
||||
desired_max_climbrate,
|
||||
desired_max_sinkrate,
|
||||
_body_acceleration(0),
|
||||
-_local_pos.vz,
|
||||
hgt_rate_sp);
|
||||
|
||||
newTECS::TECS::DebugOutput new_status{_tecsnew.getStatus()};
|
||||
|
||||
printf("Compare Airspeed Filter: \n ");
|
||||
printf("\t\t raw\t old \t new \n");
|
||||
printf("Airspeed: \t %.2f \t %.2f \t %.2f \n",(double)(_airspeed*_eas2tas), (double)_tecs.tas_state(), (double)new_status.true_airspeed_filtered);
|
||||
printf("Airspeed_deriv:\t %.2f \t %.2f \t %.2f \n", (double)_body_acceleration(0), (double)_tecs.speed_derivative(), (double)new_status.true_airspeed_derivative);
|
||||
|
||||
printf("Compare Altitude reference filter: \n ");
|
||||
printf("\t\t\t raw\t\t old \t\t new \n");
|
||||
printf("Altitude Ref:\t\t %.2f \t %.2f \t %.2f \n",(double)alt_sp, (double)_tecs.hgt_setpoint(), (double)new_status.altitude_sp);
|
||||
printf("Alt rate from Alt:\t %.2f \t\t %.2f \t\t %.2f \n",0.0, (double)_tecs.hgt_rate_from_hgt_setpoint(), (double)new_status.altitude_rate);
|
||||
printf("Altitude Rate Ref:\t %.2f \t\t %.2f \t\t %.2f \n",(double)hgt_rate_sp, (double)_tecs.smooth_hgt_rate_setpoint(), (double)new_status.altitude_rate_setpoint);
|
||||
|
||||
printf("Compare Tecs control: \n ");
|
||||
printf("\t\t old \t\t new \n");
|
||||
printf("alt_rate_ref:\t %.2f \t %.2f \n", (double)_tecs.hgt_rate_setpoint(), (double)new_status.altitude_rate_control);
|
||||
printf("speed_rate_ref:\t %.2f \t %.2f \n", (double)_tecs.TAS_rate_setpoint(), (double)new_status.true_airspeed_derivative_control);
|
||||
printf("ste_rate_error:\t %.2f \t %.2f \n", (double)_tecs.STE_rate_error(), (double)new_status.total_energy_rate_error);
|
||||
printf("ste_rate_sp:\t %.2f \t %.2f \n", (double)_tecs.STE_rate_setpoint(), (double)new_status.total_energy_rate_sp);
|
||||
printf("seb_rate_error:\t %.2f \t %.2f \n", (double)_tecs.SEB_rate_error(), (double)new_status.energy_balance_rate_error);
|
||||
printf("seb_rate_sp:\t %.2f \t %.2f \n", (double)_tecs.SEB_rate_setpoint(), (double)new_status.energy_balance_rate_sp);
|
||||
|
||||
printf("Compare Tecs output: \n ");
|
||||
printf("\t\t old \t\t new \n");
|
||||
printf("pitch control:\t %.2f \t %.2f \n", (double)_tecs.get_pitch_setpoint(), (double)_tecsnew.get_pitch_setpoint());
|
||||
printf("throttle control:\t %.2f \t %.2f \n", (double)_tecs.get_throttle_setpoint(), (double)_tecsnew.get_throttle_setpoint());
|
||||
|
||||
|
||||
_tecs.update(_pitch - radians(_param_fw_psp_off.get()),
|
||||
_current_altitude,
|
||||
alt_sp,
|
||||
airspeed_sp,
|
||||
_airspeed,
|
||||
_eas2tas,
|
||||
climbout_mode,
|
||||
climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
|
||||
throttle_min,
|
||||
throttle_max,
|
||||
throttle_trim_comp,
|
||||
pitch_min_rad - radians(_param_fw_psp_off.get()),
|
||||
pitch_max_rad - radians(_param_fw_psp_off.get()),
|
||||
desired_max_climbrate,
|
||||
desired_max_sinkrate,
|
||||
_body_acceleration(0),
|
||||
-_local_pos.vz,
|
||||
hgt_rate_sp);
|
||||
|
||||
tecs_status_publish();
|
||||
}
|
||||
|
||||
@ -58,7 +58,6 @@
|
||||
#include <lib/l1/ECL_L1_Pos_Controller.hpp>
|
||||
#include <lib/npfg/npfg.hpp>
|
||||
#include <lib/tecs/TECS.hpp>
|
||||
#include <lib/tecs/TECSnew.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/slew_rate/SlewRate.hpp>
|
||||
@ -389,7 +388,6 @@ private:
|
||||
|
||||
// total energy control system - airspeed / altitude control
|
||||
TECS _tecs;
|
||||
newTECS::TECS _tecsnew;
|
||||
|
||||
bool _reinitialize_tecs{true};
|
||||
bool _tecs_is_running{false};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user