mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-24 19:43:35 +08:00
move ecl L1, TECS, and data validator to PX4/Firmware
This commit is contained in:
Daniel Agar
@@ -48,8 +48,10 @@ add_subdirectory(drivers)
|
||||
add_subdirectory(ecl)
|
||||
add_subdirectory(flight_tasks)
|
||||
add_subdirectory(hysteresis)
|
||||
add_subdirectory(l1)
|
||||
add_subdirectory(landing_slope)
|
||||
add_subdirectory(led)
|
||||
add_subdirectory(mag_compensation)
|
||||
add_subdirectory(mathlib)
|
||||
add_subdirectory(mixer)
|
||||
add_subdirectory(mixer_module)
|
||||
@@ -58,8 +60,8 @@ add_subdirectory(perf)
|
||||
add_subdirectory(pid)
|
||||
add_subdirectory(rc)
|
||||
add_subdirectory(systemlib)
|
||||
add_subdirectory(tecs)
|
||||
add_subdirectory(terrain_estimation)
|
||||
add_subdirectory(tunes)
|
||||
add_subdirectory(version)
|
||||
add_subdirectory(weather_vane)
|
||||
add_subdirectory(mag_compensation)
|
||||
|
||||
+1
-1
Submodule src/lib/ecl updated: a296fe7d8c...a8bb8ea99f
@@ -0,0 +1,40 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2018-2020 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_library(l1
|
||||
ECL_L1_Pos_Controller.cpp
|
||||
ECL_L1_Pos_Controller.hpp
|
||||
)
|
||||
|
||||
add_dependencies(l1 git_ecl)
|
||||
target_link_libraries(l1 PRIVATE ecl_geo)
|
||||
@@ -0,0 +1,388 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2020 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ECL_L1_Pos_Controller.cpp
|
||||
* Implementation of L1 position control.
|
||||
* Authors and acknowledgements in header.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "ECL_L1_Pos_Controller.hpp"
|
||||
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
#include <float.h>
|
||||
|
||||
using matrix::Vector2f;
|
||||
using matrix::wrap_pi;
|
||||
|
||||
void ECL_L1_Pos_Controller::update_roll_setpoint()
|
||||
{
|
||||
float roll_new = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
|
||||
roll_new = math::constrain(roll_new, -_roll_lim_rad, _roll_lim_rad);
|
||||
|
||||
if (_dt > 0.0f && _roll_slew_rate > 0.0f) {
|
||||
// slew rate limiting active
|
||||
roll_new = math::constrain(roll_new, _roll_setpoint - _roll_slew_rate * _dt, _roll_setpoint + _roll_slew_rate * _dt);
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(roll_new)) {
|
||||
_roll_setpoint = roll_new;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
|
||||
{
|
||||
/* following [2], switching on L1 distance */
|
||||
return math::min(wp_radius, _L1_distance);
|
||||
}
|
||||
|
||||
void
|
||||
ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B,
|
||||
const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector)
|
||||
{
|
||||
/* this follows the logic presented in [1] */
|
||||
float eta = 0.0f;
|
||||
float xtrack_vel = 0.0f;
|
||||
float ltrack_vel = 0.0f;
|
||||
|
||||
/* get the direction between the last (visited) and next waypoint */
|
||||
_target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1),
|
||||
(double)vector_B(0), (double)vector_B(1));
|
||||
|
||||
/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
|
||||
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
|
||||
|
||||
/* calculate the L1 length required for the desired period */
|
||||
_L1_distance = _L1_ratio * ground_speed;
|
||||
|
||||
/* calculate vector from A to B */
|
||||
Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
|
||||
|
||||
/*
|
||||
* check if waypoints are on top of each other. If yes,
|
||||
* skip A and directly continue to B
|
||||
*/
|
||||
if (vector_AB.length() < 1.0e-6f) {
|
||||
vector_AB = get_local_planar_vector(vector_curr_position, vector_B);
|
||||
}
|
||||
|
||||
vector_AB.normalize();
|
||||
|
||||
/* calculate the vector from waypoint A to the aircraft */
|
||||
Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
|
||||
|
||||
/* calculate crosstrack error (output only) */
|
||||
_crosstrack_error = vector_AB % vector_A_to_airplane;
|
||||
|
||||
/*
|
||||
* If the current position is in a +-135 degree angle behind waypoint A
|
||||
* and further away from A than the L1 distance, then A becomes the L1 point.
|
||||
* If the aircraft is already between A and B normal L1 logic is applied.
|
||||
*/
|
||||
float distance_A_to_airplane = vector_A_to_airplane.length();
|
||||
float alongTrackDist = vector_A_to_airplane * vector_AB;
|
||||
|
||||
/* estimate airplane position WRT to B */
|
||||
Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
|
||||
|
||||
/* calculate angle of airplane position vector relative to line) */
|
||||
|
||||
// XXX this could probably also be based solely on the dot product
|
||||
float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB);
|
||||
|
||||
/* extension from [2], fly directly to A */
|
||||
if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane, 1.0f) < -0.7071f) {
|
||||
|
||||
/* calculate eta to fly to waypoint A */
|
||||
|
||||
/* unit vector from waypoint A to current position */
|
||||
Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
|
||||
/* velocity across / orthogonal to line */
|
||||
xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
|
||||
/* velocity along line */
|
||||
ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
|
||||
eta = atan2f(xtrack_vel, ltrack_vel);
|
||||
/* bearing from current position to L1 point */
|
||||
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
|
||||
|
||||
/*
|
||||
* If the AB vector and the vector from B to airplane point in the same
|
||||
* direction, we have missed the waypoint. At +- 90 degrees we are just passing it.
|
||||
*/
|
||||
|
||||
} else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) {
|
||||
/*
|
||||
* Extension, fly back to waypoint.
|
||||
*
|
||||
* This corner case is possible if the system was following
|
||||
* the AB line from waypoint A to waypoint B, then is
|
||||
* switched to manual mode (or otherwise misses the waypoint)
|
||||
* and behind the waypoint continues to follow the AB line.
|
||||
*/
|
||||
|
||||
/* calculate eta to fly to waypoint B */
|
||||
|
||||
/* velocity across / orthogonal to line */
|
||||
xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
|
||||
/* velocity along line */
|
||||
ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
|
||||
eta = atan2f(xtrack_vel, ltrack_vel);
|
||||
/* bearing from current position to L1 point */
|
||||
_nav_bearing = atan2f(-vector_B_to_P_unit(1), -vector_B_to_P_unit(0));
|
||||
|
||||
} else {
|
||||
|
||||
/* calculate eta to fly along the line between A and B */
|
||||
|
||||
/* velocity across / orthogonal to line */
|
||||
xtrack_vel = ground_speed_vector % vector_AB;
|
||||
/* velocity along line */
|
||||
ltrack_vel = ground_speed_vector * vector_AB;
|
||||
/* calculate eta2 (angle of velocity vector relative to line) */
|
||||
float eta2 = atan2f(xtrack_vel, ltrack_vel);
|
||||
/* calculate eta1 (angle to L1 point) */
|
||||
float xtrackErr = vector_A_to_airplane % vector_AB;
|
||||
float sine_eta1 = xtrackErr / math::max(_L1_distance, 0.1f);
|
||||
/* limit output to 45 degrees */
|
||||
sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
|
||||
float eta1 = asinf(sine_eta1);
|
||||
eta = eta1 + eta2;
|
||||
/* bearing from current position to L1 point */
|
||||
_nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;
|
||||
|
||||
}
|
||||
|
||||
/* limit angle to +-90 degrees */
|
||||
eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
|
||||
_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
|
||||
|
||||
/* flying to waypoints, not circling them */
|
||||
_circle_mode = false;
|
||||
|
||||
/* the bearing angle, in NED frame */
|
||||
_bearing_error = eta;
|
||||
|
||||
update_roll_setpoint();
|
||||
}
|
||||
|
||||
void
|
||||
ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius,
|
||||
int8_t loiter_direction, const Vector2f &ground_speed_vector)
|
||||
{
|
||||
/* the complete guidance logic in this section was proposed by [2] */
|
||||
|
||||
/* calculate the gains for the PD loop (circle tracking) */
|
||||
float omega = (2.0f * M_PI_F / _L1_period);
|
||||
float K_crosstrack = omega * omega;
|
||||
float K_velocity = 2.0f * _L1_damping * omega;
|
||||
|
||||
/* update bearing to next waypoint */
|
||||
_target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1),
|
||||
(double)vector_A(0), (double)vector_A(1));
|
||||
|
||||
/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
|
||||
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
|
||||
|
||||
/* calculate the L1 length required for the desired period */
|
||||
_L1_distance = _L1_ratio * ground_speed;
|
||||
|
||||
/* calculate the vector from waypoint A to current position */
|
||||
Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
|
||||
|
||||
Vector2f vector_A_to_airplane_unit;
|
||||
|
||||
/* prevent NaN when normalizing */
|
||||
if (vector_A_to_airplane.length() > FLT_EPSILON) {
|
||||
/* store the normalized vector from waypoint A to current position */
|
||||
vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
|
||||
|
||||
} else {
|
||||
vector_A_to_airplane_unit = vector_A_to_airplane;
|
||||
}
|
||||
|
||||
/* calculate eta angle towards the loiter center */
|
||||
|
||||
/* velocity across / orthogonal to line from waypoint to current position */
|
||||
float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector;
|
||||
/* velocity along line from waypoint to current position */
|
||||
float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit);
|
||||
float eta = atan2f(xtrack_vel_center, ltrack_vel_center);
|
||||
/* limit eta to 90 degrees */
|
||||
eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f);
|
||||
|
||||
/* calculate the lateral acceleration to capture the center point */
|
||||
float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
|
||||
|
||||
/* for PD control: Calculate radial position and velocity errors */
|
||||
|
||||
/* radial velocity error */
|
||||
float xtrack_vel_circle = -ltrack_vel_center;
|
||||
/* radial distance from the loiter circle (not center) */
|
||||
float xtrack_err_circle = vector_A_to_airplane.length() - radius;
|
||||
|
||||
/* cross track error for feedback */
|
||||
_crosstrack_error = xtrack_err_circle;
|
||||
|
||||
/* calculate PD update to circle waypoint */
|
||||
float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);
|
||||
|
||||
/* calculate velocity on circle / along tangent */
|
||||
float tangent_vel = xtrack_vel_center * loiter_direction;
|
||||
|
||||
/* prevent PD output from turning the wrong way */
|
||||
if (tangent_vel < 0.0f) {
|
||||
lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd, 0.0f);
|
||||
}
|
||||
|
||||
/* calculate centripetal acceleration setpoint */
|
||||
float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius),
|
||||
(radius + xtrack_err_circle));
|
||||
|
||||
/* add PD control on circle and centripetal acceleration for total circle command */
|
||||
float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal);
|
||||
|
||||
/*
|
||||
* Switch between circle (loiter) and capture (towards waypoint center) mode when
|
||||
* the commands switch over. Only fly towards waypoint if outside the circle.
|
||||
*/
|
||||
|
||||
// XXX check switch over
|
||||
if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) ||
|
||||
(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
|
||||
_lateral_accel = lateral_accel_sp_center;
|
||||
_circle_mode = false;
|
||||
/* angle between requested and current velocity vector */
|
||||
_bearing_error = eta;
|
||||
/* bearing from current position to L1 point */
|
||||
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
|
||||
|
||||
} else {
|
||||
_lateral_accel = lateral_accel_sp_circle;
|
||||
_circle_mode = true;
|
||||
_bearing_error = 0.0f;
|
||||
/* bearing from current position to L1 point */
|
||||
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
|
||||
}
|
||||
|
||||
update_roll_setpoint();
|
||||
}
|
||||
|
||||
void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading,
|
||||
const Vector2f &ground_speed_vector)
|
||||
{
|
||||
/* the complete guidance logic in this section was proposed by [2] */
|
||||
|
||||
/*
|
||||
* As the commanded heading is the only reference
|
||||
* (and no crosstrack correction occurs),
|
||||
* target and navigation bearing become the same
|
||||
*/
|
||||
_target_bearing = _nav_bearing = wrap_pi(navigation_heading);
|
||||
|
||||
float eta = wrap_pi(_target_bearing - wrap_pi(current_heading));
|
||||
|
||||
/* consequently the bearing error is exactly eta: */
|
||||
_bearing_error = eta;
|
||||
|
||||
/* ground speed is the length of the ground speed vector */
|
||||
float ground_speed = ground_speed_vector.length();
|
||||
|
||||
/* adjust L1 distance to keep constant frequency */
|
||||
_L1_distance = ground_speed / _heading_omega;
|
||||
float omega_vel = ground_speed * _heading_omega;
|
||||
|
||||
/* not circling a waypoint */
|
||||
_circle_mode = false;
|
||||
|
||||
/* navigating heading means by definition no crosstrack error */
|
||||
_crosstrack_error = 0;
|
||||
|
||||
/* limit eta to 90 degrees */
|
||||
eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
|
||||
_lateral_accel = 2.0f * sinf(eta) * omega_vel;
|
||||
|
||||
update_roll_setpoint();
|
||||
}
|
||||
|
||||
void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
|
||||
{
|
||||
/* the logic in this section is trivial, but originally proposed by [2] */
|
||||
|
||||
/* reset all heading / error measures resulting in zero roll */
|
||||
_target_bearing = current_heading;
|
||||
_nav_bearing = current_heading;
|
||||
_bearing_error = 0;
|
||||
_crosstrack_error = 0;
|
||||
_lateral_accel = 0;
|
||||
|
||||
/* not circling a waypoint when flying level */
|
||||
_circle_mode = false;
|
||||
|
||||
update_roll_setpoint();
|
||||
}
|
||||
|
||||
Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2f &origin, const Vector2f &target) const
|
||||
{
|
||||
/* this is an approximation for small angles, proposed by [2] */
|
||||
Vector2f out(math::radians((target(0) - origin(0))),
|
||||
math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
|
||||
|
||||
return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
|
||||
}
|
||||
|
||||
void ECL_L1_Pos_Controller::set_l1_period(float period)
|
||||
{
|
||||
_L1_period = period;
|
||||
|
||||
/* calculate the ratio introduced in [2] */
|
||||
_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
|
||||
|
||||
/* calculate normalized frequency for heading tracking */
|
||||
_heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period;
|
||||
}
|
||||
|
||||
void ECL_L1_Pos_Controller::set_l1_damping(float damping)
|
||||
{
|
||||
_L1_damping = damping;
|
||||
|
||||
/* calculate the ratio introduced in [2] */
|
||||
_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
|
||||
|
||||
/* calculate the L1 gain (following [2]) */
|
||||
_K_L1 = 4.0f * _L1_damping * _L1_damping;
|
||||
}
|
||||
@@ -0,0 +1,249 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2020 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ecl_l1_pos_control.h
|
||||
* Implementation of L1 position control.
|
||||
*
|
||||
*
|
||||
* Acknowledgements and References:
|
||||
*
|
||||
* This implementation has been built for PX4 based on the original
|
||||
* publication from [1] and does include a lot of the ideas (not code)
|
||||
* from [2].
|
||||
*
|
||||
*
|
||||
* [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
|
||||
* Proceedings of the AIAA Guidance, Navigation and Control
|
||||
* Conference, Aug 2004. AIAA-2004-4900.
|
||||
*
|
||||
* [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013.
|
||||
* - Explicit control over frequency and damping
|
||||
* - Explicit control over track capture angle
|
||||
* - Ability to use loiter radius smaller than L1 length
|
||||
* - Modified to use PD control for circle tracking to enable loiter radius less than L1 length
|
||||
* - Modified to enable period and damping of guidance loop to be set explicitly
|
||||
* - Modified to provide explicit control over capture angle
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef ECL_L1_POS_CONTROLLER_H
|
||||
#define ECL_L1_POS_CONTROLLER_H
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
/**
|
||||
* L1 Nonlinear Guidance Logic
|
||||
*/
|
||||
class ECL_L1_Pos_Controller
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* The current target bearing
|
||||
*
|
||||
* @return bearing angle (-pi..pi, in NED frame)
|
||||
*/
|
||||
float nav_bearing() { return matrix::wrap_pi(_nav_bearing); }
|
||||
|
||||
/**
|
||||
* Get lateral acceleration demand.
|
||||
*
|
||||
* @return Lateral acceleration in m/s^2
|
||||
*/
|
||||
float nav_lateral_acceleration_demand() { return _lateral_accel; }
|
||||
|
||||
/**
|
||||
* Heading error.
|
||||
*
|
||||
* The heading error is either compared to the current track
|
||||
* or to the tangent of the current loiter radius.
|
||||
*/
|
||||
float bearing_error() { return _bearing_error; }
|
||||
|
||||
/**
|
||||
* Bearing from aircraft to current target.
|
||||
*
|
||||
* @return bearing angle (-pi..pi, in NED frame)
|
||||
*/
|
||||
float target_bearing() { return _target_bearing; }
|
||||
|
||||
/**
|
||||
* Get roll angle setpoint for fixed wing.
|
||||
*
|
||||
* @return Roll angle (in NED frame)
|
||||
*/
|
||||
float get_roll_setpoint() { return _roll_setpoint; }
|
||||
|
||||
/**
|
||||
* Get the current crosstrack error.
|
||||
*
|
||||
* @return Crosstrack error in meters.
|
||||
*/
|
||||
float crosstrack_error() { return _crosstrack_error; }
|
||||
|
||||
/**
|
||||
* Returns true if the loiter waypoint has been reached
|
||||
*/
|
||||
bool reached_loiter_target() { return _circle_mode; }
|
||||
|
||||
/**
|
||||
* Returns true if following a circle (loiter)
|
||||
*/
|
||||
bool circle_mode() { return _circle_mode; }
|
||||
|
||||
/**
|
||||
* Get the switch distance
|
||||
*
|
||||
* This is the distance at which the system will
|
||||
* switch to the next waypoint. This depends on the
|
||||
* period and damping
|
||||
*
|
||||
* @param waypoint_switch_radius The switching radius the waypoint has set.
|
||||
*/
|
||||
float switch_distance(float waypoint_switch_radius);
|
||||
|
||||
/**
|
||||
* Navigate between two waypoints
|
||||
*
|
||||
* Calling this function with two waypoints results in the
|
||||
* control outputs to fly to the line segment defined by
|
||||
* the points and once captured following the line segment.
|
||||
* This follows the logic in [1].
|
||||
*
|
||||
* @return sets _lateral_accel setpoint
|
||||
*/
|
||||
void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B,
|
||||
const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed);
|
||||
|
||||
/**
|
||||
* Navigate on an orbit around a loiter waypoint.
|
||||
*
|
||||
* This allow orbits smaller than the L1 length,
|
||||
* this modification was introduced in [2].
|
||||
*
|
||||
* @return sets _lateral_accel setpoint
|
||||
*/
|
||||
void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius,
|
||||
int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector);
|
||||
|
||||
/**
|
||||
* Navigate on a fixed bearing.
|
||||
*
|
||||
* This only holds a certain direction and does not perform cross
|
||||
* track correction. Helpful for semi-autonomous modes. Introduced
|
||||
* by [2].
|
||||
*
|
||||
* @return sets _lateral_accel setpoint
|
||||
*/
|
||||
void navigate_heading(float navigation_heading, float current_heading, const matrix::Vector2f &ground_speed);
|
||||
|
||||
/**
|
||||
* Keep the wings level.
|
||||
*
|
||||
* This is typically needed for maximum-lift-demand situations,
|
||||
* such as takeoff or near stall. Introduced in [2].
|
||||
*/
|
||||
void navigate_level_flight(float current_heading);
|
||||
|
||||
/**
|
||||
* Set the L1 period.
|
||||
*/
|
||||
void set_l1_period(float period);
|
||||
|
||||
/**
|
||||
* Set the L1 damping factor.
|
||||
*
|
||||
* The original publication recommends a default of sqrt(2) / 2 = 0.707
|
||||
*/
|
||||
void set_l1_damping(float damping);
|
||||
|
||||
/**
|
||||
* Set the maximum roll angle output in radians
|
||||
*/
|
||||
void set_l1_roll_limit(float roll_lim_rad) { _roll_lim_rad = roll_lim_rad; }
|
||||
|
||||
/**
|
||||
* Set roll angle slew rate. Set to zero to deactivate.
|
||||
*/
|
||||
void set_roll_slew_rate(float roll_slew_rate) { _roll_slew_rate = roll_slew_rate; }
|
||||
|
||||
/**
|
||||
* Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting.
|
||||
*/
|
||||
void set_dt(float dt) { _dt = dt;}
|
||||
|
||||
private:
|
||||
|
||||
float _lateral_accel{0.0f}; ///< Lateral acceleration setpoint in m/s^2
|
||||
float _L1_distance{20.0f}; ///< L1 lead distance, defined by period and damping
|
||||
bool _circle_mode{false}; ///< flag for loiter mode
|
||||
float _nav_bearing{0.0f}; ///< bearing to L1 reference point
|
||||
float _bearing_error{0.0f}; ///< bearing error
|
||||
float _crosstrack_error{0.0f}; ///< crosstrack error in meters
|
||||
float _target_bearing{0.0f}; ///< the heading setpoint
|
||||
|
||||
float _L1_period{25.0f}; ///< L1 tracking period in seconds
|
||||
float _L1_damping{0.75f}; ///< L1 damping ratio
|
||||
float _L1_ratio{5.0f}; ///< L1 ratio for navigation
|
||||
float _K_L1{2.0f}; ///< L1 control gain for _L1_damping
|
||||
float _heading_omega{1.0f}; ///< Normalized frequency
|
||||
|
||||
float _roll_lim_rad{math::radians(30.0f)}; ///<maximum roll angle in radians
|
||||
float _roll_setpoint{0.0f}; ///< current roll angle setpoint in radians
|
||||
float _roll_slew_rate{0.0f}; ///< roll angle setpoint slew rate limit in rad/s
|
||||
float _dt{0}; ///< control loop time in seconds
|
||||
|
||||
/**
|
||||
* Convert a 2D vector from WGS84 to planar coordinates.
|
||||
*
|
||||
* This converts from latitude and longitude to planar
|
||||
* coordinates with (0,0) being at the position of ref and
|
||||
* returns a vector in meters towards wp.
|
||||
*
|
||||
* @param ref The reference position in WGS84 coordinates
|
||||
* @param wp The point to convert to into the local coordinates, in WGS84 coordinates
|
||||
* @return The vector in meters pointing from the reference position to the coordinates
|
||||
*/
|
||||
matrix::Vector2f get_local_planar_vector(const matrix::Vector2f &origin, const matrix::Vector2f &target) const;
|
||||
|
||||
/**
|
||||
* Update roll angle setpoint. This will also apply slew rate limits if set.
|
||||
*
|
||||
*/
|
||||
void update_roll_setpoint();
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif /* ECL_L1_POS_CONTROLLER_H */
|
||||
@@ -0,0 +1,40 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2018-2020 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_library(tecs
|
||||
TECS.cpp
|
||||
TECS.hpp
|
||||
)
|
||||
|
||||
add_dependencies(tecs git_ecl)
|
||||
target_link_libraries(tecs PRIVATE ecl_geo)
|
||||
@@ -0,0 +1,609 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2020 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "TECS.hpp"
|
||||
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
using math::constrain;
|
||||
using math::max;
|
||||
using math::min;
|
||||
|
||||
static constexpr float DT_MIN = 0.001f; ///< minimum allowed value of _dt (sec)
|
||||
static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a filter state reset is performed (sec)
|
||||
|
||||
/**
|
||||
* @file TECS.cpp
|
||||
*
|
||||
* @author Paul Riseborough
|
||||
*/
|
||||
|
||||
/*
|
||||
* This function implements a complementary filter to estimate the climb rate when
|
||||
* inertial nav data is not available. It also calculates a true airspeed derivative
|
||||
* which is used by the airspeed complimentary filter.
|
||||
*/
|
||||
void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat,
|
||||
const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air,
|
||||
float altitude, float vz)
|
||||
{
|
||||
// calculate the time lapsed since the last update
|
||||
uint64_t now = hrt_absolute_time();
|
||||
float dt = constrain((now - _state_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX);
|
||||
|
||||
bool reset_altitude = false;
|
||||
|
||||
if (_state_update_timestamp == 0 || dt > DT_MAX) {
|
||||
dt = DT_DEFAULT;
|
||||
reset_altitude = true;
|
||||
}
|
||||
|
||||
if (!altitude_lock || !in_air) {
|
||||
reset_altitude = true;
|
||||
}
|
||||
|
||||
if (reset_altitude) {
|
||||
_states_initialized = false;
|
||||
}
|
||||
|
||||
_state_update_timestamp = now;
|
||||
_EAS = airspeed;
|
||||
|
||||
_in_air = in_air;
|
||||
|
||||
// Set the velocity and position state to the the INS data
|
||||
_vert_vel_state = -vz;
|
||||
_vert_pos_state = altitude;
|
||||
|
||||
// Update and average speed rate of change if airspeed is being measured
|
||||
if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) {
|
||||
// Assuming the vehicle is flying X axis forward, use the X axis measured acceleration
|
||||
// compensated for gravity to estimate the rate of change of speed
|
||||
float speed_deriv_raw = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0);
|
||||
|
||||
// Apply some noise filtering
|
||||
_speed_derivative = 0.95f * _speed_derivative + 0.05f * speed_deriv_raw;
|
||||
|
||||
} else {
|
||||
_speed_derivative = 0.0f;
|
||||
}
|
||||
|
||||
if (!_in_air) {
|
||||
_states_initialized = false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspeed, float EAS2TAS)
|
||||
{
|
||||
// Calculate the time in seconds since the last update and use the default time step value if out of bounds
|
||||
uint64_t now = hrt_absolute_time();
|
||||
const float dt = constrain((now - _speed_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX);
|
||||
|
||||
// Convert equivalent airspeed quantities to true airspeed
|
||||
_EAS_setpoint = airspeed_setpoint;
|
||||
_TAS_setpoint = _EAS_setpoint * EAS2TAS;
|
||||
_TAS_max = _indicated_airspeed_max * EAS2TAS;
|
||||
_TAS_min = _indicated_airspeed_min * EAS2TAS;
|
||||
|
||||
// If airspeed measurements are not being used, fix the airspeed estimate to halfway between
|
||||
// min and max limits
|
||||
if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) {
|
||||
_EAS = 0.5f * (_indicated_airspeed_min + _indicated_airspeed_max);
|
||||
|
||||
} else {
|
||||
_EAS = indicated_airspeed;
|
||||
}
|
||||
|
||||
// If first time through or not flying, reset airspeed states
|
||||
if (_speed_update_timestamp == 0 || !_in_air) {
|
||||
_tas_rate_state = 0.0f;
|
||||
_tas_state = (_EAS * EAS2TAS);
|
||||
}
|
||||
|
||||
// Obtain a smoothed airspeed estimate using a second order complementary filter
|
||||
|
||||
// Update TAS rate state
|
||||
float tas_error = (_EAS * EAS2TAS) - _tas_state;
|
||||
float tas_rate_state_input = tas_error * _tas_estimate_freq * _tas_estimate_freq;
|
||||
|
||||
// limit integrator input to prevent windup
|
||||
if (_tas_state < 3.1f) {
|
||||
tas_rate_state_input = max(tas_rate_state_input, 0.0f);
|
||||
|
||||
}
|
||||
|
||||
// Update TAS state
|
||||
_tas_rate_state = _tas_rate_state + tas_rate_state_input * dt;
|
||||
float tas_state_input = _tas_rate_state + _speed_derivative + tas_error * _tas_estimate_freq * 1.4142f;
|
||||
_tas_state = _tas_state + tas_state_input * dt;
|
||||
|
||||
// Limit the airspeed state to a minimum of 3 m/s
|
||||
_tas_state = max(_tas_state, 3.0f);
|
||||
_speed_update_timestamp = now;
|
||||
|
||||
}
|
||||
|
||||
void TECS::_update_speed_setpoint()
|
||||
{
|
||||
// Set the airspeed demand to the minimum value if an underspeed or
|
||||
// or a uncontrolled descent condition exists to maximise climb rate
|
||||
if ((_uncommanded_descent_recovery) || (_underspeed_detected)) {
|
||||
_TAS_setpoint = _TAS_min;
|
||||
}
|
||||
|
||||
_TAS_setpoint = constrain(_TAS_setpoint, _TAS_min, _TAS_max);
|
||||
|
||||
// Calculate limits for the demanded rate of change of speed based on physical performance limits
|
||||
// with a 50% margin to allow the total energy controller to correct for errors.
|
||||
float velRateMax = 0.5f * _STE_rate_max / _tas_state;
|
||||
float velRateMin = 0.5f * _STE_rate_min / _tas_state;
|
||||
|
||||
_TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max);
|
||||
|
||||
// calculate the demanded rate of change of speed proportional to speed error
|
||||
// and apply performance limits
|
||||
_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _speed_error_gain, velRateMin, velRateMax);
|
||||
|
||||
}
|
||||
|
||||
void TECS::_update_height_setpoint(float desired, float state)
|
||||
{
|
||||
// Detect first time through and initialize previous value to demand
|
||||
if (PX4_ISFINITE(desired) && fabsf(_hgt_setpoint_in_prev) < 0.1f) {
|
||||
_hgt_setpoint_in_prev = desired;
|
||||
}
|
||||
|
||||
// Apply a 2 point moving average to demanded height to reduce
|
||||
// intersampling noise effects.
|
||||
if (PX4_ISFINITE(desired)) {
|
||||
_hgt_setpoint = 0.5f * (desired + _hgt_setpoint_in_prev);
|
||||
|
||||
} else {
|
||||
_hgt_setpoint = _hgt_setpoint_in_prev;
|
||||
}
|
||||
|
||||
_hgt_setpoint_in_prev = _hgt_setpoint;
|
||||
|
||||
// Apply a rate limit to respect vehicle performance limitations
|
||||
if ((_hgt_setpoint - _hgt_setpoint_prev) > (_max_climb_rate * _dt)) {
|
||||
_hgt_setpoint = _hgt_setpoint_prev + _max_climb_rate * _dt;
|
||||
|
||||
} else if ((_hgt_setpoint - _hgt_setpoint_prev) < (-_max_sink_rate * _dt)) {
|
||||
_hgt_setpoint = _hgt_setpoint_prev - _max_sink_rate * _dt;
|
||||
}
|
||||
|
||||
_hgt_setpoint_prev = _hgt_setpoint;
|
||||
|
||||
// Apply a first order noise filter
|
||||
_hgt_setpoint_adj = 0.1f * _hgt_setpoint + 0.9f * _hgt_setpoint_adj_prev;
|
||||
|
||||
// Calculate the demanded climb rate proportional to height error plus a feedforward term to provide
|
||||
// tight tracking during steady climb and descent manoeuvres.
|
||||
_hgt_rate_setpoint = (_hgt_setpoint_adj - state) * _height_error_gain + _height_setpoint_gain_ff *
|
||||
(_hgt_setpoint_adj - _hgt_setpoint_adj_prev) / _dt;
|
||||
_hgt_setpoint_adj_prev = _hgt_setpoint_adj;
|
||||
|
||||
// Limit the rate of change of height demand to respect vehicle performance limits
|
||||
if (_hgt_rate_setpoint > _max_climb_rate) {
|
||||
_hgt_rate_setpoint = _max_climb_rate;
|
||||
|
||||
} else if (_hgt_rate_setpoint < -_max_sink_rate) {
|
||||
_hgt_rate_setpoint = -_max_sink_rate;
|
||||
}
|
||||
}
|
||||
|
||||
void TECS::_detect_underspeed()
|
||||
{
|
||||
if (!_detect_underspeed_enabled) {
|
||||
_underspeed_detected = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (((_tas_state < _TAS_min * 0.9f) && (_throttle_setpoint >= _throttle_setpoint_max * 0.95f))
|
||||
|| ((_vert_pos_state < _hgt_setpoint_adj) && _underspeed_detected)) {
|
||||
|
||||
_underspeed_detected = true;
|
||||
|
||||
} else {
|
||||
_underspeed_detected = false;
|
||||
}
|
||||
}
|
||||
|
||||
void TECS::_update_energy_estimates()
|
||||
{
|
||||
// Calculate specific energy demands in units of (m**2/sec**2)
|
||||
_SPE_setpoint = _hgt_setpoint_adj * CONSTANTS_ONE_G; // potential energy
|
||||
_SKE_setpoint = 0.5f * _TAS_setpoint_adj * _TAS_setpoint_adj; // kinetic energy
|
||||
|
||||
// Calculate specific energy rate demands in units of (m**2/sec**3)
|
||||
_SPE_rate_setpoint = _hgt_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change
|
||||
_SKE_rate_setpoint = _tas_state * _TAS_rate_setpoint; // kinetic energy rate of change
|
||||
|
||||
// Calculate specific energies in units of (m**2/sec**2)
|
||||
_SPE_estimate = _vert_pos_state * CONSTANTS_ONE_G; // potential energy
|
||||
_SKE_estimate = 0.5f * _tas_state * _tas_state; // kinetic energy
|
||||
|
||||
// Calculate specific energy rates in units of (m**2/sec**3)
|
||||
_SPE_rate = _vert_vel_state * CONSTANTS_ONE_G; // potential energy rate of change
|
||||
_SKE_rate = _tas_state * _speed_derivative;// kinetic energy rate of change
|
||||
}
|
||||
|
||||
void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::Dcmf &rotMat)
|
||||
{
|
||||
// Calculate total energy error
|
||||
_STE_error = _SPE_setpoint - _SPE_estimate + _SKE_setpoint - _SKE_estimate;
|
||||
|
||||
// Calculate demanded rate of change of total energy, respecting vehicle limits
|
||||
float STE_rate_setpoint = constrain((_SPE_rate_setpoint + _SKE_rate_setpoint), _STE_rate_min, _STE_rate_max);
|
||||
|
||||
// Calculate the total energy rate error, applying a first order IIR filter
|
||||
// to reduce the effect of accelerometer noise
|
||||
_STE_rate_error = 0.2f * (STE_rate_setpoint - _SPE_rate - _SKE_rate) + 0.8f * _STE_rate_error;
|
||||
|
||||
// Calculate the throttle demand
|
||||
if (_underspeed_detected) {
|
||||
// always use full throttle to recover from an underspeed condition
|
||||
_throttle_setpoint = 1.0f;
|
||||
|
||||
} else {
|
||||
// Adjust the demanded total energy rate to compensate for induced drag rise in turns.
|
||||
// Assume induced drag scales linearly with normal load factor.
|
||||
// The additional normal load factor is given by (1/cos(bank angle) - 1)
|
||||
float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
|
||||
STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (1.0f / constrain(cosPhi, 0.1f, 1.0f) - 1.0f);
|
||||
|
||||
// Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle
|
||||
// as the starting point. Assume:
|
||||
// Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max
|
||||
// Specific total energy rate = 0 at cruise throttle
|
||||
// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
|
||||
float throttle_predicted = 0.0f;
|
||||
|
||||
if (STE_rate_setpoint >= 0) {
|
||||
// throttle is between cruise and maximum
|
||||
throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max - throttle_cruise);
|
||||
|
||||
} else {
|
||||
// throttle is between cruise and minimum
|
||||
throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min - throttle_cruise);
|
||||
|
||||
}
|
||||
|
||||
// Calculate gain scaler from specific energy error to throttle
|
||||
float STE_to_throttle = 1.0f / (_throttle_time_constant * (_STE_rate_max - _STE_rate_min));
|
||||
|
||||
// Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits
|
||||
_throttle_setpoint = (_STE_error + _STE_rate_error * _throttle_damping_gain) * STE_to_throttle + throttle_predicted;
|
||||
_throttle_setpoint = constrain(_throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
|
||||
|
||||
// Rate limit the throttle demand
|
||||
if (fabsf(_throttle_slewrate) > 0.01f) {
|
||||
float throttle_increment_limit = _dt * (_throttle_setpoint_max - _throttle_setpoint_min) * _throttle_slewrate;
|
||||
_throttle_setpoint = constrain(_throttle_setpoint, _last_throttle_setpoint - throttle_increment_limit,
|
||||
_last_throttle_setpoint + throttle_increment_limit);
|
||||
}
|
||||
|
||||
_last_throttle_setpoint = _throttle_setpoint;
|
||||
|
||||
if (_integrator_gain > 0.0f) {
|
||||
// Calculate throttle integrator state upper and lower limits with allowance for
|
||||
// 10% throttle saturation to accommodate noise on the demand.
|
||||
float integ_state_max = _throttle_setpoint_max - _throttle_setpoint + 0.1f;
|
||||
float integ_state_min = _throttle_setpoint_min - _throttle_setpoint - 0.1f;
|
||||
|
||||
// Calculate a throttle demand from the integrated total energy error
|
||||
// This will be added to the total throttle demand to compensate for steady state errors
|
||||
_throttle_integ_state = _throttle_integ_state + (_STE_error * _integrator_gain) * _dt * STE_to_throttle;
|
||||
|
||||
if (_climbout_mode_active) {
|
||||
// During climbout, set the integrator to maximum throttle to prevent transient throttle drop
|
||||
// at end of climbout when we transition to closed loop throttle control
|
||||
_throttle_integ_state = integ_state_max;
|
||||
|
||||
} else {
|
||||
// Respect integrator limits during closed loop operation.
|
||||
_throttle_integ_state = constrain(_throttle_integ_state, integ_state_min, integ_state_max);
|
||||
}
|
||||
|
||||
} else {
|
||||
_throttle_integ_state = 0.0f;
|
||||
}
|
||||
|
||||
if (airspeed_sensor_enabled()) {
|
||||
// Add the integrator feedback during closed loop operation with an airspeed sensor
|
||||
_throttle_setpoint = _throttle_setpoint + _throttle_integ_state;
|
||||
|
||||
} else {
|
||||
// when flying without an airspeed sensor, use the predicted throttle only
|
||||
_throttle_setpoint = throttle_predicted;
|
||||
|
||||
}
|
||||
|
||||
_throttle_setpoint = constrain(_throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
|
||||
}
|
||||
}
|
||||
|
||||
void TECS::_detect_uncommanded_descent()
|
||||
{
|
||||
/*
|
||||
* This function detects a condition that can occur when the demanded airspeed is greater than the
|
||||
* aircraft can achieve in level flight. When this occurs, the vehicle will continue to reduce height
|
||||
* while attempting to maintain speed.
|
||||
*/
|
||||
|
||||
// Calculate rate of change of total specific energy
|
||||
float STE_rate = _SPE_rate + _SKE_rate;
|
||||
|
||||
// If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode
|
||||
bool enter_mode = !_uncommanded_descent_recovery && !_underspeed_detected && (_STE_error > 200.0f) && (STE_rate < 0.0f)
|
||||
&& (_throttle_setpoint >= _throttle_setpoint_max * 0.9f);
|
||||
|
||||
// If we enter an underspeed condition or recover the required total energy, then exit uncommanded descent recovery mode
|
||||
bool exit_mode = _uncommanded_descent_recovery && (_underspeed_detected || (_STE_error < 0.0f));
|
||||
|
||||
if (enter_mode) {
|
||||
_uncommanded_descent_recovery = true;
|
||||
|
||||
} else if (exit_mode) {
|
||||
_uncommanded_descent_recovery = false;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void TECS::_update_pitch_setpoint()
|
||||
{
|
||||
/*
|
||||
* The SKE_weighting variable controls how speed and height control are prioritised by the pitch demand calculation.
|
||||
* A weighting of 1 givea equal speed and height priority
|
||||
* A weighting of 0 gives 100% priority to height control and must be used when no airspeed measurement is available.
|
||||
* A weighting of 2 provides 100% priority to speed control and is used when:
|
||||
* a) an underspeed condition is detected.
|
||||
* b) during climbout where a minimum pitch angle has been set to ensure height is gained. If the airspeed
|
||||
* rises above the demanded value, the pitch angle demand is increased by the TECS controller to prevent the vehicle overspeeding.
|
||||
* The weighting can be adjusted between 0 and 2 depending on speed and height accuracy requirements.
|
||||
*/
|
||||
|
||||
// Calculate the weighting applied to control of specific kinetic energy error
|
||||
float SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f);
|
||||
|
||||
if ((_underspeed_detected || _climbout_mode_active) && airspeed_sensor_enabled()) {
|
||||
SKE_weighting = 2.0f;
|
||||
|
||||
} else if (!airspeed_sensor_enabled()) {
|
||||
SKE_weighting = 0.0f;
|
||||
}
|
||||
|
||||
// Calculate the weighting applied to control of specific potential energy error
|
||||
float SPE_weighting = 2.0f - SKE_weighting;
|
||||
|
||||
// Calculate the specific energy balance demand which specifies how the available total
|
||||
// energy should be allocated to speed (kinetic energy) and height (potential energy)
|
||||
float SEB_setpoint = _SPE_setpoint * SPE_weighting - _SKE_setpoint * SKE_weighting;
|
||||
|
||||
// Calculate the specific energy balance rate demand
|
||||
float SEB_rate_setpoint = _SPE_rate_setpoint * SPE_weighting - _SKE_rate_setpoint * SKE_weighting;
|
||||
|
||||
// Calculate the specific energy balance and balance rate error
|
||||
_SEB_error = SEB_setpoint - (_SPE_estimate * SPE_weighting - _SKE_estimate * SKE_weighting);
|
||||
_SEB_rate_error = SEB_rate_setpoint - (_SPE_rate * SPE_weighting - _SKE_rate * SKE_weighting);
|
||||
|
||||
// Calculate derivative from change in climb angle to rate of change of specific energy balance
|
||||
float climb_angle_to_SEB_rate = _tas_state * _pitch_time_constant * CONSTANTS_ONE_G;
|
||||
|
||||
if (_integrator_gain > 0.0f) {
|
||||
// Calculate pitch integrator input term
|
||||
float pitch_integ_input = _SEB_error * _integrator_gain;
|
||||
|
||||
// Prevent the integrator changing in a direction that will increase pitch demand saturation
|
||||
// Decay the integrator at the control loop time constant if the pitch demand from the previous time step is saturated
|
||||
if (_pitch_setpoint_unc > _pitch_setpoint_max) {
|
||||
pitch_integ_input = min(pitch_integ_input,
|
||||
min((_pitch_setpoint_max - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f));
|
||||
|
||||
} else if (_pitch_setpoint_unc < _pitch_setpoint_min) {
|
||||
pitch_integ_input = max(pitch_integ_input,
|
||||
max((_pitch_setpoint_min - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f));
|
||||
}
|
||||
|
||||
// Update the pitch integrator state.
|
||||
_pitch_integ_state = _pitch_integ_state + pitch_integ_input * _dt;
|
||||
|
||||
} else {
|
||||
_pitch_integ_state = 0.0f;
|
||||
}
|
||||
|
||||
// Calculate a specific energy correction that doesn't include the integrator contribution
|
||||
float SEB_correction = _SEB_error + _SEB_rate_error * _pitch_damping_gain + SEB_rate_setpoint * _pitch_time_constant;
|
||||
|
||||
// During climbout, bias the demanded pitch angle so that a zero speed error produces a pitch angle
|
||||
// demand equal to the minimum pitch angle set by the mission plan. This prevents the integrator
|
||||
// having to catch up before the nose can be raised to reduce excess speed during climbout.
|
||||
if (_climbout_mode_active) {
|
||||
SEB_correction += _pitch_setpoint_min * climb_angle_to_SEB_rate;
|
||||
}
|
||||
|
||||
// Sum the correction terms and convert to a pitch angle demand. This calculation assumes:
|
||||
// a) The climb angle follows pitch angle with a lag that is small enough not to destabilise the control loop.
|
||||
// b) The offset between climb angle and pitch angle (angle of attack) is constant, excluding the effect of
|
||||
// pitch transients due to control action or turbulence.
|
||||
_pitch_setpoint_unc = (SEB_correction + _pitch_integ_state) / climb_angle_to_SEB_rate;
|
||||
_pitch_setpoint = constrain(_pitch_setpoint_unc, _pitch_setpoint_min, _pitch_setpoint_max);
|
||||
|
||||
// Comply with the specified vertical acceleration limit by applying a pitch rate limit
|
||||
float ptchRateIncr = _dt * _vert_accel_limit / _tas_state;
|
||||
|
||||
if ((_pitch_setpoint - _last_pitch_setpoint) > ptchRateIncr) {
|
||||
_pitch_setpoint = _last_pitch_setpoint + ptchRateIncr;
|
||||
|
||||
} else if ((_pitch_setpoint - _last_pitch_setpoint) < -ptchRateIncr) {
|
||||
_pitch_setpoint = _last_pitch_setpoint - ptchRateIncr;
|
||||
}
|
||||
|
||||
_last_pitch_setpoint = _pitch_setpoint;
|
||||
}
|
||||
|
||||
void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout,
|
||||
float EAS2TAS)
|
||||
{
|
||||
if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initialized) {
|
||||
// On first time through or when not using TECS of if there has been a large time slip,
|
||||
// states must be reset to allow filters to a clean start
|
||||
_vert_vel_state = 0.0f;
|
||||
_vert_pos_state = baro_altitude;
|
||||
_tas_rate_state = 0.0f;
|
||||
_tas_state = _EAS * EAS2TAS;
|
||||
_throttle_integ_state = 0.0f;
|
||||
_pitch_integ_state = 0.0f;
|
||||
_last_throttle_setpoint = (_in_air ? throttle_cruise : 0.0f);;
|
||||
_last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max);
|
||||
_pitch_setpoint_unc = _last_pitch_setpoint;
|
||||
_hgt_setpoint_adj_prev = baro_altitude;
|
||||
_hgt_setpoint_adj = _hgt_setpoint_adj_prev;
|
||||
_hgt_setpoint_prev = _hgt_setpoint_adj_prev;
|
||||
_hgt_setpoint_in_prev = _hgt_setpoint_adj_prev;
|
||||
_TAS_setpoint_last = _EAS * EAS2TAS;
|
||||
_TAS_setpoint_adj = _TAS_setpoint_last;
|
||||
_underspeed_detected = false;
|
||||
_uncommanded_descent_recovery = false;
|
||||
_STE_rate_error = 0.0f;
|
||||
|
||||
if (_dt > DT_MAX || _dt < DT_MIN) {
|
||||
_dt = DT_DEFAULT;
|
||||
}
|
||||
|
||||
} else if (_climbout_mode_active) {
|
||||
// During climbout use the lower pitch angle limit specified by the
|
||||
// calling controller
|
||||
_pitch_setpoint_min = pitch_min_climbout;
|
||||
|
||||
// throttle lower limit is set to a value that prevents throttle reduction
|
||||
_throttle_setpoint_min = _throttle_setpoint_max - 0.01f;
|
||||
|
||||
// height demand and associated states are set to track the measured height
|
||||
_hgt_setpoint_adj_prev = baro_altitude;
|
||||
_hgt_setpoint_adj = _hgt_setpoint_adj_prev;
|
||||
_hgt_setpoint_prev = _hgt_setpoint_adj_prev;
|
||||
|
||||
// airspeed demand states are set to track the measured airspeed
|
||||
_TAS_setpoint_last = _EAS * EAS2TAS;
|
||||
_TAS_setpoint_adj = _EAS * EAS2TAS;
|
||||
|
||||
// disable speed and decent error condition checks
|
||||
_underspeed_detected = false;
|
||||
_uncommanded_descent_recovery = false;
|
||||
}
|
||||
|
||||
_states_initialized = true;
|
||||
}
|
||||
|
||||
void TECS::_update_STE_rate_lim()
|
||||
{
|
||||
// Calculate the specific total energy upper rate limits from the max throttle climb rate
|
||||
_STE_rate_max = _max_climb_rate * CONSTANTS_ONE_G;
|
||||
|
||||
// Calculate the specific total energy lower rate limits from the min throttle sink rate
|
||||
_STE_rate_min = - _min_sink_rate * CONSTANTS_ONE_G;
|
||||
}
|
||||
|
||||
void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint,
|
||||
float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
|
||||
float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max)
|
||||
{
|
||||
// Calculate the time since last update (seconds)
|
||||
uint64_t now = hrt_absolute_time();
|
||||
_dt = constrain((now - _pitch_update_timestamp) * 1e-6f, DT_MIN, DT_MAX);
|
||||
|
||||
// Set class variables from inputs
|
||||
_throttle_setpoint_max = throttle_max;
|
||||
_throttle_setpoint_min = throttle_min;
|
||||
_pitch_setpoint_max = pitch_limit_max;
|
||||
_pitch_setpoint_min = pitch_limit_min;
|
||||
_climbout_mode_active = climb_out_setpoint;
|
||||
|
||||
// Initialize selected states and variables as required
|
||||
_initialize_states(pitch, throttle_cruise, baro_altitude, pitch_min_climbout, eas_to_tas);
|
||||
|
||||
// Don't run TECS control algorithms when not in flight
|
||||
if (!_in_air) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Update the true airspeed state estimate
|
||||
_update_speed_states(EAS_setpoint, indicated_airspeed, eas_to_tas);
|
||||
|
||||
// Calculate rate limits for specific total energy
|
||||
_update_STE_rate_lim();
|
||||
|
||||
// Detect an underspeed condition
|
||||
_detect_underspeed();
|
||||
|
||||
// Detect an uncommanded descent caused by an unachievable airspeed demand
|
||||
_detect_uncommanded_descent();
|
||||
|
||||
// Calculate the demanded true airspeed
|
||||
_update_speed_setpoint();
|
||||
|
||||
// Calculate the demanded height
|
||||
_update_height_setpoint(hgt_setpoint, baro_altitude);
|
||||
|
||||
// Calculate the specific energy values required by the control loop
|
||||
_update_energy_estimates();
|
||||
|
||||
// Calculate the throttle demand
|
||||
_update_throttle_setpoint(throttle_cruise, rotMat);
|
||||
|
||||
// Calculate the pitch demand
|
||||
_update_pitch_setpoint();
|
||||
|
||||
// Update time stamps
|
||||
_pitch_update_timestamp = now;
|
||||
|
||||
// Set TECS mode for next frame
|
||||
if (_underspeed_detected) {
|
||||
_tecs_mode = ECL_TECS_MODE_UNDERSPEED;
|
||||
|
||||
} else if (_uncommanded_descent_recovery) {
|
||||
_tecs_mode = ECL_TECS_MODE_BAD_DESCENT;
|
||||
|
||||
} else if (_climbout_mode_active) {
|
||||
_tecs_mode = ECL_TECS_MODE_CLIMBOUT;
|
||||
|
||||
} else {
|
||||
// This is the default operation mode
|
||||
_tecs_mode = ECL_TECS_MODE_NORMAL;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,327 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2020 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file tecs.cpp
|
||||
*
|
||||
* @author Paul Riseborough
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
class TECS
|
||||
{
|
||||
public:
|
||||
TECS() = default;
|
||||
~TECS() = default;
|
||||
|
||||
// no copy, assignment, move, move assignment
|
||||
TECS(const TECS &) = delete;
|
||||
TECS &operator=(const TECS &) = delete;
|
||||
TECS(TECS &&) = delete;
|
||||
TECS &operator=(TECS &&) = delete;
|
||||
|
||||
/**
|
||||
* Get the current airspeed status
|
||||
*
|
||||
* @return true if airspeed is enabled for control
|
||||
*/
|
||||
bool airspeed_sensor_enabled() { return _airspeed_enabled; }
|
||||
|
||||
/**
|
||||
* Set the airspeed enable state
|
||||
*/
|
||||
void enable_airspeed(bool enabled) { _airspeed_enabled = enabled; }
|
||||
|
||||
/**
|
||||
* Updates the following vehicle kineamtic state estimates:
|
||||
* Vertical position, velocity and acceleration.
|
||||
* Speed derivative
|
||||
* Must be called prior to udating tecs control loops
|
||||
* Must be called at 50Hz or greater
|
||||
*/
|
||||
void update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat,
|
||||
const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air,
|
||||
float altitude, float vz);
|
||||
|
||||
/**
|
||||
* Update the control loop calculations
|
||||
*/
|
||||
void update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint,
|
||||
float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
|
||||
float throttle_min, float throttle_setpoint_max, float throttle_cruise,
|
||||
float pitch_limit_min, float pitch_limit_max);
|
||||
|
||||
float get_throttle_setpoint() { return _throttle_setpoint; }
|
||||
float get_pitch_setpoint() { return _pitch_setpoint; }
|
||||
float get_speed_weight() { return _pitch_speed_weight; }
|
||||
|
||||
void reset_state() { _states_initialized = false; }
|
||||
|
||||
enum ECL_TECS_MODE {
|
||||
ECL_TECS_MODE_NORMAL = 0,
|
||||
ECL_TECS_MODE_UNDERSPEED,
|
||||
ECL_TECS_MODE_BAD_DESCENT,
|
||||
ECL_TECS_MODE_CLIMBOUT
|
||||
};
|
||||
|
||||
void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; }
|
||||
|
||||
// setters for controller parameters
|
||||
void set_time_const(float time_const) { _pitch_time_constant = time_const; }
|
||||
void set_integrator_gain(float gain) { _integrator_gain = gain; }
|
||||
|
||||
void set_min_sink_rate(float rate) { _min_sink_rate = rate; }
|
||||
void set_max_sink_rate(float sink_rate) { _max_sink_rate = sink_rate; }
|
||||
void set_max_climb_rate(float climb_rate) { _max_climb_rate = climb_rate; }
|
||||
|
||||
void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; }
|
||||
void set_heightrate_p(float heightrate_p) { _height_error_gain = heightrate_p; }
|
||||
|
||||
void set_indicated_airspeed_max(float airspeed) { _indicated_airspeed_max = airspeed; }
|
||||
void set_indicated_airspeed_min(float airspeed) { _indicated_airspeed_min = airspeed; }
|
||||
|
||||
void set_pitch_damping(float damping) { _pitch_damping_gain = damping; }
|
||||
void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; }
|
||||
|
||||
void set_speed_comp_filter_omega(float omega) { _tas_estimate_freq = omega; }
|
||||
void set_speed_weight(float weight) { _pitch_speed_weight = weight; }
|
||||
void set_speedrate_p(float speedrate_p) { _speed_error_gain = speedrate_p; }
|
||||
|
||||
void set_time_const_throt(float time_const_throt) { _throttle_time_constant = time_const_throt; }
|
||||
void set_throttle_damp(float throttle_damp) { _throttle_damping_gain = throttle_damp; }
|
||||
void set_throttle_slewrate(float slewrate) { _throttle_slewrate = slewrate; }
|
||||
|
||||
void set_roll_throttle_compensation(float compensation) { _load_factor_correction = compensation; }
|
||||
|
||||
// TECS status
|
||||
uint64_t timestamp() { return _pitch_update_timestamp; }
|
||||
ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
|
||||
|
||||
float hgt_setpoint_adj() { return _hgt_setpoint_adj; }
|
||||
float vert_pos_state() { return _vert_pos_state; }
|
||||
|
||||
float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
|
||||
float tas_state() { return _tas_state; }
|
||||
|
||||
float hgt_rate_setpoint() { return _hgt_rate_setpoint; }
|
||||
float vert_vel_state() { return _vert_vel_state; }
|
||||
|
||||
float TAS_rate_setpoint() { return _TAS_rate_setpoint; }
|
||||
float speed_derivative() { return _speed_derivative; }
|
||||
|
||||
float STE_error() { return _STE_error; }
|
||||
float STE_rate_error() { return _STE_rate_error; }
|
||||
|
||||
float SEB_error() { return _SEB_error; }
|
||||
float SEB_rate_error() { return _SEB_rate_error; }
|
||||
|
||||
float throttle_integ_state() { return _throttle_integ_state; }
|
||||
float pitch_integ_state() { return _pitch_integ_state; }
|
||||
|
||||
/**
|
||||
* Handle the altitude reset
|
||||
*
|
||||
* If the estimation system resets the height in one discrete step this
|
||||
* will gracefully even out the reset over time.
|
||||
*/
|
||||
void handle_alt_step(float delta_alt, float altitude)
|
||||
{
|
||||
// add height reset delta to all variables involved
|
||||
// in filtering the demanded height
|
||||
_hgt_setpoint_in_prev += delta_alt;
|
||||
_hgt_setpoint_prev += delta_alt;
|
||||
_hgt_setpoint_adj_prev += delta_alt;
|
||||
|
||||
// reset height states
|
||||
_vert_pos_state = altitude;
|
||||
_vert_vel_state = 0.0f;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL};
|
||||
|
||||
// timestamps
|
||||
uint64_t _state_update_timestamp{0}; ///< last timestamp of the 50 Hz function call
|
||||
uint64_t _speed_update_timestamp{0}; ///< last timestamp of the speed function call
|
||||
uint64_t _pitch_update_timestamp{0}; ///< last timestamp of the pitch function call
|
||||
|
||||
// controller parameters
|
||||
float _tas_estimate_freq{0.0f}; ///< cross-over frequency of the true airspeed complementary filter (rad/sec)
|
||||
float _max_climb_rate{2.0f}; ///< climb rate produced by max allowed throttle (m/sec)
|
||||
float _min_sink_rate{1.0f}; ///< sink rate produced by min allowed throttle (m/sec)
|
||||
float _max_sink_rate{2.0f}; ///< maximum safe sink rate (m/sec)
|
||||
float _pitch_time_constant{5.0f}; ///< control time constant used by the pitch demand calculation (sec)
|
||||
float _throttle_time_constant{8.0f}; ///< control time constant used by the throttle demand calculation (sec)
|
||||
float _pitch_damping_gain{0.0f}; ///< damping gain of the pitch demand calculation (sec)
|
||||
float _throttle_damping_gain{0.0f}; ///< damping gain of the throttle demand calculation (sec)
|
||||
float _integrator_gain{0.0f}; ///< integrator gain used by the throttle and pitch demand calculation
|
||||
float _vert_accel_limit{0.0f}; ///< magnitude of the maximum vertical acceleration allowed (m/sec**2)
|
||||
float _load_factor_correction{0.0f}; ///< gain from normal load factor increase to total energy rate demand (m**2/sec**3)
|
||||
float _pitch_speed_weight{1.0f}; ///< speed control weighting used by pitch demand calculation
|
||||
float _height_error_gain{0.0f}; ///< gain from height error to demanded climb rate (1/sec)
|
||||
float _height_setpoint_gain_ff{0.0f}; ///< gain from height demand derivative to demanded climb rate
|
||||
float _speed_error_gain{0.0f}; ///< gain from speed error to demanded speed rate (1/sec)
|
||||
float _indicated_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec)
|
||||
float _indicated_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec)
|
||||
float _throttle_slewrate{0.0f}; ///< throttle demand slew rate limit (1/sec)
|
||||
|
||||
// controller outputs
|
||||
float _throttle_setpoint{0.0f}; ///< normalized throttle demand (0..1)
|
||||
float _pitch_setpoint{0.0f}; ///< pitch angle demand (radians)
|
||||
|
||||
// complimentary filter states
|
||||
float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec)
|
||||
float _vert_pos_state{0.0f}; ///< complimentary filter state - height (m)
|
||||
float _tas_rate_state{0.0f}; ///< complimentary filter state - true airspeed first derivative (m/sec**2)
|
||||
float _tas_state{0.0f}; ///< complimentary filter state - true airspeed (m/sec)
|
||||
|
||||
// controller states
|
||||
float _throttle_integ_state{0.0f}; ///< throttle integrator state
|
||||
float _pitch_integ_state{0.0f}; ///< pitch integrator state (rad)
|
||||
float _last_throttle_setpoint{0.0f}; ///< throttle demand rate limiter state (1/sec)
|
||||
float _last_pitch_setpoint{0.0f}; ///< pitch demand rate limiter state (rad/sec)
|
||||
float _speed_derivative{0.0f}; ///< rate of change of speed along X axis (m/sec**2)
|
||||
|
||||
// speed demand calculations
|
||||
float _EAS{0.0f}; ///< equivalent airspeed (m/sec)
|
||||
float _TAS_max{30.0f}; ///< true airpeed demand upper limit (m/sec)
|
||||
float _TAS_min{3.0f}; ///< true airpeed demand lower limit (m/sec)
|
||||
float _TAS_setpoint{0.0f}; ///< current airpeed demand (m/sec)
|
||||
float _TAS_setpoint_last{0.0f}; ///< previous true airpeed demand (m/sec)
|
||||
float _EAS_setpoint{0.0f}; ///< Equivalent airspeed demand (m/sec)
|
||||
float _TAS_setpoint_adj{0.0f}; ///< true airspeed demand tracked by the TECS algorithm (m/sec)
|
||||
float _TAS_rate_setpoint{0.0f}; ///< true airspeed rate demand tracked by the TECS algorithm (m/sec**2)
|
||||
|
||||
// height demand calculations
|
||||
float _hgt_setpoint{0.0f}; ///< demanded height tracked by the TECS algorithm (m)
|
||||
float _hgt_setpoint_in_prev{0.0f}; ///< previous value of _hgt_setpoint after noise filtering (m)
|
||||
float _hgt_setpoint_prev{0.0f}; ///< previous value of _hgt_setpoint after noise filtering and rate limiting (m)
|
||||
float _hgt_setpoint_adj{0.0f}; ///< demanded height used by the control loops after all filtering has been applied (m)
|
||||
float _hgt_setpoint_adj_prev{0.0f}; ///< value of _hgt_setpoint_adj from previous frame (m)
|
||||
float _hgt_rate_setpoint{0.0f}; ///< demanded climb rate tracked by the TECS algorithm
|
||||
|
||||
// vehicle physical limits
|
||||
float _pitch_setpoint_unc{0.0f}; ///< pitch demand before limiting (rad)
|
||||
float _STE_rate_max{0.0f}; ///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3)
|
||||
float _STE_rate_min{0.0f}; ///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3)
|
||||
float _throttle_setpoint_max{0.0f}; ///< normalised throttle upper limit
|
||||
float _throttle_setpoint_min{0.0f}; ///< normalised throttle lower limit
|
||||
float _pitch_setpoint_max{0.5f}; ///< pitch demand upper limit (rad)
|
||||
float _pitch_setpoint_min{-0.5f}; ///< pitch demand lower limit (rad)
|
||||
|
||||
// specific energy quantities
|
||||
float _SPE_setpoint{0.0f}; ///< specific potential energy demand (m**2/sec**2)
|
||||
float _SKE_setpoint{0.0f}; ///< specific kinetic energy demand (m**2/sec**2)
|
||||
float _SPE_rate_setpoint{0.0f}; ///< specific potential energy rate demand (m**2/sec**3)
|
||||
float _SKE_rate_setpoint{0.0f}; ///< specific kinetic energy rate demand (m**2/sec**3)
|
||||
float _SPE_estimate{0.0f}; ///< specific potential energy estimate (m**2/sec**2)
|
||||
float _SKE_estimate{0.0f}; ///< specific kinetic energy estimate (m**2/sec**2)
|
||||
float _SPE_rate{0.0f}; ///< specific potential energy rate estimate (m**2/sec**3)
|
||||
float _SKE_rate{0.0f}; ///< specific kinetic energy rate estimate (m**2/sec**3)
|
||||
|
||||
// specific energy error quantities
|
||||
float _STE_error{0.0f}; ///< specific total energy error (m**2/sec**2)
|
||||
float _STE_rate_error{0.0f}; ///< specific total energy rate error (m**2/sec**3)
|
||||
float _SEB_error{0.0f}; ///< specific energy balance error (m**2/sec**2)
|
||||
float _SEB_rate_error{0.0f}; ///< specific energy balance rate error (m**2/sec**3)
|
||||
|
||||
// time steps (non-fixed)
|
||||
float _dt{DT_DEFAULT}; ///< Time since last update of main TECS loop (sec)
|
||||
static constexpr float DT_DEFAULT = 0.02f; ///< default value for _dt (sec)
|
||||
|
||||
// controller mode logic
|
||||
bool _underspeed_detected{false}; ///< true when an underspeed condition has been detected
|
||||
bool _detect_underspeed_enabled{true}; ///< true when underspeed detection is enabled
|
||||
bool _uncommanded_descent_recovery{false}; ///< true when a continuous descent caused by an unachievable airspeed demand has been detected
|
||||
bool _climbout_mode_active{false}; ///< true when in climbout mode
|
||||
bool _airspeed_enabled{false}; ///< true when airspeed use has been enabled
|
||||
bool _states_initialized{false}; ///< true when TECS states have been iniitalized
|
||||
bool _in_air{false}; ///< true when the vehicle is flying
|
||||
|
||||
/**
|
||||
* Update the airspeed internal state using a second order complementary filter
|
||||
*/
|
||||
void _update_speed_states(float airspeed_setpoint, float indicated_airspeed, float eas_to_tas);
|
||||
|
||||
/**
|
||||
* Update the desired airspeed
|
||||
*/
|
||||
void _update_speed_setpoint();
|
||||
|
||||
/**
|
||||
* Update the desired height
|
||||
*/
|
||||
void _update_height_setpoint(float desired, float state);
|
||||
|
||||
/**
|
||||
* Detect if the system is not capable of maintaining airspeed
|
||||
*/
|
||||
void _detect_underspeed();
|
||||
|
||||
/**
|
||||
* Update specific energy
|
||||
*/
|
||||
void _update_energy_estimates();
|
||||
|
||||
/**
|
||||
* Update throttle setpoint
|
||||
*/
|
||||
void _update_throttle_setpoint(float throttle_cruise, const matrix::Dcmf &rotMat);
|
||||
|
||||
/**
|
||||
* Detect an uncommanded descent
|
||||
*/
|
||||
void _detect_uncommanded_descent();
|
||||
|
||||
/**
|
||||
* Update the pitch setpoint
|
||||
*/
|
||||
void _update_pitch_setpoint();
|
||||
|
||||
/**
|
||||
* Initialize the controller
|
||||
*/
|
||||
void _initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout,
|
||||
float eas_to_tas);
|
||||
|
||||
/**
|
||||
* Calculate specific total energy rate limits
|
||||
*/
|
||||
void _update_STE_rate_lim();
|
||||
|
||||
};
|
||||
Reference in New Issue
Block a user