mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-24 07:09:48 +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
|
||||
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.
|
||||
|
||||
@@ -117,7 +117,6 @@ set(config_module_list
|
||||
lib/mathlib/math/filter
|
||||
lib/rc
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/conversion
|
||||
|
||||
@@ -137,7 +137,6 @@ set(config_module_list
|
||||
lib/mathlib
|
||||
lib/mathlib/math/filter
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/conversion
|
||||
|
||||
@@ -85,7 +85,6 @@ set(config_module_list
|
||||
lib/mathlib/math/filter
|
||||
lib/rc
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/conversion
|
||||
|
||||
@@ -142,7 +142,6 @@ set(config_module_list
|
||||
lib/mathlib/math/filter
|
||||
lib/rc
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/conversion
|
||||
|
||||
@@ -119,7 +119,6 @@ set(config_module_list
|
||||
lib/mathlib/math/filter
|
||||
lib/rc
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/conversion
|
||||
|
||||
@@ -40,7 +40,6 @@ set(config_module_list
|
||||
lib/mathlib
|
||||
lib/mathlib/math/filter
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/conversion
|
||||
lib/version
|
||||
|
||||
@@ -137,7 +137,6 @@ set(config_module_list
|
||||
lib/mathlib
|
||||
lib/mathlib/math/filter
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/conversion
|
||||
|
||||
@@ -133,7 +133,6 @@ set(config_module_list
|
||||
lib/mathlib
|
||||
lib/mathlib/math/filter
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/conversion
|
||||
|
||||
@@ -145,7 +145,6 @@ set(config_module_list
|
||||
lib/conversion
|
||||
lib/DriverFramework/framework
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/launchdetection
|
||||
|
||||
@@ -147,7 +147,6 @@ set(config_module_list
|
||||
lib/mathlib/math/filter
|
||||
lib/rc
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/conversion
|
||||
|
||||
@@ -146,7 +146,6 @@ set(config_module_list
|
||||
lib/conversion
|
||||
lib/DriverFramework/framework
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/launchdetection
|
||||
|
||||
@@ -146,7 +146,6 @@ set(config_module_list
|
||||
lib/conversion
|
||||
lib/DriverFramework/framework
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/launchdetection
|
||||
|
||||
@@ -125,7 +125,6 @@ set(config_module_list
|
||||
lib/mathlib/math/filter
|
||||
lib/rc
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/conversion
|
||||
|
||||
@@ -91,7 +91,6 @@ set(config_module_list
|
||||
lib/mathlib
|
||||
lib/mathlib/math/filter
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/conversion
|
||||
|
||||
@@ -80,7 +80,6 @@ set(config_module_list
|
||||
lib/ecl
|
||||
lib/geo_lookup
|
||||
lib/launchdetection
|
||||
lib/external_lgpl
|
||||
lib/conversion
|
||||
lib/terrain_estimation
|
||||
lib/runway_takeoff
|
||||
|
||||
@@ -85,7 +85,6 @@ set(config_module_list
|
||||
lib/ecl
|
||||
lib/geo_lookup
|
||||
lib/launchdetection
|
||||
lib/external_lgpl
|
||||
lib/conversion
|
||||
lib/terrain_estimation
|
||||
lib/runway_takeoff
|
||||
|
||||
@@ -84,7 +84,6 @@ set(config_module_list
|
||||
lib/ecl
|
||||
lib/geo_lookup
|
||||
lib/launchdetection
|
||||
lib/external_lgpl
|
||||
lib/conversion
|
||||
lib/terrain_estimation
|
||||
lib/runway_takeoff
|
||||
|
||||
@@ -101,7 +101,6 @@ set(config_module_list
|
||||
lib/geo_lookup
|
||||
lib/launchdetection
|
||||
lib/led
|
||||
lib/external_lgpl
|
||||
lib/conversion
|
||||
lib/terrain_estimation
|
||||
lib/runway_takeoff
|
||||
|
||||
@@ -121,7 +121,6 @@ set(config_module_list
|
||||
lib/conversion
|
||||
lib/DriverFramework/framework
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
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
|
||||
platforms__common
|
||||
git_ecl
|
||||
lib__external_lgpl
|
||||
lib__ecl
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
|
||||
@@ -43,7 +43,7 @@
|
||||
* Conference, Aug 2004. AIAA-2004-4900.
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
@@ -66,7 +66,7 @@
|
||||
|
||||
#include <drivers/drv_hrt.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 <launchdetection/LaunchDetector.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
@@ -40,7 +40,6 @@ px4_add_module(
|
||||
DEPENDS
|
||||
platforms__common
|
||||
git_ecl
|
||||
lib__external_lgpl
|
||||
lib__ecl
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
|
||||
Reference in New Issue
Block a user