aboutsummaryrefslogtreecommitdiff
path: root/src/lib/external_lgpl
diff options
context:
space:
mode:
Diffstat (limited to 'src/lib/external_lgpl')
-rw-r--r--src/lib/external_lgpl/module.mk2
-rw-r--r--src/lib/external_lgpl/tecs/tecs.cpp72
-rw-r--r--src/lib/external_lgpl/tecs/tecs.h93
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;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-02 01:36:18 +0000