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

Merge branch 'beta' into beta_merge4

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-22 14:09:20 +02:00
parent 21d1b4ba2e 6cdceb1829
commit 85007d8952
7 changed files with 105 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/external_lgpl/tecs/tecs.cpp
+29 -20
View File
@@ -309,12 +309,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
// Calculate total energy values
_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
_STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
// Apply 0.5 second first order filter to STEdot_error
// This is required to remove accelerometer noise from the measurement
STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast;
_STEdotErrLast = STEdot_error;
_STEdot_error = 0.2f * _STEdot_error + 0.8f * _STEdotErrLast;
_STEdotErrLast = _STEdot_error;
// Calculate throttle demand
// If underspeed condition is set, then demand full throttle
@@ -342,7 +342,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
}
// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
_throttle_dem = (_STE_error + _STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
// Rate limit PD + FF throttle
@@ -438,11 +438,11 @@ void TECS::_update_pitch(void)
// Calculate Specific Energy Balance demand, and error
float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
_SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
_SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
// Calculate integrator state, constraining input if pitch limits are exceeded
float integ7_input = SEB_error * _integGain;
float integ7_input = _SEB_error * _integGain;
if (_pitch_dem_unc > _PITCHmaxf) {
integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
@@ -460,7 +460,7 @@ void TECS::_update_pitch(void)
// 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 temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
if (_climbOutDem)
{
temp += _PITCHminf * gainInv;
@@ -598,18 +598,27 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
_tecs_state.mode = ECL_TECS_MODE_NORMAL;
}
_tecs_state.hgt_dem = _hgt_dem_adj;
_tecs_state.hgt = _integ3_state;
_tecs_state.dhgt_dem = _hgt_rate_dem;
_tecs_state.dhgt = _integ2_state;
_tecs_state.spd_dem = _TAS_dem_adj;
_tecs_state.spd = _integ5_state;
_tecs_state.dspd = _vel_dot;
_tecs_state.ithr = _integ6_state;
_tecs_state.iptch = _integ7_state;
_tecs_state.thr = _throttle_dem;
_tecs_state.ptch = _pitch_dem;
_tecs_state.dspd_dem = _TAS_rate_dem;
_tecs_state.altitude_sp = _hgt_dem_adj;
_tecs_state.altitude_filtered = _integ3_state;
_tecs_state.altitude_rate_sp = _hgt_rate_dem;
_tecs_state.altitude_rate = _integ2_state;
_tecs_state.airspeed_sp = _TAS_dem_adj;
_tecs_state.airspeed_rate_sp = _TAS_rate_dem;
_tecs_state.airspeed_filtered = _integ5_state;
_tecs_state.airspeed_rate = _vel_dot;
_tecs_state.total_energy_error = _STE_error;
_tecs_state.energy_distribution_error = _SEB_error;
_tecs_state.total_energy_rate_error = _STEdot_error;
_tecs_state.energy_distribution_error = _SEBdot_error;
_tecs_state.energy_error_integ = _integ6_state;
_tecs_state.energy_distribution_error_integ = _integ7_state;
_tecs_state.throttle_sp = _throttle_dem;
_tecs_state.pitch_sp = _pitch_dem;
_update_pitch_throttle_last_usec = now;
src/lib/external_lgpl/tecs/tecs.h
+29 -12
View File
@@ -79,6 +79,10 @@ public:
_SKE_est(0.0f),
_SPEdot(0.0f),
_SKEdot(0.0f),
_STE_error(0.0f),
_STEdot_error(0.0f),
_SEB_error(0.0f),
_SEBdot_error(0.0f),
_airspeed_enabled(false),
_states_initalized(false),
_in_air(false),
@@ -137,18 +141,22 @@ public:
struct tecs_state {
uint64_t timestamp;
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;
float altitude_filtered;
float altitude_sp;
float altitude_rate;
float altitude_rate_sp;
float airspeed_filtered;
float airspeed_sp;
float airspeed_rate;
float airspeed_rate_sp;
float energy_error_integ;
float energy_distribution_error_integ;
float total_energy_error;
float total_energy_rate_error;
float energy_distribution_error;
float energy_distribution_rate_error;
float throttle_sp;
float pitch_sp;
enum ECL_TECS_MODE mode;
};
@@ -376,6 +384,15 @@ private:
// Specific energy error quantities
float _STE_error;
// Energy error rate
float _STEdot_error;
// Specific energy balance error
float _SEB_error;
// Specific energy balance error rate
float _SEBdot_error;
// Time since last update of main TECS loop (seconds)
float _DT;