From 4e0c5f948902cbcd38eb71967e936b2526b8da72 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 20:26:50 +0200 Subject: Compile fixes, cleanups, better references --- src/lib/ecl/l1/ecl_l1_pos_control.cpp | 12 +++++----- src/lib/ecl/l1/ecl_l1_pos_control.h | 42 +++++++++++++++++++++++------------ 2 files changed, 34 insertions(+), 20 deletions(-) (limited to 'src/lib/ecl') diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.cpp b/src/lib/ecl/l1/ecl_l1_pos_control.cpp index 6f5065960..533409217 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_control.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_control.cpp @@ -6,9 +6,9 @@ * modification, are permitted provided that the following conditions * are met: * - * 1. Redistributions of source code must retain the above copyright + * 1. Redistributions of source code must retain the vector_ABove copyright * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright + * 2. Redistributions in binary form must reproduce the vector_ABove copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. @@ -18,14 +18,14 @@ * * 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 + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTvector_ABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIvector_ABLE 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 + * AND ON ANY THEORY OF LIvector_ABILITY, WHETHER IN CONTRACT, STRICT + * LIvector_ABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.h b/src/lib/ecl/l1/ecl_l1_pos_control.h index 661651f08..8bb94d3eb 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_control.h +++ b/src/lib/ecl/l1/ecl_l1_pos_control.h @@ -39,18 +39,22 @@ * * Acknowledgements and References: * - * Original publication: - * S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," + * 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. * - * Original navigation logic modified by Paul Riseborough 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 + * [2] Paul Riseborough 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 * */ @@ -79,6 +83,7 @@ public: */ float nav_bearing(); + /** * Get lateral acceleration demand. * @@ -86,8 +91,6 @@ public: */ float nav_lateral_acceleration_demand(); - // return the heading error angle +ve to left of track - /** * Heading error. @@ -146,6 +149,7 @@ public: * 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 */ @@ -156,6 +160,9 @@ public: /** * 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 math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, @@ -166,7 +173,8 @@ public: * Navigate on a fixed bearing. * * This only holds a certain direction and does not perform cross - * track correction. + * track correction. Helpful for semi-autonomous modes. Introduced + * by [2]. * * @return sets _lateral_accel setpoint */ @@ -174,7 +182,10 @@ public: /** - * Keep the wings level + * 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); @@ -184,6 +195,7 @@ public: */ void 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; @@ -197,14 +209,16 @@ public: */ void 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; } private: float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2 - float _L1_distance; ///< L1 lead distance, defined by period and damping + float _L1_distance; ///< L1 lead distance, defined by period and damping bool _circle_mode; ///< flag for loiter mode float _nav_bearing; ///< bearing to L1 reference point float _bearing_error; ///< bearing error -- cgit v1.2.3