1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file tecs.h
/// @brief Combined Total Energy Speed & Height Control.
/*
* Written by Paul Riseborough 2013 to provide:
* - Combined control of speed and height using throttle to control
* total energy and pitch angle to control exchange of energy between
* 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
* height priority
* - 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
*/
#ifndef TECS_H
#define TECS_H
#include <mathlib/mathlib.h>
#include <stdint.h>
class __EXPORT TECS
{
public:
TECS() :
_airspeed_enabled(false),
_throttle_slewrate(0.0f),
_climbOutDem(false),
_hgt_dem_prev(0.0f),
_hgt_dem_adj_last(0.0f),
_hgt_dem_in_old(0.0f),
_TAS_dem_last(0.0f),
_TAS_dem_adj(0.0f),
_TAS_dem(0.0f),
_integ1_state(0.0f),
_integ2_state(0.0f),
_integ3_state(0.0f),
_integ4_state(0.0f),
_integ5_state(0.0f),
_integ6_state(0.0f),
_integ7_state(0.0f),
_pitch_dem(0.0f),
_last_pitch_dem(0.0f),
_SPE_dem(0.0f),
_SKE_dem(0.0f),
_SPEdot_dem(0.0f),
_SKEdot_dem(0.0f),
_SPE_est(0.0f),
_SKE_est(0.0f),
_SPEdot(0.0f),
_SKEdot(0.0f) {
}
bool airspeed_sensor_enabled() {
return _airspeed_enabled;
}
void enable_airspeed(bool enabled) {
_airspeed_enabled = enabled;
}
// 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 baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth);
// Update the control loop calculations
void update_pitch_throttle(const math::Dcm &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);
// demanded throttle in percentage
// should return 0 to 100
float get_throttle_demand(void) {
return _throttle_dem;
}
int32_t get_throttle_demand_percent(void) {
return get_throttle_demand();
}
float get_pitch_demand() { return _pitch_dem; }
// demanded pitch angle in centi-degrees
// should return between -9000 to +9000
int32_t get_pitch_demand_cd() { return int32_t(get_pitch_demand() * 5729.5781f);}
// Rate of change of velocity along X body axis in m/s^2
float get_VXdot(void) { return _vel_dot; }
// 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;
void set_time_const(float time_const) {
_timeConst = time_const;
}
void set_min_sink_rate(float rate) {
_minSinkRate = rate;
}
void set_max_sink_rate(float sink_rate) {
_maxSinkRate = sink_rate;
}
void set_max_climb_rate(float climb_rate) {
_maxClimbRate = climb_rate;
}
void set_throttle_damp(float throttle_damp) {
_thrDamp = throttle_damp;
}
void set_integrator_gain(float gain) {
_integGain = gain;
}
void set_vertical_accel_limit(float limit) {
_vertAccLim = limit;
}
void set_height_comp_filter_omega(float omega) {
_hgtCompFiltOmega = omega;
}
void set_speed_comp_filter_omega(float omega) {
_spdCompFiltOmega = omega;
}
void set_roll_throttle_compensation(float compensation) {
_rollComp = compensation;
}
void set_speed_weight(float weight) {
_spdWeight = weight;
}
void set_pitch_damping(float damping) {
_ptchDamp = damping;
}
void set_throttle_slewrate(float slewrate) {
_throttle_slewrate = slewrate;
}
void set_indicated_airspeed_min(float airspeed) {
_indicated_airspeed_min = airspeed;
}
void set_indicated_airspeed_max(float airspeed) {
_indicated_airspeed_max = airspeed;
}
private:
// Last time update_50Hz was called
uint64_t _update_50hz_last_usec;
// Last time update_speed was called
uint64_t _update_speed_last_usec;
// Last time update_pitch_throttle was called
uint64_t _update_pitch_throttle_last_usec;
// TECS tuning parameters
float _hgtCompFiltOmega;
float _spdCompFiltOmega;
float _maxClimbRate;
float _minSinkRate;
float _maxSinkRate;
float _timeConst;
float _ptchDamp;
float _thrDamp;
float _integGain;
float _vertAccLim;
float _rollComp;
float _spdWeight;
// throttle demand in the range from 0.0 to 1.0
float _throttle_dem;
// pitch angle demand in radians
float _pitch_dem;
// Integrator state 1 - height filter second derivative
float _integ1_state;
// Integrator state 2 - height rate
float _integ2_state;
// Integrator state 3 - height
float _integ3_state;
// Integrator state 4 - airspeed filter first derivative
float _integ4_state;
// Integrator state 5 - true airspeed
float _integ5_state;
// Integrator state 6 - throttle integrator
float _integ6_state;
// Integrator state 6 - pitch integrator
float _integ7_state;
// throttle demand rate limiter state
float _last_throttle_dem;
// pitch demand rate limiter state
float _last_pitch_dem;
// Rate of change of speed along X axis
float _vel_dot;
// Equivalent airspeed
float _EAS;
// True airspeed limits
float _TASmax;
float _TASmin;
// Current and last true airspeed demand
float _TAS_dem;
float _TAS_dem_last;
// Equivalent airspeed demand
float _EAS_dem;
// height demands
float _hgt_dem;
float _hgt_dem_in_old;
float _hgt_dem_adj;
float _hgt_dem_adj_last;
float _hgt_rate_dem;
float _hgt_dem_prev;
// Speed demand after application of rate limiting
// This is the demand tracked by the TECS control loops
float _TAS_dem_adj;
// Speed rate demand after application of rate limiting
// This is the demand tracked by the TECS control loops
float _TAS_rate_dem;
// Total energy rate filter state
float _STEdotErrLast;
// Underspeed condition
bool _underspeed;
// 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;
// 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 pitch limits
float _PITCHmaxf;
float _PITCHminf;
// Specific energy quantities
float _SPE_dem;
float _SKE_dem;
float _SPEdot_dem;
float _SKEdot_dem;
float _SPE_est;
float _SKE_est;
float _SPEdot;
float _SKEdot;
// Specific energy error quantities
float _STE_error;
// Time since last update of main TECS loop (seconds)
float _DT;
bool _airspeed_enabled;
float _throttle_slewrate;
float _indicated_airspeed_min;
float _indicated_airspeed_max;
// Update the airspeed internal state using a second order complementary filter
void _update_speed(float airspeed_demand, float indicated_airspeed,
float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS);
// Update the demanded airspeed
void _update_speed_demand(void);
// Update the demanded height
void _update_height_demand(float demand);
// Detect an underspeed condition
void _detect_underspeed(void);
// Update Specific Energy Quantities
void _update_energies(void);
// Update Demanded Throttle
void _update_throttle(float throttle_cruise, const math::Dcm &rotMat);
// Detect Bad Descent
void _detect_bad_descent(void);
// Update Demanded Pitch Angle
void _update_pitch(void);
// Initialise states and variables
void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad);
// Calculate specific total energy rate limits
void _update_STE_rate_lim(void);
};
#endif //TECS_H
|