Line data Source code
1 : /// @file AP_TECS.h 2 : /// @brief Combined Total Energy Speed & Height Control. This is a instance of an 3 : 4 : /* 5 : * Written by Paul Riseborough 2013 to provide: 6 : * - Combined control of speed and height using throttle to control 7 : * total energy and pitch angle to control exchange of energy between 8 : * potential and kinetic. 9 : * Selectable speed or height priority modes when calculating pitch angle 10 : * - Fallback mode when no airspeed measurement is available that 11 : * sets throttle based on height rate demand and switches pitch angle control to 12 : * height priority 13 : * - Underspeed protection that demands maximum throttle switches pitch angle control 14 : * to speed priority mode 15 : * - Relative ease of tuning through use of intuitive time constant, trim rate and damping parameters and the use 16 : * of easy to measure aircraft performance data 17 : */ 18 : #pragma once 19 : 20 : #include <AP_Math/AP_Math.h> 21 : #include <AP_AHRS/AP_AHRS.h> 22 : #include <AP_Param/AP_Param.h> 23 : #include <AP_Vehicle/AP_FixedWing.h> 24 : #include <Filter/AverageFilter.h> 25 : 26 : class AP_Landing; 27 : class AP_TECS { 28 : public: 29 692 : AP_TECS(AP_AHRS &ahrs, const AP_FixedWing &parms, const AP_Landing &landing, const uint32_t log_bitmask) 30 692 : : _ahrs(ahrs) 31 692 : , aparm(parms) 32 692 : , _landing(landing) 33 692 : , _log_bitmask(log_bitmask) 34 : { 35 692 : AP_Param::setup_object_defaults(this, var_info); 36 692 : } 37 : 38 : /* Do not allow copies */ 39 : CLASS_NO_COPY(AP_TECS); 40 : 41 : // Update of the estimated height and height rate internal state 42 : // Update of the inertial speed rate internal state 43 : // Should be called at 50Hz or greater 44 : void update_50hz(void); 45 : 46 : // Update the control loop calculations 47 : // Do not call slower than 10Hz or faster than 500Hz 48 : void update_pitch_throttle(int32_t hgt_dem_cm, 49 : int32_t EAS_dem_cm, 50 : enum AP_FixedWing::FlightStage flight_stage, 51 : float distance_beyond_land_wp, 52 : int32_t ptchMinCO_cd, 53 : int16_t throttle_nudge, 54 : float hgt_afe, 55 : float load_factor, 56 : float pitch_trim_deg); 57 : 58 : // demanded throttle in percentage 59 : // should return -100 to 100, usually positive unless reverse thrust is enabled via _THRminf < 0 60 5077750 : float get_throttle_demand(void) { 61 5077750 : return _throttle_dem * 100.0f; 62 : } 63 : 64 : // demanded pitch angle in centi-degrees 65 : // should return between -9000 to +9000 66 3888434 : int32_t get_pitch_demand(void) { 67 3888434 : return int32_t(_pitch_dem * 5729.5781f); 68 : } 69 : 70 : // Rate of change of velocity along X body axis in m/s^2 71 383 : float get_VXdot(void) { 72 383 : return _vel_dot; 73 : } 74 : 75 : // return current target airspeed 76 667518 : float get_target_airspeed(void) const { 77 667518 : return _TAS_dem_adj / _ahrs.get_EAS2TAS(); 78 : } 79 : 80 : // return maximum climb rate 81 60548 : float get_max_climbrate(void) const { 82 60548 : return _maxClimbRate; 83 : } 84 : 85 : // return maximum sink rate (+ve number down) 86 19436 : float get_max_sinkrate(void) const { 87 19436 : return _maxSinkRate; 88 : } 89 : 90 : // added to let SoaringContoller reset pitch integrator to zero 91 5 : void reset_pitch_I(void) { 92 5 : _integSEBdot = 0.0f; 93 5 : _integKE = 0.0f; 94 5 : } 95 : 96 : // reset throttle integrator 97 21278 : void reset_throttle_I(void) { 98 21278 : _integTHR_state = 0.0; 99 21278 : } 100 : 101 : // return landing sink rate 102 6097 : float get_land_sinkrate(void) const { 103 6097 : return _land_sink; 104 : } 105 : 106 : // return landing airspeed 107 205125 : float get_land_airspeed(void) const { 108 205125 : return _landAirspeed; 109 : } 110 : 111 : // return height rate demand, in m/s 112 : float get_height_rate_demand(void) const { 113 : return _hgt_rate_dem; 114 : } 115 : 116 : // set path_proportion 117 515533 : void set_path_proportion(float path_proportion) { 118 515533 : _path_proportion = constrain_float(path_proportion, 0.0f, 1.0f); 119 515533 : } 120 : 121 : // set soaring flag 122 17049 : void set_gliding_requested_flag(bool gliding_requested) { 123 17049 : _flags.gliding_requested = gliding_requested; 124 17049 : } 125 : 126 : // set propulsion failed flag 127 : void set_propulsion_failed_flag(bool propulsion_failed) { 128 : _flags.propulsion_failed = propulsion_failed; 129 : } 130 : 131 : // set minimum throttle override, [-1, -1] range 132 : // if reset_output is true the output slew limiter will also be reset to respect the lower limit 133 : // its decay is controlled by TECS_THR_ERATE 134 : void set_throttle_min(const float thr_min, bool reset_output = false); 135 : 136 : // set maximum throttle override, [0, -1] range 137 : // it is applicable for one control cycle only 138 : void set_throttle_max(const float thr_max); 139 : 140 : // set minimum pitch override, in degrees. 141 : // it is applicable for one control cycle only 142 : void set_pitch_min(const float pitch_min); 143 : 144 : // set maximum pitch override, in degrees. 145 : // it is applicable for one control cycle only 146 : void set_pitch_max(const float pitch_max); 147 : 148 : // force use of synthetic airspeed for one loop 149 394556 : void use_synthetic_airspeed(void) { 150 394556 : _use_synthetic_airspeed_once = true; 151 394556 : } 152 : 153 : // reset on next loop 154 813598 : void reset(void) { 155 813598 : _need_reset = true; 156 813598 : } 157 : 158 : // Apply an altitude offset, to compensate for changes in home alt. 159 : void offset_altitude(const float alt_offset); 160 : 161 : // Return true if airspeed should be used 162 : bool use_airspeed() const; 163 : 164 : // this supports the TECS_* user settable parameters 165 : static const struct AP_Param::GroupInfo var_info[]; 166 : 167 : private: 168 : // Last time update_50Hz was called 169 : uint64_t _update_50hz_last_usec; 170 : 171 : // Last time update_pitch_throttle was called 172 : uint64_t _update_pitch_throttle_last_usec; 173 : 174 : // reference to the AHRS object 175 : AP_AHRS &_ahrs; 176 : 177 : const AP_FixedWing &aparm; 178 : 179 : // reference to const AP_Landing to access it's params 180 : const AP_Landing &_landing; 181 : 182 : // Logging bitmask 183 : const uint32_t _log_bitmask; 184 : 185 : // TECS tuning parameters 186 : AP_Float _hgtCompFiltOmega; 187 : AP_Float _spdCompFiltOmega; 188 : AP_Float _maxClimbRate; 189 : AP_Float _minSinkRate; 190 : AP_Float _maxSinkRate; 191 : AP_Float _timeConst; 192 : AP_Float _landTimeConst; 193 : AP_Float _ptchDamp; 194 : AP_Float _land_pitch_damp; 195 : AP_Float _landDamp; 196 : AP_Float _thrDamp; 197 : AP_Float _land_throttle_damp; 198 : AP_Float _integGain; 199 : AP_Float _integGain_takeoff; 200 : AP_Float _integGain_land; 201 : AP_Float _vertAccLim; 202 : AP_Float _rollComp; 203 : AP_Float _spdWeight; 204 : AP_Float _spdWeightLand; 205 : AP_Float _landThrottle; 206 : AP_Float _landAirspeed; 207 : AP_Float _land_sink; 208 : AP_Float _land_sink_rate_change; 209 : AP_Int8 _pitch_max; 210 : AP_Int8 _pitch_min; 211 : AP_Int8 _land_pitch_max; 212 : AP_Float _maxSinkRate_approach; 213 : AP_Int32 _options; 214 : AP_Float _flare_holdoff_hgt; 215 : AP_Float _hgt_dem_tconst; 216 : 217 : enum class Option { 218 : GLIDER_ONLY = (1<<0), 219 : DESCENT_SPEEDUP = (1<<1) 220 : }; 221 : 222 627784 : bool option_is_set(const Option option) const { 223 627784 : return (_options.get() & int32_t(option)) != 0; 224 : } 225 : 226 : AP_Float _pitch_ff_v0; 227 : AP_Float _pitch_ff_k; 228 : AP_Float _accel_gf; 229 : AP_Int8 _thr_min_pct_ext_rate_lim; 230 : 231 : // current height estimate (above field elevation) 232 : float _height; 233 : 234 : // throttle demand in the range from -1.0 to 1.0, usually positive unless reverse thrust is enabled via _THRminf < 0 235 : float _throttle_dem; 236 : 237 : // pitch angle demand in radians 238 : float _pitch_dem; 239 : 240 : // estimated climb rate (m/s) 241 : float _climb_rate; 242 : 243 : // climb and sink rate limits 244 : float _climb_rate_limit; 245 : float _sink_rate_limit; 246 : 247 : /* 248 : a filter to estimate climb rate if we don't have it from the EKF 249 : */ 250 : struct { 251 : // height filter second derivative 252 : float dd_height; 253 : 254 : // height integration 255 : float height; 256 : } _height_filter; 257 : 258 : // Integrator state 4 - airspeed filter first derivative 259 : float _integDTAS_state; 260 : 261 : // Integrator state 5 - true airspeed 262 : float _TAS_state; 263 : 264 : // Integrator state 6 - throttle integrator 265 : float _integTHR_state; 266 : 267 : // energy balance error integral 268 : float _integSEBdot; 269 : 270 : // pitch demand kinetic energy error integral 271 : float _integKE; 272 : 273 : // throttle demand rate limiter state 274 : float _last_throttle_dem; 275 : 276 : // pitch demand rate limiter state 277 : float _last_pitch_dem; 278 : 279 : // Rate of change of speed along X axis 280 : float _vel_dot; 281 : float _vel_dot_lpf; 282 : 283 : // True airspeed limits 284 : float _TASmax; 285 : float _TASmin; 286 : 287 : // Current true airspeed demand 288 : float _TAS_dem; 289 : 290 : // Equivalent airspeed demand 291 : float _EAS_dem; 292 : 293 : // height demands 294 : float _hgt_dem_in_raw; // height demand input from autopilot before any modification (m) 295 : float _hgt_dem_in; // height demand input from autopilot after unachievable climb or descent limiting (m) 296 : float _hgt_dem_in_prev; // previous value of _hgt_dem_in (m) 297 : float _hgt_dem_lpf; // height demand after application of low pass filtering (m) 298 : float _flare_hgt_dem_adj; // height rate demand during flare adjusted for height tracking offset at flare entry (m) 299 : float _flare_hgt_dem_ideal; // height we want to fly at during flare (m) 300 : float _hgt_dem; // height demand sent to control loops (m) 301 : float _hgt_dem_prev; // _hgt_dem from previous frame (m) 302 : 303 : // height rate demands 304 : float _hgt_dem_rate_ltd; // height demand after application of the rate limiter (m) 305 : float _hgt_rate_dem; // height rate demand sent to control loops 306 : 307 : // offset applied to height demand post takeoff to compensate for height demand filter lag 308 : float _post_TO_hgt_offset; 309 : 310 : // last lag compensation offset applied to height demand 311 : float _lag_comp_hgt_offset; 312 : 313 : // Speed demand after application of rate limiting 314 : // This is the demand tracked by the TECS control loops 315 : float _TAS_dem_adj; 316 : 317 : // Speed rate demand after application of rate limiting 318 : // This is the demand tracked by the TECS control loops 319 : float _TAS_rate_dem; 320 : float _TAS_rate_dem_lpf; 321 : 322 : // Total energy rate filter state 323 : float _STEdotErrLast; 324 : 325 : struct flags { 326 : // Underspeed condition 327 : bool underspeed:1; 328 : 329 : // Bad descent condition caused by unachievable airspeed demand 330 : bool badDescent:1; 331 : 332 : // true when plane is in auto mode and executing a land mission item 333 : bool is_doing_auto_land:1; 334 : 335 : // true when we have reached target speed in takeoff 336 : bool reached_speed_takeoff:1; 337 : 338 : // true if the soaring feature has requested gliding flight 339 : bool gliding_requested:1; 340 : 341 : // true when we are in gliding flight, in one of three situations; 342 : // - THR_MAX=0 343 : // - gliding has been requested e.g. by soaring feature 344 : // - engine failure detected (detection not implemented currently) 345 : bool is_gliding:1; 346 : 347 : // true if a propulsion failure is detected. 348 : bool propulsion_failed:1; 349 : 350 : // true when a reset of airspeed and height states to current is performed on this frame 351 : bool reset:1; 352 : 353 : }; 354 : union { 355 : struct flags _flags; 356 : uint8_t _flags_byte; 357 : }; 358 : 359 : // time when underspeed started 360 : uint32_t _underspeed_start_ms; 361 : 362 : // auto mode flightstage 363 : enum AP_FixedWing::FlightStage _flight_stage; 364 : 365 : // pitch demand before limiting 366 : float _pitch_dem_unc; 367 : 368 : // Specific total energy rate limits 369 : float _STEdot_max; // Specific total energy rate gain at cruise airspeed & THR_MAX (m/s/s) 370 : float _STEdot_min; // Specific total energy rate loss at cruise airspeed & THR_MIN (m/s/s) 371 : float _STEdot_neg_max; // Specific total energy rate loss at max airspeed & THR_MIN (m/s/s) 372 : 373 : // Maximum and minimum floating point throttle limits 374 : float _THRmaxf; 375 : float _THRminf; 376 : // Maximum and minimum throttle safety limits, set externally, typically by servos.cpp:apply_throttle_limits() 377 : float _THRmaxf_ext = 1.0f; 378 : float _THRminf_ext = -1.0f; 379 : // Maximum and minimum pitch limits, set externally, typically by the takeoff logic. 380 : float _PITCHmaxf_ext = 90.0f; 381 : float _PITCHminf_ext = -90.0f; 382 : 383 : // Maximum and minimum floating point pitch limits 384 : float _PITCHmaxf; 385 : float _PITCHminf; 386 : 387 : // 1 if throttle is clipping at max value, -1 if clipping at min value, 0 otherwise 388 : enum class clipStatus : int8_t { 389 : MIN = -1, 390 : NONE = 0, 391 : MAX = 1, 392 : }; 393 : clipStatus _thr_clip_status; 394 : 395 : // Specific energy quantities 396 : float _SPE_dem; 397 : float _SKE_dem; 398 : float _SPEdot_dem; 399 : float _SKEdot_dem; 400 : float _SPE_est; 401 : float _SKE_est; 402 : float _SPEdot; 403 : float _SKEdot; 404 : 405 : // variables used for precision landing pitch control 406 : float _hgt_at_start_of_flare; 407 : float _hgt_rate_dem_at_flare_entry; 408 : float _hgt_afe; 409 : float _pitch_min_at_flare_entry; 410 : 411 : // used to scale max climb and sink limits to match vehicle ability 412 : float _max_climb_scaler; 413 : float _max_sink_scaler; 414 : float _sink_fraction; 415 : 416 : // Specific energy error quantities 417 : float _STE_error; 418 : 419 : // 1 when specific energy balance rate demand is clipping in the up direction 420 : // -1 when specific energy balance rate demand is clipping in the down direction 421 : // 0 when not clipping 422 : clipStatus _SEBdot_dem_clip; 423 : 424 : // Time since last update of main TECS loop (seconds) 425 : float _DT; 426 : 427 : // true when class variables used for flare control have been initialised 428 : // on flare entry 429 : bool _flare_initialised; 430 : 431 : // percent traveled along the previous and next waypoints 432 : float _path_proportion; 433 : 434 : float _distance_beyond_land_wp; 435 : 436 : float _land_pitch_min = -90; 437 : 438 : // need to reset on next loop 439 : bool _need_reset; 440 : // Flag if someone else drives throttle externally. 441 : bool _flag_throttle_forced; 442 : 443 : // Checks if we reset at the beginning of takeoff. 444 : bool _flag_have_reset_after_takeoff; 445 : 446 : float _SKE_weighting; 447 : 448 : AP_Int8 _use_synthetic_airspeed; 449 : 450 : // use synthetic airspeed for next loop 451 : bool _use_synthetic_airspeed_once; 452 : 453 : // using airspeed in throttle calculation this frame 454 : bool _using_airspeed_for_throttle; 455 : 456 : // low pass filters used for crossover filter that combines demanded and measured pitch 457 : // when calculating a pitch to throttle mapping. 458 : LowPassFilterFloat _pitch_demand_lpf; 459 : LowPassFilterFloat _pitch_measured_lpf; 460 : 461 : // aerodynamic load factor 462 : float _load_factor; 463 : 464 : // Update the airspeed internal state using a second order complementary filter 465 : void _update_speed(float DT); 466 : 467 : // Update the demanded airspeed 468 : void _update_speed_demand(void); 469 : 470 : // Update the demanded height 471 : void _update_height_demand(void); 472 : 473 : // Detect an underspeed condition 474 : void _detect_underspeed(void); 475 : 476 : // Update Specific Energy Quantities 477 : void _update_energies(void); 478 : 479 : // Update Demanded Throttle 480 : void _update_throttle_with_airspeed(void); 481 : 482 : // Update Demanded Throttle Non-Airspeed 483 : void _update_throttle_without_airspeed(int16_t throttle_nudge, float pitch_trim_deg); 484 : 485 : // Constrain throttle demand and record clipping 486 : void constrain_throttle(); 487 : 488 : // get integral gain which is flight_stage dependent 489 : float _get_i_gain(void); 490 : 491 : // Detect Bad Descent 492 : void _detect_bad_descent(void); 493 : 494 : // Update Demanded Pitch Angle 495 : void _update_pitch(void); 496 : 497 : // Initialise states and variables 498 : void _initialise_states(float hgt_afe); 499 : 500 : // Calculate specific total energy rate limits 501 : void _update_STE_rate_lim(void); 502 : 503 : // declares a 5point average filter using floats 504 : AverageFilterFloat_Size5 _vdot_filter; 505 : 506 : // current time constant 507 : float timeConstant(void) const; 508 : 509 : // Update the allowable throttle range. 510 : void _update_throttle_limits(); 511 : 512 : // Update the allowable pitch range. 513 : void _update_pitch_limits(const int32_t ptchMinCO_cd); 514 : };
