Browse Source
The TECS controller belongs really into the ECL (estimation & control library) where we have collected a number of vehicle control systems. It is being replaced by a new implementation of the algorithm, contributed by Paul Riseborough.sbg
Lorenz Meier
8 years ago
28 changed files with 83 additions and 1223 deletions
@ -1,41 +1,28 @@
@@ -1,41 +1,28 @@
|
||||
The PX4 firmware is licensed generally under a permissive 3-clause BSD license. Contributions are required |
||||
to be made under the same license. Any exception to this general rule is listed below. |
||||
Copyright (c) 2012-2017 PX4 Development Team |
||||
All rights reserved. |
||||
|
||||
/**************************************************************************** |
||||
* |
||||
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
Redistribution and use in source and binary forms, with or without |
||||
modification, are permitted provided that the following conditions are met: |
||||
|
||||
1. Redistributions of source code must retain the above copyright |
||||
notice, this list of conditions and the following disclaimer. |
||||
|
||||
- PX4 middleware: BSD 3-clause |
||||
- PX4 flight control stack: BSD 3-clause |
||||
- NuttX operating system: BSD 3-clause |
||||
- Exceptions: Currently only this [400 LOC file](https://github.com/PX4/Firmware/blob/master/src/lib/external_lgpl/tecs/tecs.cpp) remains LGPL, but will be replaced with a BSD implementation. |
||||
2. Redistributions in binary form must reproduce the above copyright |
||||
notice, this list of conditions and the following disclaimer in |
||||
the documentation and/or other materials provided with the |
||||
distribution. |
||||
|
||||
3. Neither the name PX4 nor the names of its contributors may be |
||||
used to endorse or promote products derived from this software |
||||
without specific prior written permission. |
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND |
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED |
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR |
||||
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES |
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND |
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS |
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
||||
|
||||
@ -0,0 +1,55 @@
@@ -0,0 +1,55 @@
|
||||
include(posix/px4_impl_posix) |
||||
|
||||
set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) |
||||
|
||||
set(config_module_list |
||||
drivers/device |
||||
drivers/boards/sitl |
||||
platforms/common |
||||
platforms/posix/px4_layer |
||||
platforms/posix/work_queue |
||||
systemcmds/param |
||||
systemcmds/ver |
||||
systemcmds/perf |
||||
modules/uORB |
||||
modules/systemlib/param |
||||
modules/systemlib |
||||
modules/ekf2 |
||||
modules/ekf2_replay |
||||
modules/sdlog2 |
||||
modules/logger |
||||
lib/controllib |
||||
lib/mathlib |
||||
lib/mathlib/math/filter |
||||
lib/conversion |
||||
lib/ecl |
||||
lib/geo |
||||
lib/geo_lookup |
||||
lib/version |
||||
lib/DriverFramework/framework |
||||
lib/micro-CDR |
||||
) |
||||
|
||||
set(config_extra_builtin_cmds |
||||
serdis |
||||
sercon |
||||
) |
||||
|
||||
set(config_sitl_rcS_dir |
||||
posix-configs/SITL/init/replay |
||||
CACHE INTERNAL "init script dir for sitl" |
||||
) |
||||
|
||||
set(config_sitl_viewer |
||||
replay |
||||
CACHE STRING "viewer for sitl" |
||||
) |
||||
set_property(CACHE config_sitl_viewer |
||||
PROPERTY STRINGS "replay;none") |
||||
|
||||
set(config_sitl_debugger |
||||
disable |
||||
CACHE STRING "debugger for sitl" |
||||
) |
||||
set_property(CACHE config_sitl_debugger |
||||
PROPERTY STRINGS "disable;gdb;lldb") |
||||
@ -1 +1 @@
@@ -1 +1 @@
|
||||
Subproject commit 61e0c0481130ec6f07240298bc6a6aff5feb8418 |
||||
Subproject commit 2bae55753dc3cb83ae8bd92be5dcc76f27cf8948 |
||||
@ -1,41 +0,0 @@
@@ -1,41 +0,0 @@
|
||||
############################################################################ |
||||
# |
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved. |
||||
# |
||||
# Redistribution and use in source and binary forms, with or without |
||||
# modification, are permitted provided that the following conditions |
||||
# are met: |
||||
# |
||||
# 1. Redistributions of source code must retain the above copyright |
||||
# notice, this list of conditions and the following disclaimer. |
||||
# 2. Redistributions in binary form must reproduce the above copyright |
||||
# notice, this list of conditions and the following disclaimer in |
||||
# the documentation and/or other materials provided with the |
||||
# distribution. |
||||
# 3. Neither the name PX4 nor the names of its contributors may be |
||||
# used to endorse or promote products derived from this software |
||||
# without specific prior written permission. |
||||
# |
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
# POSSIBILITY OF SUCH DAMAGE. |
||||
# |
||||
############################################################################ |
||||
px4_add_module( |
||||
MODULE lib__external_lgpl |
||||
COMPILE_FLAGS |
||||
SRCS |
||||
tecs/tecs.cpp |
||||
DEPENDS |
||||
platforms__common |
||||
) |
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix : |
||||
@ -1,633 +0,0 @@
@@ -1,633 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
#include "tecs.h" |
||||
|
||||
#include <ecl/ecl.h> |
||||
#include <systemlib/err.h> |
||||
#include <geo/geo.h> |
||||
|
||||
using math::constrain; |
||||
using math::max; |
||||
using math::min; |
||||
|
||||
/**
|
||||
* @file tecs.cpp |
||||
* |
||||
* @author Paul Riseborough |
||||
* |
||||
* Written by Paul Riseborough 2013 to provide: |
||||
* - Combined control of speed and height using throttle to control |
||||
* total energy and pitch angle to control exchange of energy between |
||||
* potential and kinetic. |
||||
* Selectable speed or height priority modes when calculating pitch angle |
||||
* - Fallback mode when no airspeed measurement is available that |
||||
* sets throttle based on height rate demand and switches pitch angle control to |
||||
* height priority |
||||
* - Underspeed protection that demands maximum throttle and switches pitch angle control |
||||
* to speed priority mode |
||||
* - Relative ease of tuning through use of intuitive time constant, integrator and damping gains and the use |
||||
* of easy to measure aircraft performance data |
||||
* |
||||
*/ |
||||
|
||||
void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<3, 3> &rotMat, |
||||
const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air) |
||||
{ |
||||
// Implement third order complementary filter for height and height rate
|
||||
// estimted height rate = _integ2_state
|
||||
// estimated height = _integ3_state
|
||||
// Reference Paper :
|
||||
// Optimising the Gains of the Baro-Inertial Vertical Channel
|
||||
// Widnall W.S, Sinha P.K,
|
||||
// AIAA Journal of Guidance and Control, 78-1307R
|
||||
|
||||
// Calculate time in seconds since last update
|
||||
uint64_t now = ecl_absolute_time(); |
||||
float DT = max((now - _update_50hz_last_usec), UINT64_C(0)) * 1.0e-6f; |
||||
|
||||
bool reset_altitude = false; |
||||
|
||||
if (_update_50hz_last_usec == 0 || DT > DT_MAX) { |
||||
DT = DT_DEFAULT; // when first starting TECS, use small time constant
|
||||
reset_altitude = true; |
||||
} |
||||
|
||||
if (!altitude_lock || !in_air) { |
||||
reset_altitude = true; |
||||
} |
||||
|
||||
if (reset_altitude) { |
||||
_integ3_state = baro_altitude; |
||||
_integ2_state = 0.0f; |
||||
_integ1_state = 0.0f; |
||||
|
||||
// Reset the filter state as we just switched from non-altitude control
|
||||
// to altitude control mode
|
||||
_states_initalized = false; |
||||
} |
||||
|
||||
_update_50hz_last_usec = now; |
||||
_EAS = airspeed; |
||||
|
||||
_in_air = in_air; |
||||
|
||||
// Get height acceleration
|
||||
float hgt_ddot_mea = -(accel_earth(2) + CONSTANTS_ONE_G); |
||||
|
||||
// Perform filter calculation using backwards Euler integration
|
||||
// Coefficients selected to place all three filter poles at omega
|
||||
float omega2 = _hgtCompFiltOmega * _hgtCompFiltOmega; |
||||
float hgt_err = baro_altitude - _integ3_state; |
||||
float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega; |
||||
_integ1_state = _integ1_state + integ1_input * DT; |
||||
float integ2_input = _integ1_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f; |
||||
_integ2_state = _integ2_state + integ2_input * DT; |
||||
float integ3_input = _integ2_state + hgt_err * _hgtCompFiltOmega * 3.0f; |
||||
|
||||
// If more than 1 second has elapsed since last update then reset the integrator state
|
||||
// to the measured height
|
||||
if (reset_altitude) { |
||||
_integ3_state = baro_altitude; |
||||
|
||||
} else { |
||||
_integ3_state = _integ3_state + integ3_input * DT; |
||||
} |
||||
|
||||
// Update and average speed rate of change
|
||||
// Only required if airspeed is being measured and controlled
|
||||
if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) { |
||||
// Get DCM
|
||||
// Calculate speed rate of change
|
||||
// XXX check
|
||||
float temp = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0); |
||||
// take 5 point moving average
|
||||
//_vel_dot = _vdot_filter.apply(temp);
|
||||
// XXX resolve this properly
|
||||
_vel_dot = 0.95f * _vel_dot + 0.05f * temp; |
||||
|
||||
} else { |
||||
_vel_dot = 0.0f; |
||||
} |
||||
|
||||
if (!_in_air) { |
||||
_states_initalized = false; |
||||
} |
||||
|
||||
} |
||||
|
||||
void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, float EAS2TAS) |
||||
{ |
||||
// Calculate time in seconds since last update
|
||||
uint64_t now = ecl_absolute_time(); |
||||
float DT = max((now - _update_speed_last_usec), UINT64_C(0)) * 1.0e-6f; |
||||
|
||||
// Convert equivalent airspeeds to true airspeeds
|
||||
_EAS_dem = airspeed_demand; |
||||
_TAS_dem = _EAS_dem * EAS2TAS; |
||||
_TASmax = _indicated_airspeed_max * EAS2TAS; |
||||
_TASmin = _indicated_airspeed_min * EAS2TAS; |
||||
|
||||
// Get airspeed or default to halfway between min and max if
|
||||
// airspeed is not being used and set speed rate to zero
|
||||
if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) { |
||||
// If no airspeed available use average of min and max
|
||||
_EAS = 0.5f * (_indicated_airspeed_min + _indicated_airspeed_max); |
||||
|
||||
} else { |
||||
_EAS = indicated_airspeed; |
||||
} |
||||
|
||||
// Reset states on initial execution or if not active
|
||||
if (_update_speed_last_usec == 0 || !_in_air) { |
||||
_integ4_state = 0.0f; |
||||
_integ5_state = (_EAS * EAS2TAS); |
||||
} |
||||
|
||||
if (DT < DT_MIN || DT > DT_MAX) { |
||||
DT = DT_DEFAULT; // when first starting TECS, use small time constant
|
||||
} |
||||
|
||||
// Implement a second order complementary filter to obtain a
|
||||
// smoothed airspeed estimate
|
||||
// airspeed estimate is held in _integ5_state
|
||||
float aspdErr = (_EAS * EAS2TAS) - _integ5_state; |
||||
float integ4_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega; |
||||
|
||||
// Prevent state from winding up
|
||||
if (_integ5_state < 3.1f) { |
||||
integ4_input = max(integ4_input, 0.0f); |
||||
} |
||||
|
||||
_integ4_state = _integ4_state + integ4_input * DT; |
||||
float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f; |
||||
_integ5_state = _integ5_state + integ5_input * DT; |
||||
|
||||
// limit the airspeed to a minimum of 3 m/s
|
||||
_integ5_state = max(_integ5_state, 3.0f); |
||||
_update_speed_last_usec = now; |
||||
} |
||||
|
||||
void TECS::_update_speed_demand() |
||||
{ |
||||
// Set the airspeed demand to the minimum value if an underspeed condition exists
|
||||
// or a bad descent condition exists
|
||||
// This will minimise the rate of descent resulting from an engine failure,
|
||||
// enable the maximum climb rate to be achieved and prevent continued full power descent
|
||||
// into the ground due to an unachievable airspeed value
|
||||
if ((_badDescent) || (_underspeed)) { |
||||
_TAS_dem = _TASmin; |
||||
} |
||||
|
||||
// Constrain speed demand
|
||||
_TAS_dem = constrain(_TAS_dem, _TASmin, _TASmax); |
||||
|
||||
// calculate velocity rate limits based on physical performance limits
|
||||
// provision to use a different rate limit if bad descent or underspeed condition exists
|
||||
// Use 50% of maximum energy rate to allow margin for total energy controller
|
||||
float velRateMax; |
||||
float velRateMin; |
||||
|
||||
if ((_badDescent) || (_underspeed)) { |
||||
velRateMax = 0.5f * _STEdot_max / _integ5_state; |
||||
velRateMin = 0.5f * _STEdot_min / _integ5_state; |
||||
|
||||
} else { |
||||
velRateMax = 0.5f * _STEdot_max / _integ5_state; |
||||
velRateMin = 0.5f * _STEdot_min / _integ5_state; |
||||
} |
||||
|
||||
// // Apply rate limit
|
||||
// if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * _DT)) {
|
||||
// _TAS_dem_adj = _TAS_dem_adj + velRateMax * _DT;
|
||||
// _TAS_rate_dem = velRateMax;
|
||||
//
|
||||
// } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * _DT)) {
|
||||
// _TAS_dem_adj = _TAS_dem_adj + velRateMin * _DT;
|
||||
// _TAS_rate_dem = velRateMin;
|
||||
//
|
||||
// } else {
|
||||
// _TAS_dem_adj = _TAS_dem;
|
||||
//
|
||||
//
|
||||
// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / _DT;
|
||||
// }
|
||||
|
||||
_TAS_dem_adj = constrain(_TAS_dem, _TASmin, _TASmax); |
||||
|
||||
//xxx: using a p loop for now
|
||||
_TAS_rate_dem = constrain((_TAS_dem_adj - _integ5_state) * _speedrate_p, velRateMin, velRateMax); |
||||
|
||||
// _TAS_dem_last = _TAS_dem;
|
||||
|
||||
} |
||||
|
||||
void TECS::_update_height_demand(float demand, float state) |
||||
{ |
||||
// Handle initialization
|
||||
if (PX4_ISFINITE(demand) && fabsf(_hgt_dem_in_old) < 0.1f) { |
||||
_hgt_dem_in_old = demand; |
||||
} |
||||
|
||||
// Apply 2 point moving average to demanded height
|
||||
// This is required because height demand is updated in steps
|
||||
if (PX4_ISFINITE(demand)) { |
||||
_hgt_dem = 0.5f * (demand + _hgt_dem_in_old); |
||||
|
||||
} else { |
||||
_hgt_dem = _hgt_dem_in_old; |
||||
} |
||||
|
||||
_hgt_dem_in_old = _hgt_dem; |
||||
|
||||
// Limit height demand
|
||||
// this is important to avoid a windup
|
||||
if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * _DT)) { |
||||
_hgt_dem = _hgt_dem_prev + _maxClimbRate * _DT; |
||||
|
||||
} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * _DT)) { |
||||
_hgt_dem = _hgt_dem_prev - _maxSinkRate * _DT; |
||||
} |
||||
|
||||
_hgt_dem_prev = _hgt_dem; |
||||
|
||||
// Apply first order lag to height demand
|
||||
_hgt_dem_adj = 0.1f * _hgt_dem + 0.9f * _hgt_dem_adj_last; |
||||
_hgt_rate_dem = (_hgt_dem_adj - state) * _heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last) / _DT; |
||||
_hgt_dem_adj_last = _hgt_dem_adj; |
||||
|
||||
// Limit height rate of change
|
||||
if (_hgt_rate_dem > _maxClimbRate) { |
||||
_hgt_rate_dem = _maxClimbRate; |
||||
|
||||
} else if (_hgt_rate_dem < -_maxSinkRate) { |
||||
_hgt_rate_dem = -_maxSinkRate; |
||||
} |
||||
} |
||||
|
||||
void TECS::_detect_underspeed() |
||||
{ |
||||
if (!_detect_underspeed_enabled) { |
||||
_underspeed = false; |
||||
return; |
||||
} |
||||
|
||||
if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) |
||||
&& _underspeed)) { |
||||
|
||||
_underspeed = true; |
||||
|
||||
} else { |
||||
_underspeed = false; |
||||
} |
||||
} |
||||
|
||||
void TECS::_update_energies() |
||||
{ |
||||
// Calculate specific energy demands
|
||||
_SPE_dem = _hgt_dem_adj * CONSTANTS_ONE_G; |
||||
_SKE_dem = 0.5f * _TAS_dem_adj * _TAS_dem_adj; |
||||
|
||||
// Calculate specific energy rate demands
|
||||
_SPEdot_dem = _hgt_rate_dem * CONSTANTS_ONE_G; |
||||
_SKEdot_dem = _integ5_state * _TAS_rate_dem; |
||||
|
||||
// Calculate specific energy
|
||||
_SPE_est = _integ3_state * CONSTANTS_ONE_G; |
||||
_SKE_est = 0.5f * _integ5_state * _integ5_state; |
||||
|
||||
// Calculate specific energy rate
|
||||
_SPEdot = _integ2_state * CONSTANTS_ONE_G; |
||||
_SKEdot = _integ5_state * _vel_dot; |
||||
} |
||||
|
||||
void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3, 3> &rotMat) |
||||
{ |
||||
// Calculate total energy values
|
||||
_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; |
||||
float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); |
||||
_STEdot_error = STEdot_dem - _SPEdot - _SKEdot; |
||||
|
||||
// Apply 0.5 second first order filter to STEdot_error
|
||||
// This is required to remove accelerometer noise from the measurement
|
||||
_STEdot_error = 0.2f * _STEdot_error + 0.8f * _STEdotErrLast; |
||||
_STEdotErrLast = _STEdot_error; |
||||
|
||||
// Calculate throttle demand
|
||||
// If underspeed condition is set, then demand full throttle
|
||||
if (_underspeed) { |
||||
_throttle_dem = 1.0f; |
||||
|
||||
} else { |
||||
// Calculate gain scaler from specific energy error to throttle
|
||||
float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min)); |
||||
|
||||
// Calculate feed-forward throttle
|
||||
float ff_throttle = 0; |
||||
float nomThr = throttle_cruise; |
||||
// Use the demanded rate of change of total energy as the feed-forward demand, but add
|
||||
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
|
||||
// drag increase during turns.
|
||||
float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1))); |
||||
STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi, 0.1f, 1.0f) - 1.0f); |
||||
|
||||
if (STEdot_dem >= 0) { |
||||
ff_throttle = nomThr + STEdot_dem / _STEdot_max * (_THRmaxf - nomThr); |
||||
|
||||
} else { |
||||
ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr; |
||||
} |
||||
|
||||
// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
|
||||
_throttle_dem = (_STE_error + _STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; |
||||
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); |
||||
|
||||
// Rate limit PD + FF throttle
|
||||
// Calculate the throttle increment from the specified slew time
|
||||
if (fabsf(_throttle_slewrate) > 0.01f) { |
||||
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; |
||||
_throttle_dem = constrain(_throttle_dem, _last_throttle_dem - thrRateIncr, _last_throttle_dem + thrRateIncr); |
||||
} |
||||
|
||||
// Ensure _last_throttle_dem is always initialized properly
|
||||
_last_throttle_dem = _throttle_dem; |
||||
|
||||
// Calculate integrator state upper and lower limits
|
||||
// Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
|
||||
float integ_max = (_THRmaxf - _throttle_dem + 0.1f); |
||||
float integ_min = (_THRminf - _throttle_dem - 0.1f); |
||||
|
||||
// Calculate integrator state, constraining state
|
||||
// Set integrator to a max throttle value during climbout
|
||||
_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr; |
||||
|
||||
if (_climbOutDem) { |
||||
_integ6_state = integ_max; |
||||
|
||||
} else { |
||||
_integ6_state = constrain(_integ6_state, integ_min, integ_max); |
||||
} |
||||
|
||||
// Sum the components.
|
||||
// Only use feed-forward component if airspeed is not being used
|
||||
if (airspeed_sensor_enabled()) { |
||||
_throttle_dem = _throttle_dem + _integ6_state; |
||||
|
||||
} else { |
||||
_throttle_dem = ff_throttle; |
||||
} |
||||
|
||||
// Constrain throttle demand
|
||||
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); |
||||
} |
||||
} |
||||
|
||||
void TECS::_detect_bad_descent() |
||||
{ |
||||
// Detect a demanded airspeed too high for the aircraft to achieve. This will be
|
||||
// evident by the the following conditions:
|
||||
// 1) Underspeed protection not active
|
||||
// 2) Specific total energy error > 200 (greater than ~20m height error)
|
||||
// 3) Specific total energy reducing
|
||||
// 4) throttle demand > 90%
|
||||
// If these four conditions exist simultaneously, then the protection
|
||||
// mode will be activated.
|
||||
// Once active, the following condition are required to stay in the mode
|
||||
// 1) Underspeed protection not active
|
||||
// 2) Specific total energy error > 0
|
||||
// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
|
||||
// float STEdot = _SPEdot + _SKEdot;
|
||||
//
|
||||
// if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
|
||||
//
|
||||
// warnx("bad descent detected: _STE_error %.1f, STEdot %.1f, _throttle_dem %.1f", _STE_error, STEdot, _throttle_dem);
|
||||
// _badDescent = true;
|
||||
//
|
||||
// } else {
|
||||
// _badDescent = false;
|
||||
// }
|
||||
|
||||
_badDescent = false; |
||||
} |
||||
|
||||
void TECS::_update_pitch() |
||||
{ |
||||
// Calculate Speed/Height Control Weighting
|
||||
// This is used to determine how the pitch control prioritises speed and height control
|
||||
// A weighting of 1 provides equal priority (this is the normal mode of operation)
|
||||
// A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available
|
||||
// A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected
|
||||
// or during takeoff/climbout where a minimum pitch angle is set to ensure height is gained. In this instance, if airspeed
|
||||
// rises above the demanded value, the pitch angle will be increased by the TECS controller.
|
||||
float SKE_weighting = constrain(_spdWeight, 0.0f, 2.0f); |
||||
|
||||
if ((_underspeed || _climbOutDem) && airspeed_sensor_enabled()) { |
||||
SKE_weighting = 2.0f; |
||||
|
||||
} else if (!airspeed_sensor_enabled()) { |
||||
SKE_weighting = 0.0f; |
||||
} |
||||
|
||||
float SPE_weighting = 2.0f - SKE_weighting; |
||||
|
||||
// Calculate Specific Energy Balance demand, and error
|
||||
float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; |
||||
float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; |
||||
_SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); |
||||
_SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); |
||||
|
||||
// Calculate factor relating an error in specific energy to a desired delta pitch angle
|
||||
float gainInv = _integ5_state * _timeConst * CONSTANTS_ONE_G; |
||||
|
||||
// Calculate integrator state, constraining input if pitch limits are exceeded
|
||||
float integ7_input = _SEB_error * _integGain; |
||||
|
||||
// constrain the integrator input to prevent it changing in the direction that increases pitch demand saturation
|
||||
// if the pitch demand is saturated, then decay the integrator at the control loop time constant
|
||||
if (_pitch_dem_unc > _PITCHmaxf) { |
||||
integ7_input = min(integ7_input, min((_PITCHmaxf - _pitch_dem_unc) * gainInv / _timeConst, 0.0f)); |
||||
|
||||
} else if (_pitch_dem_unc < _PITCHminf) { |
||||
integ7_input = max(integ7_input, max((_PITCHminf - _pitch_dem_unc) * gainInv / _timeConst, 0.0f)); |
||||
} |
||||
|
||||
// pitch loop integration
|
||||
_integ7_state = _integ7_state + integ7_input * _DT; |
||||
|
||||
// Specific Energy Balance correction excluding integrator contribution
|
||||
float SEB_correction = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; |
||||
|
||||
// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
|
||||
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
|
||||
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
|
||||
if (_climbOutDem) { |
||||
SEB_correction += _PITCHminf * gainInv; |
||||
} |
||||
|
||||
// Calculate pitch demand from specific energy balance signals
|
||||
_pitch_dem_unc = (SEB_correction + _integ7_state) / gainInv; |
||||
|
||||
// Constrain pitch demand
|
||||
_pitch_dem = constrain(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); |
||||
|
||||
// Rate limit the pitch demand to comply with specified vertical
|
||||
// acceleration limit
|
||||
float ptchRateIncr = _DT * _vertAccLim / _integ5_state; |
||||
|
||||
if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) { |
||||
_pitch_dem = _last_pitch_dem + ptchRateIncr; |
||||
|
||||
} else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) { |
||||
_pitch_dem = _last_pitch_dem - ptchRateIncr; |
||||
} |
||||
|
||||
_last_pitch_dem = _pitch_dem; |
||||
} |
||||
|
||||
void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad, |
||||
float EAS2TAS) |
||||
{ |
||||
// Initialise states and variables if DT > 1 second or in climbout
|
||||
if (_update_pitch_throttle_last_usec == 0 || _DT > DT_MAX || !_in_air || !_states_initalized) { |
||||
_integ1_state = 0.0f; |
||||
_integ2_state = 0.0f; |
||||
_integ3_state = baro_altitude; |
||||
_integ4_state = 0.0f; |
||||
_integ5_state = _EAS * EAS2TAS; |
||||
_integ6_state = 0.0f; |
||||
_integ7_state = 0.0f; |
||||
|
||||
_last_throttle_dem = throttle_cruise; |
||||
_last_pitch_dem = constrain(pitch, _PITCHminf, _PITCHmaxf); |
||||
_pitch_dem_unc = _last_pitch_dem; |
||||
|
||||
_hgt_dem_adj_last = baro_altitude; |
||||
_hgt_dem_adj = _hgt_dem_adj_last; |
||||
_hgt_dem_prev = _hgt_dem_adj_last; |
||||
_hgt_dem_in_old = _hgt_dem_adj_last; |
||||
|
||||
_TAS_dem_last = _EAS * EAS2TAS; |
||||
_TAS_dem_adj = _TAS_dem_last; |
||||
|
||||
_underspeed = false; |
||||
_badDescent = false; |
||||
|
||||
if (_DT > DT_MAX || _DT < DT_MIN) { |
||||
_DT = DT_DEFAULT; |
||||
} |
||||
|
||||
} else if (_climbOutDem) { |
||||
_PITCHminf = ptchMinCO_rad; |
||||
_THRminf = _THRmaxf - 0.01f; |
||||
|
||||
_hgt_dem_adj_last = baro_altitude; |
||||
_hgt_dem_adj = _hgt_dem_adj_last; |
||||
_hgt_dem_prev = _hgt_dem_adj_last; |
||||
|
||||
_TAS_dem_last = _EAS * EAS2TAS; |
||||
_TAS_dem_adj = _EAS * EAS2TAS; |
||||
|
||||
_underspeed = false; |
||||
_badDescent = false; |
||||
} |
||||
|
||||
_states_initalized = true; |
||||
} |
||||
|
||||
void TECS::_update_STE_rate_lim() |
||||
{ |
||||
// Calculate Specific Total Energy Rate Limits
|
||||
// This is a trivial calculation at the moment but will get bigger once we start adding altitude effects
|
||||
_STEdot_max = _maxClimbRate * CONSTANTS_ONE_G; |
||||
_STEdot_min = - _minSinkRate * CONSTANTS_ONE_G; |
||||
} |
||||
|
||||
void TECS::update_pitch_throttle(const math::Matrix<3, 3> &rotMat, float pitch, float baro_altitude, float hgt_dem, |
||||
float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, |
||||
float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) |
||||
{ |
||||
|
||||
// Calculate time in seconds since last update
|
||||
uint64_t now = ecl_absolute_time(); |
||||
_DT = max((now - _update_pitch_throttle_last_usec), UINT64_C(0)) * 1.0e-6f; |
||||
|
||||
// Convert inputs
|
||||
_THRmaxf = throttle_max; |
||||
_THRminf = throttle_min; |
||||
_PITCHmaxf = pitch_limit_max; |
||||
_PITCHminf = pitch_limit_min; |
||||
_climbOutDem = climbOutDem; |
||||
|
||||
// initialise selected states and variables if DT > 1 second or in climbout
|
||||
_initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO, EAS2TAS); |
||||
|
||||
if (!_in_air) { |
||||
return; |
||||
} |
||||
|
||||
// Update the speed estimate using a 2nd order complementary filter
|
||||
_update_speed(EAS_dem, indicated_airspeed, EAS2TAS); |
||||
|
||||
// Calculate Specific Total Energy Rate Limits
|
||||
_update_STE_rate_lim(); |
||||
|
||||
// Detect underspeed condition
|
||||
_detect_underspeed(); |
||||
|
||||
// Calculate the speed demand
|
||||
_update_speed_demand(); |
||||
|
||||
// Calculate the height demand
|
||||
_update_height_demand(hgt_dem, baro_altitude); |
||||
|
||||
// Calculate specific energy quantitiues
|
||||
_update_energies(); |
||||
|
||||
// Calculate throttle demand
|
||||
_update_throttle(throttle_cruise, rotMat); |
||||
|
||||
// Detect bad descent due to demanded airspeed being too high
|
||||
_detect_bad_descent(); |
||||
|
||||
// Calculate pitch demand
|
||||
_update_pitch(); |
||||
|
||||
_tecs_state.timestamp = now; |
||||
|
||||
if (_underspeed) { |
||||
_tecs_state.mode = ECL_TECS_MODE_UNDERSPEED; |
||||
|
||||
} else if (_badDescent) { |
||||
_tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT; |
||||
|
||||
} else if (_climbOutDem) { |
||||
_tecs_state.mode = ECL_TECS_MODE_CLIMBOUT; |
||||
|
||||
} else { |
||||
// If no error flag applies, conclude normal
|
||||
_tecs_state.mode = ECL_TECS_MODE_NORMAL; |
||||
} |
||||
|
||||
_tecs_state.altitude_sp = _hgt_dem_adj; |
||||
_tecs_state.altitude_filtered = _integ3_state; |
||||
_tecs_state.altitude_rate_sp = _hgt_rate_dem; |
||||
_tecs_state.altitude_rate = _integ2_state; |
||||
|
||||
_tecs_state.airspeed_sp = _TAS_dem_adj; |
||||
_tecs_state.airspeed_rate_sp = _TAS_rate_dem; |
||||
_tecs_state.airspeed_filtered = _integ5_state; |
||||
_tecs_state.airspeed_rate = _vel_dot; |
||||
|
||||
_tecs_state.total_energy_error = _STE_error; |
||||
_tecs_state.energy_distribution_error = _SEB_error; |
||||
_tecs_state.total_energy_rate_error = _STEdot_error; |
||||
_tecs_state.energy_distribution_rate_error = _SEBdot_error; |
||||
|
||||
_tecs_state.energy_error_integ = _integ6_state; |
||||
_tecs_state.energy_distribution_error_integ = _integ7_state; |
||||
|
||||
_tecs_state.throttle_integ = _integ6_state; |
||||
_tecs_state.pitch_integ = _integ7_state; |
||||
|
||||
_update_pitch_throttle_last_usec = now; |
||||
|
||||
} |
||||
@ -1,487 +0,0 @@
@@ -1,487 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
/// @file tecs.h
|
||||
/// @brief Combined Total Energy Speed & Height Control.
|
||||
|
||||
/*
|
||||
* Written by Paul Riseborough 2013 to provide: |
||||
* - Combined control of speed and height using throttle to control |
||||
* total energy and pitch angle to control exchange of energy between |
||||
* potential and kinetic. |
||||
* Selectable speed or height priority modes when calculating pitch angle |
||||
* - Fallback mode when no airspeed measurement is available that |
||||
* sets throttle based on height rate demand and switches pitch angle control to |
||||
* height priority |
||||
* - Underspeed protection that demands maximum throttle switches pitch angle control |
||||
* to speed priority mode |
||||
* - Relative ease of tuning through use of intuitive time constant, trim rate and damping parameters and the use |
||||
* of easy to measure aircraft performance data |
||||
*/ |
||||
|
||||
#ifndef TECS_H |
||||
#define TECS_H |
||||
|
||||
#include <mathlib/mathlib.h> |
||||
#include <stdint.h> |
||||
|
||||
class __EXPORT TECS |
||||
{ |
||||
public: |
||||
TECS() : |
||||
_tecs_state {}, |
||||
_update_50hz_last_usec(0), |
||||
_update_speed_last_usec(0), |
||||
_update_pitch_throttle_last_usec(0), |
||||
// TECS tuning parameters
|
||||
_hgtCompFiltOmega(0.0f), |
||||
_spdCompFiltOmega(0.0f), |
||||
_maxClimbRate(2.0f), |
||||
_minSinkRate(1.0f), |
||||
_maxSinkRate(2.0f), |
||||
_timeConst(5.0f), |
||||
_timeConstThrot(8.0f), |
||||
_ptchDamp(0.0f), |
||||
_thrDamp(0.0f), |
||||
_integGain(0.0f), |
||||
_vertAccLim(0.0f), |
||||
_rollComp(0.0f), |
||||
_spdWeight(1.0f), |
||||
_heightrate_p(0.0f), |
||||
_heightrate_ff(0.0f), |
||||
_speedrate_p(0.0f), |
||||
_throttle_dem(0.0f), |
||||
_pitch_dem(0.0f), |
||||
_integ1_state(0.0f), |
||||
_integ2_state(0.0f), |
||||
_integ3_state(0.0f), |
||||
_integ4_state(0.0f), |
||||
_integ5_state(0.0f), |
||||
_integ6_state(0.0f), |
||||
_integ7_state(0.0f), |
||||
_last_throttle_dem(0.0f), |
||||
_last_pitch_dem(0.0f), |
||||
_vel_dot(0.0f), |
||||
_EAS(0.0f), |
||||
_TASmax(30.0f), |
||||
_TASmin(3.0f), |
||||
_TAS_dem(0.0f), |
||||
_TAS_dem_last(0.0f), |
||||
_EAS_dem(0.0f), |
||||
_hgt_dem(0.0f), |
||||
_hgt_dem_in_old(0.0f), |
||||
_hgt_dem_adj(0.0f), |
||||
_hgt_dem_adj_last(0.0f), |
||||
_hgt_rate_dem(0.0f), |
||||
_hgt_dem_prev(0.0f), |
||||
_TAS_dem_adj(0.0f), |
||||
_TAS_rate_dem(0.0f), |
||||
_STEdotErrLast(0.0f), |
||||
_underspeed(false), |
||||
_detect_underspeed_enabled(true), |
||||
_badDescent(false), |
||||
_climbOutDem(false), |
||||
_pitch_dem_unc(0.0f), |
||||
_STEdot_max(0.0f), |
||||
_STEdot_min(0.0f), |
||||
_THRmaxf(0.0f), |
||||
_THRminf(0.0f), |
||||
_PITCHmaxf(0.5f), |
||||
_PITCHminf(-0.5f), |
||||
_SPE_dem(0.0f), |
||||
_SKE_dem(0.0f), |
||||
_SPEdot_dem(0.0f), |
||||
_SKEdot_dem(0.0f), |
||||
_SPE_est(0.0f), |
||||
_SKE_est(0.0f), |
||||
_SPEdot(0.0f), |
||||
_SKEdot(0.0f), |
||||
_STE_error(0.0f), |
||||
_STEdot_error(0.0f), |
||||
_SEB_error(0.0f), |
||||
_SEBdot_error(0.0f), |
||||
_DT(0.02f), |
||||
_airspeed_enabled(false), |
||||
_states_initalized(false), |
||||
_in_air(false), |
||||
_throttle_slewrate(0.0f), |
||||
_indicated_airspeed_min(3.0f), |
||||
_indicated_airspeed_max(30.0f) |
||||
{ |
||||
} |
||||
|
||||
bool airspeed_sensor_enabled() |
||||
{ |
||||
return _airspeed_enabled; |
||||
} |
||||
|
||||
void enable_airspeed(bool enabled) |
||||
{ |
||||
_airspeed_enabled = enabled; |
||||
} |
||||
|
||||
// Update of the estimated height and height rate internal state
|
||||
// Update of the inertial speed rate internal state
|
||||
// Should be called at 50Hz or greater
|
||||
void update_state(float baro_altitude, float airspeed, const math::Matrix<3, 3> &rotMat, |
||||
const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air); |
||||
|
||||
// Update the control loop calculations
|
||||
void update_pitch_throttle(const math::Matrix<3, 3> &rotMat, float pitch, float baro_altitude, float hgt_dem, |
||||
float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, |
||||
float throttle_min, float throttle_max, float throttle_cruise, |
||||
float pitch_limit_min, float pitch_limit_max); |
||||
|
||||
float get_throttle_demand(void) { return _throttle_dem; } |
||||
float get_pitch_demand() { return _pitch_dem; } |
||||
float get_speed_weight() { return _spdWeight; } |
||||
|
||||
void reset_state() |
||||
{ |
||||
_states_initalized = false; |
||||
} |
||||
|
||||
enum ECL_TECS_MODE { |
||||
ECL_TECS_MODE_NORMAL = 0, |
||||
ECL_TECS_MODE_UNDERSPEED, |
||||
ECL_TECS_MODE_BAD_DESCENT, |
||||
ECL_TECS_MODE_CLIMBOUT |
||||
}; |
||||
|
||||
struct tecs_state { |
||||
uint64_t timestamp; |
||||
float altitude_filtered; |
||||
float altitude_sp; |
||||
float altitude_rate; |
||||
float altitude_rate_sp; |
||||
float airspeed_filtered; |
||||
float airspeed_sp; |
||||
float airspeed_rate; |
||||
float airspeed_rate_sp; |
||||
float energy_error_integ; |
||||
float energy_distribution_error_integ; |
||||
float total_energy_error; |
||||
float total_energy_rate_error; |
||||
float energy_distribution_error; |
||||
float energy_distribution_rate_error; |
||||
float throttle_integ; |
||||
float pitch_integ; |
||||
enum ECL_TECS_MODE mode; |
||||
}; |
||||
|
||||
void get_tecs_state(struct tecs_state &state) |
||||
{ |
||||
state = _tecs_state; |
||||
} |
||||
|
||||
void set_time_const(float time_const) |
||||
{ |
||||
_timeConst = time_const; |
||||
} |
||||
|
||||
void set_time_const_throt(float time_const_throt) |
||||
{ |
||||
_timeConstThrot = time_const_throt; |
||||
} |
||||
|
||||
void set_min_sink_rate(float rate) |
||||
{ |
||||
_minSinkRate = rate; |
||||
} |
||||
|
||||
void set_max_sink_rate(float sink_rate) |
||||
{ |
||||
_maxSinkRate = sink_rate; |
||||
} |
||||
|
||||
void set_max_climb_rate(float climb_rate) |
||||
{ |
||||
_maxClimbRate = climb_rate; |
||||
} |
||||
|
||||
void set_throttle_damp(float throttle_damp) |
||||
{ |
||||
_thrDamp = throttle_damp; |
||||
} |
||||
|
||||
void set_integrator_gain(float gain) |
||||
{ |
||||
_integGain = gain; |
||||
} |
||||
|
||||
void set_vertical_accel_limit(float limit) |
||||
{ |
||||
_vertAccLim = limit; |
||||
} |
||||
|
||||
void set_height_comp_filter_omega(float omega) |
||||
{ |
||||
_hgtCompFiltOmega = omega; |
||||
} |
||||
|
||||
void set_speed_comp_filter_omega(float omega) |
||||
{ |
||||
_spdCompFiltOmega = omega; |
||||
} |
||||
|
||||
void set_roll_throttle_compensation(float compensation) |
||||
{ |
||||
_rollComp = compensation; |
||||
} |
||||
|
||||
void set_speed_weight(float weight) |
||||
{ |
||||
_spdWeight = weight; |
||||
} |
||||
|
||||
void set_pitch_damping(float damping) |
||||
{ |
||||
_ptchDamp = damping; |
||||
} |
||||
|
||||
void set_throttle_slewrate(float slewrate) |
||||
{ |
||||
_throttle_slewrate = slewrate; |
||||
} |
||||
|
||||
void set_indicated_airspeed_min(float airspeed) |
||||
{ |
||||
_indicated_airspeed_min = airspeed; |
||||
} |
||||
|
||||
void set_indicated_airspeed_max(float airspeed) |
||||
{ |
||||
_indicated_airspeed_max = airspeed; |
||||
} |
||||
|
||||
void set_heightrate_p(float heightrate_p) |
||||
{ |
||||
_heightrate_p = heightrate_p; |
||||
} |
||||
|
||||
void set_heightrate_ff(float heightrate_ff) |
||||
{ |
||||
_heightrate_ff = heightrate_ff; |
||||
} |
||||
|
||||
void set_speedrate_p(float speedrate_p) |
||||
{ |
||||
_speedrate_p = speedrate_p; |
||||
} |
||||
|
||||
void set_detect_underspeed_enabled(bool enabled) |
||||
{ |
||||
_detect_underspeed_enabled = enabled; |
||||
} |
||||
|
||||
// in case of a height reset driven by the estimator we need
|
||||
// to allow TECS to swallow the step in height and demanded height instantaneously
|
||||
void handle_alt_step(float delta_alt, float altitude) |
||||
{ |
||||
// add height reset delta to all variables involved
|
||||
// in filtering the demanded height
|
||||
_hgt_dem_in_old += delta_alt; |
||||
_hgt_dem_prev += delta_alt; |
||||
_hgt_dem_adj_last += delta_alt; |
||||
|
||||
// reset height states
|
||||
_integ3_state = altitude; |
||||
_integ1_state = 0.0f; |
||||
_integ2_state = 0.0f; |
||||
} |
||||
|
||||
private: |
||||
|
||||
struct tecs_state _tecs_state; |
||||
|
||||
// Last time update_50Hz was called
|
||||
uint64_t _update_50hz_last_usec; |
||||
|
||||
// Last time update_speed was called
|
||||
uint64_t _update_speed_last_usec; |
||||
|
||||
// Last time update_pitch_throttle was called
|
||||
uint64_t _update_pitch_throttle_last_usec; |
||||
|
||||
// TECS tuning parameters
|
||||
float _hgtCompFiltOmega; |
||||
float _spdCompFiltOmega; |
||||
float _maxClimbRate; |
||||
float _minSinkRate; |
||||
float _maxSinkRate; |
||||
float _timeConst; |
||||
float _timeConstThrot; |
||||
float _ptchDamp; |
||||
float _thrDamp; |
||||
float _integGain; |
||||
float _vertAccLim; |
||||
float _rollComp; |
||||
float _spdWeight; |
||||
float _heightrate_p; |
||||
float _heightrate_ff; |
||||
float _speedrate_p; |
||||
|
||||
// throttle demand in the range from 0.0 to 1.0
|
||||
float _throttle_dem; |
||||
|
||||
// pitch angle demand in radians
|
||||
float _pitch_dem; |
||||
|
||||
// Integrator state 1 - height filter second derivative
|
||||
float _integ1_state; |
||||
|
||||
// Integrator state 2 - height rate
|
||||
float _integ2_state; |
||||
|
||||
// Integrator state 3 - height
|
||||
float _integ3_state; |
||||
|
||||
// Integrator state 4 - airspeed filter first derivative
|
||||
float _integ4_state; |
||||
|
||||
// Integrator state 5 - true airspeed
|
||||
float _integ5_state; |
||||
|
||||
// Integrator state 6 - throttle integrator
|
||||
float _integ6_state; |
||||
|
||||
// Integrator state 7 - pitch integrator
|
||||
float _integ7_state; |
||||
|
||||
// throttle demand rate limiter state
|
||||
float _last_throttle_dem; |
||||
|
||||
// pitch demand rate limiter state
|
||||
float _last_pitch_dem; |
||||
|
||||
// Rate of change of speed along X axis
|
||||
float _vel_dot; |
||||
|
||||
// Equivalent airspeed
|
||||
float _EAS; |
||||
|
||||
// True airspeed limits
|
||||
float _TASmax; |
||||
float _TASmin; |
||||
|
||||
// Current and last true airspeed demand
|
||||
float _TAS_dem; |
||||
float _TAS_dem_last; |
||||
|
||||
// Equivalent airspeed demand
|
||||
float _EAS_dem; |
||||
|
||||
// height demands
|
||||
float _hgt_dem; |
||||
float _hgt_dem_in_old; |
||||
float _hgt_dem_adj; |
||||
float _hgt_dem_adj_last; |
||||
float _hgt_rate_dem; |
||||
float _hgt_dem_prev; |
||||
|
||||
// Speed demand after application of rate limiting
|
||||
// This is the demand tracked by the TECS control loops
|
||||
float _TAS_dem_adj; |
||||
|
||||
// Speed rate demand after application of rate limiting
|
||||
// This is the demand tracked by the TECS control loops
|
||||
float _TAS_rate_dem; |
||||
|
||||
// Total energy rate filter state
|
||||
float _STEdotErrLast; |
||||
|
||||
// Underspeed condition
|
||||
bool _underspeed; |
||||
|
||||
// Underspeed detection enabled
|
||||
bool _detect_underspeed_enabled; |
||||
|
||||
// Bad descent condition caused by unachievable airspeed demand
|
||||
bool _badDescent; |
||||
|
||||
// climbout mode
|
||||
bool _climbOutDem; |
||||
|
||||
// pitch demand before limiting
|
||||
float _pitch_dem_unc; |
||||
|
||||
// Maximum and minimum specific total energy rate limits
|
||||
float _STEdot_max; |
||||
float _STEdot_min; |
||||
|
||||
// Maximum and minimum floating point throttle limits
|
||||
float _THRmaxf; |
||||
float _THRminf; |
||||
|
||||
// Maximum and minimum floating point pitch limits
|
||||
float _PITCHmaxf; |
||||
float _PITCHminf; |
||||
|
||||
// Specific energy quantities
|
||||
float _SPE_dem; |
||||
float _SKE_dem; |
||||
float _SPEdot_dem; |
||||
float _SKEdot_dem; |
||||
float _SPE_est; |
||||
float _SKE_est; |
||||
float _SPEdot; |
||||
float _SKEdot; |
||||
|
||||
// Specific energy error quantities
|
||||
float _STE_error; |
||||
|
||||
// Energy error rate
|
||||
float _STEdot_error; |
||||
|
||||
// Specific energy balance error
|
||||
float _SEB_error; |
||||
|
||||
// Specific energy balance error rate
|
||||
float _SEBdot_error; |
||||
|
||||
// Time since last update of main TECS loop (seconds)
|
||||
float _DT; |
||||
|
||||
static constexpr float DT_MIN = 0.001f; |
||||
static constexpr float DT_DEFAULT = 0.02f; |
||||
static constexpr float DT_MAX = 1.0f; |
||||
|
||||
bool _airspeed_enabled; |
||||
bool _states_initalized; |
||||
bool _in_air; |
||||
float _throttle_slewrate; |
||||
float _indicated_airspeed_min; |
||||
float _indicated_airspeed_max; |
||||
|
||||
// Update the airspeed internal state using a second order complementary filter
|
||||
void _update_speed(float airspeed_demand, float indicated_airspeed, float EAS2TAS); |
||||
|
||||
// Update the demanded airspeed
|
||||
void _update_speed_demand(void); |
||||
|
||||
// Update the demanded height
|
||||
void _update_height_demand(float demand, float state); |
||||
|
||||
// Detect an underspeed condition
|
||||
void _detect_underspeed(void); |
||||
|
||||
// Update Specific Energy Quantities
|
||||
void _update_energies(void); |
||||
|
||||
// Update Demanded Throttle
|
||||
void _update_throttle(float throttle_cruise, const math::Matrix<3, 3> &rotMat); |
||||
|
||||
// Detect Bad Descent
|
||||
void _detect_bad_descent(void); |
||||
|
||||
// Update Demanded Pitch Angle
|
||||
void _update_pitch(void); |
||||
|
||||
// Initialise states and variables
|
||||
void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad, float EAS2TAS); |
||||
|
||||
// Calculate specific total energy rate limits
|
||||
void _update_STE_rate_lim(void); |
||||
|
||||
}; |
||||
|
||||
#endif //TECS_H
|
||||
Loading…
Reference in new issue