mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-18 18:49:05 +08:00
369 lines
14 KiB
C++
369 lines
14 KiB
C++
|
|
/****************************************************************************
|
|
*
|
|
* 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/AirspeedDirectionController.hpp>
|
|
|
|
using namespace matrix;
|
|
|
|
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 East
|
|
bearing = M_PI_2_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 East with a min airspeed equal to min_ground_speed
|
|
EXPECT_NEAR(heading_setpoint, M_PI_2_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: bearing due North and wind from the North
|
|
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 East and wind from the West
|
|
const Vector2f wind_vel(0.f, 16.f);
|
|
float bearing = M_PI_2_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 East with a min airspeed at 0
|
|
EXPECT_NEAR(heading_setpoint, M_PI_2_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: bearing due North and wind from the North
|
|
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 East and wind from the West
|
|
const Vector2f wind_vel(0.f, 25.f);
|
|
float bearing = M_PI_2_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
|
|
const 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 East with a min airspeed equal to 0
|
|
EXPECT_NEAR(heading_setpoint, M_PI_2_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: bearing due North, strong wind due East
|
|
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)
|
|
{
|
|
AirspeedDirectionController _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);
|
|
}
|