From 84a0331856f91e6f0a5cd81aaa1f42d1c88afbd3 Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Wed, 16 Apr 2025 15:57:34 +0200 Subject: [PATCH] NPFG: add unit tests and add back feasible bearing check back --- src/lib/npfg/CMakeLists.txt | 1 + src/lib/npfg/CourseToAirspeedRefMapper.cpp | 31 +- src/lib/npfg/NpfgTest.cpp | 398 +++++++++++++++++++++ 3 files changed, 418 insertions(+), 12 deletions(-) create mode 100644 src/lib/npfg/NpfgTest.cpp diff --git a/src/lib/npfg/CMakeLists.txt b/src/lib/npfg/CMakeLists.txt index a8306fbcea..de489b18c8 100644 --- a/src/lib/npfg/CMakeLists.txt +++ b/src/lib/npfg/CMakeLists.txt @@ -41,3 +41,4 @@ px4_add_library(npfg ) target_link_libraries(npfg PRIVATE geo) +px4_add_unit_gtest(SRC NpfgTest.cpp LINKLIBS npfg) diff --git a/src/lib/npfg/CourseToAirspeedRefMapper.cpp b/src/lib/npfg/CourseToAirspeedRefMapper.cpp index 7e745486b4..170348f139 100644 --- a/src/lib/npfg/CourseToAirspeedRefMapper.cpp +++ b/src/lib/npfg/CourseToAirspeedRefMapper.cpp @@ -40,11 +40,18 @@ CourseToAirspeedRefMapper::mapCourseSetpointToHeadingSetpoint(const float bearin { 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 float airsp_dot_bearing = projectAirspOnBearing(airspeed_sp, wind_cross_bearing); - const Vector2f air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vector); + Vector2f air_vel_ref; - // TODO check if we need to use infeasibleAirVelRef or other mitigation functions in some cases (high wind) + if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_sp, wind_vel.norm())) { + + const float airsp_dot_bearing = projectAirspOnBearing(airspeed_sp, wind_cross_bearing); + air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vector); + + } else { + air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vector, wind_vel.norm(), airspeed_sp); + } return atan2f(air_vel_ref(1), air_vel_ref(0)); } @@ -96,12 +103,12 @@ CourseToAirspeedRefMapper::solveWindTriangle(const float wind_cross_bearing, con 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; -// } +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/NpfgTest.cpp b/src/lib/npfg/NpfgTest.cpp new file mode 100644 index 0000000000..299753cf76 --- /dev/null +++ b/src/lib/npfg/NpfgTest.cpp @@ -0,0 +1,398 @@ + +/**************************************************************************** + * + * Copyright (C) 2025 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. + * + ****************************************************************************/ + +/****************************************************************** + * Test code for the NPFG algorithm + * Run this test only using "make tests TESTFILTER=Npfg" + * + * + * NOTE: + * + * +******************************************************************/ + +#include +#include +#include + +using namespace matrix; + +TEST(NpfgTest, Test) +{ + // V C + // / + // / + // / + // P + const Vector2f curr_wp_ned(10.f, 10.f); + float target_bearing1 = NAN; + // NaN speed + EXPECT_FALSE(PX4_ISFINITE(target_bearing1)); +} + +TEST(NpfgTest, NoWind) +{ + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN + const Vector2f wind_vel(0.f, 0.f); + float bearing = 0.f; + float airspeed_max = 20.f; + float min_ground_speed = 5.0f; + float airspeed_setpoint = 15.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = _course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted); + + // THEN: expect heading due North with a min airspeed equal to min_ground_speed + EXPECT_NEAR(heading_setpoint, 0.f, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, min_ground_speed, FLT_EPSILON); + + // GIVEN: bearing due South + bearing = M_PI_F; + airspeed_max = 20.f; + min_ground_speed = 5.0f; + + // WHEN: we update bearing and airspeed magnitude augmentation + min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + + // THEN: expect heading due South with a min airspeed equal to min_ground_speed + EXPECT_NEAR(heading_setpoint, -M_PI_F, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, min_ground_speed, FLT_EPSILON); +} + +TEST(NpfgTest, LightCrossWind) +{ + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN + const Vector2f wind_vel(0.f, 6.f); + float bearing = 0.f; + float airspeed_max = 20.f; + float min_ground_speed = 5.0f; + float airspeed_setpoint = 15.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading -0.4115168 with a min airspeed of to 7.8 (sqrt(25+36)) + EXPECT_NEAR(heading_setpoint, -0.4115168, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, 7.8f, 0.1f); + + // GIVEN: bearing due South + bearing = M_PI_F; + airspeed_max = 20.f; + min_ground_speed = 5.0f; + + // WHEN: we update bearing and airspeed magnitude augmentation + min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading of -2.73 and a min airspeed of 7.8 (sqrt(25+36)) + EXPECT_NEAR(heading_setpoint, -2.73f, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, 7.8f, 0.1f); +} + +TEST(NpfgTest, StrongHeadWind) +{ + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN + const Vector2f wind_vel(-16.f, 0.f); + float bearing = 0.f; + float airspeed_max = 25.f; + float min_ground_speed = 5.0f; + float airspeed_setpoint = 15.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + + // THEN: expect heading due North with a min airspeed equal to 16+min_ground_speed + EXPECT_NEAR(heading_setpoint, 0.f, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, 16 + min_ground_speed, 0.1f); + + +} + +TEST(NpfgTest, StrongTailWind) +{ + + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN: bearing due South + const Vector2f wind_vel(-16.f, 0.f); + float bearing = M_PI_F; + float airspeed_max = 25.f; + float min_ground_speed = 5.0f; + float airspeed_setpoint = 15.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading due South with a min airspeed at 0 + EXPECT_NEAR(heading_setpoint, -M_PI_F, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, 0.f, 0.1f); +} + + + +TEST(NpfgTest, ExcessHeadWind) +{ + + // TEST DESCRIPTION: infeasible bearing, with |wind| = |airspeed|. Align with wind + + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN + const Vector2f wind_vel(-25.f, 0.f); + float bearing = 0.f; + float airspeed_max = 25.f; + float min_ground_speed = 5.0f; + float airspeed_setpoint = 15.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading sp due North with a min airspeed equal to airspeed_max + EXPECT_NEAR(heading_setpoint, 0.f, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, airspeed_max, 0.1f); + + // WHEN: we increase the maximum airspeed + airspeed_max = 35.f; + + min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + // THEN: expect the minimum airspeed to be high enough to maintain minimum groundspeed + EXPECT_NEAR(min_airspeed_for_bearing, 30.f, 0.1f); + + // TEST DESCRIPTION: infeasible bearing, with |wind| = |airspeed|. Align with wind + + // GIVEN: bearing east + bearing = M_PI_F / 2.f; + airspeed_max = 25.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading sp due North with a min airspeed equal to airspeed_max + EXPECT_NEAR(heading_setpoint, 0.f, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, airspeed_max, 0.1f); + + + // TEST DESCRIPTION: infeasible bearing, with |wind| > |airspeed|. Aircraft should have a heading between the target bearing + // and wind direction to minimize drift while still attempting to reach the bearing. + + // GIVEN: bearing NE + bearing = M_PI_F / 4.f; + airspeed_max = 20.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading setpoint to be between the target bearing and the cross wind + // & the minimum airspeed to be = maximum airspeed + EXPECT_TRUE((heading_setpoint > -M_PI_F / 2.f) && (heading_setpoint < bearing)); + EXPECT_NEAR(min_airspeed_for_bearing, airspeed_max, 0.1f); + +} + +TEST(NpfgTest, ExcessTailWind) +{ + + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN: bearing due South + const Vector2f wind_vel(-25.f, 0.f); + float bearing = M_PI_F; + float airspeed_max = 25.f; + float min_ground_speed = 5.0f; + float airspeed_setpoint = 15.f; + + // WHEN: we update bearing and airspeed magnitude augmentation + float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading due South with a min airspeed equal to 0 + EXPECT_NEAR(heading_setpoint, -M_PI_F, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, 0.f, 0.1f); + +} + +TEST(NpfgTest, ExcessCrossWind) +{ + + // TEST DESCRIPTION: infeasible bearing, with |wind| > |airspeed|. Aircraft should have a heading between the target bearing + // and wind direction to minimize drift while still attempting to reach the bearing. + + CourseToAirspeedRefMapper _course_to_airspeed; + + // GIVEN + const Vector2f wind_vel(0, 30.f); + float bearing = 0.f; + float airspeed_max = 25.f; + float min_ground_speed = 5.f; + float airspeed_setpoint = 15.f; + + + // WHEN: we update bearing and airspeed magnitude augmentation + float min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + float airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + float heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + // THEN: expect heading setpoint to be between the target bearing and the cross wind + // & the minimum airspeed to be = maximum airspeed + EXPECT_TRUE((heading_setpoint > -M_PI_F / 2.f) && (heading_setpoint < bearing)); + EXPECT_NEAR(min_airspeed_for_bearing, airspeed_max, 0.1f); + + + // TEST DESCRIPTION: infeasible bearing, with |wind| = |airspeed|. Align with wind. + + airspeed_max = 30.f; + + min_airspeed_for_bearing = _course_to_airspeed.getMinAirspeedForCurrentBearing(bearing, wind_vel, + airspeed_max, min_ground_speed); + + airspeed_setpoint_adapted = math::constrain(airspeed_setpoint, min_airspeed_for_bearing, airspeed_max); + + heading_setpoint = matrix::wrap_pi(_course_to_airspeed.mapCourseSetpointToHeadingSetpoint(bearing, wind_vel, + airspeed_setpoint_adapted)); + + EXPECT_NEAR(heading_setpoint, -M_PI_F / 2.f, 0.01f); + EXPECT_NEAR(min_airspeed_for_bearing, airspeed_max, 0.1f); + +} + +TEST(NpfgTest, HeadingControl) +{ + + AirspeedReferenceController _airspeed_reference_controller; + const float p_gain = 0.8885f; + + // GIVEN: that we are already aligned with out heading setpoint + float heading_sp = 0.f; + float heading = 0.f; + float airspeed = 15.f; + + // WHEN: we compute the lateral acceleration setpoint + float lateral_acceleration_setpoint = _airspeed_reference_controller.controlHeading(heading_sp, heading, airspeed); + + // THEN: we expect 0 lateral acceleration + EXPECT_NEAR(lateral_acceleration_setpoint, 0.f, 0.01f); + + // GIVEN: current heading 45 deg NW + heading = - M_PI_F / 4.f; + + // WHEN: we compute the lateral acceleration setpoint + lateral_acceleration_setpoint = _airspeed_reference_controller.controlHeading(heading_sp, heading, airspeed); + + // THEN: we expect a positive lateral acceleration input. = Airspeed vector + // required to correct the difference between the setpoint and the current heading, + // scaled by p_gain. + EXPECT_NEAR(lateral_acceleration_setpoint, airspeed * sinf(heading_sp - heading) * p_gain, 0.01f); + + // GIVEN: current heading 180 (South) + heading = M_PI_F; + + // WHEN: we compute the lateral acceleration setpoint + lateral_acceleration_setpoint = _airspeed_reference_controller.controlHeading(heading_sp, heading, airspeed); + + // THEN: we we expect maxmimum lateral acceleration setpoint + EXPECT_NEAR(lateral_acceleration_setpoint, airspeed * p_gain, 0.01f); + +}