diff --git a/LICENSE.md b/LICENSE.md
index 075d48c4529158d9bd8bf792cb745c5d6ab5de90..5a11a895e07bc37fb2cacc99ae58d8d583cf06d9 100644
--- a/LICENSE.md
+++ b/LICENSE.md
@@ -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.
diff --git a/cmake/configs/nuttx_aerocore2_default.cmake b/cmake/configs/nuttx_aerocore2_default.cmake
index c2ed66b898bf89c2ceb9f9d8328d653f42e12a8c..92954df56ab565ad66a7713da5ff2cc587728991 100644
--- a/cmake/configs/nuttx_aerocore2_default.cmake
+++ b/cmake/configs/nuttx_aerocore2_default.cmake
@@ -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
diff --git a/cmake/configs/nuttx_auav-x21_default.cmake b/cmake/configs/nuttx_auav-x21_default.cmake
index 34c7700aceba3a1f0dee6755951838363c746d96..a1c619a0da48bdf88e8b45ef985a41a907cc27dd 100644
--- a/cmake/configs/nuttx_auav-x21_default.cmake
+++ b/cmake/configs/nuttx_auav-x21_default.cmake
@@ -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
diff --git a/cmake/configs/nuttx_crazyflie_default.cmake b/cmake/configs/nuttx_crazyflie_default.cmake
index 9a5ad86ec2db552ca3eaea2757d90b167b88093b..9da7097d1ebc390c21e4bc87095a8896048cd3d5 100644
--- a/cmake/configs/nuttx_crazyflie_default.cmake
+++ b/cmake/configs/nuttx_crazyflie_default.cmake
@@ -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
diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake
index c354da6d2f11c9c738384accd3cbbfa20d0b5be6..ff7468fdb938e9f19f51e92fc9e0c23d8179d45f 100644
--- a/cmake/configs/nuttx_mindpx-v2_default.cmake
+++ b/cmake/configs/nuttx_mindpx-v2_default.cmake
@@ -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
diff --git a/cmake/configs/nuttx_px4-same70xplained-v1_default.cmake b/cmake/configs/nuttx_px4-same70xplained-v1_default.cmake
index f07b27350de2db7884450d28bb0f9576a4f7d23a..1efffbed1492f8d9f6d9a2ba1cb57cb67ae76377 100644
--- a/cmake/configs/nuttx_px4-same70xplained-v1_default.cmake
+++ b/cmake/configs/nuttx_px4-same70xplained-v1_default.cmake
@@ -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
diff --git a/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
index 7d4110ef05d9705cfeb4a1e13b0b653525665550..735d1f2d50c064704e4146395dd659783fe19c8b 100644
--- a/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
+++ b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
@@ -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
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 50cb205e5bcb1a9e45fdd8d19e5fcd0b25df6f82..cff7c243ba9e8b2c6ed62553338e51bcd6b9b469 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -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
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index 59d16b2d66e07b96ee6cb5f221de35cb8483a7ed..5871b120cf26bde465088a0ec608708472bf8bf2 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -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
diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake
index 76dfee191be4c7676dfdcc63290b76a4ac260224..9dc94c1de143b65310dd3d15d54cd9ae0e4df3a7 100644
--- a/cmake/configs/nuttx_px4fmu-v3_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake
@@ -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
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index 28fa0efa57820e7a849d388e1ee9a95bd3ca39d2..63f316cb7edac19050e46533192c74b49615a9c9 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -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
diff --git a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
index fc54b471c6fe17fe37cb52e4c2449a4670f7f4b2..2acd6a322967a9eab61569c0d55ab5d9b7732de9 100644
--- a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
@@ -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
diff --git a/cmake/configs/nuttx_px4fmu-v5_default.cmake b/cmake/configs/nuttx_px4fmu-v5_default.cmake
index f6f70824f24dd008c3f9cd15d5c7ed6bfd37754a..f4d3165aee4d42ec4e00d50d4cd5f53a9f0c8036 100644
--- a/cmake/configs/nuttx_px4fmu-v5_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v5_default.cmake
@@ -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
diff --git a/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake b/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake
index cbf98a728a03eb0e4ad36d053b349d9af1beb204..2e8fec7d02d492a08a19aca274de712ad5fa5b5b 100644
--- a/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake
+++ b/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake
@@ -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
diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake
index 717641a45df6bc38a0f796add5d1d93575eb1fdc..932097bdeecf7266c0b18764b0403dab4d8265a7 100644
--- a/cmake/configs/nuttx_tap-v1_default.cmake
+++ b/cmake/configs/nuttx_tap-v1_default.cmake
@@ -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
diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index 3fa4c44142acf8ec8ac89be8daed15688f1f48ae..38899b0e0be0733a2c8b070579d49b39b1208a78 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -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
diff --git a/cmake/configs/posix_ocpoc_cross.cmake b/cmake/configs/posix_ocpoc_cross.cmake
index 9eda20cc3e15ff02a178cf2b24a5e3ed526ad5bb..0e2bf2a606edc14162bbcc3d6a7ffe3d0260f125 100644
--- a/cmake/configs/posix_ocpoc_cross.cmake
+++ b/cmake/configs/posix_ocpoc_cross.cmake
@@ -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
diff --git a/cmake/configs/posix_ocpoc_ubuntu.cmake b/cmake/configs/posix_ocpoc_ubuntu.cmake
index 87762770ee924c536165bc697de08fcc5e1f097f..1674d8f6e0c2f1638444e9a4047bf66fd5090968 100644
--- a/cmake/configs/posix_ocpoc_ubuntu.cmake
+++ b/cmake/configs/posix_ocpoc_ubuntu.cmake
@@ -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
diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake
index 10a711189a8e7708dc0061ed5c1a252b814ca7d1..b0adeabfa488759c5027f1c5efaa19d2d13c6085 100644
--- a/cmake/configs/posix_rpi_common.cmake
+++ b/cmake/configs/posix_rpi_common.cmake
@@ -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
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index c9836ab8e726b06a815bbdc0df8e3dd9a02dea5d..475c5b6e229edc6c5591a2b95e68426f5c728f12 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -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
diff --git a/cmake/configs/posix_sitl_replay.cmake b/cmake/configs/posix_sitl_replay.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..83960ac41b7b2f78e90c9f039f938ecf7e7981d1
--- /dev/null
+++ b/cmake/configs/posix_sitl_replay.cmake
@@ -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")
diff --git a/src/lib/ecl b/src/lib/ecl
index 61e0c0481130ec6f07240298bc6a6aff5feb8418..2bae55753dc3cb83ae8bd92be5dcc76f27cf8948 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 61e0c0481130ec6f07240298bc6a6aff5feb8418
+Subproject commit 2bae55753dc3cb83ae8bd92be5dcc76f27cf8948
diff --git a/src/lib/external_lgpl/CMakeLists.txt b/src/lib/external_lgpl/CMakeLists.txt
deleted file mode 100644
index 396a41ce154e1fd13d642cc7cb0c7e0b4656f3e5..0000000000000000000000000000000000000000
--- a/src/lib/external_lgpl/CMakeLists.txt
+++ /dev/null
@@ -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 :
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
deleted file mode 100644
index 37019a479e8605ed3555db977a8c0c5b64a7e2cc..0000000000000000000000000000000000000000
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ /dev/null
@@ -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;
-
-}
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
deleted file mode 100644
index 522c7a29ab47e93f5fc27d6c7c45d8e3ee73f183..0000000000000000000000000000000000000000
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ /dev/null
@@ -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
diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt
index 340ef90aac800ba07355ded101fb82638523b80a..20b6fadbe6cdf6f1f1ead53d6f7e66447d1091bf 100644
--- a/src/modules/fw_pos_control_l1/CMakeLists.txt
+++ b/src/modules/fw_pos_control_l1/CMakeLists.txt
@@ -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 :
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
index b24740962335c75f14bec6c5537d6a4d0bed3ed4..3753a2603ea379bf2b8ab637c46b76ad87603ec7 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
@@ -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>
diff --git a/src/modules/gnd_pos_control/CMakeLists.txt b/src/modules/gnd_pos_control/CMakeLists.txt
index 89a10ebf5e752818a219a7607eb2b7eca23516c6..df6257a0d9aa962e39a3cdda2d3eaa1c2fed53d9 100644
--- a/src/modules/gnd_pos_control/CMakeLists.txt
+++ b/src/modules/gnd_pos_control/CMakeLists.txt
@@ -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 :