px4_autopilot/tecs/tecs.h

/****************************************************************************
 *
 *   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 <stdint.h>

class __EXPORT TECS
{
public:
	TECS() :
		_state_update_timestamp(0),
		_speed_update_timestamp(0),
		_pitch_update_timestamp(0),
		_hgt_estimate_freq(0.0f),
		_tas_estimate_freq(0.0f),
		_max_climb_rate(2.0f),
		_min_sink_rate(1.0f),
		_max_sink_rate(2.0f),
		_pitch_time_constant(5.0f),
		_throttle_time_constant(8.0f),
		_pitch_damping_gain(0.0f),
		_throttle_damping_gain(0.0f),
		_integrator_gain(0.0f),
		_vert_accel_limit(0.0f),
		_load_factor_correction(0.0f),
		_pitch_speed_weight(1.0f),
		_height_error_gain(0.0f),
		_height_setpoint_gain_ff(0.0f),
		_speed_error_gain(0.0f),
		_throttle_setpoint(0.0f),
		_pitch_setpoint(0.0f),
		_vert_accel_state(0.0f),
		_vert_vel_state(0.0f),
		_vert_pos_state(0.0f),
		_tas_rate_state(0.0f),
		_tas_state(0.0f),
		_throttle_integ_state(0.0f),
		_pitch_integ_state(0.0f),
		_last_throttle_setpoint(0.0f),
		_last_pitch_setpoint(0.0f),
		_speed_derivative(0.0f),
		_EAS(0.0f),
		_TAS_max(30.0f),
		_TAS_min(3.0f),
		_TAS_setpoint(0.0f),
		_TAS_setpoint_last(0.0f),
		_EAS_setpoint(0.0f),
		_TAS_setpoint_adj(0.0f),
		_TAS_rate_setpoint(0.0f),
		_indicated_airspeed_min(3.0f),
		_indicated_airspeed_max(30.0f),
		_hgt_setpoint(0.0f),
		_hgt_setpoint_in_prev(0.0f),
		_hgt_setpoint_prev(0.0f),
		_hgt_setpoint_adj(0.0f),
		_hgt_setpoint_adj_prev(0.0f),
		_hgt_rate_setpoint(0.0f),
		_pitch_setpoint_unc(0.0f),
		_STE_rate_max(0.0f),
		_STE_rate_min(0.0f),
		_throttle_setpoint_max(0.0f),
		_throttle_setpoint_min(0.0f),
		_pitch_setpoint_max(0.5f),
		_pitch_setpoint_min(-0.5f),
		_throttle_slewrate(0.0f),
		_SPE_setpoint(0.0f),
		_SKE_setpoint(0.0f),
		_SPE_rate_setpoint(0.0f),
		_SKE_rate_setpoint(0.0f),
		_SPE_estimate(0.0f),
		_SKE_estimate(0.0f),
		_SPE_rate(0.0f),
		_SKE_rate(0.0f),
		_STE_error(0.0f),
		_STE_rate_error(0.0f),
		_SEB_error(0.0f),
		_SEB_rate_error(0.0f),
		_dt(0.02f),
		_underspeed_detected(false),
		_detect_underspeed_enabled(true),
		_uncommanded_descent_recovery(false),
		_climbout_mode_active(false),
		_airspeed_enabled(false),
		_states_initalized(false),
		_in_air(false)
	{
	}

	/**
	 * 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 math::Matrix<3, 3> &rotMat,
					    const math::Vector<3> &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 math::Matrix<3, 3> &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
	};

	enum ECL_TECS_MODE _tecs_mode;

	void set_time_const(float time_const)
	{
		_pitch_time_constant = time_const;
	}

	void set_time_const_throt(float time_const_throt)
	{
		_throttle_time_constant = time_const_throt;
	}

	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_throttle_damp(float throttle_damp)
	{
		_throttle_damping_gain = throttle_damp;
	}

	void set_integrator_gain(float gain)
	{
		_integrator_gain = gain;
	}

	void set_vertical_accel_limit(float limit)
	{
		_vert_accel_limit = limit;
	}

	void set_height_comp_filter_omega(float omega)
	{
		_hgt_estimate_freq = omega;
	}

	void set_speed_comp_filter_omega(float omega)
	{
		_tas_estimate_freq = omega;
	}

	void set_roll_throttle_compensation(float compensation)
	{
		_load_factor_correction = compensation;
	}

	void set_speed_weight(float weight)
	{
		_pitch_speed_weight = weight;
	}

	void set_pitch_damping(float damping)
	{
		_pitch_damping_gain = damping;
	}

	void set_throttle_slewrate(float slewrate)
	{
		_throttle_slewrate = slewrate;
	}

	void set_indicated_airspeed_min(float airspeed)
	{
		_indicated_airspeed_min = airspeed;
	}

	void set_indicated_airspeed_max(float airspeed)
	{
		_indicated_airspeed_max = airspeed;
	}

	void set_heightrate_p(float heightrate_p)
	{
		_height_error_gain = heightrate_p;
	}

	void set_heightrate_ff(float heightrate_ff)
	{
		_height_setpoint_gain_ff = heightrate_ff;
	}

	void set_speedrate_p(float speedrate_p)
	{
		_speed_error_gain = speedrate_p;
	}

	void set_detect_underspeed_enabled(bool enabled)
	{
		_detect_underspeed_enabled = enabled;
	}

	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; }

	int tecs_mode() { return _tecs_mode; }

	uint64_t timestamp() { return _pitch_update_timestamp; }

	/**
	 * 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:

	// timestamps
	uint64_t _state_update_timestamp;			///< last timestamp of the 50 Hz function call
	uint64_t _speed_update_timestamp;			///< last timestamp of the speed function call
	uint64_t _pitch_update_timestamp;			///< last timestamp of the pitch function call

	// controller parameters
	float _hgt_estimate_freq;					///< cross-over frequency of the height rate complementary filter (rad/sec)
	float _tas_estimate_freq;					///< cross-over frequency of the true airspeed complementary filter (rad/sec)
	float _max_climb_rate;						///< climb rate produced by max allowed throttle (m/sec)
	float _min_sink_rate;						///< sink rate produced by min allowed throttle (m/sec)
	float _max_sink_rate;						///< maximum safe sink rate (m/sec)
	float _pitch_time_constant;					///< control time constant used by the pitch demand calculation (sec)
	float _throttle_time_constant;				///< control time constant used by the throttle demand calculation (sec)
	float _pitch_damping_gain;					///< damping gain of the pitch demand calculation (sec)
	float _throttle_damping_gain;				///< damping gain of the throttle demand calculation (sec)
	float _integrator_gain;						///< integrator gain used by the throttle and pitch demand calculation
	float _vert_accel_limit;					///< magnitude of the maximum vertical acceleration allowed (m/sec**2)
	float _load_factor_correction;				///< gain from normal load factor increase to total energy rate demand (m**2/sec**3)
	float _pitch_speed_weight;					///< speed control weighting used by pitch demand calculation
	float _height_error_gain;					///< gain from height error to demanded climb rate (1/sec)
	float _height_setpoint_gain_ff;				///< gain from height demand derivative to demanded climb rate
	float _speed_error_gain;					///< gain from speed error to demanded speed rate (1/sec)

	// controller outputs
	float _throttle_setpoint;					///< normalized throttle demand (0..1)
	float _pitch_setpoint;						///< pitch angle demand (radians)

	// complimentary filter states
	float _vert_accel_state;					///< complimentary filter state - height second derivative (m/sec**2)
	float _vert_vel_state;						///< complimentary filter state - height rate (m/sec)
	float _vert_pos_state;						///< complimentary filter state - height (m)
	float _tas_rate_state;						///< complimentary filter state - true airspeed first derivative (m/sec**2)
	float _tas_state;							///< complimentary filter state - true airspeed (m/sec)

	// controller states
	float _throttle_integ_state;				///< throttle integrator state
	float _pitch_integ_state;					///< pitch integrator state (rad)
	float _last_throttle_setpoint;				///< throttle demand rate limiter state (1/sec)
	float _last_pitch_setpoint;					///< pitch demand rate limiter state (rad/sec)
	float _speed_derivative;					///< rate of change of speed along X axis (m/sec**2)

	// speed demand calculations
	float _EAS;									///< equivalent airspeed (m/sec)
	float _TAS_max;								///< true airpeed demand upper limit (m/sec)
	float _TAS_min;								///< true airpeed demand lower limit (m/sec)
	float _TAS_setpoint;						///< current airpeed demand (m/sec)
	float _TAS_setpoint_last;					///< previous true airpeed demand (m/sec)
	float _EAS_setpoint;						///< Equivalent airspeed demand (m/sec)
	float _TAS_setpoint_adj;					///< true airspeed demand tracked by the TECS algorithm (m/sec)
	float _TAS_rate_setpoint;					///< true airspeed rate demand tracked by the TECS algorithm (m/sec**2)
	float _indicated_airspeed_min;				///< equivalent airspeed demand lower limit (m/sec)
	float _indicated_airspeed_max;				///< equivalent airspeed demand upper limit (m/sec)

	// height demand calculations
	float _hgt_setpoint;						///< demanded height tracked by the TECS algorithm (m)
	float _hgt_setpoint_in_prev;				///< previous value of _hgt_setpoint after noise filtering (m)
	float _hgt_setpoint_prev;					///< previous value of _hgt_setpoint after noise filtering and rate limiting (m)
	float _hgt_setpoint_adj;					///< demanded height used by the control loops after all filtering has been applied (m)
	float _hgt_setpoint_adj_prev;				///< value of _hgt_setpoint_adj from previous frame (m)
	float _hgt_rate_setpoint;					///< demanded climb rate tracked by the TECS algorithm

	// vehicle physical limits
	float _pitch_setpoint_unc;					///< pitch demand before limiting (rad)
	float _STE_rate_max;						///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3)
	float _STE_rate_min;						///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3)
	float _throttle_setpoint_max;				///< normalised throttle upper limit
	float _throttle_setpoint_min;				///< normalised throttle lower limit
	float _pitch_setpoint_max;					///< pitch demand upper limit (rad)
	float _pitch_setpoint_min;					///< pitch demand lower limit (rad)
	float _throttle_slewrate;					///< throttle demand slew rate limit (1/sec)

	// specific energy quantities
	float _SPE_setpoint;						///< specific potential energy demand (m**2/sec**2)
	float _SKE_setpoint;						///< specific kinetic energy demand (m**2/sec**2)
	float _SPE_rate_setpoint;					///< specific potential energy rate demand (m**2/sec**3)
	float _SKE_rate_setpoint;					///< specific kinetic energy rate demand (m**2/sec**3)
	float _SPE_estimate;						///< specific potential energy estimate (m**2/sec**2)
	float _SKE_estimate;						///< specific kinetic energy estimate (m**2/sec**2)
	float _SPE_rate;							///< specific potential energy rate estimate (m**2/sec**3)
	float _SKE_rate;							///< specific kinetic energy rate estimate (m**2/sec**3)

	// specific energy error quantities
	float _STE_error;							///< specific total energy error (m**2/sec**2)
	float _STE_rate_error;						///< specific total energy rate error (m**2/sec**3)
	float _SEB_error;							///< specific energy balance error (m**2/sec**2)
	float _SEB_rate_error;						///< specific energy balance rate error (m**2/sec**3)

	// time steps (non-fixed)
	float _dt;									///< Time since last update of main TECS loop (sec)
	static constexpr float DT_MIN = 0.001f;		///< minimum allowed value of _dt (sec)
	static constexpr float DT_DEFAULT = 0.02f;	///< default value for _dt (sec)
	static constexpr float DT_MAX = 1.0f;		///< max value of _dt allowed before a filter state reset is performed (sec)

	// controller mode logic
	bool _underspeed_detected;					///< true when an underspeed condition has been detected
	bool _detect_underspeed_enabled;			///< true when underspeed detection is enabled
	bool _uncommanded_descent_recovery;			///< true when a continuous descent caused by an unachievable airspeed demand has been detected
	bool _climbout_mode_active;					///< true when in climbout mode
	bool _airspeed_enabled;						///< true when airspeed use has been enabled
	bool _states_initalized;					///< true when TECS states have been iniitalized
	bool _in_air;								///< 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(void);

	/**
	 * Update specific energy
	 */
	void _update_energy_estimates(void);

	/**
	 * Update throttle setpoint
	 */
	void _update_throttle_setpoint(float throttle_cruise, const math::Matrix<3, 3> &rotMat);

	/**
	 * Detect an uncommanded descent
	 */
	void _detect_uncommanded_descent(void);

	/**
	 * Update the pitch setpoint
	 */
	void _update_pitch_setpoint(void);

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

};