Line data Source code
1 : #include "AP_TECS.h"
2 :
3 : #include <AP_HAL/AP_HAL.h>
4 : #include <AP_Baro/AP_Baro.h>
5 : #include <AP_Logger/AP_Logger.h>
6 : #include <AP_Landing/AP_Landing.h>
7 :
8 : extern const AP_HAL::HAL& hal;
9 :
10 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
11 : #include <stdio.h>
12 : # define Debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
13 : #else
14 : # define Debug(fmt, args ...)
15 : #endif
16 : //Debug("%.2f %.2f %.2f %.2f \n", var1, var2, var3, var4);
17 :
18 : // table of user settable parameters
19 : const AP_Param::GroupInfo AP_TECS::var_info[] = {
20 :
21 : // @Param: CLMB_MAX
22 : // @DisplayName: Maximum Climb Rate (metres/sec)
23 : // @Description: Maximum demanded climb rate. Do not set higher than the climb speed at THR_MAX at AIRSPEED_CRUISE when the battery is at low voltage. Reduce value if airspeed cannot be maintained on ascent. Increase value if throttle does not increase significantly to ascend.
24 : // @Increment: 0.1
25 : // @Range: 0.1 20.0
26 : // @User: Standard
27 : AP_GROUPINFO("CLMB_MAX", 0, AP_TECS, _maxClimbRate, 5.0f),
28 :
29 : // @Param: SINK_MIN
30 : // @DisplayName: Minimum Sink Rate (metres/sec)
31 : // @Description: Minimum sink rate when at THR_MIN and AIRSPEED_CRUISE.
32 : // @Increment: 0.1
33 : // @Range: 0.1 10.0
34 : // @User: Standard
35 : AP_GROUPINFO("SINK_MIN", 1, AP_TECS, _minSinkRate, 2.0f),
36 :
37 : // @Param: TIME_CONST
38 : // @DisplayName: Controller time constant (sec)
39 : // @Description: Time constant of the TECS control algorithm. Small values make faster altitude corrections but can cause overshoot and aggressive behavior.
40 : // @Range: 3.0 10.0
41 : // @Increment: 0.2
42 : // @User: Advanced
43 : AP_GROUPINFO("TIME_CONST", 2, AP_TECS, _timeConst, 5.0f),
44 :
45 : // @Param: THR_DAMP
46 : // @DisplayName: Controller throttle damping
47 : // @Description: Damping gain for throttle demand loop. Increase to add throttle activity to dampen oscillations in speed and height.
48 : // @Range: 0.1 1.0
49 : // @Increment: 0.1
50 : // @User: Advanced
51 : AP_GROUPINFO("THR_DAMP", 3, AP_TECS, _thrDamp, 0.5f),
52 :
53 : // @Param: INTEG_GAIN
54 : // @DisplayName: Controller integrator
55 : // @Description: Integrator gain to trim out long-term speed and height errors.
56 : // @Range: 0.0 0.5
57 : // @Increment: 0.02
58 : // @User: Advanced
59 : AP_GROUPINFO("INTEG_GAIN", 4, AP_TECS, _integGain, 0.3f),
60 :
61 : // @Param: VERT_ACC
62 : // @DisplayName: Vertical Acceleration Limit (metres/sec^2)
63 : // @Description: Maximum vertical acceleration used to correct speed or height errors.
64 : // @Range: 1.0 10.0
65 : // @Increment: 0.5
66 : // @User: Advanced
67 : AP_GROUPINFO("VERT_ACC", 5, AP_TECS, _vertAccLim, 7.0f),
68 :
69 : // @Param: HGT_OMEGA
70 : // @DisplayName: Height complementary filter frequency (radians/sec)
71 : // @Description: This is the cross-over frequency of the complementary filter used to fuse vertical acceleration and baro alt to obtain an estimate of height rate and height.
72 : // @Range: 1.0 5.0
73 : // @Increment: 0.05
74 : // @User: Advanced
75 : AP_GROUPINFO("HGT_OMEGA", 6, AP_TECS, _hgtCompFiltOmega, 3.0f),
76 :
77 : // @Param: SPD_OMEGA
78 : // @DisplayName: Speed complementary filter frequency (radians/sec)
79 : // @Description: This is the cross-over frequency of the complementary filter used to fuse longitudinal acceleration and airspeed to obtain a lower noise and lag estimate of airspeed.
80 : // @Range: 0.5 2.0
81 : // @Increment: 0.05
82 : // @User: Advanced
83 : AP_GROUPINFO("SPD_OMEGA", 7, AP_TECS, _spdCompFiltOmega, 2.0f),
84 :
85 : // @Param: RLL2THR
86 : // @DisplayName: Bank angle compensation gain
87 : // @Description: Gain from bank angle to throttle to compensate for loss of airspeed from drag in turns. Set to approximately 10x the sink rate in m/s caused by a 45-degree turn. High efficiency models may need less while less efficient aircraft may need more. Should be tuned in an automatic mission with waypoints and turns greater than 90 degrees. Tune with PTCH2SRV_RLL and KFF_RDDRMIX to achieve constant airspeed, constant altitude turns.
88 : // @Range: 5.0 30.0
89 : // @Increment: 1.0
90 : // @User: Advanced
91 : AP_GROUPINFO("RLL2THR", 8, AP_TECS, _rollComp, 10.0f),
92 :
93 : // @Param: SPDWEIGHT
94 : // @DisplayName: Weighting applied to speed control
95 : // @Description: Mixing of pitch and throttle correction for height and airspeed errors. Pitch controls altitude and throttle controls airspeed if set to 0. Pitch controls airspeed and throttle controls altitude if set to 2 (good for gliders). Blended if set to 1.
96 : // @Range: 0.0 2.0
97 : // @Increment: 0.1
98 : // @User: Advanced
99 : AP_GROUPINFO("SPDWEIGHT", 9, AP_TECS, _spdWeight, 1.0f),
100 :
101 : // @Param: PTCH_DAMP
102 : // @DisplayName: Controller pitch damping
103 : // @Description: Damping gain for pitch control from TECS control. Increasing may correct for oscillations in speed and height, but too much may cause additional oscillation and degraded control.
104 : // @Range: 0.1 1.0
105 : // @Increment: 0.1
106 : // @User: Advanced
107 : AP_GROUPINFO("PTCH_DAMP", 10, AP_TECS, _ptchDamp, 0.3f),
108 :
109 : // @Param: SINK_MAX
110 : // @DisplayName: Maximum Descent Rate (metres/sec)
111 : // @Description: Maximum demanded descent rate. Do not set higher than the vertical speed the aircraft can maintain at THR_MIN, TECS_PITCH_MIN, and AIRSPEED_MAX.
112 : // @Increment: 0.1
113 : // @Range: 0.0 20.0
114 : // @User: Standard
115 : AP_GROUPINFO("SINK_MAX", 11, AP_TECS, _maxSinkRate, 5.0f),
116 :
117 : // @Param: LAND_ARSPD
118 : // @DisplayName: Airspeed during landing approach (m/s)
119 : // @Description: When performing an autonomous landing, this value is used as the goal airspeed during approach. Max airspeed allowed is Trim Airspeed or AIRSPEED_MAX as defined by LAND_OPTIONS bitmask. Note that this parameter is not useful if your platform does not have an airspeed sensor (use TECS_LAND_THR instead). If negative then this value is halfway between AIRSPEED_MIN and AIRSPEED_CRUISE speed for fixed wing autolandings.
120 : // @Range: -1 127
121 : // @Increment: 1
122 : // @User: Standard
123 : AP_GROUPINFO("LAND_ARSPD", 12, AP_TECS, _landAirspeed, -1),
124 :
125 : // @Param: LAND_THR
126 : // @DisplayName: Cruise throttle during landing approach (percentage)
127 : // @Description: Use this parameter instead of LAND_ARSPD if your platform does not have an airspeed sensor. It is the cruise throttle during landing approach. If this value is negative then it is disabled and TECS_LAND_ARSPD is used instead.
128 : // @Range: -1 100
129 : // @Increment: 0.1
130 : // @User: Standard
131 : AP_GROUPINFO("LAND_THR", 13, AP_TECS, _landThrottle, -1),
132 :
133 : // @Param: LAND_SPDWGT
134 : // @DisplayName: Weighting applied to speed control during landing.
135 : // @Description: Same as SPDWEIGHT parameter, with the exception that this parameter is applied during landing flight stages. A value closer to 2 will result in the plane ignoring height error during landing and our experience has been that the plane will therefore keep the nose up -- sometimes good for a glider landing (with the side effect that you will likely glide a ways past the landing point). A value closer to 0 results in the plane ignoring speed error -- use caution when lowering the value below 1 -- ignoring speed could result in a stall. Values between 0 and 2 are valid values for a fixed landing weight. When using -1 the weight will be scaled during the landing. At the start of the landing approach it starts with TECS_SPDWEIGHT and scales down to 0 by the time you reach the land point. Example: Halfway down the landing approach you'll effectively have a weight of TECS_SPDWEIGHT/2.
136 : // @Range: -1.0 2.0
137 : // @Increment: 0.1
138 : // @User: Advanced
139 : AP_GROUPINFO("LAND_SPDWGT", 14, AP_TECS, _spdWeightLand, -1.0f),
140 :
141 : // @Param: PITCH_MAX
142 : // @DisplayName: Maximum pitch in auto flight
143 : // @Description: Overrides PTCH_LIM_MAX_DEG in automatic throttle modes to reduce climb rates. Uses PTCH_LIM_MAX_DEG if set to 0. For proper TECS tuning, set to the angle that the aircraft can climb at AIRSPEED_CRUISE and THR_MAX.
144 : // @Range: 0 45
145 : // @Increment: 1
146 : // @User: Advanced
147 : AP_GROUPINFO("PITCH_MAX", 15, AP_TECS, _pitch_max, 15),
148 :
149 : // @Param: PITCH_MIN
150 : // @DisplayName: Minimum pitch in auto flight
151 : // @Description: Overrides PTCH_LIM_MIN_DEG in automatic throttle modes to reduce descent rates. Uses PTCH_LIM_MIN_DEG if set to 0. For proper TECS tuning, set to the angle that the aircraft can descend at without overspeeding.
152 : // @Range: -45 0
153 : // @Increment: 1
154 : // @User: Advanced
155 : AP_GROUPINFO("PITCH_MIN", 16, AP_TECS, _pitch_min, 0),
156 :
157 : // @Param: LAND_SINK
158 : // @DisplayName: Sink rate for final landing stage
159 : // @Description: The sink rate in meters/second for the final stage of landing.
160 : // @Range: 0.0 2.0
161 : // @Increment: 0.1
162 : // @User: Advanced
163 : AP_GROUPINFO("LAND_SINK", 17, AP_TECS, _land_sink, 0.25f),
164 :
165 : // @Param: LAND_TCONST
166 : // @DisplayName: Land controller time constant (sec)
167 : // @Description: This is the time constant of the TECS control algorithm when in final landing stage of flight. It should be smaller than TECS_TIME_CONST to allow for faster flare
168 : // @Range: 1.0 5.0
169 : // @Increment: 0.2
170 : // @User: Advanced
171 : AP_GROUPINFO("LAND_TCONST", 18, AP_TECS, _landTimeConst, 2.0f),
172 :
173 : // @Param: LAND_DAMP
174 : // @DisplayName: Controller sink rate to pitch gain during flare
175 : // @Description: This is the sink rate gain for the pitch demand loop when in final landing stage of flight. It should be larger than TECS_PTCH_DAMP to allow for better sink rate control during flare.
176 : // @Range: 0.1 1.0
177 : // @Increment: 0.1
178 : // @User: Advanced
179 : AP_GROUPINFO("LAND_DAMP", 19, AP_TECS, _landDamp, 0.5f),
180 :
181 : // @Param: LAND_PMAX
182 : // @DisplayName: Maximum pitch during final stage of landing
183 : // @Description: This limits the pitch used during the final stage of automatic landing. During the final landing stage most planes need to keep their pitch small to avoid stalling. A maximum of 10 degrees is usually good. A value of zero means to use the normal pitch limits.
184 : // @Range: -5 40
185 : // @Increment: 1
186 : // @User: Advanced
187 : AP_GROUPINFO("LAND_PMAX", 20, AP_TECS, _land_pitch_max, 10),
188 :
189 : // @Param: APPR_SMAX
190 : // @DisplayName: Sink rate max for landing approach stage
191 : // @Description: The sink rate max for the landing approach stage of landing. This will need to be large for steep landing approaches especially when using reverse thrust. If 0, then use TECS_SINK_MAX.
192 : // @Range: 0.0 20.0
193 : // @Units: m/s
194 : // @Increment: 0.1
195 : // @User: Advanced
196 : AP_GROUPINFO("APPR_SMAX", 21, AP_TECS, _maxSinkRate_approach, 0),
197 :
198 : // @Param: LAND_SRC
199 : // @DisplayName: Land sink rate change
200 : // @Description: When zero, the flare sink rate (TECS_LAND_SINK) is a fixed sink demand. With this enabled the flare sink rate will increase/decrease the flare sink demand as you get further beyond the LAND waypoint. Has no effect before the waypoint. This value is added to TECS_LAND_SINK proportional to distance traveled after wp. With an increasing sink rate you can still land in a given distance if you're traveling too fast and cruise passed the land point. A positive value will force the plane to land sooner proportional to distance passed land point. A negative number will tell the plane to slowly climb allowing for a pitched-up stall landing. Recommend 0.2 as initial value.
201 : // @Range: -2.0 2.0
202 : // @Units: m/s/m
203 : // @Increment: 0.1
204 : // @User: Advanced
205 : AP_GROUPINFO("LAND_SRC", 22, AP_TECS, _land_sink_rate_change, 0),
206 :
207 : // @Param: LAND_TDAMP
208 : // @DisplayName: Controller throttle damping when landing
209 : // @Description: Damping gain for the throttle demand loop during an auto-landing. Same as TECS_THR_DAMP but only in effect during an auto-land. Increase to add throttle activity to dampen oscillations in speed and height. When set to 0 landing throttle damping is controlled by TECS_THR_DAMP.
210 : // @Range: 0.1 1.0
211 : // @Increment: 0.1
212 : // @User: Advanced
213 : AP_GROUPINFO("LAND_TDAMP", 23, AP_TECS, _land_throttle_damp, 0),
214 :
215 : // @Param: LAND_IGAIN
216 : // @DisplayName: Controller integrator during landing
217 : // @Description: This is the integrator gain on the control loop during landing. When set to 0 then TECS_INTEG_GAIN is used. Increase to increase the rate at which speed and height offsets are trimmed out. Typically values lower than TECS_INTEG_GAIN work best
218 : // @Range: 0.0 0.5
219 : // @Increment: 0.02
220 : // @User: Advanced
221 : AP_GROUPINFO("LAND_IGAIN", 24, AP_TECS, _integGain_land, 0),
222 :
223 : // @Param: TKOFF_IGAIN
224 : // @DisplayName: Controller integrator during takeoff
225 : // @Description: This is the integrator gain on the control loop during takeoff. Increase to increase the rate at which speed and height offsets are trimmed out.
226 : // @Range: 0.0 0.5
227 : // @Increment: 0.02
228 : // @User: Advanced
229 : AP_GROUPINFO("TKOFF_IGAIN", 25, AP_TECS, _integGain_takeoff, 0),
230 :
231 : // @Param: LAND_PDAMP
232 : // @DisplayName: Pitch damping gain when landing
233 : // @Description: This is the damping gain for the pitch demand loop during landing. Increase to add damping to correct for oscillations in speed and height. If set to 0 then TECS_PTCH_DAMP will be used instead.
234 : // @Range: 0.1 1.0
235 : // @Increment: 0.1
236 : // @User: Advanced
237 : AP_GROUPINFO("LAND_PDAMP", 26, AP_TECS, _land_pitch_damp, 0),
238 :
239 : // @Param: SYNAIRSPEED
240 : // @DisplayName: Enable the use of synthetic airspeed
241 : // @Description: This enables the use of synthetic airspeed in TECS for aircraft that don't have a real airspeed sensor. This is useful for development testing where the user is aware of the considerable limitations of the synthetic airspeed system, such as very poor estimates when a wind estimate is not accurate. Do not enable this option unless you fully understand the limitations of a synthetic airspeed estimate. This option has no effect if a healthy airspeed sensor is being used for airspeed measurements.
242 : // @Values: 0:Disable,1:Enable
243 : // @User: Advanced
244 : AP_GROUPINFO("SYNAIRSPEED", 27, AP_TECS, _use_synthetic_airspeed, 0),
245 :
246 : // @Param: OPTIONS
247 : // @DisplayName: Extra TECS options
248 : // @Description: This allows the enabling of special features in the speed/height controller.
249 : // @Bitmask: 0:GliderOnly,1:AllowDescentSpeedup
250 : // @User: Advanced
251 : AP_GROUPINFO("OPTIONS", 28, AP_TECS, _options, 0),
252 :
253 : // @Param: PTCH_FF_V0
254 : // @DisplayName: Baseline airspeed for pitch feed-forward.
255 : // @Description: This parameter sets the airspeed at which no feed-forward is applied between demanded airspeed and pitch. It should correspond to the airspeed in metres per second at which the plane glides at neutral pitch including STAB_PITCH_DOWN.
256 : // @Range: 5.0 50.0
257 : // @User: Advanced
258 : AP_GROUPINFO("PTCH_FF_V0", 29, AP_TECS, _pitch_ff_v0, 12.0),
259 :
260 : // @Param: PTCH_FF_K
261 : // @DisplayName: Gain for pitch feed-forward.
262 : // @Description: This parameter sets the gain between demanded airspeed and pitch. It has units of radians per metre per second and should generally be negative. A good starting value is -0.04 for gliders and -0.08 for draggy airframes. The default (0.0) disables this feed-forward.
263 : // @Range: -5.0 0.0
264 : // @User: Advanced
265 : AP_GROUPINFO("PTCH_FF_K", 30, AP_TECS, _pitch_ff_k, 0.0),
266 :
267 : // 31 previously used by AP_Int8 TECS_LAND_PTRIM which was removed in November 2022
268 :
269 : // @Param: THR_ERATE
270 : // @DisplayName: Forward throttle external limit slew rate
271 : // @Description: The externally set forward throttle lower limit applied within TECS will be reduced by this many percentage points per second after being set. Set to a non positive value to hold the lower limit for one frame only.
272 : // @Units: %/s
273 : // @Range: 0 100
274 : // @User: Advanced
275 : AP_GROUPINFO("THR_ERATE", 31, AP_TECS, _thr_min_pct_ext_rate_lim, 20),
276 :
277 : // @Param: FLARE_HGT
278 : // @DisplayName: Flare holdoff height
279 : // @Description: When height above ground is below this, the sink rate will be held at TECS_LAND_SINK. Use this to perform a hold-off manoeuvre when combined with small values for TECS_LAND_SINK.
280 : // @Range: 0 15
281 : // @Units: m
282 : // @User: Advanced
283 : AP_GROUPINFO("FLARE_HGT", 32, AP_TECS, _flare_holdoff_hgt, 1.0f),
284 :
285 : // @Param: HDEM_TCONST
286 : // @DisplayName: Height Demand Time Constant
287 : // @Description: This sets the time constant of the low pass filter that is applied to the height demand input when bit 0 of TECS_OPTIONS is not selected.
288 : // @Range: 1.0 5.0
289 : // @Units: s
290 : // @Increment: 0.2
291 : // @User: Advanced
292 : AP_GROUPINFO("HDEM_TCONST", 33, AP_TECS, _hgt_dem_tconst, 3.0f),
293 :
294 : AP_GROUPEND
295 : };
296 :
297 : /*
298 : * Written by Paul Riseborough 2013 to provide:
299 : * - Combined control of speed and height using throttle to control
300 : * total energy and pitch angle to control exchange of energy between
301 : * potential and kinetic.
302 : * Selectable speed or height priority modes when calculating pitch angle
303 : * - Fallback mode when no airspeed measurement is available that
304 : * sets throttle based on height rate demand and switches pitch angle control to
305 : * height priority
306 : * - Underspeed protection that demands maximum throttle and switches pitch angle control
307 : * to speed priority mode
308 : * - Relative ease of tuning through use of intuitive time constant, integrator and damping gains and the use
309 : * of easy to measure aircraft performance data
310 : *
311 : */
312 :
313 1657292 : void AP_TECS::update_50hz(void)
314 : {
315 : // Implement third order complementary filter for height and height rate
316 : // estimated height rate = _climb_rate
317 : // estimated height above field elevation = _height
318 : // Reference Paper :
319 : // Optimizing the Gains of the Baro-Inertial Vertical Channel
320 : // Widnall W.S, Sinha P.K,
321 : // AIAA Journal of Guidance and Control, 78-1307R
322 :
323 : /*
324 : if we have a vertical position estimate from the EKF then use
325 : it, otherwise use barometric altitude
326 : */
327 1657292 : _ahrs.get_relative_position_D_home(_height);
328 1657292 : _height *= -1.0f;
329 :
330 : // Calculate time in seconds since last update
331 1657292 : uint64_t now = AP_HAL::micros64();
332 1657292 : float DT = (now - _update_50hz_last_usec) * 1.0e-6f;
333 1657292 : _flags.reset = DT > 1.0f;
334 1657292 : if (_flags.reset) {
335 368 : _climb_rate = 0.0f;
336 368 : _height_filter.dd_height = 0.0f;
337 368 : DT = 0.02f; // when first starting TECS, use most likely time constant
338 368 : _vdot_filter.reset();
339 : }
340 1657292 : _update_50hz_last_usec = now;
341 :
342 : // Use inertial nav vertical velocity and height if available
343 1657292 : Vector3f velned;
344 1657292 : if (_ahrs.get_velocity_NED(velned)) {
345 : // if possible use the EKF vertical velocity
346 1648957 : _climb_rate = -velned.z;
347 : } else {
348 : /*
349 : use a complimentary filter to calculate climb_rate. This is
350 : designed to minimise lag
351 : */
352 8335 : const float baro_alt = AP::baro().get_altitude();
353 : // Get height acceleration
354 8335 : float hgt_ddot_mea = -(_ahrs.get_accel_ef().z + GRAVITY_MSS);
355 : // Perform filter calculation using backwards Euler integration
356 : // Coefficients selected to place all three filter poles at omega
357 8335 : float omega2 = _hgtCompFiltOmega*_hgtCompFiltOmega;
358 8335 : float hgt_err = baro_alt - _height_filter.height;
359 8335 : float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega;
360 :
361 8335 : _height_filter.dd_height += integ1_input * DT;
362 :
363 8335 : float integ2_input = _height_filter.dd_height + hgt_ddot_mea + hgt_err * omega2 * 3.0f;
364 :
365 8335 : _climb_rate += integ2_input * DT;
366 :
367 8335 : float integ3_input = _climb_rate + hgt_err * _hgtCompFiltOmega * 3.0f;
368 : // If more than 1 second has elapsed since last update then reset the integrator state
369 : // to the measured height
370 8335 : if (_flags.reset) {
371 38 : _height_filter.height = _height;
372 : } else {
373 8297 : _height_filter.height += integ3_input*DT;
374 : }
375 : }
376 :
377 : // Update the speed estimate using a 2nd order complementary filter
378 1657292 : _update_speed(DT);
379 1657292 : }
380 :
381 1657292 : void AP_TECS::_update_speed(float DT)
382 : {
383 : // Update and average speed rate of change
384 :
385 : // calculate a low pass filtered _vel_dot
386 1657292 : if (_flags.reset) {
387 368 : _vdot_filter.reset();
388 368 : _vel_dot_lpf = _vel_dot;
389 : } else {
390 : // Get DCM
391 1656924 : const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned();
392 : // Calculate speed rate of change
393 1656924 : float temp = rotMat.c.x * GRAVITY_MSS + AP::ins().get_accel().x;
394 : // take 5 point moving average
395 1656924 : _vel_dot = _vdot_filter.apply(temp);
396 1656924 : const float alpha = DT / (DT + timeConstant());
397 1656924 : _vel_dot_lpf = _vel_dot_lpf * (1.0f - alpha) + _vel_dot * alpha;
398 : }
399 :
400 1657292 : const bool should_use_airspeed = use_airspeed();
401 :
402 : // Convert equivalent airspeeds to true airspeeds and harmonise limits
403 :
404 1657292 : float EAS2TAS = _ahrs.get_EAS2TAS();
405 1657292 : _TAS_dem = _EAS_dem * EAS2TAS;
406 1657292 : if (_flags.reset || !should_use_airspeed) {
407 77821 : _TASmax = aparm.airspeed_max * EAS2TAS;
408 1579471 : } else if (_thr_clip_status == clipStatus::MAX) {
409 : // wind down airspeed upper limit to prevent a situation where the aircraft can't climb
410 : // at the maximum speed
411 177500 : const float velRateMin = 0.5f * _STEdot_min / MAX(_TAS_state, aparm.airspeed_min * EAS2TAS);
412 177500 : _TASmax += _DT * velRateMin;
413 177500 : _TASmax = MAX(_TASmax, aparm.airspeed_cruise * EAS2TAS);
414 : } else {
415 : // wind airspeed upper limit back to parameter defined value
416 1401971 : const float velRateMax = 0.5f * _STEdot_max / MAX(_TAS_state, aparm.airspeed_min * EAS2TAS);
417 1401971 : _TASmax += _DT * velRateMax;
418 : }
419 1657292 : _TASmax = MIN(_TASmax, aparm.airspeed_max * EAS2TAS);
420 1657292 : _TASmin = aparm.airspeed_min * EAS2TAS;
421 :
422 1657292 : if (_landing.is_on_final() && is_positive(aparm.airspeed_stall)) {
423 0 : _TASmin = aparm.airspeed_stall * EAS2TAS;
424 : }
425 :
426 1657292 : if (aparm.stall_prevention) {
427 : // when stall prevention is active we raise the minimum
428 : // airspeed based on aerodynamic load factor
429 1629071 : if (is_positive(aparm.airspeed_stall)) {
430 0 : _TASmin = MAX(_TASmin, aparm.airspeed_stall*EAS2TAS*sqrtf(_load_factor));
431 : } else {
432 1629071 : _TASmin *= sqrtf(_load_factor);
433 : }
434 : }
435 :
436 1657292 : if (_TASmax < _TASmin) {
437 339 : _TASmax = _TASmin;
438 : }
439 :
440 : // Get measured airspeed or default to trim speed and constrain to range between min and max if
441 : // airspeed sensor data cannot be used
442 :
443 : // Equivalent airspeed
444 : float _EAS;
445 1657292 : if (!should_use_airspeed || !_ahrs.airspeed_EAS(_EAS)) {
446 : // If no airspeed available, assume we are at cruise speed.
447 77471 : _EAS = constrain_float(aparm.airspeed_cruise.get(), (float)aparm.airspeed_min.get(), (float)aparm.airspeed_max.get());
448 : }
449 :
450 : // limit the airspeed to a minimum of 3 m/s
451 1657292 : const float min_airspeed = 3.0;
452 :
453 : // Reset states of time since last update is too large
454 1657292 : if (_flags.reset) {
455 368 : _TAS_state = (_EAS * EAS2TAS);
456 368 : _TAS_state = MAX(_TAS_state, min_airspeed);
457 368 : _integDTAS_state = 0.0f;
458 368 : return;
459 : }
460 :
461 : // Implement a second order complementary filter to obtain a
462 : // smoothed airspeed estimate
463 : // airspeed estimate is held in _TAS_state
464 1656924 : float aspdErr = (_EAS * EAS2TAS) - _TAS_state;
465 1656924 : float integDTAS_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega;
466 : // Prevent state from winding up
467 1656924 : if (_TAS_state < 3.1f) {
468 163166 : integDTAS_input = MAX(integDTAS_input, 0.0f);
469 : }
470 1656924 : _integDTAS_state = _integDTAS_state + integDTAS_input * DT;
471 1656924 : float TAS_input = _integDTAS_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f;
472 1656924 : _TAS_state = _TAS_state + TAS_input * DT;
473 1656924 : _TAS_state = MAX(_TAS_state, min_airspeed);
474 :
475 : }
476 :
477 313892 : void AP_TECS::_update_speed_demand(void)
478 : {
479 313892 : if (option_is_set(Option::DESCENT_SPEEDUP)) {
480 : // Allow demanded speed to go to maximum when descending at maximum descent rate
481 4799 : _TAS_dem = _TAS_dem + (_TASmax - _TAS_dem) * _sink_fraction;
482 : }
483 :
484 : // Set the airspeed demand to the minimum value if an underspeed condition exists
485 : // or a bad descent condition exists
486 : // This will minimise the rate of descent resulting from an engine failure,
487 : // enable the maximum climb rate to be achieved and prevent continued full power descent
488 : // into the ground due to an unachievable airspeed value
489 313892 : if ((_flags.badDescent) || (_flags.underspeed)) {
490 6291 : _TAS_dem = _TASmin;
491 : }
492 :
493 : // Constrain speed demand, taking into account the load factor
494 313892 : _TAS_dem = constrain_float(_TAS_dem, _TASmin, _TASmax);
495 :
496 : // Determine the true cruising airspeed (m/s)
497 313892 : const float TAScruise = aparm.airspeed_cruise * _ahrs.get_EAS2TAS();
498 :
499 : // calculate velocity rate limits based on physical performance limits
500 : // provision to use a different rate limit if bad descent or underspeed condition exists
501 : // Use 50% of maximum energy rate on gain, 90% on dissipation to allow margin for total energy controller
502 313892 : const float velRateMax = 0.5f * _STEdot_max / _TAS_state;
503 : // Maximum permissible rate of deceleration value at max airspeed
504 313892 : const float velRateNegMax = 0.9f * _STEdot_neg_max / _TASmax;
505 : // Maximum permissible rate of deceleration value at cruise speed
506 313892 : const float velRateNegCruise = 0.9f * _STEdot_min / TAScruise;
507 : // Linear interpolation between velocity rate at cruise and max speeds, capped at those speeds
508 313892 : const float velRateMin = linear_interpolate(velRateNegMax, velRateNegCruise, _TAS_state, _TASmax, TAScruise);
509 313892 : const float TAS_dem_previous = _TAS_dem_adj;
510 :
511 : // Apply rate limit
512 313892 : if ((_TAS_dem - TAS_dem_previous) > (velRateMax * _DT)) {
513 11020 : _TAS_dem_adj = TAS_dem_previous + velRateMax * _DT;
514 11020 : _TAS_rate_dem = velRateMax;
515 302872 : } else if ((_TAS_dem - TAS_dem_previous) < (velRateMin * _DT)) {
516 20530 : _TAS_dem_adj = TAS_dem_previous + velRateMin * _DT;
517 20530 : _TAS_rate_dem = velRateMin;
518 : } else {
519 282342 : _TAS_rate_dem = (_TAS_dem - TAS_dem_previous) / _DT;
520 282342 : _TAS_dem_adj = _TAS_dem;
521 : }
522 :
523 : // calculate a low pass filtered _TAS_rate_dem
524 313892 : if (_flags.reset) {
525 516 : _TAS_dem_adj = _TAS_state;
526 516 : _TAS_rate_dem_lpf = _TAS_rate_dem;
527 : } else {
528 313376 : const float alpha = _DT / (_DT + timeConstant());
529 313376 : _TAS_rate_dem_lpf = _TAS_rate_dem_lpf * (1.0f - alpha) + _TAS_rate_dem * alpha;
530 : }
531 :
532 : // Constrain speed demand again to protect against bad values on initialisation.
533 313892 : _TAS_dem_adj = constrain_float(_TAS_dem_adj, _TASmin, _TASmax);
534 313892 : }
535 :
536 313892 : void AP_TECS::_update_height_demand(void)
537 : {
538 313892 : _climb_rate_limit = _maxClimbRate * _max_climb_scaler;
539 313892 : _sink_rate_limit = _maxSinkRate * _max_sink_scaler;
540 313892 : if (_maxSinkRate_approach > 0 && _flags.is_doing_auto_land) {
541 : // special sink rate for approach to accommodate steep slopes and reverse thrust.
542 : // A special check must be done to see if we're LANDing on approach but also if
543 : // we're in that tiny window just starting NAV_LAND but still in NORMAL mode. If
544 : // we have a steep slope with a short approach we'll want to allow acquiring the
545 : // glide slope right away.
546 0 : _sink_rate_limit = _maxSinkRate_approach;
547 : }
548 :
549 :
550 313892 : if (!_landing.is_flaring()) {
551 : // Apply 2 point moving average to demanded height
552 290636 : const float hgt_dem = 0.5f * (_hgt_dem_in + _hgt_dem_in_prev);
553 290636 : _hgt_dem_in_prev = _hgt_dem_in;
554 :
555 : // Limit height rate of change
556 290636 : if ((hgt_dem - _hgt_dem_rate_ltd) > (_climb_rate_limit * _DT)) {
557 35958 : _hgt_dem_rate_ltd = _hgt_dem_rate_ltd + _climb_rate_limit * _DT;
558 35958 : _sink_fraction = 0.0f;
559 254678 : } else if ((hgt_dem - _hgt_dem_rate_ltd) < (-_sink_rate_limit * _DT)) {
560 24717 : _hgt_dem_rate_ltd = _hgt_dem_rate_ltd - _sink_rate_limit * _DT;
561 24717 : _sink_fraction = 1.0f;
562 : } else {
563 229961 : const float numerator = hgt_dem - _hgt_dem_rate_ltd;
564 229961 : const float denominator = - _sink_rate_limit * _DT;
565 229961 : if (is_negative(numerator) && is_negative(denominator)) {
566 67109 : _sink_fraction = numerator / denominator;
567 : } else {
568 162852 : _sink_fraction = 0.0f;
569 : }
570 229961 : _hgt_dem_rate_ltd = hgt_dem;
571 : }
572 :
573 : // Apply a first order lag to height demand and compensate for lag when commencing height
574 : // control after takeoff to prevent plane pushing nose to level before climbing again. Post takeoff
575 : // compensation offset is decayed using the same time constant as the height demand filter.
576 290636 : const float coef = MIN(_DT / (_DT + MAX(_hgt_dem_tconst, _DT)), 1.0f);
577 290636 : _hgt_rate_dem = (_hgt_dem_rate_ltd - _hgt_dem_lpf) / _hgt_dem_tconst;
578 290636 : _hgt_dem_lpf = _hgt_dem_rate_ltd * coef + (1.0f - coef) * _hgt_dem_lpf;
579 290636 : _post_TO_hgt_offset *= (1.0f - coef);
580 290636 : _hgt_dem = _hgt_dem_lpf + _post_TO_hgt_offset;
581 :
582 : // during approach compensate for height filter lag
583 290636 : if (_flags.is_doing_auto_land) {
584 12850 : _hgt_dem += _hgt_dem_tconst * _hgt_rate_dem;
585 : } else {
586 : // Don't allow height demand to get too far ahead of the vehicles current height
587 : // if vehicle is unable to follow the demanded climb or descent
588 545060 : bool max_climb_condition = (_pitch_dem_unc > _PITCHmaxf) ||
589 267274 : (_SEBdot_dem_clip == clipStatus::MAX);
590 545899 : bool max_descent_condition = (_pitch_dem_unc < _PITCHminf) ||
591 268113 : (_SEBdot_dem_clip == clipStatus::MIN);
592 277786 : if (_using_airspeed_for_throttle) {
593 : // large height errors will result in the throttle saturating
594 276507 : max_climb_condition |= (_thr_clip_status == clipStatus::MAX) &&
595 11736 : !((_flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || (_flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING));
596 264771 : max_descent_condition |= (_thr_clip_status == clipStatus::MIN) && !_landing.is_flaring();
597 : }
598 277786 : const float hgt_dem_alpha = _DT / MAX(_DT + _hgt_dem_tconst, _DT);
599 277786 : if (max_climb_condition && _hgt_dem > _hgt_dem_prev) {
600 18641 : _max_climb_scaler *= (1.0f - hgt_dem_alpha);
601 259145 : } else if (max_descent_condition && _hgt_dem < _hgt_dem_prev) {
602 6123 : _max_sink_scaler *= (1.0f - hgt_dem_alpha);
603 : } else {
604 253022 : _max_climb_scaler = _max_climb_scaler * (1.0f - hgt_dem_alpha) + hgt_dem_alpha;
605 253022 : _max_sink_scaler = _max_sink_scaler * (1.0f - hgt_dem_alpha) + hgt_dem_alpha;
606 : }
607 : }
608 290636 : _hgt_dem_prev = _hgt_dem;
609 : } else {
610 : // when flaring force height rate demand to the
611 : // configured sink rate and adjust the demanded height to
612 : // be kinematically consistent with the height rate.
613 :
614 : // set all height filter states to current height to prevent large pitch transients if flare is aborted
615 23256 : _hgt_dem_lpf = _height;
616 23256 : _hgt_dem_rate_ltd = _height;
617 23256 : _hgt_dem_in_prev = _height;
618 :
619 23256 : if (!_flare_initialised) {
620 110 : _flare_hgt_dem_adj = _hgt_dem;
621 110 : _flare_hgt_dem_ideal = _height;
622 110 : _hgt_at_start_of_flare = _hgt_afe;
623 110 : _hgt_rate_dem_at_flare_entry = _hgt_rate_dem;
624 110 : _flare_initialised = true;
625 : }
626 :
627 : // adjust the flare sink rate to increase/decrease as your travel further beyond the land wp
628 23256 : float land_sink_rate_adj = _land_sink + _land_sink_rate_change*_distance_beyond_land_wp;
629 :
630 : // bring it in linearly with height
631 : float p;
632 23256 : if (_hgt_at_start_of_flare > _flare_holdoff_hgt) {
633 22723 : p = constrain_float((_hgt_at_start_of_flare - _hgt_afe) / (_hgt_at_start_of_flare - _flare_holdoff_hgt), 0.0f, 1.0f);
634 : } else {
635 533 : p = 1.0f;
636 : }
637 23256 : _hgt_rate_dem = _hgt_rate_dem_at_flare_entry * (1.0f - p) - land_sink_rate_adj * p;
638 :
639 23256 : _flare_hgt_dem_ideal += _DT * _hgt_rate_dem; // the ideal height profile to follow
640 23256 : _flare_hgt_dem_adj += _DT * _hgt_rate_dem; // the demanded height profile that includes the pre-flare height tracking offset
641 :
642 : // fade across to the ideal height profile
643 23256 : _hgt_dem = _flare_hgt_dem_adj * (1.0f - p) + _flare_hgt_dem_ideal * p;
644 : }
645 313892 : }
646 :
647 313892 : void AP_TECS::_detect_underspeed(void)
648 : {
649 : // see if we can clear a previous underspeed condition. We clear
650 : // it if we are now more than 15% above min speed, and haven't
651 : // been below min speed for at least 3 seconds.
652 630382 : if (_flags.underspeed &&
653 314121 : _TAS_state >= _TASmin * 1.15f &&
654 229 : AP_HAL::millis() - _underspeed_start_ms > 3000U) {
655 1 : _flags.underspeed = false;
656 : }
657 :
658 313892 : if (_flight_stage == AP_FixedWing::FlightStage::VTOL) {
659 18726 : _flags.underspeed = false;
660 610613 : } else if (((_TAS_state < _TASmin * 0.9f) &&
661 20281 : (_throttle_dem >= _THRmaxf * 0.95f) &&
662 604873 : !_landing.is_flaring()) ||
663 291364 : ((_height < _hgt_dem) && _flags.underspeed))
664 : {
665 4229 : _flags.underspeed = true;
666 4229 : if (_TAS_state < _TASmin * 0.9f) {
667 : // reset start time as we are still underspeed
668 3802 : _underspeed_start_ms = AP_HAL::millis();
669 : }
670 : } else {
671 : // this clears underspeed if we reach our demanded height and
672 : // we are either below 95% throttle or we above 90% of min
673 : // airspeed
674 290937 : _flags.underspeed = false;
675 : }
676 313892 : }
677 :
678 313892 : void AP_TECS::_update_energies(void)
679 : {
680 : // Calculate specific energy demands
681 313892 : _SPE_dem = _hgt_dem * GRAVITY_MSS;
682 313892 : _SKE_dem = 0.5f * _TAS_dem_adj * _TAS_dem_adj;
683 :
684 : // Calculate specific energy rate demands and high pass filter demanded airspeed
685 : // rate of change to match the filtering applied to the measurement
686 313892 : _SKEdot_dem = _TAS_state * (_TAS_rate_dem - _TAS_rate_dem_lpf);
687 :
688 : // Calculate specific energy
689 313892 : _SPE_est = _height * GRAVITY_MSS;
690 313892 : _SKE_est = 0.5f * _TAS_state * _TAS_state;
691 :
692 : // Calculate specific energy rate and high pass filter airspeed rate of change
693 : // to remove effect of complementary filter induced bias errors
694 313892 : _SPEdot = _climb_rate * GRAVITY_MSS;
695 313892 : _SKEdot = _TAS_state * (_vel_dot - _vel_dot_lpf);
696 :
697 313892 : }
698 :
699 : /*
700 : current time constant. It is lower in landing to try to give a precise approach
701 : */
702 3177779 : float AP_TECS::timeConstant(void) const
703 : {
704 3177779 : if (_flags.is_doing_auto_land) {
705 377445 : if (_landTimeConst < 0.1f) {
706 0 : return 0.1f;
707 : }
708 377445 : return _landTimeConst;
709 : }
710 2800334 : if (_timeConst < 0.1f) {
711 0 : return 0.1f;
712 : }
713 2800334 : return _timeConst;
714 : }
715 :
716 : /*
717 : calculate throttle demand - airspeed enabled case
718 : */
719 299136 : void AP_TECS::_update_throttle_with_airspeed(void)
720 : {
721 : // Calculate limits to be applied to potential energy error to prevent over or underspeed occurring due to large height errors
722 299136 : float SPE_err_max = MAX(_SKE_est - 0.5f * _TASmin * _TASmin, 0.0f);
723 299136 : float SPE_err_min = MIN(_SKE_est - 0.5f * _TASmax * _TASmax, 0.0f);
724 :
725 299136 : if (_flight_stage == AP_FixedWing::FlightStage::VTOL) {
726 : /*
727 : when we are in a VTOL state then we ignore potential energy
728 : errors as we have vertical motors that interfere with the
729 : total energy calculation.
730 : */
731 18726 : SPE_err_max = SPE_err_min = 0;
732 : }
733 :
734 : // rate of change of potential energy is proportional to height error
735 299136 : _SPEdot_dem = (_SPE_dem - _SPE_est) / timeConstant();
736 :
737 : // Calculate total energy error
738 299136 : _STE_error = constrain_float((_SPE_dem - _SPE_est), SPE_err_min, SPE_err_max) + _SKE_dem - _SKE_est;
739 299136 : float STEdot_dem = constrain_float((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
740 299136 : float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
741 :
742 : // Apply 0.5 second first order filter to STEdot_error
743 : // This is required to remove accelerometer noise from the measurement
744 299136 : const float filt_coef = 2.0f * _DT;
745 299136 : STEdot_error = filt_coef * STEdot_error + (1.0f - filt_coef) * _STEdotErrLast;
746 299136 : _STEdotErrLast = STEdot_error;
747 :
748 : // Calculate throttle demand
749 : // If underspeed condition is set, then demand full throttle
750 299136 : if (_flags.underspeed) {
751 4228 : _throttle_dem = 1.0f;
752 294908 : } else if (_flags.is_gliding) {
753 14349 : _throttle_dem = 0.0f;
754 : } else {
755 : // Calculate gain scaler from specific energy error to throttle
756 280559 : const float K_thr2STE = (_STEdot_max - _STEdot_min) / (_THRmaxf - _THRminf); // This is the derivative of STEdot wrt throttle measured across the max allowed throttle range.
757 280559 : const float K_STE2Thr = 1 / (timeConstant() * K_thr2STE);
758 :
759 : // Calculate feed-forward throttle
760 280559 : const float nomThr = aparm.throttle_cruise * 0.01f;
761 : // Use the demanded rate of change of total energy as the feed-forward demand, but add
762 : // additional component which scales with (1/(cos(bank angle)**2) - 1) to compensate for induced
763 : // drag increase during turns.
764 280559 : STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(sq(_ahrs.cos_roll()), 0.1f, 1.0f) - 1.0f);
765 280559 : const float ff_throttle = nomThr + STEdot_dem / K_thr2STE;
766 :
767 : // Calculate PD + FF throttle
768 280559 : float throttle_damp = _thrDamp;
769 280559 : if (_flags.is_doing_auto_land && !is_zero(_land_throttle_damp)) {
770 0 : throttle_damp = _land_throttle_damp;
771 : }
772 280559 : _throttle_dem = (_STE_error + STEdot_error * throttle_damp) * K_STE2Thr + ff_throttle;
773 :
774 280559 : float THRminf_clipped_to_zero = constrain_float(_THRminf, 0, _THRmaxf);
775 :
776 : // Calculate integrator state upper and lower limits
777 : // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
778 : // Additionally constrain the integrator state amplitude so that the integrator comes off limits faster.
779 280559 : const float maxAmp = 0.5f*(_THRmaxf - THRminf_clipped_to_zero);
780 280559 : const float integ_max = constrain_float((_THRmaxf - _throttle_dem + 0.1f),-maxAmp,maxAmp);
781 280559 : const float integ_min = constrain_float((_THRminf - _throttle_dem - 0.1f),-maxAmp,maxAmp);
782 :
783 : // Calculate integrator state, constraining state
784 : // Set integrator to a max throttle value during climbout
785 280559 : _integTHR_state = _integTHR_state + (_STE_error * _get_i_gain()) * _DT * K_STE2Thr;
786 280559 : if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
787 5840 : if (!_flags.reached_speed_takeoff) {
788 : // ensure we run at full throttle until we reach the target airspeed
789 222 : _throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state);
790 : }
791 : } else {
792 274719 : _integTHR_state = constrain_float(_integTHR_state, integ_min, integ_max);
793 : }
794 :
795 : // Rate limit PD + FF throttle
796 : // Calculate the throttle increment from the specified slew time
797 280559 : int16_t throttle_slewrate = aparm.throttle_slewrate;
798 280559 : if (_landing.is_on_approach()) {
799 5188 : const int16_t land_slewrate = _landing.get_throttle_slewrate();
800 5188 : if (land_slewrate > 0) {
801 0 : throttle_slewrate = land_slewrate;
802 : }
803 : }
804 :
805 280559 : if (throttle_slewrate != 0) {
806 280230 : const float thrRateIncr = _DT * (_THRmaxf - THRminf_clipped_to_zero) * throttle_slewrate * 0.01f;
807 :
808 280230 : _throttle_dem = constrain_float(_throttle_dem,
809 : _last_throttle_dem - thrRateIncr,
810 : _last_throttle_dem + thrRateIncr);
811 280230 : _last_throttle_dem = _throttle_dem;
812 : }
813 :
814 : // Sum the components.
815 280559 : _throttle_dem = _throttle_dem + _integTHR_state;
816 :
817 : #if HAL_LOGGING_ENABLED
818 280559 : if (AP::logger().should_log(_log_bitmask)){
819 : // @LoggerMessage: TEC3
820 : // @Vehicles: Plane
821 : // @Description: Additional additional information about the Total Energy Control System
822 : // @URL: http://ardupilot.club/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html
823 : // @Field: TimeUS: Time since system startup
824 : // @Field: KED: Kinetic Energy Dot (1st derivative of KE)
825 : // @Field: PED: Potential Energy Dot (1st derivative of PE)
826 : // @Field: KEDD: Kinetic Energy Dot Demand
827 : // @Field: PEDD: Potential Energy Dot Demand
828 : // @Field: TEE: Total energy error
829 : // @Field: TEDE: Total energy dot error (1st derivative of total energy error)
830 : // @Field: FFT: feed-forward throttle
831 : // @Field: Imin: integrator limit based on throttle values
832 : // @Field: Imax: integrator limit based on throttle values
833 : // @Field: I: integrator state for throttle
834 : // @Field: Emin: lower limit for potential energy error
835 : // @Field: Emax: upper limit for potential energy error
836 561118 : AP::logger().WriteStreaming(
837 : "TEC3",
838 : "TimeUS," "KED," "PED," "KEDD," "PEDD," "TEE," "TEDE," "FFT," "Imin," "Imax," "I," "Emin," "Emax",
839 : "s" "?" "?" "?" "?" "?" "?" "-" "-" "-" "-" "?" "?",
840 : "F" "0" "0" "0" "0" "0" "0" "0" "0" "0" "0" "0" "0",
841 : "Q" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f",
842 : AP_HAL::micros64(),
843 280559 : _SKEdot,
844 280559 : _SPEdot,
845 280559 : _SKEdot_dem,
846 280559 : _SPEdot_dem,
847 280559 : _STE_error,
848 : STEdot_error,
849 : ff_throttle,
850 : integ_min,
851 : integ_max,
852 280559 : _integTHR_state,
853 : SPE_err_min,
854 : SPE_err_max
855 : );
856 :
857 : // @LoggerMessage: TEC4
858 : // @Vehicles: Plane
859 : // @Description: Additional additional additional information about the Total Energy Control System
860 : // @URL: http://ardupilot.club/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html
861 : // @Field: TimeUS: Time since system startup
862 : // @Field: P: estimate of potential energy
863 : // @Field: K: estimate of kinetic energy
864 : // @Field: Pdem: demanded potential energy
865 : // @Field: Kdem: demanded kinetic energy
866 : // @Field: ClimbMax: Climb rate upper limit
867 : // @Field: ClimbMin: Climb rate lower limit
868 561118 : AP::logger().WriteStreaming(
869 : "TEC4",
870 : "TimeUS," "P," "K," "Pdem," "Kdem," "ClimbMax," "ClimbMin",
871 : "s" "?" "?" "?" "?" "n" "n",
872 : "F" "0" "0" "0" "0" "0" "0",
873 : "Q" "f" "f" "f" "f" "f" "f",
874 : AP_HAL::micros64(),
875 280559 : _SPE_est,
876 280559 : _SKE_est,
877 280559 : _SPE_dem,
878 280559 : _SKE_dem,
879 280559 : _climb_rate_limit,
880 280559 : -_sink_rate_limit
881 : );
882 : }
883 : #endif
884 : }
885 :
886 299136 : constrain_throttle();
887 :
888 299136 : }
889 :
890 : // Constrain throttle demand and record clipping
891 313892 : void AP_TECS::constrain_throttle() {
892 313892 : if (_throttle_dem > _THRmaxf) {
893 33805 : _thr_clip_status = clipStatus::MAX;
894 33805 : _throttle_dem = _THRmaxf;
895 280087 : } else if (_throttle_dem < _THRminf) {
896 12214 : _thr_clip_status = clipStatus::MIN;
897 12214 : _throttle_dem = _THRminf;
898 : } else {
899 267873 : _thr_clip_status = clipStatus::NONE;
900 : }
901 313892 : }
902 :
903 594451 : float AP_TECS::_get_i_gain(void)
904 : {
905 594451 : float i_gain = _integGain;
906 594451 : if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
907 13836 : i_gain = _integGain_takeoff;
908 580615 : } else if (_flags.is_doing_auto_land) {
909 69075 : if (!is_zero(_integGain_land)) {
910 0 : i_gain = _integGain_land;
911 : }
912 : }
913 594451 : return i_gain;
914 : }
915 :
916 : /*
917 : calculate throttle, non-airspeed case
918 : */
919 14756 : void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pitch_trim_deg)
920 : {
921 : // reset clip status after possible use of synthetic airspeed
922 14756 : _thr_clip_status = clipStatus::NONE;
923 :
924 : // Calculate throttle demand by interpolating between pitch and throttle limits
925 : float nomThr;
926 : //If landing and we don't have an airspeed sensor and we have a non-zero
927 : //TECS_LAND_THR param then use it
928 14756 : if (_flags.is_doing_auto_land && _landThrottle >= 0) {
929 0 : nomThr = (_landThrottle + throttle_nudge) * 0.01f;
930 : } else { //not landing or not using TECS_LAND_THR parameter
931 14756 : nomThr = (aparm.throttle_cruise + throttle_nudge)* 0.01f;
932 : }
933 :
934 : // Use a pitch angle that is the sum of a highpassed _pitch_dem and a lowpassed ahrs pitch
935 : // so that the throttle mapping adjusts for the effect of pitch control errors
936 14756 : _pitch_demand_lpf.apply(_pitch_dem, _DT);
937 14756 : const float pitch_demand_hpf = _pitch_dem - _pitch_demand_lpf.get();
938 14756 : _pitch_measured_lpf.apply(_ahrs.get_pitch_rad(), _DT);
939 14756 : const float pitch_corrected_lpf = _pitch_measured_lpf.get() - radians(pitch_trim_deg);
940 14756 : const float pitch_blended = pitch_demand_hpf + pitch_corrected_lpf;
941 :
942 14756 : if (pitch_blended > 0.0f && _PITCHmaxf > 0.0f)
943 : {
944 1952 : _throttle_dem = nomThr + (_THRmaxf - nomThr) * pitch_blended / _PITCHmaxf;
945 : }
946 12804 : else if (pitch_blended < 0.0f && _PITCHminf < 0.0f)
947 : {
948 12703 : _throttle_dem = nomThr + (_THRminf - nomThr) * pitch_blended / _PITCHminf;
949 : }
950 : else
951 : {
952 101 : _throttle_dem = nomThr;
953 : }
954 :
955 14756 : if (_flags.is_gliding) {
956 0 : _throttle_dem = 0.0f;
957 0 : return;
958 : }
959 :
960 : // Calculate additional throttle for turn drag compensation including throttle nudging
961 : // Use the demanded rate of change of total energy as the feed-forward demand, but add
962 : // additional component which scales with (1/(cos(bank angle)**2) - 1) to compensate for induced
963 : // drag increase during turns.
964 14756 : float STEdot_dem = _rollComp * (1.0f/constrain_float(sq(_ahrs.cos_roll()), 0.1f, 1.0f) - 1.0f);
965 14756 : _throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
966 :
967 14756 : constrain_throttle();
968 : }
969 :
970 313892 : void AP_TECS::_detect_bad_descent(void)
971 : {
972 : // Don't detect bad descents when gliding, transitioning, or when underspeed.
973 313892 : if (_flags.is_gliding || _flight_stage == AP_FixedWing::FlightStage::VTOL || _flags.underspeed) {
974 37304 : _flags.badDescent = false;
975 37304 : return;
976 : }
977 :
978 : // Detect a demanded airspeed too high for the aircraft to achieve. This will be
979 : // evident by the following conditions:
980 : // 1) Specific total energy error > 200 (greater than ~20m height error)
981 : // 2) Specific total energy reducing
982 : // 3) throttle demand > 90%
983 : // If these conditions exist simultaneously, then the protection mode will be activated.
984 : // Once active, it will remain active until the specific total energy error drops below 0.
985 : // This mode will produce an undulating speed and height response as it cuts in and out, but it
986 : // will prevent the aircraft from descending into the ground if an unachievable speed demand is set.
987 276588 : float STEdot = _SPEdot + _SKEdot;
988 276588 : if (((_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_flags.badDescent && (_STE_error > 0.0f))) {
989 3719 : _flags.badDescent = true;
990 : } else {
991 272869 : _flags.badDescent = false;
992 : }
993 : }
994 :
995 313892 : void AP_TECS::_update_pitch(void)
996 : {
997 : // Calculate Speed/Height Control Weighting
998 : // This is used to determine how the pitch control prioritises speed and height control
999 : // A weighting of 1 provides equal priority (this is the normal mode of operation)
1000 : // A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available
1001 : // A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected. In this instance, if airspeed
1002 : // rises above the demanded value, the pitch angle will be increased by the TECS controller.
1003 313892 : _SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f);
1004 313892 : if (!use_airspeed()) {
1005 14756 : _SKE_weighting = 0.0f;
1006 299136 : } else if (_flight_stage == AP_FixedWing::FlightStage::VTOL) {
1007 : // if we are in VTOL mode then control pitch without regard to
1008 : // speed. Speed is also taken care of independently of
1009 : // height. This is needed as the usual relationship of speed
1010 : // and height is broken by the VTOL motors
1011 18726 : _SKE_weighting = 0.0f;
1012 280410 : } else if ( _flags.underspeed || _flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING || _flags.is_gliding) {
1013 24417 : _SKE_weighting = 2.0f;
1014 255993 : } else if (_flags.is_doing_auto_land) {
1015 32969 : if (_spdWeightLand < 0) {
1016 : // use sliding scale from normal weight down to zero at landing
1017 32969 : float scaled_weight = _spdWeight * (1.0f - constrain_float(_path_proportion,0,1));
1018 32969 : _SKE_weighting = constrain_float(scaled_weight, 0.0f, 2.0f);
1019 : } else {
1020 0 : _SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f);
1021 : }
1022 : }
1023 :
1024 313892 : float SPE_weighting = 2.0f - _SKE_weighting;
1025 :
1026 : // either weight can fade to 0, but don't go above 1 to prevent instability if tuned at a speed weight of 1 and weighting is varied to end points in flight.
1027 313892 : SPE_weighting = MIN(SPE_weighting, 1.0f);
1028 313892 : _SKE_weighting = MIN(_SKE_weighting, 1.0f);
1029 :
1030 : // Calculate demanded specific energy balance and error
1031 313892 : float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * _SKE_weighting;
1032 313892 : float SEB_est = _SPE_est * SPE_weighting - _SKE_est * _SKE_weighting;
1033 313892 : float SEB_error = SEB_dem - SEB_est;
1034 :
1035 : // track demanded height using the specified time constant
1036 313892 : float SEBdot_dem = _hgt_rate_dem * GRAVITY_MSS * SPE_weighting + SEB_error / timeConstant();
1037 313892 : const float SEBdot_dem_min = - _maxSinkRate * GRAVITY_MSS;
1038 313892 : const float SEBdot_dem_max = _maxClimbRate * GRAVITY_MSS;
1039 313892 : if (SEBdot_dem < SEBdot_dem_min) {
1040 9161 : SEBdot_dem = SEBdot_dem_min;
1041 9161 : _SEBdot_dem_clip = clipStatus::MIN;
1042 304731 : } else if (SEBdot_dem > SEBdot_dem_max) {
1043 5334 : SEBdot_dem = SEBdot_dem_max;
1044 5334 : _SEBdot_dem_clip = clipStatus::MAX;
1045 : } else {
1046 299397 : _SEBdot_dem_clip = clipStatus::NONE;
1047 : }
1048 :
1049 : // calculate specific energy balance rate error
1050 313892 : const float SEBdot_est = _SPEdot * SPE_weighting - _SKEdot * _SKE_weighting;
1051 313892 : float SEBdot_error = SEBdot_dem - SEBdot_est;
1052 :
1053 : // sum predicted plus damping correction
1054 : // integral correction is added later
1055 : // During flare a different damping gain is used
1056 313892 : float pitch_damp = _ptchDamp;
1057 313892 : if (_landing.is_flaring()) {
1058 23256 : pitch_damp = _landDamp;
1059 290636 : } else if (!is_zero(_land_pitch_damp) && _flags.is_doing_auto_land) {
1060 0 : pitch_damp = _land_pitch_damp;
1061 : }
1062 313892 : float SEBdot_dem_total = SEBdot_dem + SEBdot_error * pitch_damp;
1063 :
1064 : // inverse of gain from SEB to pitch angle
1065 313892 : float gainInv = (_TAS_state * GRAVITY_MSS);
1066 :
1067 : // During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
1068 : // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
1069 : // integrator has to catch up before the nose can be raised to reduce speed during climbout.
1070 313892 : if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
1071 7996 : SEBdot_dem_total += _PITCHminf * gainInv;
1072 : }
1073 :
1074 : // don't allow the integrator to rise by more than 20% of its full
1075 : // Calculate max and min values for integrator state that will allow for no more than
1076 : // 5deg of saturation. This allows for some pitch variation due to gusts before the
1077 : // integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
1078 313892 : float integSEBdot_min = (gainInv * (_PITCHminf - radians(5.0f))) - SEBdot_dem_total;
1079 313892 : float integSEBdot_max = (gainInv * (_PITCHmaxf + radians(5.0f))) - SEBdot_dem_total;
1080 :
1081 : // Calculate integrator state, constraining input if pitch limits are exceeded
1082 : // don't allow the integrator to rise by more than 10% of its full
1083 : // range in one step. This prevents single value glitches from
1084 : // causing massive integrator changes. See Issue#4066
1085 313892 : float integSEB_range = integSEBdot_max - integSEBdot_min;
1086 313892 : float integSEB_delta = constrain_float(SEBdot_error * _get_i_gain() * _DT, -integSEB_range*0.1f, integSEB_range*0.1f);
1087 :
1088 : // predict what pitch will be with uncontrained integration
1089 313892 : _pitch_dem_unc = (SEBdot_dem_total + _integSEBdot + integSEB_delta + _integKE) / gainInv;
1090 :
1091 : // integrate SEB rate error and apply integrator state limits
1092 614820 : const bool inhibit_integrator = ((_pitch_dem_unc > _PITCHmaxf) && integSEB_delta > 0.0f) ||
1093 300928 : ((_pitch_dem_unc < _PITCHminf) && integSEB_delta < 0.0f);
1094 313892 : if (!inhibit_integrator) {
1095 288436 : _integSEBdot += integSEB_delta;
1096 288436 : _integKE += (_SKE_est - _SKE_dem) * _SKE_weighting * _DT / timeConstant();
1097 : } else {
1098 : // fade out integrator if saturating
1099 25456 : const float coef = 1.0f - _DT / (_DT + timeConstant());
1100 25456 : _integSEBdot *= coef;
1101 25456 : _integKE *= coef;
1102 : }
1103 313892 : _integSEBdot = constrain_float(_integSEBdot, integSEBdot_min, integSEBdot_max);
1104 313892 : const float KE_integ_limit = 0.25f * (_PITCHmaxf - _PITCHminf) * gainInv; // allow speed trim integrator to access 505 of pitch range
1105 313892 : _integKE = constrain_float(_integKE, - KE_integ_limit, KE_integ_limit);
1106 :
1107 : // Calculate pitch demand from specific energy balance signals
1108 313892 : _pitch_dem_unc = (SEBdot_dem_total + _integSEBdot + _integKE) / gainInv;
1109 :
1110 : // Add a feedforward term from demanded airspeed to pitch
1111 313892 : if (_flags.is_gliding) {
1112 14349 : _pitch_dem_unc += (_TAS_dem_adj - _pitch_ff_v0) * _pitch_ff_k;
1113 : }
1114 :
1115 : // Constrain pitch demand
1116 313892 : _pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
1117 :
1118 : // Rate limit the pitch demand to comply with specified vertical
1119 : // acceleration limit
1120 313892 : float ptchRateIncr = _DT * _vertAccLim / _TAS_state;
1121 :
1122 313892 : if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) {
1123 4594 : _pitch_dem = _last_pitch_dem + ptchRateIncr;
1124 309298 : } else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) {
1125 5826 : _pitch_dem = _last_pitch_dem - ptchRateIncr;
1126 : }
1127 :
1128 313892 : _last_pitch_dem = _pitch_dem;
1129 :
1130 : #if HAL_LOGGING_ENABLED
1131 313892 : if (AP::logger().should_log(_log_bitmask)){
1132 : // log to AP_Logger
1133 : // @LoggerMessage: TEC2
1134 : // @Vehicles: Plane
1135 : // @Description: Additional information about the Total Energy Control System
1136 : // @URL: http://ardupilot.club/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html
1137 : // @Field: TimeUS: Time since system startup
1138 : // @Field: PEW: Potential energy weighting
1139 : // @Field: KEW: Kinetic energy weighting
1140 : // @Field: EBD: Energy balance demand
1141 : // @Field: EBE: Energy balance estimate
1142 : // @Field: EBDD: Energy balance rate demand
1143 : // @Field: EBDE: Energy balance rate estimate
1144 : // @Field: EBDDT: Energy balance rate demand + Energy balance rate error*pitch_damping
1145 : // @Field: Imin: Minimum integrator value
1146 : // @Field: Imax: Maximum integrator value
1147 : // @Field: I: Energy balance error integral
1148 : // @Field: KI: Pitch demand kinetic energy integral
1149 : // @Field: tmin: Throttle min
1150 : // @Field: tmax: Throttle max
1151 627784 : AP::logger().WriteStreaming(
1152 : "TEC2",
1153 : "TimeUS," "PEW," "KEW," "EBD," "EBE," "EBDD," "EBDE," "EBDDT," "Imin," "Imax," "I," "KI," "tmin," "tmax",
1154 : "s" "-" "-" "?" "?" "?" "?" "-" "-" "-" "-" "-" "-" "-",
1155 : "F" "0" "0" "0" "0" "0" "0" "0" "0" "0" "0" "0" "0" "0",
1156 : "Q" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f",
1157 : AP_HAL::micros64(),
1158 : SPE_weighting,
1159 313892 : _SKE_weighting,
1160 : SEB_dem,
1161 : SEB_est,
1162 : SEBdot_dem,
1163 : SEBdot_est,
1164 : SEBdot_dem_total,
1165 : integSEBdot_min,
1166 : integSEBdot_max,
1167 313892 : _integSEBdot,
1168 313892 : _integKE,
1169 313892 : _THRminf,
1170 313892 : _THRmaxf
1171 : );
1172 : }
1173 : #endif
1174 313892 : }
1175 :
1176 313892 : void AP_TECS::_initialise_states(float hgt_afe)
1177 : {
1178 : // Initialise states and variables if DT > 0.2 second or TECS is getting overridden or in climbout.
1179 313892 : _flags.reset = false;
1180 :
1181 313892 : if (_DT > 0.2f || _need_reset) {
1182 402 : _SKE_weighting = 1.0f;
1183 402 : _integTHR_state = 0.0f;
1184 402 : _integSEBdot = 0.0f;
1185 402 : _integKE = 0.0f;
1186 402 : _last_throttle_dem = aparm.throttle_cruise * 0.01f;
1187 402 : _last_pitch_dem = _ahrs.get_pitch_rad();
1188 402 : _hgt_dem_in_prev = hgt_afe;
1189 402 : _hgt_dem_lpf = hgt_afe;
1190 402 : _hgt_dem_rate_ltd = hgt_afe;
1191 402 : _hgt_dem_prev = hgt_afe;
1192 402 : _TAS_dem_adj = _TAS_dem;
1193 402 : _flags.reset = true;
1194 402 : _DT = 0.02f; // when first starting TECS, use the most likely time constant
1195 402 : _lag_comp_hgt_offset = 0.0f;
1196 402 : _post_TO_hgt_offset = 0.0f;
1197 402 : _use_synthetic_airspeed_once = false;
1198 :
1199 402 : _flags.underspeed = false;
1200 402 : _flags.badDescent = false;
1201 402 : _flags.reached_speed_takeoff = false;
1202 402 : _need_reset = false;
1203 :
1204 : // misc variables used for alternative precision landing pitch control
1205 402 : _hgt_at_start_of_flare = 0.0f;
1206 402 : _hgt_rate_dem_at_flare_entry = 0.0f;
1207 402 : _hgt_afe = 0.0f;
1208 402 : _pitch_min_at_flare_entry = 0.0f;
1209 :
1210 402 : _max_climb_scaler = 1.0f;
1211 402 : _max_sink_scaler = 1.0f;
1212 :
1213 402 : const float fc = 1.0f / (M_2PI * _timeConst);
1214 402 : _pitch_demand_lpf.set_cutoff_frequency(fc);
1215 402 : _pitch_measured_lpf.set_cutoff_frequency(fc);
1216 402 : _pitch_demand_lpf.reset(_ahrs.get_pitch_rad());
1217 402 : _pitch_measured_lpf.reset(_ahrs.get_pitch_rad());
1218 :
1219 313892 : } else if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
1220 :
1221 7891 : if (!_flag_throttle_forced) {
1222 : // Calculate the takeoff target height offset before _hgt_dem_in_raw gets reset below.
1223 : // Prevent the offset from becoming negative.
1224 121 : _post_TO_hgt_offset = MAX(MIN(_climb_rate_limit * _hgt_dem_tconst, _hgt_dem_in_raw - hgt_afe), 0);
1225 : } else {
1226 : // If throttle is externally forced, this mechanism of adding energy is unnecessary.
1227 7770 : _post_TO_hgt_offset = 0;
1228 : }
1229 :
1230 7891 : _hgt_afe = hgt_afe;
1231 7891 : _hgt_dem_lpf = hgt_afe;
1232 7891 : _hgt_dem_rate_ltd = hgt_afe;
1233 7891 : _hgt_dem_prev = hgt_afe;
1234 7891 : _hgt_dem = hgt_afe;
1235 7891 : _hgt_dem_in_prev = hgt_afe;
1236 7891 : _hgt_dem_in_raw = hgt_afe;
1237 7891 : _flags.underspeed = false;
1238 7891 : _flags.badDescent = false;
1239 7891 : _TAS_dem_adj = _TAS_dem;
1240 7891 : _max_climb_scaler = 1.0f;
1241 7891 : _max_sink_scaler = 1.0f;
1242 7891 : _pitch_demand_lpf.reset(_ahrs.get_pitch_rad());
1243 7891 : _pitch_measured_lpf.reset(_ahrs.get_pitch_rad());
1244 :
1245 :
1246 7891 : if (!_flag_have_reset_after_takeoff) {
1247 114 : _flags.reset = true;
1248 114 : _flag_have_reset_after_takeoff = true;
1249 : }
1250 : }
1251 :
1252 313892 : if (_flight_stage != AP_FixedWing::FlightStage::TAKEOFF && _flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) {
1253 : // reset takeoff speed flag when not in takeoff
1254 305896 : _flags.reached_speed_takeoff = false;
1255 305896 : _flag_have_reset_after_takeoff = false;
1256 : }
1257 313892 : }
1258 :
1259 313892 : void AP_TECS::_update_STE_rate_lim(void)
1260 : {
1261 : // Calculate Specific Total Energy Rate Limits & deceleration rate bounds
1262 : // Keep the 50% energy margin from the original velRate calculation for now
1263 : // This is a trivial calculation at the moment but will get bigger once we start adding altitude effects
1264 313892 : _STEdot_max = _climb_rate_limit * GRAVITY_MSS;
1265 313892 : _STEdot_min = - _minSinkRate * GRAVITY_MSS;
1266 313892 : _STEdot_neg_max = - _maxSinkRate * GRAVITY_MSS;
1267 313892 : }
1268 :
1269 :
1270 313893 : void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
1271 : int32_t EAS_dem_cm,
1272 : enum AP_FixedWing::FlightStage flight_stage,
1273 : float distance_beyond_land_wp,
1274 : int32_t ptchMinCO_cd,
1275 : int16_t throttle_nudge,
1276 : float hgt_afe,
1277 : float load_factor,
1278 : float pitch_trim_deg)
1279 : {
1280 313893 : uint64_t now = AP_HAL::micros64();
1281 : // check how long since we last did the 50Hz update; do nothing in
1282 : // this loop if that hasn't run for some significant period of
1283 : // time. Notably, it may never have run, leaving _TAS_state as
1284 : // zero and subsequently division-by-zero errors.
1285 313893 : const float _DT_for_update_50hz = (now - _update_50hz_last_usec) * 1.0e-6f;
1286 313893 : if (_update_50hz_last_usec == 0 || _DT_for_update_50hz > 1.0) {
1287 : // more than 1 second since it was run, don't do anything yet:
1288 1 : return;
1289 : }
1290 :
1291 : // Calculate time in seconds since last update
1292 313892 : _DT = (now - _update_pitch_throttle_last_usec) * 1.0e-6f;
1293 313892 : _DT = MAX(_DT, 0.001f);
1294 313892 : _update_pitch_throttle_last_usec = now;
1295 :
1296 313892 : _flags.is_gliding = _flags.gliding_requested || _flags.propulsion_failed || aparm.throttle_max==0;
1297 313892 : _flags.is_doing_auto_land = (flight_stage == AP_FixedWing::FlightStage::LAND);
1298 313892 : _distance_beyond_land_wp = distance_beyond_land_wp;
1299 313892 : _flight_stage = flight_stage;
1300 :
1301 : // Convert inputs
1302 313892 : _hgt_dem_in_raw = hgt_dem_cm * 0.01f;
1303 313892 : _EAS_dem = EAS_dem_cm * 0.01f;
1304 313892 : _hgt_afe = hgt_afe;
1305 313892 : _load_factor = load_factor;
1306 :
1307 : // Don't allow height deamnd to continue changing in a direction that saturates vehicle manoeuvre limits
1308 : // if vehicle is unable to follow the demanded climb or descent.
1309 356724 : const bool max_climb_condition = (_pitch_dem_unc > _PITCHmaxf || _thr_clip_status == clipStatus::MAX) &&
1310 42832 : !(_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);
1311 313892 : const bool max_descent_condition = _pitch_dem_unc < _PITCHminf || _thr_clip_status == clipStatus::MIN;
1312 313892 : if (max_climb_condition && _hgt_dem_in_raw > _hgt_dem_in_prev) {
1313 10550 : _hgt_dem_in = _hgt_dem_in_prev;
1314 303342 : } else if (max_descent_condition && _hgt_dem_in_raw < _hgt_dem_in_prev) {
1315 5683 : _hgt_dem_in = _hgt_dem_in_prev;
1316 : } else {
1317 297659 : _hgt_dem_in = _hgt_dem_in_raw;
1318 : }
1319 :
1320 : // Update the throttle limits.
1321 313892 : _update_throttle_limits();
1322 :
1323 313892 : _update_pitch_limits(ptchMinCO_cd);
1324 :
1325 313892 : if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
1326 7996 : if (!_flags.reached_speed_takeoff && _TAS_state >= _TASmin && _TASmin > 0) {
1327 : // we have reached our target speed in takeoff, allow for
1328 : // normal throttle control
1329 118 : _flags.reached_speed_takeoff = true;
1330 : }
1331 : }
1332 :
1333 : // initialise selected states and variables if DT > 1 second or in climbout
1334 313892 : _initialise_states(hgt_afe);
1335 :
1336 : // Calculate Specific Total Energy Rate Limits
1337 313892 : _update_STE_rate_lim();
1338 :
1339 : // Calculate the speed demand
1340 313892 : _update_speed_demand();
1341 :
1342 : // Calculate the height demand
1343 313892 : _update_height_demand();
1344 :
1345 : // Detect underspeed condition
1346 313892 : _detect_underspeed();
1347 :
1348 : // Calculate specific energy quantitiues
1349 313892 : _update_energies();
1350 :
1351 : // Calculate pitch demand
1352 313892 : _update_pitch();
1353 :
1354 : // Calculate throttle demand - use simple pitch to throttle if no airspeed estimate.
1355 313892 : if (use_airspeed()) {
1356 299136 : _update_throttle_with_airspeed();
1357 299136 : _use_synthetic_airspeed_once = false;
1358 299136 : _using_airspeed_for_throttle = true;
1359 : } else {
1360 14756 : _update_throttle_without_airspeed(throttle_nudge, pitch_trim_deg);
1361 14756 : _using_airspeed_for_throttle = false;
1362 : }
1363 :
1364 : // Detect bad descent due to demanded airspeed being too high
1365 313892 : _detect_bad_descent();
1366 :
1367 313892 : if (option_is_set(Option::GLIDER_ONLY)) {
1368 4799 : _flags.badDescent = false;
1369 : }
1370 :
1371 : #if HAL_LOGGING_ENABLED
1372 313892 : if (AP::logger().should_log(_log_bitmask)){
1373 : // log to AP_Logger
1374 : // @LoggerMessage: TECS
1375 : // @Vehicles: Plane
1376 : // @Description: Information about the Total Energy Control System
1377 : // @URL: http://ardupilot.club/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html
1378 : // @Field: TimeUS: Time since system startup
1379 : // @Field: h: height estimate (UP) currently in use by TECS
1380 : // @Field: dh: current climb rate ("delta-height")
1381 : // @Field: hin: height demand received by TECS
1382 : // @Field: hdem: height demand after rate limiting and filtering that TECS is currently trying to achieve
1383 : // @Field: dhdem: climb rate TECS is currently trying to achieve
1384 : // @Field: spdem: True AirSpeed TECS is currently trying to achieve
1385 : // @Field: sp: current estimated True AirSpeed
1386 : // @Field: dsp: x-axis acceleration estimate ("delta-speed")
1387 : // @Field: th: throttle output
1388 : // @Field: ph: pitch output
1389 : // @Field: pmin: pitch lower limit
1390 : // @Field: pmax: pitch upper limit
1391 : // @Field: dspdem: demanded acceleration output ("delta-speed demand")
1392 : // @Field: f: flags
1393 : // @FieldBits: f: Underspeed,UnachievableDescent,AutoLanding,ReachedTakeoffSpd,GlidingRequested,isGliding,PropulsionFailed,Reset
1394 313892 : AP::logger().WriteStreaming(
1395 : "TECS",
1396 : "TimeUS," "h," "dh," "hin," "hdem," "dhdem," "spdem," "sp," "dsp," "th," "ph," "pmin," "pmax," "dspdem," "f",
1397 : "s" "m" "n" "m" "m" "n" "n" "n" "n" "-" "-" "-" "-" "-" "-",
1398 : "F" "0" "0" "0" "0" "0" "0" "0" "0" "-" "-" "-" "-" "-" "-",
1399 : "Q" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "B",
1400 : now,
1401 313892 : _height,
1402 313892 : _climb_rate,
1403 313892 : _hgt_dem_in_raw,
1404 313892 : _hgt_dem,
1405 313892 : _hgt_rate_dem,
1406 313892 : _TAS_dem_adj,
1407 313892 : _TAS_state,
1408 313892 : _vel_dot,
1409 313892 : _throttle_dem,
1410 313892 : _pitch_dem,
1411 313892 : _PITCHminf,
1412 313892 : _PITCHmaxf,
1413 313892 : _TAS_rate_dem,
1414 313892 : _flags_byte
1415 : );
1416 : }
1417 : #endif
1418 : }
1419 :
1420 : // set minimum throttle override, [-1, -1] range
1421 : // if reset_output is true the output slew limiter will also be reset to respect the lower limit
1422 : // its decay is controlled by TECS_THR_ERATE
1423 7338766 : void AP_TECS::set_throttle_min(const float thr_min, bool reset_output) {
1424 : // Don't change the limit if it is already covered.
1425 7338766 : if (thr_min > _THRminf_ext) {
1426 306693 : _THRminf_ext = thr_min;
1427 306693 : if (reset_output) {
1428 6 : _last_throttle_dem = MAX(_last_throttle_dem, _THRminf_ext);
1429 6 : _throttle_dem = _last_throttle_dem;
1430 : }
1431 : }
1432 7338766 : }
1433 :
1434 : // set minimum throttle override, [0, -1] range
1435 : // it is applicable for one control cycle only
1436 7338760 : void AP_TECS::set_throttle_max(const float thr_max) {
1437 : // Don't change the limit if it is already covered.
1438 7338760 : if (thr_max < _THRmaxf_ext) {
1439 9931 : _THRmaxf_ext = thr_max;
1440 : }
1441 7338760 : }
1442 :
1443 313892 : void AP_TECS::_update_throttle_limits() {
1444 :
1445 : // Configure max throttle; constrain to the external safety limits.
1446 313892 : _THRmaxf = MIN(1.0f, _THRmaxf_ext);
1447 : // Configure min throttle; constrain to the external safety limits.
1448 313892 : _THRminf = MAX(-1.0f, _THRminf_ext);
1449 :
1450 : // Allow a minimum of 1% throttle range, primarily to prevent TECS numerical errors.
1451 313892 : const float thr_eps = 0.01;
1452 313892 : if (fabsf(_THRminf-_THRmaxf) < thr_eps) {
1453 12724 : _flag_throttle_forced = true;
1454 12724 : if (_THRmaxf < 1) {
1455 5252 : _THRmaxf = MAX(_THRmaxf, _THRminf + 0.01f);
1456 : } else {
1457 7472 : _THRminf = MIN(_THRminf, _THRmaxf - 0.01f);
1458 : }
1459 : } else {
1460 301168 : _flag_throttle_forced = false;
1461 : }
1462 :
1463 : // Reset the external throttle limits.
1464 : // Caller will have to reset them in the next iteration.
1465 313892 : if (_thr_min_pct_ext_rate_lim > 0) {
1466 313892 : _THRminf_ext -= 0.01f * float(_thr_min_pct_ext_rate_lim) * _DT;
1467 313892 : _THRminf_ext = MAX(_THRminf_ext, -1.0f);
1468 : } else {
1469 0 : _THRminf_ext = -1.0f;
1470 : }
1471 313892 : _THRmaxf_ext = 1.0f;
1472 313892 : }
1473 :
1474 392752 : void AP_TECS::set_pitch_min(const float pitch_min) {
1475 : // Don't change the limit if it is already covered.
1476 392752 : if (pitch_min > _PITCHminf_ext) {
1477 19694 : _PITCHminf_ext = pitch_min;
1478 : }
1479 392752 : }
1480 :
1481 329054 : void AP_TECS::set_pitch_max(const float pitch_max) {
1482 : // Don't change the limit if it is already covered.
1483 329054 : if (pitch_max < _PITCHmaxf_ext) {
1484 12129 : _PITCHmaxf_ext = pitch_max;
1485 : }
1486 329054 : }
1487 :
1488 313892 : void AP_TECS::_update_pitch_limits(const int32_t ptchMinCO_cd) {
1489 : // If TECS_PITCH_{MAX,MIN} isn't set then use LIM_PITCH_{MAX,MIN}.
1490 : // Don't allow TECS_PITCH_{MAX,MIN} to be larger than LIM_PITCH_{MAX,MIN}.
1491 313892 : if (_pitch_max == 0) {
1492 0 : _PITCHmaxf = aparm.pitch_limit_max;
1493 : } else {
1494 313892 : _PITCHmaxf = _pitch_max;
1495 : }
1496 :
1497 313892 : if (_pitch_min == 0) {
1498 309093 : _PITCHminf = aparm.pitch_limit_min;
1499 : } else {
1500 4799 : _PITCHminf = _pitch_min;
1501 : }
1502 :
1503 313892 : if (!_landing.is_on_approach()) {
1504 : // reset land pitch min when not landing
1505 307639 : _land_pitch_min = _PITCHminf;
1506 : }
1507 :
1508 : // calculate the expected pitch angle from the demanded climb rate and airspeed for use during approach and flare
1509 313892 : if (_landing.is_flaring()) {
1510 : // smoothly move the min pitch to the required minimum at touchdown
1511 : float p; // 0 at start of flare, 1 at finish
1512 23256 : if (!_flare_initialised) {
1513 110 : p = 0.0f;
1514 23146 : } else if (_hgt_at_start_of_flare > _flare_holdoff_hgt) {
1515 22616 : p = constrain_float((_hgt_at_start_of_flare - _hgt_afe) / _hgt_at_start_of_flare, 0.0f, 1.0f);
1516 : } else {
1517 530 : p = 1.0f;
1518 : }
1519 23256 : const float pitch_limit_deg = (1.0f - p) * _pitch_min_at_flare_entry + p * 0.01f * _landing.get_pitch_cd();
1520 :
1521 : // in flare use min pitch from LAND_PITCH_DEG
1522 23256 : _PITCHminf = MAX(_PITCHminf, pitch_limit_deg);
1523 :
1524 : // and use max pitch from TECS_LAND_PMAX
1525 23256 : if (_land_pitch_max != 0) {
1526 : // note that this allows a flare pitch outside the normal TECS auto limits
1527 23256 : _PITCHmaxf = _land_pitch_max;
1528 : }
1529 290636 : } else if (_landing.is_on_approach()) {
1530 6253 : _PITCHminf = MAX(_PITCHminf, aparm.pitch_limit_min);
1531 6253 : _pitch_min_at_flare_entry = _PITCHminf;
1532 6253 : _flare_initialised = false;
1533 : } else {
1534 284383 : _flare_initialised = false;
1535 : }
1536 :
1537 313892 : if (_landing.is_on_approach()) {
1538 : // don't allow the lower bound of pitch to decrease, nor allow
1539 : // it to increase rapidly. This prevents oscillation of pitch
1540 : // demand while in landing approach based on rapidly changing
1541 : // time to flare estimate
1542 6253 : if (_land_pitch_min <= -90) {
1543 0 : _land_pitch_min = _PITCHminf;
1544 : }
1545 6253 : const float flare_pitch_range = 20;
1546 6253 : const float delta_per_loop = (flare_pitch_range/_landTimeConst) * _DT;
1547 6253 : _PITCHminf = MIN(_PITCHminf, _land_pitch_min+delta_per_loop);
1548 6253 : _land_pitch_min = MAX(_land_pitch_min, _PITCHminf);
1549 6253 : _PITCHminf = MAX(_land_pitch_min, _PITCHminf);
1550 : }
1551 :
1552 : // Apply TAKEOFF minimum pitch
1553 313892 : if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF
1554 306143 : || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)
1555 : {
1556 7996 : _PITCHminf = cd_to_rad(ptchMinCO_cd);
1557 : }
1558 :
1559 : // Apply external limits.
1560 313892 : _PITCHmaxf = MIN(_PITCHmaxf, _PITCHmaxf_ext);
1561 313892 : _PITCHminf = MAX(_PITCHminf, _PITCHminf_ext);
1562 :
1563 : // Reset the external pitch limits.
1564 313892 : _PITCHminf_ext = -90.0f;
1565 313892 : _PITCHmaxf_ext = 90.0f;
1566 :
1567 : // convert to radians
1568 313892 : _PITCHmaxf = radians(_PITCHmaxf);
1569 313892 : _PITCHminf = radians(_PITCHminf);
1570 :
1571 : // don't allow max pitch to go below min pitch
1572 313892 : _PITCHmaxf = MAX(_PITCHmaxf, _PITCHminf);
1573 313892 : }
1574 :
1575 291 : void AP_TECS::offset_altitude(const float alt_offset)
1576 : {
1577 : // Convention: When alt_offset is positive it means that the altitude of
1578 : // home has increased. Thus, the relative altitude of the vehicle has
1579 : // decreased.
1580 : //
1581 : // Assumption: This method is called more often and before
1582 : // `update_pitch_throttle()`. This is necessary to ensure that new height
1583 : // demands which incorporate the home change are compatible with the
1584 : // (now updated) internal height state.
1585 :
1586 291 : _flare_hgt_dem_ideal -= alt_offset;
1587 291 : _flare_hgt_dem_adj -= alt_offset;
1588 291 : _hgt_at_start_of_flare -= alt_offset;
1589 291 : _hgt_dem_in_prev -= alt_offset;
1590 291 : _hgt_dem_lpf -= alt_offset;
1591 291 : _hgt_dem_rate_ltd -= alt_offset;
1592 291 : _hgt_dem_prev -= alt_offset;
1593 291 : _height_filter.height -= alt_offset;
1594 :
1595 : // The following variables are updated anew in every call of
1596 : // `update_pitch_throttle()`. There's no need to update those.
1597 : // _hgt_dem
1598 : // _hgt_dem_in_raw
1599 : // _hgt_dem_in
1600 : // Energies
1601 291 : }
1602 :
1603 : // Return true if airspeed should be used (either from a sensor or synthetic)
1604 2658142 : bool AP_TECS::use_airspeed() const
1605 : {
1606 : // Note that caller can demand the use of
1607 : // synthetic airspeed for one loop if needed. This is required
1608 : // during QuadPlane transition when pitch is constrained
1609 2658142 : return _ahrs.using_airspeed_sensor() || _use_synthetic_airspeed || _use_synthetic_airspeed_once;
1610 : }
|
