mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-03-24 10:34:00 +08:00
lib: remove unused l1 library
This commit is contained in:
chfriedrich98
committed by
chfriedrich98
chfriedrich98
parent
e266d1f22f
commit
d2d3d8a5fd
@@ -52,7 +52,6 @@ add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(geo EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(heatshrink EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(hysteresis EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(l1 EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(lat_lon_alt EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(led EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(matrix EXCLUDE_FROM_ALL)
|
||||
|
||||
@@ -1,39 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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
|
||||
)
|
||||
|
||||
target_link_libraries(l1 PRIVATE geo)
|
||||
@@ -1,201 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2021 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/geo/geo.h>
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
#include <float.h>
|
||||
|
||||
using matrix::Vector2f;
|
||||
|
||||
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;
|
||||
|
||||
/* get the direction between the last (visited) and next waypoint */
|
||||
Vector2f vector_P_to_B = vector_B - vector_curr_position;
|
||||
Vector2f vector_P_to_B_unit = vector_P_to_B.normalized();
|
||||
_target_bearing = atan2f(vector_P_to_B_unit(1), vector_P_to_B_unit(0));
|
||||
|
||||
/* 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 = vector_B - vector_A;
|
||||
|
||||
/*
|
||||
* 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 = vector_B - vector_curr_position;
|
||||
}
|
||||
|
||||
vector_AB.normalize();
|
||||
|
||||
/* calculate the vector from waypoint A to the aircraft */
|
||||
Vector2f vector_A_to_airplane = vector_curr_position - vector_A;
|
||||
|
||||
/* 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 = vector_curr_position - vector_B;
|
||||
Vector2f vector_B_to_P_unit = vector_B_to_P.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 */
|
||||
float xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
|
||||
|
||||
/* velocity along line */
|
||||
float 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 */
|
||||
float xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
|
||||
|
||||
/* velocity along line */
|
||||
float 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 */
|
||||
float xtrack_vel = ground_speed_vector % vector_AB;
|
||||
|
||||
/* velocity along line */
|
||||
float 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 feasible values */
|
||||
sine_eta1 = math::constrain(sine_eta1, -1.0f, 1.0f);
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -1,141 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.hpp
|
||||
* 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; }
|
||||
|
||||
/**
|
||||
* Bearing from aircraft to current target.
|
||||
*
|
||||
* @return bearing angle (-pi..pi, in NED frame)
|
||||
*/
|
||||
float target_bearing() { return _target_bearing; }
|
||||
|
||||
/**
|
||||
* Get the current crosstrack error.
|
||||
*
|
||||
* @return Crosstrack error in meters.
|
||||
*/
|
||||
float crosstrack_error() { return _crosstrack_error; }
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
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
|
||||
float _nav_bearing{0.0f}; ///< bearing to L1 reference point
|
||||
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
|
||||
};
|
||||
|
||||
|
||||
#endif /* ECL_L1_POS_CONTROLLER_H */
|
||||
Reference in New Issue
Block a user