ardupilot/libraries/AP_TECS/AP_TECS.h

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

/// @file    AP_TECS.h
/// @brief   Combined Total Energy Speed & Height Control. This is a instance of an
/// AP_SpdHgtControl class

/*
 *  Written by Paul Riseborough 2013 to provide:
 *  - Combined control of speed and height using throttle to control
 *    total energy and pitch angle to control exchange of energy between
 *    potential and kinetic.
 *    Selectable speed or height priority modes when calculating pitch angle
 *  - Fallback mode when no airspeed measurement is available that
 *    sets throttle based on height rate demand and switches pitch angle control to 
 *    height priority
 *  - Underspeed protection that demands maximum throttle switches pitch angle control 
 *    to speed priority mode
 *  - Relative ease of tuning through use of intuitive time constant, trim rate and damping parameters and the use
 *    of easy to measure aircraft performance data
 */

#ifndef AP_TECS_H
#define AP_TECS_H

#include <AP_Math/AP_Math.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Param/AP_Param.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#include <DataFlash/DataFlash.h>

class AP_TECS : public AP_SpdHgtControl {
public:
	AP_TECS(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) :
		_ahrs(ahrs),
		aparm(parms)
		{
			AP_Param::setup_object_defaults(this, var_info);
		}

	// Update of the estimated height and height rate internal state
	// Update of the inertial speed rate internal state
	// Should be called at 50Hz or greater
	void update_50hz(float hgt_afe);

	// Update the control loop calculations
    void update_pitch_throttle(int32_t hgt_dem_cm, 
                               int32_t EAS_dem_cm, 
                               enum FlightStage flight_stage,
                               int32_t ptchMinCO_cd,
                               int16_t throttle_nudge,
							   float hgt_afe,
							   float load_factor);

	// demanded throttle in percentage
	// should return 0 to 100
	int32_t get_throttle_demand(void) {return int32_t(_throttle_dem * 100.0f);}
	
	// demanded pitch angle in centi-degrees
	// should return between -9000 to +9000
	int32_t get_pitch_demand(void) { return int32_t(_pitch_dem * 5729.5781f);}
	
	// Rate of change of velocity along X body axis in m/s^2
	float get_VXdot(void) { return _vel_dot; }

	// log data on internal state of the controller. Called at 10Hz
	void log_data(DataFlash_Class &dataflash, uint8_t msgid);

	// return current target airspeed
	float get_target_airspeed(void) const { return _TAS_dem / _ahrs.get_EAS2TAS(); }

	// return maximum climb rate
	float get_max_climbrate(void) const { return _maxClimbRate; }

	// return landing sink rate
	float get_land_sinkrate(void) const { return _land_sink; }

	// this supports the TECS_* user settable parameters
    static const struct AP_Param::GroupInfo var_info[];

	struct PACKED log_TECS_Tuning {
		LOG_PACKET_HEADER;
		uint64_t time_us;
		float hgt;
		float dhgt;
		float hgt_dem;
		float dhgt_dem;
		float spd_dem;
		float spd;
		float dspd;
		float ithr;
		float iptch;
		float thr;
		float ptch;
		float dspd_dem;
	} log_tuning;

private:
    // Last time update_50Hz was called
    uint32_t _update_50hz_last_usec;
	
    // Last time update_speed was called
    uint32_t _update_speed_last_usec;
	
    // Last time update_pitch_throttle was called
    uint32_t _update_pitch_throttle_last_usec;

	// reference to the AHRS object
    AP_AHRS &_ahrs;

	const AP_Vehicle::FixedWing &aparm;

	// TECS tuning parameters
	AP_Float _hgtCompFiltOmega;
    AP_Float _spdCompFiltOmega;
    AP_Float _maxClimbRate;
    AP_Float _minSinkRate;
    AP_Float _maxSinkRate;
    AP_Float _timeConst;
    AP_Float _landTimeConst;
    AP_Float _ptchDamp;
    AP_Float _landDamp;
    AP_Float _thrDamp;
    AP_Float _integGain;
    AP_Float _vertAccLim;
	AP_Float _rollComp;
	AP_Float _spdWeight;
	AP_Float _spdWeightLand;
    AP_Float _landThrottle;
    AP_Float _landAirspeed;
    AP_Float _land_sink;
	AP_Int8  _pitch_max;
	AP_Int8  _pitch_min;
	AP_Int8  _land_pitch_max;

