mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-04 05:05:19 +08:00
Remove TECS from system codebase
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.
This commit is contained in:
Lorenz Meier
+25
-38
@@ -1,41 +1,28 @@
|
|||||||
The PX4 firmware is licensed generally under a permissive 3-clause BSD license. Contributions are required
|
Copyright (c) 2012-2017 PX4 Development Team
|
||||||
to be made under the same license. Any exception to this general rule is listed below.
|
All rights reserved.
|
||||||
|
|
||||||
/****************************************************************************
|
Redistribution and use in source and binary forms, with or without
|
||||||
*
|
modification, are permitted provided that the following conditions are met:
|
||||||
* 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.
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
|
1. Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
|
||||||
- PX4 middleware: BSD 3-clause
|
2. Redistributions in binary form must reproduce the above copyright
|
||||||
- PX4 flight control stack: BSD 3-clause
|
notice, this list of conditions and the following disclaimer in
|
||||||
- NuttX operating system: BSD 3-clause
|
the documentation and/or other materials provided with the
|
||||||
- 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.
|
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.
|
||||||
|
|||||||
@@ -117,7 +117,6 @@ set(config_module_list
|
|||||||
lib/mathlib/math/filter
|
lib/mathlib/math/filter
|
||||||
lib/rc
|
lib/rc
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/conversion
|
lib/conversion
|
||||||
|
|||||||
@@ -137,7 +137,6 @@ set(config_module_list
|
|||||||
lib/mathlib
|
lib/mathlib
|
||||||
lib/mathlib/math/filter
|
lib/mathlib/math/filter
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/conversion
|
lib/conversion
|
||||||
|
|||||||
@@ -85,7 +85,6 @@ set(config_module_list
|
|||||||
lib/mathlib/math/filter
|
lib/mathlib/math/filter
|
||||||
lib/rc
|
lib/rc
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/conversion
|
lib/conversion
|
||||||
|
|||||||
@@ -142,7 +142,6 @@ set(config_module_list
|
|||||||
lib/mathlib/math/filter
|
lib/mathlib/math/filter
|
||||||
lib/rc
|
lib/rc
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/conversion
|
lib/conversion
|
||||||
|
|||||||
@@ -119,7 +119,6 @@ set(config_module_list
|
|||||||
lib/mathlib/math/filter
|
lib/mathlib/math/filter
|
||||||
lib/rc
|
lib/rc
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/conversion
|
lib/conversion
|
||||||
|
|||||||
@@ -40,7 +40,6 @@ set(config_module_list
|
|||||||
lib/mathlib
|
lib/mathlib
|
||||||
lib/mathlib/math/filter
|
lib/mathlib/math/filter
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/conversion
|
lib/conversion
|
||||||
lib/version
|
lib/version
|
||||||
|
|||||||
@@ -137,7 +137,6 @@ set(config_module_list
|
|||||||
lib/mathlib
|
lib/mathlib
|
||||||
lib/mathlib/math/filter
|
lib/mathlib/math/filter
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/conversion
|
lib/conversion
|
||||||
|
|||||||
@@ -133,7 +133,6 @@ set(config_module_list
|
|||||||
lib/mathlib
|
lib/mathlib
|
||||||
lib/mathlib/math/filter
|
lib/mathlib/math/filter
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/conversion
|
lib/conversion
|
||||||
|
|||||||
@@ -145,7 +145,6 @@ set(config_module_list
|
|||||||
lib/conversion
|
lib/conversion
|
||||||
lib/DriverFramework/framework
|
lib/DriverFramework/framework
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/launchdetection
|
lib/launchdetection
|
||||||
|
|||||||
@@ -147,7 +147,6 @@ set(config_module_list
|
|||||||
lib/mathlib/math/filter
|
lib/mathlib/math/filter
|
||||||
lib/rc
|
lib/rc
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/conversion
|
lib/conversion
|
||||||
|
|||||||
@@ -146,7 +146,6 @@ set(config_module_list
|
|||||||
lib/conversion
|
lib/conversion
|
||||||
lib/DriverFramework/framework
|
lib/DriverFramework/framework
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/launchdetection
|
lib/launchdetection
|
||||||
|
|||||||
@@ -146,7 +146,6 @@ set(config_module_list
|
|||||||
lib/conversion
|
lib/conversion
|
||||||
lib/DriverFramework/framework
|
lib/DriverFramework/framework
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/launchdetection
|
lib/launchdetection
|
||||||
|
|||||||
@@ -125,7 +125,6 @@ set(config_module_list
|
|||||||
lib/mathlib/math/filter
|
lib/mathlib/math/filter
|
||||||
lib/rc
|
lib/rc
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/conversion
|
lib/conversion
|
||||||
|
|||||||
@@ -91,7 +91,6 @@ set(config_module_list
|
|||||||
lib/mathlib
|
lib/mathlib
|
||||||
lib/mathlib/math/filter
|
lib/mathlib/math/filter
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/conversion
|
lib/conversion
|
||||||
|
|||||||
@@ -80,7 +80,6 @@ set(config_module_list
|
|||||||
lib/ecl
|
lib/ecl
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/launchdetection
|
lib/launchdetection
|
||||||
lib/external_lgpl
|
|
||||||
lib/conversion
|
lib/conversion
|
||||||
lib/terrain_estimation
|
lib/terrain_estimation
|
||||||
lib/runway_takeoff
|
lib/runway_takeoff
|
||||||
|
|||||||
@@ -85,7 +85,6 @@ set(config_module_list
|
|||||||
lib/ecl
|
lib/ecl
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/launchdetection
|
lib/launchdetection
|
||||||
lib/external_lgpl
|
|
||||||
lib/conversion
|
lib/conversion
|
||||||
lib/terrain_estimation
|
lib/terrain_estimation
|
||||||
lib/runway_takeoff
|
lib/runway_takeoff
|
||||||
|
|||||||
@@ -84,7 +84,6 @@ set(config_module_list
|
|||||||
lib/ecl
|
lib/ecl
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/launchdetection
|
lib/launchdetection
|
||||||
lib/external_lgpl
|
|
||||||
lib/conversion
|
lib/conversion
|
||||||
lib/terrain_estimation
|
lib/terrain_estimation
|
||||||
lib/runway_takeoff
|
lib/runway_takeoff
|
||||||
|
|||||||
@@ -101,7 +101,6 @@ set(config_module_list
|
|||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/launchdetection
|
lib/launchdetection
|
||||||
lib/led
|
lib/led
|
||||||
lib/external_lgpl
|
|
||||||
lib/conversion
|
lib/conversion
|
||||||
lib/terrain_estimation
|
lib/terrain_estimation
|
||||||
lib/runway_takeoff
|
lib/runway_takeoff
|
||||||
|
|||||||
@@ -121,7 +121,6 @@ set(config_module_list
|
|||||||
lib/conversion
|
lib/conversion
|
||||||
lib/DriverFramework/framework
|
lib/DriverFramework/framework
|
||||||
lib/ecl
|
lib/ecl
|
||||||
lib/external_lgpl
|
|
||||||
lib/geo
|
lib/geo
|
||||||
lib/geo_lookup
|
lib/geo_lookup
|
||||||
lib/launchdetection
|
lib/launchdetection
|
||||||
|
|||||||
@@ -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
Submodule src/lib/ecl updated: 61e0c04811...2bae55753d
@@ -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 :
|
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
|
||||||
@@ -41,7 +41,6 @@ px4_add_module(
|
|||||||
DEPENDS
|
DEPENDS
|
||||||
platforms__common
|
platforms__common
|
||||||
git_ecl
|
git_ecl
|
||||||
lib__external_lgpl
|
|
||||||
lib__ecl
|
lib__ecl
|
||||||
)
|
)
|
||||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||||
|
|||||||
@@ -43,7 +43,7 @@
|
|||||||
* Conference, Aug 2004. AIAA-2004-4900.
|
* Conference, Aug 2004. AIAA-2004-4900.
|
||||||
*
|
*
|
||||||
* Original implementation for total energy control class:
|
* Original implementation for total energy control class:
|
||||||
* Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
|
* Paul Riseborough, ECL Library, 2017
|
||||||
*
|
*
|
||||||
* More details and acknowledgements in the referenced library headers.
|
* More details and acknowledgements in the referenced library headers.
|
||||||
*
|
*
|
||||||
@@ -66,7 +66,7 @@
|
|||||||
|
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
#include <ecl/l1/ecl_l1_pos_controller.h>
|
#include <ecl/l1/ecl_l1_pos_controller.h>
|
||||||
#include <external_lgpl/tecs/tecs.h>
|
#include <ecl/tecs/tecs.h>
|
||||||
#include <geo/geo.h>
|
#include <geo/geo.h>
|
||||||
#include <launchdetection/LaunchDetector.h>
|
#include <launchdetection/LaunchDetector.h>
|
||||||
#include <mathlib/mathlib.h>
|
#include <mathlib/mathlib.h>
|
||||||
|
|||||||
@@ -40,7 +40,6 @@ px4_add_module(
|
|||||||
DEPENDS
|
DEPENDS
|
||||||
platforms__common
|
platforms__common
|
||||||
git_ecl
|
git_ecl
|
||||||
lib__external_lgpl
|
|
||||||
lib__ecl
|
lib__ecl
|
||||||
)
|
)
|
||||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||||
|
|||||||
Reference in New Issue
Block a user