diff options
Diffstat (limited to 'src/lib/external_lgpl')
-rw-r--r-- | src/lib/external_lgpl/module.mk | 2 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 72 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.h | 93 |
3 files changed, 119 insertions, 48 deletions
diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk index 53f1629e3..29d3514f6 100644 --- a/src/lib/external_lgpl/module.mk +++ b/src/lib/external_lgpl/module.mk @@ -46,3 +46,5 @@ # SRCS = tecs/tecs.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 6386e37a0..cfcc48b62 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -236,9 +236,9 @@ void TECS::_update_height_demand(float demand, float state) // // _hgt_rate_dem); _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; + _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT; _hgt_dem_adj_last = _hgt_dem_adj; - _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; @@ -252,6 +252,11 @@ void TECS::_update_height_demand(float demand, float state) void TECS::_detect_underspeed(void) { + if (!_detect_underspeed_enabled) { + _underspeed = false; + return; + } + if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) { _underspeed = true; @@ -294,11 +299,11 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM // Calculate throttle demand // If underspeed condition is set, then demand full throttle if (_underspeed) { - _throttle_dem_unc = 1.0f; + _throttle_dem = 1.0f; } else { // Calculate gain scaler from specific energy error to throttle - float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min)); + float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min)); // Calculate feed-forward throttle float ff_throttle = 0; @@ -316,28 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr; } - // Calculate PD + FF throttle + // 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 = constrain(_throttle_dem, _THRminf, _THRmaxf); // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time if (fabsf(_throttle_slewrate) > 0.01f) { float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; - _throttle_dem = constrain(_throttle_dem, - _last_throttle_dem - thrRateIncr, - _last_throttle_dem + thrRateIncr); - _last_throttle_dem = _throttle_dem; + _last_throttle_dem - thrRateIncr, + _last_throttle_dem + thrRateIncr); } + // Ensure _last_throttle_dem is always initialized properly + _last_throttle_dem = _throttle_dem; // Calculate integrator state upper and lower limits - // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand + // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand float integ_max = (_THRmaxf - _throttle_dem + 0.1f); float integ_min = (_THRminf - _throttle_dem - 0.1f); // Calculate integrator state, constraining state - // Set integrator to a max throttle value dduring climbout + // Set integrator to a max throttle value during climbout _integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr; if (_climbOutDem) { @@ -355,10 +361,10 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } else { _throttle_dem = ff_throttle; } - } - // Constrain throttle demand - _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); + // Constrain throttle demand + _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); + } } void TECS::_detect_bad_descent(void) @@ -551,18 +557,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f // Calculate pitch demand _update_pitch(); -// // Write internal variables to the log_tuning structure. This -// // structure will be logged in dataflash at 10Hz - // log_tuning.hgt_dem = _hgt_dem_adj; - // log_tuning.hgt = _integ3_state; - // log_tuning.dhgt_dem = _hgt_rate_dem; - // log_tuning.dhgt = _integ2_state; - // log_tuning.spd_dem = _TAS_dem_adj; - // log_tuning.spd = _integ5_state; - // log_tuning.dspd = _vel_dot; - // log_tuning.ithr = _integ6_state; - // log_tuning.iptch = _integ7_state; - // log_tuning.thr = _throttle_dem; - // log_tuning.ptch = _pitch_dem; - // log_tuning.dspd_dem = _TAS_rate_dem; + _tecs_state.timestamp = now; + + if (_underspeed) { + _tecs_state.mode = ECL_TECS_MODE_UNDERSPEED; + } else if (_badDescent) { + _tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT; + } else if (_climbOutDem) { + _tecs_state.mode = ECL_TECS_MODE_CLIMBOUT; + } else { + // If no error flag applies, conclude normal + _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; + } diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 5cafb1c79..eb45237b7 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -28,6 +28,28 @@ class __EXPORT TECS { public: TECS() : + _tecs_state {}, + _update_50hz_last_usec(0), + _update_speed_last_usec(0), + _update_pitch_throttle_last_usec(0), + // TECS tuning parameters + _hgtCompFiltOmega(0.0f), + _spdCompFiltOmega(0.0f), + _maxClimbRate(2.0f), + _minSinkRate(1.0f), + _maxSinkRate(2.0f), + _timeConst(5.0f), + _timeConstThrot(8.0f), + _ptchDamp(0.0f), + _thrDamp(0.0f), + _integGain(0.0f), + _vertAccLim(0.0f), + _rollComp(0.0f), + _spdWeight(0.5f), + _heightrate_p(0.0f), + _heightrate_ff(0.0f), + _speedrate_p(0.0f), + _throttle_dem(0.0f), _pitch_dem(0.0f), _integ1_state(0.0f), _integ2_state(0.0f), @@ -45,6 +67,9 @@ public: _hgt_dem_prev(0.0f), _TAS_dem_adj(0.0f), _STEdotErrLast(0.0f), + _underspeed(false), + _detect_underspeed_enabled(true), + _badDescent(false), _climbOutDem(false), _SPE_dem(0.0f), _SKE_dem(0.0f), @@ -100,29 +125,42 @@ public: return _spdWeight; } - // log data on internal state of the controller. Called at 10Hz - // void log_data(DataFlash_Class &dataflash, uint8_t msgid); - - // struct PACKED log_TECS_Tuning { - // LOG_PACKET_HEADER; - // 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; + enum ECL_TECS_MODE { + ECL_TECS_MODE_NORMAL = 0, + ECL_TECS_MODE_UNDERSPEED, + ECL_TECS_MODE_BAD_DESCENT, + ECL_TECS_MODE_CLIMBOUT + }; + + 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; + enum ECL_TECS_MODE mode; + }; + + void get_tecs_state(struct tecs_state& state) { + state = _tecs_state; + } void set_time_const(float time_const) { _timeConst = time_const; } + void set_time_const_throt(float time_const_throt) { + _timeConstThrot = time_const_throt; + } + void set_min_sink_rate(float rate) { _minSinkRate = rate; } @@ -183,11 +221,22 @@ public: _heightrate_p = heightrate_p; } + void set_heightrate_ff(float heightrate_ff) { + _heightrate_ff = heightrate_ff; + } + void set_speedrate_p(float speedrate_p) { _speedrate_p = speedrate_p; } + void set_detect_underspeed_enabled(bool enabled) { + _detect_underspeed_enabled = enabled; + } + private: + + struct tecs_state _tecs_state; + // Last time update_50Hz was called uint64_t _update_50hz_last_usec; @@ -204,6 +253,7 @@ private: float _minSinkRate; float _maxSinkRate; float _timeConst; + float _timeConstThrot; float _ptchDamp; float _thrDamp; float _integGain; @@ -211,6 +261,7 @@ private: float _rollComp; float _spdWeight; float _heightrate_p; + float _heightrate_ff; float _speedrate_p; // throttle demand in the range from 0.0 to 1.0 @@ -285,15 +336,15 @@ private: // Underspeed condition bool _underspeed; + // Underspeed detection enabled + bool _detect_underspeed_enabled; + // Bad descent condition caused by unachievable airspeed demand bool _badDescent; // climbout mode bool _climbOutDem; - // throttle demand before limiting - float _throttle_dem_unc; - // pitch demand before limiting float _pitch_dem_unc; |