diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 6454c9e40c..e7e5b33c43 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -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) diff --git a/src/lib/l1/CMakeLists.txt b/src/lib/l1/CMakeLists.txt deleted file mode 100644 index 66e03d2358..0000000000 --- a/src/lib/l1/CMakeLists.txt +++ /dev/null @@ -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) diff --git a/src/lib/l1/ECL_L1_Pos_Controller.cpp b/src/lib/l1/ECL_L1_Pos_Controller.cpp deleted file mode 100644 index ed6fead918..0000000000 --- a/src/lib/l1/ECL_L1_Pos_Controller.cpp +++ /dev/null @@ -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 - -#include - -#include - -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; -} diff --git a/src/lib/l1/ECL_L1_Pos_Controller.hpp b/src/lib/l1/ECL_L1_Pos_Controller.hpp deleted file mode 100644 index 505e9944ce..0000000000 --- a/src/lib/l1/ECL_L1_Pos_Controller.hpp +++ /dev/null @@ -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 -#include - -/** - * 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 */