	// current height estimate (above field elevation)
	float _height;
	
	// throttle demand in the range from 0.0 to 1.0
    float _throttle_dem;
	
	// pitch angle demand in radians
    float _pitch_dem;

	// estimated climb rate (m/s)
	float _climb_rate;

	/*
	  a filter to estimate climb rate if we don't have it from the EKF
	 */
	struct {
		// height filter second derivative
		float dd_height;

		// height integration
		float height;
	} _height_filter;

	// Integrator state 4 - airspeed filter first derivative
	float _integ4_state;

	// Integrator state 5 - true airspeed
	float _integ5_state;

	// Integrator state 6 - throttle integrator
	float _integ6_state;

	// Integrator state 6 - pitch integrator
	float _integ7_state;

    // throttle demand rate limiter state
    float _last_throttle_dem;

    // pitch demand rate limiter state
    float _last_pitch_dem;

    // Rate of change of speed along X axis
	float _vel_dot;

    // Equivalent airspeed
    float _EAS;

    // True airspeed limits
    float _TASmax;
    float _TASmin;

    // Current and last true airspeed demand
    float _TAS_dem;
    float _TAS_dem_last;

    // Equivalent airspeed demand
    float _EAS_dem;

    // height demands
    float _hgt_dem;
    float _hgt_dem_in_old;
    float _hgt_dem_adj;
    float _hgt_dem_adj_last;
    float _hgt_rate_dem;
	float _hgt_dem_prev;
    float _land_hgt_dem;

    // Speed demand after application of rate limiting
    // This is the demand tracked by the TECS control loops
    float _TAS_dem_adj;

    // Speed rate demand after application of rate limiting
    // This is the demand tracked by the TECS control loops
    float _TAS_rate_dem;

	// Total energy rate filter state
	float _STEdotErrLast;

    // Underspeed condition
    bool _underspeed;

    // Bad descent condition caused by unachievable airspeed demand
    bool _badDescent;

    // climbout mode
    enum FlightStage _flight_stage;

	// pitch demand before limiting
	float _pitch_dem_unc;

	// Maximum and minimum specific total energy rate limits
	float _STEdot_max;
	float _STEdot_min;

	// Maximum and minimum floating point throttle limits
	float _THRmaxf;
	float _THRminf;

	// Maximum and minimum floating point pitch limits
	float _PITCHmaxf;
	float _PITCHminf;

    // Specific energy quantities
    float _SPE_dem;
    float _SKE_dem;
    float _SPEdot_dem;
    float _SKEdot_dem;
    float _SPE_est;
    float _SKE_est;
    float _SPEdot;
    float _SKEdot;

	// Specific energy error quantities
	float _STE_error;

	// Time since last update of main TECS loop (seconds)
	float _DT;

	// counter for demanded sink rate on land final
	uint8_t _flare_counter;

    // Update the airspeed internal state using a second order complementary filter
    void _update_speed(float load_factor);

    // Update the demanded airspeed
    void _update_speed_demand(void);

    // Update the demanded height
    void _update_height_demand(void);

	// Detect an underspeed condition
	void _detect_underspeed(void);

	// Update Specific Energy Quantities
	void _update_energies(void);

	// Update Demanded Throttle
	void _update_throttle(void);

	// Update Demanded Throttle Non-Airspeed
	void _update_throttle_option(int16_t throttle_nudge);

	// Detect Bad Descent
	void _detect_bad_descent(void);

	// Update Demanded Pitch Angle
	void _update_pitch(void);

	// Initialise states and variables
	void _initialise_states(int32_t ptchMinCO_cd, float hgt_afe);

	// Calculate specific total energy rate limits
	void _update_STE_rate_lim(void);

    // declares a 5point average filter using floats
	AverageFilterFloat_Size5 _vdot_filter;

	// current time constant
	float timeConstant(void) const;
};

#define TECS_LOG_FORMAT(msg) { msg, sizeof(AP_TECS::log_TECS_Tuning),	\
							   "TECS", "Qffffffffffff", "TimeUS,h,dh,h_dem,dh_dem,sp_dem,sp,dsp,ith,iph,th,ph,dsp_dem" }

#endif //AP_TECS_H