NPFG: add unit tests and add back feasible bearing check back

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)

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;
+}

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 <gtest/gtest.h>
+#include <lib/npfg/CourseToAirspeedRefMapper.hpp>
+#include <lib/npfg/AirspeedReferenceController.hpp>
+
+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);
+
+}