From a9337c5565ccad12ec4ec87c4dbe3472a44af0fc Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Wed, 8 Jan 2025 15:37:21 +0300 Subject: [PATCH] refactored npfg into smaller classes Signed-off-by: RomanBapst --- src/lib/npfg/AirspeedReferenceController.cpp | 63 ++ src/lib/npfg/AirspeedReferenceController.hpp | 75 +++ src/lib/npfg/CMakeLists.txt | 8 +- src/lib/npfg/CourseToAirspeedRefMapper.cpp | 100 +++ src/lib/npfg/CourseToAirspeedRefMapper.hpp | 138 +++++ src/lib/npfg/DirectionalGuidance.cpp | 319 ++++++++++ src/lib/npfg/DirectionalGuidance.hpp | 300 +++++++++ src/lib/npfg/npfg.cpp | 508 --------------- src/lib/npfg/npfg.hpp | 616 ------------------- 9 files changed, 1001 insertions(+), 1126 deletions(-) create mode 100644 src/lib/npfg/AirspeedReferenceController.cpp create mode 100644 src/lib/npfg/AirspeedReferenceController.hpp create mode 100644 src/lib/npfg/CourseToAirspeedRefMapper.cpp create mode 100644 src/lib/npfg/CourseToAirspeedRefMapper.hpp create mode 100644 src/lib/npfg/DirectionalGuidance.cpp create mode 100644 src/lib/npfg/DirectionalGuidance.hpp delete mode 100644 src/lib/npfg/npfg.cpp delete mode 100644 src/lib/npfg/npfg.hpp diff --git a/src/lib/npfg/AirspeedReferenceController.cpp b/src/lib/npfg/AirspeedReferenceController.cpp new file mode 100644 index 0000000000..d40dbdd489 --- /dev/null +++ b/src/lib/npfg/AirspeedReferenceController.cpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "AirspeedReferenceController.hpp" +#include +#include + +using matrix::Vector2f; +AirspeedReferenceController::AirspeedReferenceController() +{ + // Constructor +} + +float AirspeedReferenceController::controlHeading(const float heading_sp, const float heading, + const float airspeed) const +{ + + const Vector2f airspeed_vector = Vector2f{cosf(heading), sinf(heading)} * airspeed; + const Vector2f airspeed_sp_vector_unit = Vector2f{cosf(heading_sp), sinf(heading_sp)}; + + const float dot_air_vel_err = airspeed_vector.dot(airspeed_sp_vector_unit); + const float cross_air_vel_err = airspeed_vector.cross(airspeed_sp_vector_unit); + + if (dot_air_vel_err < 0.0f) { + // hold max lateral acceleration command above 90 deg heading error + return p_gain_ * ((cross_air_vel_err < 0.0f) ? -airspeed : airspeed); + + } else { + // airspeed/airspeed_ref is used to scale any incremented airspeed reference back to the current airspeed + // for acceleration commands in a "feedback" sense (i.e. at the current vehicle airspeed) + return p_gain_ * cross_air_vel_err; + } +} diff --git a/src/lib/npfg/AirspeedReferenceController.hpp b/src/lib/npfg/AirspeedReferenceController.hpp new file mode 100644 index 0000000000..ed69ee514f --- /dev/null +++ b/src/lib/npfg/AirspeedReferenceController.hpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 AirspeedRefController.hpp + * + * Original Author: Thomas Stastny + * Refactored to better suite new control API: Roman Bapst + * + * * Notes: + * - The wind estimate should be dynamic enough to capture ~1-2 second length gusts, + * Otherwise the performance will suffer. + * + * Acknowledgements and References: + * + * The logic is mostly based on [1] and Paper III of [2]. + * TODO: Concise, up to date documentation and stability analysis for the following + * implementation. + * + * [1] T. Stastny and R. Siegwart. "On Flying Backwards: Preventing Run-away of + * Small, Low-speed, Fixed-wing UAVs in Strong Winds". IEEE International Conference + * on Intelligent Robots and Systems (IROS). 2019. + * https://arxiv.org/pdf/1908.01381.pdf + * [2] T. Stastny. "Low-Altitude Control and Local Re-Planning Strategies for Small + * Fixed-Wing UAVs". Doctoral Thesis, ETH Zürich. 2020. + * https://tstastny.github.io/pdf/tstastny_phd_thesis_wcover.pdf + */ + +#ifndef PX4_AIRSPEEDREFERENCECONTROLLER_HPP +#define PX4_AIRSPEEDREFERENCECONTROLLER_HPP + +class AirspeedReferenceController +{ +public: + + AirspeedReferenceController(); + + + float controlHeading(const float heading_sp, const float heading, const float airspeed) const; + +private: + float p_gain_{0.8885f}; // proportional gain (computed from period_ and damping_) [rad/s] +}; + +#endif //PX4_AIRSPEEDREFERENCECONTROLLER_HPP diff --git a/src/lib/npfg/CMakeLists.txt b/src/lib/npfg/CMakeLists.txt index 739e6af155..a8306fbcea 100644 --- a/src/lib/npfg/CMakeLists.txt +++ b/src/lib/npfg/CMakeLists.txt @@ -32,8 +32,12 @@ ############################################################################ px4_add_library(npfg - npfg.cpp - npfg.hpp + DirectionalGuidance.cpp + CourseToAirspeedRefMapper.cpp + AirspeedReferenceController.cpp + DirectionalGuidance.hpp + CourseToAirspeedRefMapper.hpp + AirspeedReferenceController.hpp ) target_link_libraries(npfg PRIVATE geo) diff --git a/src/lib/npfg/CourseToAirspeedRefMapper.cpp b/src/lib/npfg/CourseToAirspeedRefMapper.cpp new file mode 100644 index 0000000000..27477b0417 --- /dev/null +++ b/src/lib/npfg/CourseToAirspeedRefMapper.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "CourseToAirspeedRefMapper.hpp" +using matrix::Vector2f; + +float +CourseToAirspeedRefMapper::mapCourseSetpointToHeadingSetpoint(const float bearing_setpoint, const Vector2f &wind_vel, + float airspeed_true) const +{ + + Vector2f bearing_vector = Vector2f{cosf(bearing_setpoint), sinf(bearing_setpoint)}; + const float wind_cross_bearing = wind_vel.cross(bearing_vector); + const float wind_dot_bearing = wind_vel.dot(bearing_vector); + + const Vector2f air_vel_ref = refAirVelocity(wind_vel, bearing_vector, wind_cross_bearing, + wind_dot_bearing, wind_vel.norm(), airspeed_true); + + return atan2f(air_vel_ref(1), air_vel_ref(0)); +} + +matrix::Vector2f CourseToAirspeedRefMapper::refAirVelocity(const Vector2f &wind_vel, const Vector2f &bearing_vec, + const float wind_cross_bearing, const float wind_dot_bearing, + const float wind_speed, float airspeed_true) const +{ + Vector2f air_vel_ref; + + if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_true, wind_speed)) { + const float airsp_dot_bearing = projectAirspOnBearing(airspeed_true, wind_cross_bearing); + air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); + + } else { + air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_true); + } + + return air_vel_ref; +} + +float CourseToAirspeedRefMapper::projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const +{ + // NOTE: wind_cross_bearing must be less than airspeed to use this function + // it is assumed that bearing feasibility is checked and found feasible (e.g. bearingIsFeasible() = true) prior to entering this method + // otherwise the return will be erroneous + return sqrtf(math::max(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f)); +} + +int CourseToAirspeedRefMapper::bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, + const float airspeed, const float wind_speed) const +{ + return (fabsf(wind_cross_bearing) < airspeed) && ((wind_dot_bearing > 0.0f) || (wind_speed < airspeed)); +} + +matrix::Vector2f +CourseToAirspeedRefMapper::solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing, + const Vector2f &bearing_vec) const +{ + // essentially a 2D rotation with the speeds (magnitudes) baked in + return Vector2f{airsp_dot_bearing * bearing_vec(0) - wind_cross_bearing * bearing_vec(1), + wind_cross_bearing * bearing_vec(0) + airsp_dot_bearing * bearing_vec(1)}; +} + +matrix::Vector2f CourseToAirspeedRefMapper::infeasibleAirVelRef(const Vector2f &wind_vel, const Vector2f &bearing_vec, + const float wind_speed, const float airspeed) const +{ + // NOTE: wind speed must be greater than airspeed, and airspeed must be greater than zero to use this function + // it is assumed that bearing feasibility is checked and found infeasible (e.g. bearingIsFeasible() = false) prior to entering this method + // otherwise the normalization of the air velocity vector could have a division by zero + Vector2f air_vel_ref = sqrtf(math::max(wind_speed * wind_speed - airspeed * airspeed, 0.0f)) * bearing_vec - wind_vel; + return air_vel_ref.normalized() * airspeed; +} diff --git a/src/lib/npfg/CourseToAirspeedRefMapper.hpp b/src/lib/npfg/CourseToAirspeedRefMapper.hpp new file mode 100644 index 0000000000..8a6d97efe8 --- /dev/null +++ b/src/lib/npfg/CourseToAirspeedRefMapper.hpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 CourseToAirspeedRefMapper.hpp + * + * Original Author: Thomas Stastny + * Refactored to better suite new control API: Roman Bapst + * + * * Notes: + * - The wind estimate should be dynamic enough to capture ~1-2 second length gusts, + * Otherwise the performance will suffer. + * + * Acknowledgements and References: + * + * The logic is mostly based on [1] and Paper III of [2]. + * TODO: Concise, up to date documentation and stability analysis for the following + * implementation. + * + * [1] T. Stastny and R. Siegwart. "On Flying Backwards: Preventing Run-away of + * Small, Low-speed, Fixed-wing UAVs in Strong Winds". IEEE International Conference + * on Intelligent Robots and Systems (IROS). 2019. + * https://arxiv.org/pdf/1908.01381.pdf + * [2] T. Stastny. "Low-Altitude Control and Local Re-Planning Strategies for Small + * Fixed-Wing UAVs". Doctoral Thesis, ETH Zürich. 2020. + * https://tstastny.github.io/pdf/tstastny_phd_thesis_wcover.pdf + */ + +#ifndef PX4_COURSETOAIRSPEEDREFMAPPER_HPP +#define PX4_COURSETOAIRSPEEDREFMAPPER_HPP + + +#include +#include + +class CourseToAirspeedRefMapper +{ +public: + + CourseToAirspeedRefMapper() {}; + + ~CourseToAirspeedRefMapper() = default; + + float mapCourseSetpointToHeadingSetpoint(const float bearing_setpoint, + const matrix::Vector2f &wind_vel, float airspeed_true) const; + +private: + /* + * Determines a reference air velocity *without curvature compensation, but + * including "optimal" true airspeed reference compensation in excess wind conditions. + * Nominal and maximum true airspeed member variables must be set before using this method. + * + * @param[in] wind_vel Wind velocity vector [m/s] + * @param[in] bearing_vec Bearing vector + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @param[in] min_ground_speed Minimum commanded forward ground speed [m/s] + * @return Air velocity vector [m/s] + */ + matrix::Vector2f refAirVelocity(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec, + const float wind_cross_bearing, const float wind_dot_bearing, + const float wind_speed, float airspeed_true) const; + /* + * Projection of the air velocity vector onto the bearing line considering + * a connected wind triangle. + * + * @param[in] airspeed Vehicle true airspeed [m/s] + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @return Projection of air velocity vector on bearing vector [m/s] + */ + float projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const; + /* + * Check for binary bearing feasibility. + * + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] + * @param[in] airspeed Vehicle true airspeed [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @return Binary bearing feasibility: 1 if feasible, 0 if infeasible + */ + int bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, + const float wind_speed) const; + /* + * Air velocity solution for a given wind velocity and bearing vector assuming + * a "high speed" (not backwards) solution in the excess wind case. + * + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @param[in] airsp_dot_bearing 2D dot product of air velocity (solution) and bearing vector [m/s] + * @param[in] bearing_vec Bearing vector + * @return Air velocity vector [m/s] + */ + matrix::Vector2f solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing, + const matrix::Vector2f &bearing_vec) const; + /* + * Air velocity solution for an infeasible bearing. + * + * @param[in] wind_vel Wind velocity vector [m/s] + * @param[in] bearing_vec Bearing vector + * @param[in] wind_speed Wind speed [m/s] + * @param[in] airspeed Vehicle true airspeed [m/s] + * @return Air velocity vector [m/s] + */ + matrix::Vector2f infeasibleAirVelRef(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec, + const float wind_speed, const float airspeed) const; +}; + +#endif //PX4_COURSETOAIRSPEEDREFMAPPER_HPP diff --git a/src/lib/npfg/DirectionalGuidance.cpp b/src/lib/npfg/DirectionalGuidance.cpp new file mode 100644 index 0000000000..880049e70f --- /dev/null +++ b/src/lib/npfg/DirectionalGuidance.cpp @@ -0,0 +1,319 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 DirectionalGuidance.cpp + */ +#include "DirectionalGuidance.hpp" +using matrix::Vector2d; +using matrix::Vector2f; + +DirectionalGuidance::DirectionalGuidance() +{ + +} + +DirectionalGuidanceOutput +DirectionalGuidance::guideToPath(const Vector2f &curr_pos_local, const Vector2f &ground_vel, const Vector2f &wind_vel, + const Vector2f &unit_path_tangent, const Vector2f &position_on_path, + const float path_curvature) +{ + const float ground_speed = ground_vel.norm(); + + const Vector2f air_vel = ground_vel - wind_vel; + const float airspeed = air_vel.norm(); + + const float wind_speed = wind_vel.norm(); + + const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path}; + _signed_track_error = unit_path_tangent.cross(path_pos_to_vehicle); + + // on-track wind triangle projections + const float wind_cross_upt = wind_vel.cross(unit_path_tangent); + const float wind_dot_upt = wind_vel.dot(unit_path_tangent); + + // calculate the bearing feasibility on the track at the current closest point + _feas_on_track = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed); + + const float track_error = fabsf(_signed_track_error); + + // update control parameters considering upper and lower stability bounds (if enabled) + // must be called before trackErrorBound() as it updates time_const_ + _adapted_period = adaptPeriod(ground_speed, airspeed, wind_speed, track_error, + path_curvature, wind_vel, unit_path_tangent, _feas_on_track); + //const float p_gain = pGain(adapted_period, damping_); + _time_const = timeConst(_adapted_period, damping_); + + // track error bound is dynamic depending on ground speed + _track_error_bound = trackErrorBound(ground_speed, _time_const); + const float normalized_track_error = normalizedTrackError(track_error, _track_error_bound); + + // look ahead angle based solely on track proximity + const float look_ahead_ang = lookAheadAngle(normalized_track_error); + + _track_proximity = trackProximity(look_ahead_ang); + + _bearing_vec = bearingVec(unit_path_tangent, look_ahead_ang, _signed_track_error); + + // wind triangle projections + const float wind_cross_bearing = wind_vel.cross(_bearing_vec); + const float wind_dot_bearing = wind_vel.dot(_bearing_vec); + + // continuous representation of the bearing feasibility + _feas = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed); + + // we consider feasibility of both the current bearing as well as that on the track at the current closest point + const float feas_combined = _feas * _feas_on_track; + // lateral acceleration needed to stay on curved track (assuming no heading error) + const float lateral_accel_ff = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt, + wind_cross_upt, airspeed, wind_speed, _signed_track_error, path_curvature) * feas_combined * _track_proximity; + + return DirectionalGuidanceOutput{.course_setpoint = atan2f(_bearing_vec(1), _bearing_vec(0)), + .lateral_acceleration_feedforward = lateral_accel_ff}; +} + +float +DirectionalGuidance::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, + const float wind_speed) const +{ + if (wind_dot_bearing < 0.0f) { + wind_cross_bearing = wind_speed; + + } else { + wind_cross_bearing = fabsf(wind_cross_bearing); + } + + float sin_arg = sinf(M_PI_F * 0.5f * math::constrain((airspeed - wind_cross_bearing) / AIRSPEED_BUFFER, 0.0f, 1.0f)); + return sin_arg * sin_arg; +} + +float DirectionalGuidance::adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed, + const float track_error, const float path_curvature, const Vector2f &wind_vel, + const Vector2f &unit_path_tangent, const float feas_on_track) const +{ + float period = period_; + const float air_turn_rate = fabsf(path_curvature * airspeed); + const float wind_factor = windFactor(airspeed, wind_speed); + + if (en_period_lb_ && roll_time_const_ > NPFG_EPSILON) { + // lower bound for period not considering path curvature + const float period_lb_zero_curvature = periodLowerBound(0.0f, wind_factor, feas_on_track) * period_safety_factor_; + + // lower bound for period *considering path curvature + float period_lb = periodLowerBound(air_turn_rate, wind_factor, feas_on_track) * period_safety_factor_; + + // calculate the time constant and track error bound considering the zero + // curvature, lower-bounded period and subsequently recalculate the normalized + // track error + const float time_const = timeConst(period_lb_zero_curvature, damping_); + const float track_error_bound = trackErrorBound(ground_speed, time_const); + const float normalized_track_error = normalizedTrackError(track_error, track_error_bound); + + // calculate nominal track proximity with lower bounded time constant + // (only a numerical solution can find corresponding track proximity + // and adapted gains simultaneously) + const float look_ahead_ang = lookAheadAngle(normalized_track_error); + const float track_proximity = trackProximity(look_ahead_ang); + + // linearly ramp in curvature dependent lower bound with track proximity + period_lb = period_lb * track_proximity + (1.0f - track_proximity) * period_lb_zero_curvature; + + // lower bounded period + period = math::max(period_lb, period); + + // only allow upper bounding ONLY if lower bounding is enabled (is otherwise + // dangerous to allow period decrements without stability checks). + // NOTE: if the roll time constant is not accurately known, lower-bound + // checks may be too optimistic and reducing the period can still destabilize + // the system! enable this feature at your own risk. + if (en_period_ub_) { + + const float period_ub = periodUpperBound(air_turn_rate, wind_factor, feas_on_track); + + if (en_period_ub_ && PX4_ISFINITE(period_ub) && period > period_ub) { + // NOTE: if the roll time constant is not accurately known, reducing + // the period here can destabilize the system! + // enable this feature at your own risk! + + // upper bound the period (for track keeping stability), prefer lower bound if violated + const float period_adapted = math::max(period_lb, period_ub); + + // transition from the nominal period to the adapted period as we get + // closer to the track + period = period_adapted * track_proximity + (1.0f - track_proximity) * period; + } + } + } + + return period; +} + +float DirectionalGuidance::windFactor(const float airspeed, const float wind_speed) const +{ + // See [TODO: include citation] for definition/elaboration of this approximation. + if (wind_speed > airspeed || airspeed < NPFG_EPSILON) { + return 2.0f; + + } else { + return 2.0f * (1.0f - sqrtf(1.0f - math::min(1.0f, wind_speed / airspeed))); + } +} + +float DirectionalGuidance::periodLowerBound(const float air_turn_rate, const float wind_factor, + const float feas_on_track) const +{ + // this method considers a "conservative" lower period bound, i.e. a constant + // worst case bound for any wind ratio, airspeed, and path curvature + + // the lower bound for zero curvature and no wind OR damping ratio < 0.5 + const float period_lb = M_PI_F * roll_time_const_ / damping_; + + if (air_turn_rate * wind_factor < NPFG_EPSILON || damping_ < 0.5f) { + return period_lb; + + } else { + // the lower bound for tracking a curved path in wind with damping ratio > 0.5 + const float period_windy_curved_damped = 4.0f * M_PI_F * roll_time_const_ * damping_; + + // blend the two together as the bearing on track becomes less feasible + return period_windy_curved_damped * feas_on_track + (1.0f - feas_on_track) * period_lb; + } +} + +float DirectionalGuidance::periodUpperBound(const float air_turn_rate, const float wind_factor, + const float feas_on_track) const +{ + if (air_turn_rate * wind_factor > NPFG_EPSILON) { + // multiply air turn rate by feasibility on track to zero out when we anyway + // should not consider the curvature + return 4.0f * M_PI_F * damping_ / (air_turn_rate * wind_factor * feas_on_track); + } + + return INFINITY; +} + +float DirectionalGuidance::timeConst(const float period, const float damping) const +{ + return period * damping; +} + +float DirectionalGuidance::trackErrorBound(const float ground_speed, const float time_const) const +{ + if (ground_speed > 1.0f) { + return ground_speed * time_const; + + } else { + // limit bound to some minimum ground speed to avoid singularities in track + // error normalization. the following equation assumes ground speed minimum = 1.0 + return 0.5f * time_const * (ground_speed * ground_speed + 1.0f); + } +} + +float DirectionalGuidance::normalizedTrackError(const float track_error, const float track_error_bound) const +{ + return math::constrain(track_error / track_error_bound, 0.0f, 1.0f); +} + +float DirectionalGuidance::lookAheadAngle(const float normalized_track_error) const +{ + return M_PI_2_F * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f); +} + +float DirectionalGuidance::trackProximity(const float look_ahead_ang) const +{ + const float sin_look_ahead_ang = sinf(look_ahead_ang); + return sin_look_ahead_ang * sin_look_ahead_ang; +} + +matrix::Vector2f DirectionalGuidance::bearingVec(const Vector2f &unit_path_tangent, const float look_ahead_ang, + const float signed_track_error) const +{ + const float cos_look_ahead_ang = cosf(look_ahead_ang); + const float sin_look_ahead_ang = sinf(look_ahead_ang); + + Vector2f unit_path_normal(-unit_path_tangent(1), unit_path_tangent(0)); // right handed 90 deg (clockwise) turn + Vector2f unit_track_error = -((signed_track_error < 0.0f) ? -1.0f : 1.0f) * unit_path_normal; + + return cos_look_ahead_ang * unit_track_error + sin_look_ahead_ang * unit_path_tangent; +} + +float DirectionalGuidance::lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &ground_vel, + const float wind_dot_upt, const float wind_cross_upt, const float airspeed, + const float wind_speed, const float signed_track_error, + const float path_curvature) const +{ + // NOTE: all calculations within this function take place at the closet point + // on the path, as if the aircraft were already tracking the given path at + // this point with zero angular error. this allows us to evaluate curvature + // induced requirements for lateral acceleration incrementation and ramp them + // in with the track proximity and further consider the bearing feasibility + // in excess wind conditions (this is done external to this method). + + // path frame curvature is the instantaneous curvature at our current distance + // from the actual path (considering e.g. concentric circles emanating outward/inward) + const float path_frame_curvature = path_curvature / math::max(1.0f - path_curvature * signed_track_error, + fabsf(path_curvature) * MIN_RADIUS); + + // limit tangent ground speed to along track (forward moving) direction + const float tangent_ground_speed = math::max(ground_vel.dot(unit_path_tangent), 0.0f); + + const float path_frame_rate = path_frame_curvature * tangent_ground_speed; + + // speed ratio = projection of ground vel on track / projection of air velocity + // on track + const float speed_ratio = (1.0f + wind_dot_upt / math::max(projectAirspOnBearing(airspeed, wind_cross_upt), + NPFG_EPSILON)); + + // note the use of airspeed * speed_ratio as oppose to ground_speed^2 here -- + // the prior considers that we command lateral acceleration in the air mass + // relative frame while the latter does not + return airspeed * speed_ratio * path_frame_rate; +} + +float DirectionalGuidance::projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const +{ + // NOTE: wind_cross_bearing must be less than airspeed to use this function + // it is assumed that bearing feasibility is checked and found feasible (e.g. bearingIsFeasible() = true) prior to entering this method + // otherwise the return will be erroneous + return sqrtf(math::max(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f)); +} + +float DirectionalGuidance::getBearing() +{ + return atan2f(_bearing_vec(1), _bearing_vec(0)); +} + +float DirectionalGuidance::switchDistance(float wp_radius) const +{ + return math::min(wp_radius, _track_error_bound * _switch_distance_multiplier); +} diff --git a/src/lib/npfg/DirectionalGuidance.hpp b/src/lib/npfg/DirectionalGuidance.hpp new file mode 100644 index 0000000000..ab8c3aff3a --- /dev/null +++ b/src/lib/npfg/DirectionalGuidance.hpp @@ -0,0 +1,300 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 DirectionalGuidance.hpp + * + * Original Author: Thomas Stastny + * Refactored to better suite new control API: Roman Bapst + * + * * Notes: + * - The wind estimate should be dynamic enough to capture ~1-2 second length gusts, + * Otherwise the performance will suffer. + * + * Acknowledgements and References: + * + * The logic is mostly based on [1] and Paper III of [2]. + * TODO: Concise, up to date documentation and stability analysis for the following + * implementation. + * + * [1] T. Stastny and R. Siegwart. "On Flying Backwards: Preventing Run-away of + * Small, Low-speed, Fixed-wing UAVs in Strong Winds". IEEE International Conference + * on Intelligent Robots and Systems (IROS). 2019. + * https://arxiv.org/pdf/1908.01381.pdf + * [2] T. Stastny. "Low-Altitude Control and Local Re-Planning Strategies for Small + * Fixed-Wing UAVs". Doctoral Thesis, ETH Zürich. 2020. + * https://tstastny.github.io/pdf/tstastny_phd_thesis_wcover.pdf + */ + +#ifndef PX4_DIRECTIONALGUIDANCE_HPP +#define PX4_DIRECTIONALGUIDANCE_HPP +#include +#include + +struct DirectionalGuidanceOutput { + float course_setpoint{NAN}; + float lateral_acceleration_feedforward{NAN}; +}; + +class DirectionalGuidance +{ +public: + + DirectionalGuidance(); + + DirectionalGuidanceOutput guideToPath(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_vel, + const matrix::Vector2f &wind_vel, + const matrix::Vector2f &unit_path_tangent, const matrix::Vector2f &position_on_path, + const float path_curvature); + /* + * Set the nominal controller period [s]. + */ + void setPeriod(float period) { period_ = math::max(period, NPFG_EPSILON); } + + /* + * Set the nominal controller damping ratio. + */ + void setDamping(float damping) { damping_ = math::constrain(damping, NPFG_EPSILON, 1.0f); } + + /* + * Enable automatic lower bounding of the user set controller period. + */ + void enablePeriodLB(const bool en) { en_period_lb_ = en; } + + /* + * Enable automatic adaptation of the user set controller period for track keeping + * performance. + */ + void enablePeriodUB(const bool en) { en_period_ub_ = en; } + + + /* + * Set the autopilot roll response time constant [s]. + */ + void setRollTimeConst(float tc) { roll_time_const_ = tc; } + + /* + * Set the period safety factor. + */ + void setPeriodSafetyFactor(float sf) { period_safety_factor_ = math::max(sf, 1.0f); } + void setSwitchDistanceMultiplier(float mult) { _switch_distance_multiplier = math::max(mult, 0.1f); } + /* + * [Copied directly from ECL_L1_Pos_Controller] + * + * 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[in] wp_radius The switching radius the waypoint has set. + */ + float switchDistance(float wp_radius) const; + + float getBearing(); + +private: + static constexpr float AIRSPEED_BUFFER = 1.5f; // airspeed buffer [m/s] (must be > 0) + static constexpr float NPFG_EPSILON = 1.0e-6; // small number *bigger than machine epsilon + static constexpr float MIN_RADIUS = 0.5f; // minimum effective radius (avoid singularities) [m] + float period_{10.0f}; // nominal (desired) period -- user defined [s] + float damping_{0.7071f}; // nominal (desired) damping ratio -- user defined + + float period_safety_factor_{1.5f}; // multiplied by the minimum period for conservative lower bound + bool en_period_lb_{true}; // enables automatic lower bound constraints on controller period + bool en_period_ub_{true}; // enables automatic upper bound constraints on controller period (remains disabled if lower bound is disabled) + float roll_time_const_{0.0f}; // autopilot roll response time constant [s] + + float _signed_track_error; + float _feas_on_track; + float _adapted_period; + float _time_const{7.0f}; + float _track_error_bound; + float _track_proximity; + matrix::Vector2f _bearing_vec; + float _feas; + float _switch_distance_multiplier{0.32f}; // a value multiplied by the track error boundary resulting in a lower switch distance + + /* + * user defined guidance settings + */ + + /* + * Cacluates a continuous representation of the bearing feasibility from [0,1]. + * 0 = infeasible, 1 = fully feasible, partial feasibility in between. + * + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] + * @param[in] airspeed Vehicle true airspeed [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @return bearing feasibility + */ + float bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, + const float wind_speed) const; + /* + * Adapts the controller period considering user defined inputs, current flight + * condition, path properties, and stability bounds. + * + * @param[in] ground_speed Vehicle ground speed [m/s] + * @param[in] airspeed Vehicle true airspeed [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @param[in] track_error Track error (magnitude) [m] + * @param[in] path_curvature Path curvature at closest point on track [m^-1] + * @param[in] wind_vel Wind velocity vector in inertial frame [m/s] + * @param[in] unit_path_tangent Unit vector tangent to path at closest point + * in direction of path + * @param[in] feas_on_track Bearing feasibility on track at the closest point + * @return Adapted period [s] + */ + float adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed, + const float track_error, const float path_curvature, const matrix::Vector2f &wind_vel, + const matrix::Vector2f &unit_path_tangent, const float feas_on_track) const; + /* + * Calculates a ground speed modulated track error bound under which the + * look ahead angle is quadratically transitioned from alignment with the + * track error vector to that of the path tangent vector. + * + * @param[in] ground_speed Vehicle ground speed [m/s] + * @param[in] time_const Controller time constant [s] + * @return Track error boundary [m] + */ + float trackErrorBound(const float ground_speed, const float time_const) const; + /* + * Returns normalized (unitless) and constrained track error [0,1]. + * + * @param[in] track_error Track error (magnitude) [m] + * @param[in] track_error_bound Track error boundary [m] + * @return Normalized track error + */ + float normalizedTrackError(const float track_error, const float track_error_bound) const; + /* + * Cacluates the look ahead angle as a quadratic function of the normalized + * track error. + * + * @param[in] normalized_track_error Normalized track error (track error / track error boundary) + * @return Look ahead angle [rad] + */ + float lookAheadAngle(const float normalized_track_error) const; + /* + * Computes a continous non-dimensional track proximity [0,1] - 0 when the + * vehicle is at the track error boundary, and 1 when on track. + * + * @param[in] look_ahead_ang The angle at which the bearing vector deviates + * from the unit track error vector [rad] + * @return Track proximity + */ + float trackProximity(const float look_ahead_ang) const; + /* + * Calculates the bearing vector and track proximity transitioning variable + * from the look-ahead angle mapping. + * + * @param[in] unit_path_tangent Unit vector tangent to path at closest point + * in direction of path + * @param[in] look_ahead_ang The bearing vector lies at this angle from + * the path normal vector [rad] + * @param[in] signed_track_error Signed error to track at closest point (sign + * determined by path normal direction) [m] + * @return Unit bearing vector + */ + matrix::Vector2f bearingVec(const matrix::Vector2f &unit_path_tangent, const float look_ahead_ang, + const float signed_track_error) const; + /* + * Calculates an additional feed-forward lateral acceleration demand considering + * the path curvature. + * + * @param[in] unit_path_tangent Unit vector tangent to path at closest point + * in direction of path + * @param[in] ground_vel Vehicle ground velocity vector [m/s] + * @param[in] wind_vel Wind velocity vector [m/s] + * @param[in] airspeed Vehicle true airspeed [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @param[in] signed_track_error Signed error to track at closest point (sign + * determined by path normal direction) [m] + * @param[in] path_curvature Path curvature at closest point on track [m^-1] + * @return Feed-forward lateral acceleration command [m/s^2] + */ + float lateralAccelFF(const matrix::Vector2f &unit_path_tangent, const matrix::Vector2f &ground_vel, + const float wind_dot_upt, const float wind_cross_upt, const float airspeed, + const float wind_speed, const float signed_track_error, const float path_curvature) const; + /* + * Cacluates an approximation of the wind factor (see [TODO: include citation]). + * + * @param[in] airspeed Vehicle true airspeed [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @return Non-dimensional wind factor approximation + */ + float windFactor(const float airspeed, const float wind_speed) const; + /* + * Calculates a theoretical lower bound on the user defined period to avoid + * limit cycle oscillations considering an acceleration actuation delay (e.g. + * roll response delay). Note this lower bound defines *marginal stability, + * and a safety factor should be applied in addition to the returned value. + * + * @param[in] air_turn_rate The turn rate required to track the current path + * curvature at the current true airspeed, in a no-wind condition [rad/s] + * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation]) + * @return Period lower bound [s] + */ + float periodLowerBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const; + /* + * Calculates a theoretical upper bound on the user defined period to maintain + * track keeping stability. + * + * @param[in] air_turn_rate The turn rate required to track the current path + * curvature at the current true airspeed, in a no-wind condition [rad/s] + * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation]) + * @return Period upper bound [s] + */ + float periodUpperBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const; + /* + * Calculates the required controller time constant to achieve the desired + * system period and damping ratio. NOTE: actual period and damping will vary + * when following paths with curvature in wind. + * + * @param[in] period Desired system period [s] + * @param[in] damping Desired system damping ratio + * @return Time constant [s] + */ + float timeConst(const float period, const float damping) const; + /* + * Projection of the air velocity vector onto the bearing line considering + * a connected wind triangle. + * + * @param[in] airspeed Vehicle true airspeed [m/s] + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @return Projection of air velocity vector on bearing vector [m/s] + */ + float projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const; + +}; + +#endif //PX4_DIRECTIONALGUIDANCE_HPP diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp deleted file mode 100644 index fdcfa570b0..0000000000 --- a/src/lib/npfg/npfg.cpp +++ /dev/null @@ -1,508 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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 npfg.cpp - * Implementation of a lateral-directional nonlinear path following guidance - * law with excess wind handling. Commands lateral acceleration and airspeed. - * - * Authors and acknowledgements in header. - */ - -#include "npfg.hpp" -#include -#include -#include - -using matrix::Vector2d; -using matrix::Vector2f; - -float NPFG::canRun(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const -{ - if (is_wind_valid) { - // If we have a valid wind estimate, npfg is able to handle all degenerated cases - return 1.f; - } - - // NPFG can run without wind information as long as the system is not flying backwards and has a minimal ground speed - // Check the minimal ground speed. if it is greater than twice the standard deviation, we assume that we can infer a valid track angle - const Vector2f ground_vel(local_pos.vx, local_pos.vy); - const float ground_speed(ground_vel.norm()); - const float low_ground_speed_factor(math::constrain((ground_speed - HORIZONTAL_EVH_FACTOR_COURSE_INVALID * - local_pos.evh) / ((HORIZONTAL_EVH_FACTOR_COURSE_VALID - HORIZONTAL_EVH_FACTOR_COURSE_INVALID)*local_pos.evh), - 0.f, 1.f)); - - // Check that the angle between heading and track is not off too much. if it is greater than 90° we will be pushed back from the wind and the npfg will propably give a roll command in the wrong direction. - const Vector2f heading_vector(matrix::Dcm2f(local_pos.heading)*Vector2f({1.f, 0.f})); - const Vector2f ground_vel_norm(ground_vel.normalized()); - const float flying_forward_factor(math::constrain((heading_vector.dot(ground_vel_norm) - - COS_HEADING_TRACK_ANGLE_PUSHED_BACK) / ((COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK - - COS_HEADING_TRACK_ANGLE_PUSHED_BACK)), - 0.f, 1.f)); - - return flying_forward_factor * low_ground_speed_factor; -} - -void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &ground_vel, const Vector2f &wind_vel, - const Vector2f &unit_path_tangent, - const Vector2f &position_on_path, const float path_curvature) -{ - const float ground_speed = ground_vel.norm(); - - const Vector2f air_vel = ground_vel - wind_vel; - const float airspeed = air_vel.norm(); - - const float wind_speed = wind_vel.norm(); - - const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path}; - signed_track_error_ = unit_path_tangent.cross(path_pos_to_vehicle); - - // on-track wind triangle projections - const float wind_cross_upt = wind_vel.cross(unit_path_tangent); - const float wind_dot_upt = wind_vel.dot(unit_path_tangent); - - // calculate the bearing feasibility on the track at the current closest point - feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed); - - const float track_error = fabsf(signed_track_error_); - - // update control parameters considering upper and lower stability bounds (if enabled) - // must be called before trackErrorBound() as it updates time_const_ - adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error, - path_curvature, wind_vel, unit_path_tangent, feas_on_track_); - p_gain_ = pGain(adapted_period_, damping_); - time_const_ = timeConst(adapted_period_, damping_); - - // track error bound is dynamic depending on ground speed - track_error_bound_ = trackErrorBound(ground_speed, time_const_); - const float normalized_track_error = normalizedTrackError(track_error, track_error_bound_); - - // look ahead angle based solely on track proximity - const float look_ahead_ang = lookAheadAngle(normalized_track_error); - - track_proximity_ = trackProximity(look_ahead_ang); - - bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error_); - - // wind triangle projections - const float wind_cross_bearing = wind_vel.cross(bearing_vec_); - const float wind_dot_bearing = wind_vel.dot(bearing_vec_); - - // continuous representation of the bearing feasibility - feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed); - - // we consider feasibility of both the current bearing as well as that on the track at the current closest point - const float feas_combined = feas_ * feas_on_track_; - - min_ground_speed_ref_ = minGroundSpeed(normalized_track_error, feas_combined); - - // reference air velocity with directional feedforward effect for following - // curvature in wind and magnitude incrementation depending on minimum ground - // speed violations and/or high wind conditions in general - air_vel_ref_ = refAirVelocity(wind_vel, bearing_vec_, wind_cross_bearing, - wind_dot_bearing, wind_speed, min_ground_speed_ref_); - airspeed_ref_ = air_vel_ref_.norm(); - - // lateral acceleration demand based on heading error - const float lateral_accel = lateralAccel(air_vel, air_vel_ref_, airspeed); - - // lateral acceleration needed to stay on curved track (assuming no heading error) - lateral_accel_ff_ = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt, - wind_cross_upt, airspeed, wind_speed, signed_track_error_, path_curvature); - - // total lateral acceleration to drive aircaft towards track as well as account - // for path curvature. The full effect of the feed-forward acceleration is smoothly - // ramped in as the vehicle approaches the track and is further smoothly - // zeroed out as the bearing becomes infeasible. - lateral_accel_ = lateral_accel + feas_combined * track_proximity_ * lateral_accel_ff_; - - updateRollSetpoint(); -} // guideToPath - -float NPFG::adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed, - const float track_error, const float path_curvature, const Vector2f &wind_vel, - const Vector2f &unit_path_tangent, const float feas_on_track) const -{ - float period = period_; - const float air_turn_rate = fabsf(path_curvature * airspeed); - const float wind_factor = windFactor(airspeed, wind_speed); - - if (en_period_lb_ && roll_time_const_ > NPFG_EPSILON) { - // lower bound for period not considering path curvature - const float period_lb_zero_curvature = periodLowerBound(0.0f, wind_factor, feas_on_track) * period_safety_factor_; - - // lower bound for period *considering path curvature - float period_lb = periodLowerBound(air_turn_rate, wind_factor, feas_on_track) * period_safety_factor_; - - // calculate the time constant and track error bound considering the zero - // curvature, lower-bounded period and subsequently recalculate the normalized - // track error - const float time_const = timeConst(period_lb_zero_curvature, damping_); - const float track_error_bound = trackErrorBound(ground_speed, time_const); - const float normalized_track_error = normalizedTrackError(track_error, track_error_bound); - - // calculate nominal track proximity with lower bounded time constant - // (only a numerical solution can find corresponding track proximity - // and adapted gains simultaneously) - const float look_ahead_ang = lookAheadAngle(normalized_track_error); - const float track_proximity = trackProximity(look_ahead_ang); - - // linearly ramp in curvature dependent lower bound with track proximity - period_lb = period_lb * track_proximity + (1.0f - track_proximity) * period_lb_zero_curvature; - - // lower bounded period - period = math::max(period_lb, period); - - // only allow upper bounding ONLY if lower bounding is enabled (is otherwise - // dangerous to allow period decrements without stability checks). - // NOTE: if the roll time constant is not accurately known, lower-bound - // checks may be too optimistic and reducing the period can still destabilize - // the system! enable this feature at your own risk. - if (en_period_ub_) { - - const float period_ub = periodUpperBound(air_turn_rate, wind_factor, feas_on_track); - - if (en_period_ub_ && PX4_ISFINITE(period_ub) && period > period_ub) { - // NOTE: if the roll time constant is not accurately known, reducing - // the period here can destabilize the system! - // enable this feature at your own risk! - - // upper bound the period (for track keeping stability), prefer lower bound if violated - const float period_adapted = math::max(period_lb, period_ub); - - // transition from the nominal period to the adapted period as we get - // closer to the track - period = period_adapted * track_proximity + (1.0f - track_proximity) * period; - } - } - } - - return period; -} // adaptPeriod - -float NPFG::normalizedTrackError(const float track_error, const float track_error_bound) const -{ - return math::constrain(track_error / track_error_bound, 0.0f, 1.0f); -} - -float NPFG::windFactor(const float airspeed, const float wind_speed) const -{ - // See [TODO: include citation] for definition/elaboration of this approximation. - if (wind_speed > airspeed || airspeed < NPFG_EPSILON) { - return 2.0f; - - } else { - return 2.0f * (1.0f - sqrtf(1.0f - math::min(1.0f, wind_speed / airspeed))); - } -} // windFactor - -float NPFG::periodUpperBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const -{ - if (air_turn_rate * wind_factor > NPFG_EPSILON) { - // multiply air turn rate by feasibility on track to zero out when we anyway - // should not consider the curvature - return 4.0f * M_PI_F * damping_ / (air_turn_rate * wind_factor * feas_on_track); - } - - return INFINITY; -} // periodUB - -float NPFG::periodLowerBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const -{ - // this method considers a "conservative" lower period bound, i.e. a constant - // worst case bound for any wind ratio, airspeed, and path curvature - - // the lower bound for zero curvature and no wind OR damping ratio < 0.5 - const float period_lb = M_PI_F * roll_time_const_ / damping_; - - if (air_turn_rate * wind_factor < NPFG_EPSILON || damping_ < 0.5f) { - return period_lb; - - } else { - // the lower bound for tracking a curved path in wind with damping ratio > 0.5 - const float period_windy_curved_damped = 4.0f * M_PI_F * roll_time_const_ * damping_; - - // blend the two together as the bearing on track becomes less feasible - return period_windy_curved_damped * feas_on_track + (1.0f - feas_on_track) * period_lb; - } -} // periodLB - -float NPFG::trackProximity(const float look_ahead_ang) const -{ - const float sin_look_ahead_ang = sinf(look_ahead_ang); - return sin_look_ahead_ang * sin_look_ahead_ang; -} // trackProximity - -float NPFG::trackErrorBound(const float ground_speed, const float time_const) const -{ - if (ground_speed > 1.0f) { - return ground_speed * time_const; - - } else { - // limit bound to some minimum ground speed to avoid singularities in track - // error normalization. the following equation assumes ground speed minimum = 1.0 - return 0.5f * time_const * (ground_speed * ground_speed + 1.0f); - } -} // trackErrorBound - -float NPFG::pGain(const float period, const float damping) const -{ - return 4.0f * M_PI_F * damping / period; -} // pGain - -float NPFG::timeConst(const float period, const float damping) const -{ - return period * damping; -} // timeConst - -float NPFG::lookAheadAngle(const float normalized_track_error) const -{ - return M_PI_2_F * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f); -} // lookAheadAngle - -Vector2f NPFG::bearingVec(const Vector2f &unit_path_tangent, const float look_ahead_ang, - const float signed_track_error) const -{ - const float cos_look_ahead_ang = cosf(look_ahead_ang); - const float sin_look_ahead_ang = sinf(look_ahead_ang); - - Vector2f unit_path_normal(-unit_path_tangent(1), unit_path_tangent(0)); // right handed 90 deg (clockwise) turn - Vector2f unit_track_error = -((signed_track_error < 0.0f) ? -1.0f : 1.0f) * unit_path_normal; - - return cos_look_ahead_ang * unit_track_error + sin_look_ahead_ang * unit_path_tangent; -} // bearingVec - -float NPFG::minGroundSpeed(const float normalized_track_error, const float feas) -{ - // minimum ground speed demand from track keeping logic - min_gsp_track_keeping_ = 0.0f; - - if (en_track_keeping_ && en_wind_excess_regulation_) { - // zero out track keeping speed increment when bearing is feasible - // maximum track keeping speed increment is applied until we are within - // a user defined fraction of the normalized track error - min_gsp_track_keeping_ = (1.0f - feas) * min_gsp_track_keeping_max_ * math::constrain( - normalized_track_error / NTE_FRACTION, 0.0f, - 1.0f); - } - - // minimum ground speed demand from minimum forward ground speed user setting - float min_gsp_desired = 0.0f; - - if (en_min_ground_speed_ && en_wind_excess_regulation_) { - min_gsp_desired = min_gsp_desired_; - } - - return math::max(min_gsp_track_keeping_, min_gsp_desired); -} // minGroundSpeed - -Vector2f NPFG::refAirVelocity(const Vector2f &wind_vel, const Vector2f &bearing_vec, - const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed, - const float min_ground_speed) const -{ - Vector2f air_vel_ref; - - if (min_ground_speed > wind_dot_bearing && (en_min_ground_speed_ || en_track_keeping_) && en_wind_excess_regulation_) { - // minimum ground speed and/or track keeping - - // airspeed required to achieve minimum ground speed along bearing vector - const float airspeed_min = sqrtf((min_ground_speed - wind_dot_bearing) * (min_ground_speed - wind_dot_bearing) + - wind_cross_bearing * wind_cross_bearing); - - if (airspeed_min > airspeed_max_) { - if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_max_, wind_speed)) { - // we will not maintain the minimum ground speed, but can still achieve the bearing at maximum airspeed - const float airsp_dot_bearing = projectAirspOnBearing(airspeed_max_, wind_cross_bearing); - air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); - - } else { - // bearing is maximally infeasible, employ mitigation law - air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_max_); - } - - } else if (airspeed_min > airspeed_nom_) { - // the minimum ground speed is achievable within the nom - max airspeed range - // solve wind triangle with for air velocity reference with minimum airspeed - const float airsp_dot_bearing = projectAirspOnBearing(airspeed_min, wind_cross_bearing); - air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); - - } else { - // the minimum required airspeed is less than nominal, so we can track the bearing and minimum - // ground speed with our nominal airspeed reference - const float airsp_dot_bearing = projectAirspOnBearing(airspeed_nom_, wind_cross_bearing); - air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); - } - - } else { - // wind excess regulation and/or mitigation - - if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_nom_, wind_speed)) { - // bearing is nominally feasible, solve wind triangle for air velocity reference using nominal airspeed - const float airsp_dot_bearing = projectAirspOnBearing(airspeed_nom_, wind_cross_bearing); - air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); - - } else if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_max_, wind_speed) - && en_wind_excess_regulation_) { - // bearing is maximally feasible - if (wind_dot_bearing <= 0.0f) { - // we only increment the airspeed to regulate, but not overcome, excess wind - // NOTE: in the terminal condition, this will result in a zero ground velocity configuration - air_vel_ref = wind_vel; - - } else { - // the bearing is achievable within the nom - max airspeed range - // solve wind triangle with for air velocity reference with minimum airspeed - const float airsp_dot_bearing = 0.0f; // right angle to the bearing line gives minimal airspeed usage - air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); - } - - } else { - // bearing is maximally infeasible, employ mitigation law - const float airspeed_input = (en_wind_excess_regulation_) ? airspeed_max_ : airspeed_nom_; - air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_input); - } - } - - return air_vel_ref; -} // refAirVelocity - -float NPFG::projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const -{ - // NOTE: wind_cross_bearing must be less than airspeed to use this function - // it is assumed that bearing feasibility is checked and found feasible (e.g. bearingIsFeasible() = true) prior to entering this method - // otherwise the return will be erroneous - return sqrtf(math::max(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f)); -} // projectAirspOnBearing - -int NPFG::bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, - const float wind_speed) const -{ - return (fabsf(wind_cross_bearing) < airspeed) && ((wind_dot_bearing > 0.0f) || (wind_speed < airspeed)); -} // bearingIsFeasible - -Vector2f NPFG::solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing, - const Vector2f &bearing_vec) const -{ - // essentially a 2D rotation with the speeds (magnitudes) baked in - return Vector2f{airsp_dot_bearing * bearing_vec(0) - wind_cross_bearing * bearing_vec(1), - wind_cross_bearing * bearing_vec(0) + airsp_dot_bearing * bearing_vec(1)}; -} // solveWindTriangle - -Vector2f NPFG::infeasibleAirVelRef(const Vector2f &wind_vel, const Vector2f &bearing_vec, const float wind_speed, - const float airspeed) const -{ - // NOTE: wind speed must be greater than airspeed, and airspeed must be greater than zero to use this function - // it is assumed that bearing feasibility is checked and found infeasible (e.g. bearingIsFeasible() = false) prior to entering this method - // otherwise the normalization of the air velocity vector could have a division by zero - Vector2f air_vel_ref = sqrtf(math::max(wind_speed * wind_speed - airspeed * airspeed, 0.0f)) * bearing_vec - wind_vel; - return air_vel_ref.normalized() * airspeed; -} // infeasibleAirVelRef - -float NPFG::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, - const float wind_speed) const -{ - if (wind_dot_bearing < 0.0f) { - wind_cross_bearing = wind_speed; - - } else { - wind_cross_bearing = fabsf(wind_cross_bearing); - } - - float sin_arg = sinf(M_PI_F * 0.5f * math::constrain((airspeed - wind_cross_bearing) / AIRSPEED_BUFFER, 0.0f, 1.0f)); - return sin_arg * sin_arg; -} // bearingFeasibility - -float NPFG::lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &ground_vel, - const float wind_dot_upt, const float wind_cross_upt, const float airspeed, - const float wind_speed, const float signed_track_error, const float path_curvature) const -{ - // NOTE: all calculations within this function take place at the closet point - // on the path, as if the aircraft were already tracking the given path at - // this point with zero angular error. this allows us to evaluate curvature - // induced requirements for lateral acceleration incrementation and ramp them - // in with the track proximity and further consider the bearing feasibility - // in excess wind conditions (this is done external to this method). - - // path frame curvature is the instantaneous curvature at our current distance - // from the actual path (considering e.g. concentric circles emanating outward/inward) - const float path_frame_curvature = path_curvature / math::max(1.0f - path_curvature * signed_track_error, - fabsf(path_curvature) * MIN_RADIUS); - - // limit tangent ground speed to along track (forward moving) direction - const float tangent_ground_speed = math::max(ground_vel.dot(unit_path_tangent), 0.0f); - - const float path_frame_rate = path_frame_curvature * tangent_ground_speed; - - // speed ratio = projection of ground vel on track / projection of air velocity - // on track - const float speed_ratio = (1.0f + wind_dot_upt / math::max(projectAirspOnBearing(airspeed, wind_cross_upt), - NPFG_EPSILON)); - - // note the use of airspeed * speed_ratio as oppose to ground_speed^2 here -- - // the prior considers that we command lateral acceleration in the air mass - // relative frame while the latter does not - return airspeed * speed_ratio * path_frame_rate; -} // lateralAccelFF - -float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, const float airspeed) const -{ - // lateral acceleration demand only from the heading error - - const float dot_air_vel_err = air_vel.dot(air_vel_ref); - const float cross_air_vel_err = air_vel.cross(air_vel_ref); - - if (dot_air_vel_err < 0.0f) { - // hold max lateral acceleration command above 90 deg heading error - return p_gain_ * ((cross_air_vel_err < 0.0f) ? -airspeed : airspeed); - - } else { - // airspeed/airspeed_ref is used to scale any incremented airspeed reference back to the current airspeed - // for acceleration commands in a "feedback" sense (i.e. at the current vehicle airspeed) - return p_gain_ * cross_air_vel_err / airspeed_ref_; - } -} // lateralAccel - -float NPFG::switchDistance(float wp_radius) const -{ - return math::min(wp_radius, track_error_bound_ * switch_distance_multiplier_); -} // switchDistance - -void NPFG::updateRollSetpoint() -{ - float roll_new = atanf(lateral_accel_ * 1.0f / CONSTANTS_ONE_G); - roll_new = math::constrain(roll_new, -roll_lim_rad_, roll_lim_rad_); - - if (PX4_ISFINITE(roll_new)) { - roll_setpoint_ = roll_new; - } -} // updateRollSetpoint diff --git a/src/lib/npfg/npfg.hpp b/src/lib/npfg/npfg.hpp deleted file mode 100644 index c36184ef96..0000000000 --- a/src/lib/npfg/npfg.hpp +++ /dev/null @@ -1,616 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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 npfg.hpp - * Implementation of a lateral-directional nonlinear path following guidance - * law with excess wind handling. Commands lateral acceleration and airspeed. - * - * @author Thomas Stastny - * - * Notes: - * - The wind estimate should be dynamic enough to capture ~1-2 second length gusts, - * Otherwise the performance will suffer. - * - * Acknowledgements and References: - * - * The logic is mostly based on [1] and Paper III of [2]. - * TODO: Concise, up to date documentation and stability analysis for the following - * implementation. - * - * [1] T. Stastny and R. Siegwart. "On Flying Backwards: Preventing Run-away of - * Small, Low-speed, Fixed-wing UAVs in Strong Winds". IEEE International Conference - * on Intelligent Robots and Systems (IROS). 2019. - * https://arxiv.org/pdf/1908.01381.pdf - * [2] T. Stastny. "Low-Altitude Control and Local Re-Planning Strategies for Small - * Fixed-Wing UAVs". Doctoral Thesis, ETH Zürich. 2020. - * https://tstastny.github.io/pdf/tstastny_phd_thesis_wcover.pdf - */ - -#ifndef NPFG_H_ -#define NPFG_H_ - -#include -#include - -#include - -/* - * NPFG - * Lateral-directional nonlinear path following guidance logic with excess wind handling - */ -class NPFG -{ - -public: - /** - * @brief Can run - * - * Evaluation if all the necessary information are available such that npfg can produce meaningful results. - * - * @param[in] local_pos is the current vehicle local position uorb message - * @param[in] is_wind_valid flag if the wind estimation is valid - * @return estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1]. Can be used to define proper mitigation actions. - */ - - float canRun(const vehicle_local_position_s &local_pos, bool is_wind_valid) const; - /* - * Computes the lateral acceleration and true airspeed references necessary to track - * a path in wind (including excess wind conditions). - * - * @param[in] curr_pos_local Current horizontal vehicle position in local coordinates [m] - * @param[in] ground_vel Vehicle ground velocity vector [m/s] - * @param[in] wind_vel Wind velocity vector [m/s] - * @param[in] unit_path_tangent Unit vector tangent to path at closest point - * in direction of path - * @param[in] position_on_path Horizontal point on the path to track described in local coordinates [m] - * @param[in] path_curvature Path curvature at closest point on track [m^-1] - */ - void guideToPath(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_vel, - const matrix::Vector2f &wind_vel, - const matrix::Vector2f &unit_path_tangent, const matrix::Vector2f &position_on_path, - const float path_curvature); - - /* - * Set the nominal controller period [s]. - */ - void setPeriod(float period) { period_ = math::max(period, NPFG_EPSILON); } - - /* - * Set the nominal controller damping ratio. - */ - void setDamping(float damping) { damping_ = math::constrain(damping, NPFG_EPSILON, 1.0f); } - - /* - * Enable automatic lower bounding of the user set controller period. - */ - void enablePeriodLB(const bool en) { en_period_lb_ = en; } - - /* - * Enable automatic adaptation of the user set controller period for track keeping - * performance. - */ - void enablePeriodUB(const bool en) { en_period_ub_ = en; } - - /* - * Enable minimum forward ground speed maintenance logic. - */ - void enableMinGroundSpeed(const bool en) { en_min_ground_speed_ = en; } - - /* - * Enable track keeping logic in excess wind conditions. - */ - void enableTrackKeeping(const bool en) { en_track_keeping_ = en; } - - /* - * Enable wind excess regulation. Disabling this param disables all airspeed - * reference incrementaion (airspeed reference will always be nominal). - */ - void enableWindExcessRegulation(const bool en) { en_wind_excess_regulation_ = en; } - - /* - * Set the minimum allowed forward ground speed [m/s]. - */ - void setMinGroundSpeed(float min_gsp) { min_gsp_desired_ = math::max(min_gsp, 0.0f); } - - /* - * Set the maximum value of the minimum forward ground speed command for track - * keeping (occurs at the track error boundary) [m/s]. - */ - void setMaxTrackKeepingMinGroundSpeed(float min_gsp) { min_gsp_track_keeping_max_ = math::max(min_gsp, 0.0f); } - - /* - * Set the nominal airspeed reference (true airspeed) [m/s]. - */ - void setAirspeedNom(float airsp) { airspeed_nom_ = math::max(airsp, 0.1f); } - - /* - * Set the maximum airspeed reference (true airspeed) [m/s]. - */ - void setAirspeedMax(float airsp) { airspeed_max_ = math::max(airsp, 0.1f); } - - /* - * Set the autopilot roll response time constant [s]. - */ - void setRollTimeConst(float tc) { roll_time_const_ = tc; } - - /* - * Set the switch distance multiplier. - */ - void setSwitchDistanceMultiplier(float mult) { switch_distance_multiplier_ = math::max(mult, 0.1f); } - - /* - * Set the period safety factor. - */ - void setPeriodSafetyFactor(float sf) { period_safety_factor_ = math::max(sf, 1.0f); } - - /* - * @return Controller proportional gain [rad/s] - */ - float getPGain() const { return p_gain_; } - - /* - * @return Controller time constant [s] - */ - float getTimeConst() const { return time_const_; } - - /* - * @return Adapted controller period [s] - */ - float getAdaptedPeriod() const { return adapted_period_; } - - /* - * @return Track error boundary [m] - */ - float getTrackErrorBound() const { return track_error_bound_; } - - /* - * @return Signed track error [m] - */ - float getTrackError() const { return signed_track_error_; } - - /* - * @return Airspeed reference [m/s] - */ - float getAirspeedRef() const { return airspeed_ref_; } - - /* - * @return Heading angle reference [rad] - */ - float getHeadingRef() const { return atan2f(air_vel_ref_(1), air_vel_ref_(0)); } - - /* - * @return Bearing angle [rad] - */ - float getBearing() const { return atan2f(bearing_vec_(1), bearing_vec_(0)); } - - /* - * @return Lateral acceleration command [m/s^2] - */ - float getLateralAccel() const { return lateral_accel_; } - - /* - * @return Feed-forward lateral acceleration command increment for tracking - * path curvature [m/s^2] - */ - float getLateralAccelFF() const { return lateral_accel_ff_; } - - /* - * @return Bearing feasibility [0, 1] - */ - float getBearingFeas() const { return feas_; } - - /* - * @return On-track bearing feasibility [0, 1] - */ - float getOnTrackBearingFeas() const { return feas_on_track_; } - - /* - * @return Minimum forward ground speed reference [m/s] - */ - float getMinGroundSpeedRef() const { return min_ground_speed_ref_; } - - /* - * [Copied directly from ECL_L1_Pos_Controller] - * - * Set the maximum roll angle output in radians - */ - void setRollLimit(float roll_lim_rad) { roll_lim_rad_ = roll_lim_rad; } - - /* - * [Copied directly from ECL_L1_Pos_Controller] - * - * 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[in] wp_radius The switching radius the waypoint has set. - */ - float switchDistance(float wp_radius) const; - - /* - * [Copied directly from ECL_L1_Pos_Controller] - * - * Get roll angle setpoint for fixed wing. - * - * @return Roll angle (in NED frame) - */ - float getRollSetpoint() { return roll_setpoint_; } - -private: - static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_VALID{3.f}; ///< Factor of velocity standard deviation above which course calculation is considered good enough - static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_INVALID{2.f}; ///< Factor of velocity standard deviation below which course calculation is considered unsafe - static constexpr float COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK{0.09f}; ///< Cos of Heading to track angle below which it is assumed that the vehicle is not pushed back by the wind ~cos(85°) - static constexpr float COS_HEADING_TRACK_ANGLE_PUSHED_BACK{0.f}; ///< Cos of Heading to track angle above which it is assumed that the vehicle is pushed back by the wind - - static constexpr float NPFG_EPSILON = 1.0e-6; // small number *bigger than machine epsilon - static constexpr float MIN_RADIUS = 0.5f; // minimum effective radius (avoid singularities) [m] - static constexpr float NTE_FRACTION = 0.5f; // normalized track error fraction (must be > 0) - // ^determines at what fraction of the normalized track error the maximum track keeping forward ground speed demand is reached - static constexpr float AIRSPEED_BUFFER = 1.5f; // airspeed buffer [m/s] (must be > 0) - // ^the size of the feasibility transition region at cross wind angle >= 90 deg. - // This must be non-zero to avoid jumpy airspeed incrementation while using wind - // excess handling logic. Similarly used as buffer region around zero airspeed. - - /* - * tuning - */ - - float period_{10.0f}; // nominal (desired) period -- user defined [s] - float damping_{0.7071f}; // nominal (desired) damping ratio -- user defined - float p_gain_{0.8885f}; // proportional gain (computed from period_ and damping_) [rad/s] - float time_const_{7.071f}; // time constant (computed from period_ and damping_) [s] - float adapted_period_{10.0f}; // auto-adapted period (if stability bounds enabled) [s] - - /* - * user defined guidance settings - */ - - // guidance options - bool en_period_lb_{true}; // enables automatic lower bound constraints on controller period - bool en_period_ub_{true}; // enables automatic upper bound constraints on controller period (remains disabled if lower bound is disabled) - bool en_min_ground_speed_{true}; // the airspeed reference is incremented to sustain a user defined minimum forward ground speed - bool en_track_keeping_{false}; // the airspeed reference is incremented to return to the track and sustain zero ground velocity until excess wind subsides - bool en_wind_excess_regulation_{true}; // the airspeed reference is incremented to regulate the excess wind, but not overcome it ... - // ^disabling this parameter disables all other excess wind handling options, using only the nominal airspeed for reference - - // guidance settings - float airspeed_nom_{15.0f}; // nominal (desired) true airspeed reference (generally equivalent to cruise optimized airspeed) [m/s] - float airspeed_max_{20.0f}; // maximum true airspeed reference - the maximum achievable/allowed airspeed reference [m/s] - float roll_time_const_{0.0f}; // autopilot roll response time constant [s] - float min_gsp_desired_{0.0f}; // user defined miminum desired forward ground speed [m/s] - float min_gsp_track_keeping_max_{5.0f}; // maximum, minimum forward ground speed demand from track keeping logic [m/s] - - // guidance parameters - float switch_distance_multiplier_{0.32f}; // a value multiplied by the track error boundary resulting in a lower switch distance - // ^as the bearing angle changes quadratically (instead of linearly as in L1), the time constant (automatically calculated for on track stability) proportional track error boundary typically over estimates the required switching distance - float period_safety_factor_{1.5f}; // multiplied by the minimum period for conservative lower bound - - /* - * internal guidance states - */ - - // speeds - float min_gsp_track_keeping_{0.0f}; // minimum forward ground speed demand from track keeping logic [m/s] - float min_ground_speed_ref_{0.0f}; // resultant minimum forward ground speed reference considering all active guidance logic [m/s] - - //bearing feasibility - float feas_{1.0f}; // continous representation of bearing feasibility in [0,1] (0=infeasible, 1=feasible) - float feas_on_track_{1.0f}; // continuous bearing feasibility "on track" - - // track proximity - float track_error_bound_{212.13f}; // the current ground speed dependent track error bound [m] - float track_proximity_{0.0f}; // value in [0,1] indicating proximity to track, 0 = at track error boundary or beyond, 1 = on track - - // path following states - float signed_track_error_{0.0f}; // signed track error [m] - matrix::Vector2f bearing_vec_{matrix::Vector2f{1.0f, 0.0f}}; // bearing unit vector - - /* - * guidance outputs - */ - - float airspeed_ref_{15.0f}; // true airspeed reference [m/s] - matrix::Vector2f air_vel_ref_{matrix::Vector2f{15.0f, 0.0f}}; // true air velocity reference vector [m/s] - float lateral_accel_{0.0f}; // lateral acceleration reference [m/s^2] - float lateral_accel_ff_{0.0f}; // lateral acceleration demand to maintain path curvature [m/s^2] - - /* - * ECL_L1_Pos_Controller functionality - */ - - float roll_lim_rad_{math::radians(30.0f)}; // maximum roll angle [rad] - float roll_setpoint_{0.0f}; // current roll angle setpoint [rad] - - /* - * Adapts the controller period considering user defined inputs, current flight - * condition, path properties, and stability bounds. - * - * @param[in] ground_speed Vehicle ground speed [m/s] - * @param[in] airspeed Vehicle true airspeed [m/s] - * @param[in] wind_speed Wind speed [m/s] - * @param[in] track_error Track error (magnitude) [m] - * @param[in] path_curvature Path curvature at closest point on track [m^-1] - * @param[in] wind_vel Wind velocity vector in inertial frame [m/s] - * @param[in] unit_path_tangent Unit vector tangent to path at closest point - * in direction of path - * @param[in] feas_on_track Bearing feasibility on track at the closest point - * @return Adapted period [s] - */ - float adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed, - const float track_error, const float path_curvature, const matrix::Vector2f &wind_vel, - const matrix::Vector2f &unit_path_tangent, const float feas_on_track) const; - - /* - * Returns normalized (unitless) and constrained track error [0,1]. - * - * @param[in] track_error Track error (magnitude) [m] - * @param[in] track_error_bound Track error boundary [m] - * @return Normalized track error - */ - float normalizedTrackError(const float track_error, const float track_error_bound) const; - - /* - * Cacluates an approximation of the wind factor (see [TODO: include citation]). - * - * @param[in] airspeed Vehicle true airspeed [m/s] - * @param[in] wind_speed Wind speed [m/s] - * @return Non-dimensional wind factor approximation - */ - float windFactor(const float airspeed, const float wind_speed) const; - - /* - * Calculates a theoretical upper bound on the user defined period to maintain - * track keeping stability. - * - * @param[in] air_turn_rate The turn rate required to track the current path - * curvature at the current true airspeed, in a no-wind condition [rad/s] - * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation]) - * @return Period upper bound [s] - */ - float periodUpperBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const; - - /* - * Calculates a theoretical lower bound on the user defined period to avoid - * limit cycle oscillations considering an acceleration actuation delay (e.g. - * roll response delay). Note this lower bound defines *marginal stability, - * and a safety factor should be applied in addition to the returned value. - * - * @param[in] air_turn_rate The turn rate required to track the current path - * curvature at the current true airspeed, in a no-wind condition [rad/s] - * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation]) - * @return Period lower bound [s] - */ - float periodLowerBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const; - - /* - * Computes a continous non-dimensional track proximity [0,1] - 0 when the - * vehicle is at the track error boundary, and 1 when on track. - * - * @param[in] look_ahead_ang The angle at which the bearing vector deviates - * from the unit track error vector [rad] - * @return Track proximity - */ - float trackProximity(const float look_ahead_ang) const; - - /* - * Calculates a ground speed modulated track error bound under which the - * look ahead angle is quadratically transitioned from alignment with the - * track error vector to that of the path tangent vector. - * - * @param[in] ground_speed Vehicle ground speed [m/s] - * @param[in] time_const Controller time constant [s] - * @return Track error boundary [m] - */ - float trackErrorBound(const float ground_speed, const float time_const) const; - - /* - * Calculates the required controller proportional gain to achieve the desired - * system period and damping ratio. NOTE: actual period and damping will vary - * when following paths with curvature in wind. - * - * @param[in] period Desired system period [s] - * @param[in] damping Desired system damping ratio - * @return Proportional gain [rad/s] - */ - float pGain(const float period, const float damping) const; - - /* - * Calculates the required controller time constant to achieve the desired - * system period and damping ratio. NOTE: actual period and damping will vary - * when following paths with curvature in wind. - * - * @param[in] period Desired system period [s] - * @param[in] damping Desired system damping ratio - * @return Time constant [s] - */ - float timeConst(const float period, const float damping) const; - - /* - * Cacluates the look ahead angle as a quadratic function of the normalized - * track error. - * - * @param[in] normalized_track_error Normalized track error (track error / track error boundary) - * @return Look ahead angle [rad] - */ - float lookAheadAngle(const float normalized_track_error) const; - - /* - * Calculates the bearing vector and track proximity transitioning variable - * from the look-ahead angle mapping. - * - * @param[in] unit_path_tangent Unit vector tangent to path at closest point - * in direction of path - * @param[in] look_ahead_ang The bearing vector lies at this angle from - * the path normal vector [rad] - * @param[in] signed_track_error Signed error to track at closest point (sign - * determined by path normal direction) [m] - * @return Unit bearing vector - */ - matrix::Vector2f bearingVec(const matrix::Vector2f &unit_path_tangent, const float look_ahead_ang, - const float signed_track_error) const; - - /* - * Calculates the minimum forward ground speed demand for minimum forward - * ground speed maintanence as well as track keeping logic. - * - * @param[in] normalized_track_error Normalized track error (track error / track error boundary) - * @param[in] feas Bearing feasibility - * @return Minimum forward ground speed demand [m/s] - */ - float minGroundSpeed(const float normalized_track_error, const float feas); - - /* - * Determines a reference air velocity *without curvature compensation, but - * including "optimal" true airspeed reference compensation in excess wind conditions. - * Nominal and maximum true airspeed member variables must be set before using this method. - * - * @param[in] wind_vel Wind velocity vector [m/s] - * @param[in] bearing_vec Bearing vector - * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] - * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] - * @param[in] wind_speed Wind speed [m/s] - * @param[in] min_ground_speed Minimum commanded forward ground speed [m/s] - * @return Air velocity vector [m/s] - */ - matrix::Vector2f refAirVelocity(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec, - const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed, - const float min_ground_speed) const; - - /* - * Projection of the air velocity vector onto the bearing line considering - * a connected wind triangle. - * - * @param[in] airspeed Vehicle true airspeed [m/s] - * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] - * @return Projection of air velocity vector on bearing vector [m/s] - */ - float projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const; - - /* - * Check for binary bearing feasibility. - * - * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] - * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] - * @param[in] airspeed Vehicle true airspeed [m/s] - * @param[in] wind_speed Wind speed [m/s] - * @return Binary bearing feasibility: 1 if feasible, 0 if infeasible - */ - int bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, - const float wind_speed) const; - - /* - * Air velocity solution for a given wind velocity and bearing vector assuming - * a "high speed" (not backwards) solution in the excess wind case. - * - * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] - * @param[in] airsp_dot_bearing 2D dot product of air velocity (solution) and bearing vector [m/s] - * @param[in] bearing_vec Bearing vector - * @return Air velocity vector [m/s] - */ - matrix::Vector2f solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing, - const matrix::Vector2f &bearing_vec) const; - - - /* - * Air velocity solution for an infeasible bearing. - * - * @param[in] wind_vel Wind velocity vector [m/s] - * @param[in] bearing_vec Bearing vector - * @param[in] wind_speed Wind speed [m/s] - * @param[in] airspeed Vehicle true airspeed [m/s] - * @return Air velocity vector [m/s] - */ - matrix::Vector2f infeasibleAirVelRef(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec, - const float wind_speed, const float airspeed) const; - - - /* - * Cacluates a continuous representation of the bearing feasibility from [0,1]. - * 0 = infeasible, 1 = fully feasible, partial feasibility in between. - * - * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] - * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] - * @param[in] airspeed Vehicle true airspeed [m/s] - * @param[in] wind_speed Wind speed [m/s] - * @return bearing feasibility - */ - float bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, - const float wind_speed) const; - - /* - * Calculates an additional feed-forward lateral acceleration demand considering - * the path curvature. - * - * @param[in] unit_path_tangent Unit vector tangent to path at closest point - * in direction of path - * @param[in] ground_vel Vehicle ground velocity vector [m/s] - * @param[in] wind_vel Wind velocity vector [m/s] - * @param[in] airspeed Vehicle true airspeed [m/s] - * @param[in] wind_speed Wind speed [m/s] - * @param[in] signed_track_error Signed error to track at closest point (sign - * determined by path normal direction) [m] - * @param[in] path_curvature Path curvature at closest point on track [m^-1] - * @return Feed-forward lateral acceleration command [m/s^2] - */ - float lateralAccelFF(const matrix::Vector2f &unit_path_tangent, const matrix::Vector2f &ground_vel, - const float wind_dot_upt, const float wind_cross_upt, const float airspeed, - const float wind_speed, const float signed_track_error, const float path_curvature) const; - - /* - * Calculates a lateral acceleration demand from the heading error. - * (does not consider path curvature) - * - * @param[in] air_vel Vechile air velocity vector [m/s] - * @param[in] air_vel_ref Reference air velocity vector [m/s] - * @param[in] airspeed Vehicle true airspeed [m/s] - * @return Lateral acceleration demand [m/s^2] - */ - float lateralAccel(const matrix::Vector2f &air_vel, const matrix::Vector2f &air_vel_ref, - const float airspeed) const; - - /******************************************************************************* - * PX4 POSITION SETPOINT INTERFACE FUNCTIONS - */ - - /** - * [Copied directly from ECL_L1_Pos_Controller] - * - * Update roll angle setpoint. This will also apply slew rate limits if set. - */ - void updateRollSetpoint(); - -}; // class NPFG - -#endif // NPFG_H_