AP_TECS: reformat using astyle
This commit is contained in:
Andrew Tridgell
parent
86e8c7ed2f
commit
094b9cb35e
File diff suppressed because it is too large
Load Diff
@ -1,4 +1,4 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
/// @file AP_TECS.h
|
||||
/// @brief Combined Total Energy Speed & Height Control. This is a instance of an
|
||||
@ -11,9 +11,9 @@
|
||||
* 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
|
||||
* 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
|
||||
* - 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
|
||||
@ -31,87 +31,99 @@
|
||||
|
||||
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);
|
||||
}
|
||||
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 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,
|
||||
// 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);
|
||||
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; }
|
||||
// demanded throttle in percentage
|
||||
// should return 0 to 100
|
||||
int32_t get_throttle_demand(void) {
|
||||
return int32_t(_throttle_dem * 100.0f);
|
||||
}
|
||||
|
||||
// log data on internal state of the controller. Called at 10Hz
|
||||
void log_data(DataFlash_Class &dataflash, uint8_t msgid);
|
||||
// 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);
|
||||
}
|
||||
|
||||
// return current target airspeed
|
||||
float get_target_airspeed(void) const { return _TAS_dem / _ahrs.get_EAS2TAS(); }
|
||||
// Rate of change of velocity along X body axis in m/s^2
|
||||
float get_VXdot(void) {
|
||||
return _vel_dot;
|
||||
}
|
||||
|
||||
// return maximum climb rate
|
||||
float get_max_climbrate(void) const { return _maxClimbRate; }
|
||||
// log data on internal state of the controller. Called at 10Hz
|
||||
void log_data(DataFlash_Class &dataflash, uint8_t msgid);
|
||||
|
||||
// return landing sink rate
|
||||
float get_land_sinkrate(void) const { return _land_sink; }
|
||||
// return current target airspeed
|
||||
float get_target_airspeed(void) const {
|
||||
return _TAS_dem / _ahrs.get_EAS2TAS();
|
||||
}
|
||||
|
||||
// this supports the TECS_* user settable parameters
|
||||
// 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;
|
||||
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
|
||||
// reference to the AHRS object
|
||||
AP_AHRS &_ahrs;
|
||||
|
||||
const AP_Vehicle::FixedWing &aparm;
|
||||
const AP_Vehicle::FixedWing &aparm;
|
||||
|
||||
// TECS tuning parameters
|
||||
AP_Float _hgtCompFiltOmega;
|
||||
// TECS tuning parameters
|
||||
AP_Float _hgtCompFiltOmega;
|
||||
AP_Float _spdCompFiltOmega;
|
||||
AP_Float _maxClimbRate;
|
||||
AP_Float _minSinkRate;
|
||||
@ -123,50 +135,50 @@ private:
|
||||
AP_Float _thrDamp;
|
||||
AP_Float _integGain;
|
||||
AP_Float _vertAccLim;
|
||||
AP_Float _rollComp;
|
||||
AP_Float _spdWeight;
|
||||
AP_Float _spdWeightLand;
|
||||
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;
|
||||
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
|
||||
// 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
|
||||
|
||||
// pitch angle demand in radians
|
||||
float _pitch_dem;
|
||||
|
||||
// estimated climb rate (m/s)
|
||||
float _climb_rate;
|
||||
// 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;
|
||||
/*
|
||||
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;
|
||||
// height integration
|
||||
float height;
|
||||
} _height_filter;
|
||||
|
||||
// Integrator state 4 - airspeed filter first derivative
|
||||
float _integ4_state;
|
||||
// Integrator state 4 - airspeed filter first derivative
|
||||
float _integ4_state;
|
||||
|
||||
// Integrator state 5 - true airspeed
|
||||
float _integ5_state;
|
||||
// Integrator state 5 - true airspeed
|
||||
float _integ5_state;
|
||||
|
||||
// Integrator state 6 - throttle integrator
|
||||
float _integ6_state;
|
||||
// Integrator state 6 - throttle integrator
|
||||
float _integ6_state;
|
||||
|
||||
// Integrator state 6 - pitch integrator
|
||||
float _integ7_state;
|
||||
// Integrator state 6 - pitch integrator
|
||||
float _integ7_state;
|
||||
|
||||
// throttle demand rate limiter state
|
||||
float _last_throttle_dem;
|
||||
@ -175,7 +187,7 @@ private:
|
||||
float _last_pitch_dem;
|
||||
|
||||
// Rate of change of speed along X axis
|
||||
float _vel_dot;
|
||||
float _vel_dot;
|
||||
|
||||
// Equivalent airspeed
|
||||
float _EAS;
|
||||
@ -197,7 +209,7 @@ private:
|
||||
float _hgt_dem_adj;
|
||||
float _hgt_dem_adj_last;
|
||||
float _hgt_rate_dem;
|
||||
float _hgt_dem_prev;
|
||||
float _hgt_dem_prev;
|
||||
float _land_hgt_dem;
|
||||
|
||||
// Speed demand after application of rate limiting
|
||||
@ -208,8 +220,8 @@ private:
|
||||
// This is the demand tracked by the TECS control loops
|
||||
float _TAS_rate_dem;
|
||||
|
||||
// Total energy rate filter state
|
||||
float _STEdotErrLast;
|
||||
// Total energy rate filter state
|
||||
float _STEdotErrLast;
|
||||
|
||||
// Underspeed condition
|
||||
bool _underspeed;
|
||||
@ -220,20 +232,20 @@ private:
|
||||
// climbout mode
|
||||
enum FlightStage _flight_stage;
|
||||
|
||||
// pitch demand before limiting
|
||||
float _pitch_dem_unc;
|
||||
// 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 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 throttle limits
|
||||
float _THRmaxf;
|
||||
float _THRminf;
|
||||
|
||||
// Maximum and minimum floating point pitch limits
|
||||
float _PITCHmaxf;
|
||||
float _PITCHminf;
|
||||
// Maximum and minimum floating point pitch limits
|
||||
float _PITCHmaxf;
|
||||
float _PITCHminf;
|
||||
|
||||
// Specific energy quantities
|
||||
float _SPE_dem;
|
||||
@ -245,14 +257,14 @@ private:
|
||||
float _SPEdot;
|
||||
float _SKEdot;
|
||||
|
||||
// Specific energy error quantities
|
||||
float _STE_error;
|
||||
// Specific energy error quantities
|
||||
float _STE_error;
|
||||
|
||||
// Time since last update of main TECS loop (seconds)
|
||||
float _DT;
|
||||
// Time since last update of main TECS loop (seconds)
|
||||
float _DT;
|
||||
|
||||
// counter for demanded sink rate on land final
|
||||
uint8_t _flare_counter;
|
||||
// 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);
|
||||
@ -263,35 +275,35 @@ private:
|
||||
// Update the demanded height
|
||||
void _update_height_demand(void);
|
||||
|
||||
// Detect an underspeed condition
|
||||
void _detect_underspeed(void);
|
||||
// Detect an underspeed condition
|
||||
void _detect_underspeed(void);
|
||||
|
||||
// Update Specific Energy Quantities
|
||||
void _update_energies(void);
|
||||
// Update Specific Energy Quantities
|
||||
void _update_energies(void);
|
||||
|
||||
// Update Demanded Throttle
|
||||
void _update_throttle(void);
|
||||
// Update Demanded Throttle
|
||||
void _update_throttle(void);
|
||||
|
||||
// Update Demanded Throttle Non-Airspeed
|
||||
void _update_throttle_option(int16_t throttle_nudge);
|
||||
// Update Demanded Throttle Non-Airspeed
|
||||
void _update_throttle_option(int16_t throttle_nudge);
|
||||
|
||||
// Detect Bad Descent
|
||||
void _detect_bad_descent(void);
|
||||
// Detect Bad Descent
|
||||
void _detect_bad_descent(void);
|
||||
|
||||
// Update Demanded Pitch Angle
|
||||
void _update_pitch(void);
|
||||
// Update Demanded Pitch Angle
|
||||
void _update_pitch(void);
|
||||
|
||||
// Initialise states and variables
|
||||
void _initialise_states(int32_t ptchMinCO_cd, float hgt_afe);
|
||||
// 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);
|
||||
// Calculate specific total energy rate limits
|
||||
void _update_STE_rate_lim(void);
|
||||
|
||||
// declares a 5point average filter using floats
|
||||
AverageFilterFloat_Size5 _vdot_filter;
|
||||
AverageFilterFloat_Size5 _vdot_filter;
|
||||
|
||||
// current time constant
|
||||
float timeConstant(void) const;
|
||||
// current time constant
|
||||
float timeConstant(void) const;
|
||||
};
|
||||
|
||||
#define TECS_LOG_FORMAT(msg) { msg, sizeof(AP_TECS::log_TECS_Tuning), \
|
||||
|
||||
Loading…
Reference in New Issue
Block a user