forked from Archive/PX4-Autopilot
332 lines
15 KiB
C++
332 lines
15 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2017 Estimation and Control Library (ECL). 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 ECL 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>
|
|
|
|
class TECS
|
|
{
|
|
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;
|
|
|
|
/**
|
|
* 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; }
|
|
|
|
/**
|
|
* 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
|
|
*/
|
|
void update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat,
|
|
const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air,
|
|
float altitude, bool vz_valid, float vz, float az);
|
|
|
|
/**
|
|
* Update the control loop calculations
|
|
*/
|
|
void update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint,
|
|
float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
|
|
float throttle_min, float throttle_setpoint_max, float throttle_cruise,
|
|
float pitch_limit_min, float pitch_limit_max);
|
|
|
|
float get_throttle_setpoint(void) { return _throttle_setpoint; }
|
|
float get_pitch_setpoint() { return _pitch_setpoint; }
|
|
float get_speed_weight() { return _pitch_speed_weight; }
|
|
|
|
void reset_state() { _states_initalized = false; }
|
|
|
|
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 controller parameters
|
|
void set_time_const(float time_const) { _pitch_time_constant = time_const; }
|
|
void set_integrator_gain(float gain) { _integrator_gain = 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_height_comp_filter_omega(float omega) { _hgt_estimate_freq = omega; }
|
|
void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; }
|
|
void set_heightrate_p(float heightrate_p) { _height_error_gain = heightrate_p; }
|
|
|
|
void set_indicated_airspeed_max(float airspeed) { _indicated_airspeed_max = airspeed; }
|
|
void set_indicated_airspeed_min(float airspeed) { _indicated_airspeed_min = airspeed; }
|
|
|
|
void set_pitch_damping(float damping) { _pitch_damping_gain = damping; }
|
|
void set_vertical_accel_limit(float limit) { _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_speedrate_p(float speedrate_p) { _speed_error_gain = speedrate_p; }
|
|
|
|
void set_time_const_throt(float time_const_throt) { _throttle_time_constant = time_const_throt; }
|
|
void set_throttle_damp(float throttle_damp) { _throttle_damping_gain = throttle_damp; }
|
|
void set_throttle_slewrate(float slewrate) { _throttle_slewrate = slewrate; }
|
|
|
|
void set_roll_throttle_compensation(float compensation) { _load_factor_correction = compensation; }
|
|
|
|
// TECS status
|
|
uint64_t timestamp() { return _pitch_update_timestamp; }
|
|
ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
|
|
|
|
float hgt_setpoint_adj() { return _hgt_setpoint_adj; }
|
|
float vert_pos_state() { return _vert_pos_state; }
|
|
|
|
float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
|
|
float tas_state() { return _tas_state; }
|
|
|
|
float hgt_rate_setpoint() { return _hgt_rate_setpoint; }
|
|
float vert_vel_state() { return _vert_vel_state; }
|
|
|
|
float TAS_rate_setpoint() { return _TAS_rate_setpoint; }
|
|
float speed_derivative() { return _speed_derivative; }
|
|
|
|
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 throttle_integ_state() { return _throttle_integ_state; }
|
|
float pitch_integ_state() { return _pitch_integ_state; }
|
|
|
|
/**
|
|
* Handle the altitude reset
|
|
*
|
|
* 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)
|
|
{
|
|
// add height reset delta to all variables involved
|
|
// in filtering the demanded height
|
|
_hgt_setpoint_in_prev += delta_alt;
|
|
_hgt_setpoint_prev += delta_alt;
|
|
_hgt_setpoint_adj_prev += delta_alt;
|
|
|
|
// reset height states
|
|
_vert_pos_state = altitude;
|
|
_vert_accel_state = 0.0f;
|
|
_vert_vel_state = 0.0f;
|
|
}
|
|
|
|
private:
|
|
|
|
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 _hgt_estimate_freq{0.0f}; ///< cross-over frequency of the height rate complementary filter (rad/sec)
|
|
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_time_constant{5.0f}; ///< control time constant used by the pitch demand calculation (sec)
|
|
float _throttle_time_constant{8.0f}; ///< control time constant used by the throttle demand calculation (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{0.0f}; ///< integrator gain used by the throttle and pitch demand calculation
|
|
float _vert_accel_limit{0.0f}; ///< magnitude of the maximum vertical acceleration allowed (m/sec**2)
|
|
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.0f}; ///< gain from height error to demanded climb rate (1/sec)
|
|
float _height_setpoint_gain_ff{0.0f}; ///< gain from height demand derivative to demanded climb rate
|
|
float _speed_error_gain{0.0f}; ///< gain from speed error to demanded speed rate (1/sec)
|
|
float _indicated_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec)
|
|
float _indicated_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec)
|
|
float _throttle_slewrate{0.0f}; ///< throttle demand slew rate limit (1/sec)
|
|
|
|
// controller outputs
|
|
float _throttle_setpoint{0.0f}; ///< normalized throttle demand (0..1)
|
|
float _pitch_setpoint{0.0f}; ///< pitch angle demand (radians)
|
|
|
|
// complimentary filter states
|
|
float _vert_accel_state{0.0f}; ///< complimentary filter state - height second derivative (m/sec**2)
|
|
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)
|
|
|
|
// 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 _speed_derivative{0.0f}; ///< 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)
|
|
|
|
// height demand calculations
|
|
float _hgt_setpoint{0.0f}; ///< demanded height tracked by the TECS algorithm (m)
|
|
float _hgt_setpoint_in_prev{0.0f}; ///< previous value of _hgt_setpoint after noise filtering (m)
|
|
float _hgt_setpoint_prev{0.0f}; ///< previous value of _hgt_setpoint after noise filtering and rate limiting (m)
|
|
float _hgt_setpoint_adj{0.0f}; ///< demanded height used by the control loops after all filtering has been applied (m)
|
|
float _hgt_setpoint_adj_prev{0.0f}; ///< value of _hgt_setpoint_adj from previous frame (m)
|
|
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{0.0f}; ///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3)
|
|
float _STE_rate_min{0.0f}; ///< 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 _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 _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)
|
|
|
|
// 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)
|
|
|
|
// controller mode logic
|
|
bool _underspeed_detected{false}; ///< true when an underspeed condition has been detected
|
|
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_initalized{false}; ///< true when TECS states have been iniitalized
|
|
bool _in_air{false}; ///< true when the vehicle is flying
|
|
|
|
/**
|
|
* Update the airspeed internal state using a second order complementary filter
|
|
*/
|
|
void _update_speed_states(float airspeed_setpoint, float indicated_airspeed, float eas_to_tas);
|
|
|
|
/**
|
|
* Update the desired airspeed
|
|
*/
|
|
void _update_speed_setpoint();
|
|
|
|
/**
|
|
* Update the desired height
|
|
*/
|
|
void _update_height_setpoint(float desired, float state);
|
|
|
|
/**
|
|
* Detect if the system is not capable of maintaining airspeed
|
|
*/
|
|
void _detect_underspeed();
|
|
|
|
/**
|
|
* Update specific energy
|
|
*/
|
|
void _update_energy_estimates();
|
|
|
|
/**
|
|
* Update throttle setpoint
|
|
*/
|
|
void _update_throttle_setpoint(float throttle_cruise, const matrix::Dcmf &rotMat);
|
|
|
|
/**
|
|
* Detect an uncommanded descent
|
|
*/
|
|
void _detect_uncommanded_descent();
|
|
|
|
/**
|
|
* Update the pitch setpoint
|
|
*/
|
|
void _update_pitch_setpoint();
|
|
|
|
/**
|
|
* 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();
|
|
|
|
};
|