tecs.h

Go to the documentation of this file.

/****************************************************************************
 *
 *   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();

};