fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 16:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW init TECS with PSP_OFF and constrain TAS error

Browse Source
This commit is contained in:
Daniel Agar
2016-09-27 08:49:45 +02:00
committed by Lorenz Meier
parent ebdfa2860b
commit fb75240ee9
3 changed files with 30 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/external_lgpl/tecs/tecs.cpp
+25 -17
View File
@@ -165,6 +165,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
_integ4_state = _integ4_state + integ4_input * DT;
float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f;
_integ5_state = _integ5_state + integ5_input * DT;
// limit the airspeed to a minimum of 3 m/s
_integ5_state = max(_integ5_state, 3.0f);
_update_speed_last_usec = now;
@@ -464,14 +465,14 @@ void TECS::_update_pitch(void)
// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
float gainInv = _integ5_state * _timeConst * CONSTANTS_ONE_G;
float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
if (_climbOutDem)
{
if (_climbOutDem) {
temp += _PITCHminf * gainInv;
}
_integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
float integ7_err_min = (gainInv * (_PITCHminf - math::radians(5.0f))) - temp;
float integ7_err_max = (gainInv * (_PITCHmaxf + math::radians(5.0f))) - temp;
_integ7_state = constrain(_integ7_state, integ7_err_min, integ7_err_max);
// Calculate pitch demand from specific energy balance signals
_pitch_dem_unc = (temp + _integ7_state) / gainInv;
@@ -493,7 +494,7 @@ void TECS::_update_pitch(void)
_last_pitch_dem = _pitch_dem;
}
void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad)
void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad, float EAS2TAS)
{
// Initialise states and variables if DT > 1 second or in climbout
if (_update_pitch_throttle_last_usec == 0 || _DT > DT_MAX || !_in_air || !_states_initalized) {
@@ -501,18 +502,22 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt
_integ2_state = 0.0f;
_integ3_state = baro_altitude;
_integ4_state = 0.0f;
_integ5_state = _EAS;
_integ5_state = _EAS * EAS2TAS;
_integ6_state = 0.0f;
_integ7_state = 0.0f;
_last_throttle_dem = throttle_cruise;
_last_pitch_dem = pitch;
_last_pitch_dem = constrain(pitch, _PITCHminf, _PITCHmaxf);
_pitch_dem_unc = _last_pitch_dem;
_hgt_dem_adj_last = baro_altitude;
_hgt_dem_adj = _hgt_dem_adj_last;
_hgt_dem_prev = _hgt_dem_adj_last;
_hgt_dem_in_old = _hgt_dem_adj_last;
_TAS_dem_last = _TAS_dem;
_TAS_dem_adj = _TAS_dem;
_pitch_dem_unc = pitch;
_TAS_dem_last = _EAS * EAS2TAS;
_TAS_dem_adj = _TAS_dem_last;
_underspeed = false;
_badDescent = false;
@@ -523,11 +528,14 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt
} else if (_climbOutDem) {
_PITCHminf = ptchMinCO_rad;
_THRminf = _THRmaxf - 0.01f;
_hgt_dem_adj_last = baro_altitude;
_hgt_dem_adj = _hgt_dem_adj_last;
_hgt_dem_prev = _hgt_dem_adj_last;
_TAS_dem_last = _TAS_dem;
_TAS_dem_adj = _TAS_dem;
_TAS_dem_last = _EAS * EAS2TAS;
_TAS_dem_adj = _EAS * EAS2TAS;
_underspeed = false;
_badDescent = false;
}
@@ -543,9 +551,9 @@ void TECS::_update_STE_rate_lim(void)
_STEdot_min = - _minSinkRate * CONSTANTS_ONE_G;
}
void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
float throttle_min, float throttle_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max)
void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem,
float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max)
{
// Calculate time in seconds since last update
@@ -563,7 +571,7 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
_climbOutDem = climbOutDem;
// initialise selected states and variables if DT > 1 second or in climbout
_initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO);
_initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO, EAS2TAS);
if (!_in_air) {
return;
src/lib/external_lgpl/tecs/tecs.h
+1 -1
View File
@@ -456,7 +456,7 @@ private:
void _update_pitch(void);
// Initialise states and variables
void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad);
void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad, float EAS2TAS);
// Calculate specific total energy rate limits
void _update_STE_rate_lim(void);