fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 09:20:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix confusion between trajectory_setpoint and vehicle_local_postion_setpoint

Browse Source
This commit is contained in:
Matthias Grob
2022-10-17 16:07:11 +02:00
committed by Daniel Agar
parent 7c237fca74
commit 75c63aee2a
13 changed files with 91 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
+31 -66
View File
@@ -79,22 +79,6 @@ public:
_position_control.setHorizontalThrustMargin(HORIZONTAL_THRUST_MARGIN);
_position_control.setTiltLimit(1.f);
_position_control.setHoverThrust(.5f);
resetInputSetpoint();
}
void resetInputSetpoint()
{
_input_setpoint.x = NAN;
_input_setpoint.y = NAN;
_input_setpoint.z = NAN;
_input_setpoint.yaw = NAN;
_input_setpoint.yawspeed = NAN;
_input_setpoint.vx = NAN;
_input_setpoint.vy = NAN;
_input_setpoint.vz = NAN;
Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.acceleration);
Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.thrust);
}
bool runController()
@@ -107,7 +91,7 @@ public:
}
PositionControl _position_control;
vehicle_local_position_setpoint_s _input_setpoint{};
trajectory_setpoint_s _input_setpoint{PositionControl::empty_trajectory_setpoint};
vehicle_local_position_setpoint_s _output_setpoint{};
vehicle_attitude_setpoint_s _attitude{};
@@ -134,27 +118,21 @@ public:
TEST_F(PositionControlBasicDirectionTest, PositionDirection)
{
_input_setpoint.x = .1f;
_input_setpoint.y = .1f;
_input_setpoint.z = -.1f;
Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.position);
EXPECT_TRUE(runController());
checkDirection();
}
TEST_F(PositionControlBasicDirectionTest, VelocityDirection)
{
_input_setpoint.vx = .1f;
_input_setpoint.vy = .1f;
_input_setpoint.vz = -.1f;
Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.velocity);
EXPECT_TRUE(runController());
checkDirection();
}
TEST_F(PositionControlBasicTest, TiltLimit)
{
_input_setpoint.x = 10.f;
_input_setpoint.y = 10.f;
_input_setpoint.z = -0.f;
Vector3f(10.f, 10.f, 0.f).copyTo(_input_setpoint.position);
EXPECT_TRUE(runController());
Vector3f body_z = Quatf(_attitude.q_d).dcm_z();
@@ -174,9 +152,7 @@ TEST_F(PositionControlBasicTest, TiltLimit)
TEST_F(PositionControlBasicTest, VelocityLimit)
{
_input_setpoint.x = 10.f;
_input_setpoint.y = 10.f;
_input_setpoint.z = -10.f;
Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position);
EXPECT_TRUE(runController());
Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy);
@@ -187,9 +163,7 @@ TEST_F(PositionControlBasicTest, VelocityLimit)
TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
{
// Given a setpoint that drives the controller into vertical and horizontal saturation
_input_setpoint.x = 10.f;
_input_setpoint.y = 10.f;
_input_setpoint.z = -10.f;
Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position);
// When you run it for one iteration
runController();
@@ -218,9 +192,7 @@ TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
{
_input_setpoint.x = 10.f;
_input_setpoint.y = 0.f;
_input_setpoint.z = 10.f;
Vector3f(10.f, 0.f, 10.f).copyTo(_input_setpoint.position);
runController();
Vector3f thrust(_output_setpoint.thrust);
@@ -234,9 +206,8 @@ TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
TEST_F(PositionControlBasicTest, FailsafeInput)
{
_input_setpoint.vz = .1f;
_input_setpoint.thrust[0] = _input_setpoint.thrust[1] = 0.f;
_input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f;
_input_setpoint.velocity[2] = .1f;
EXPECT_TRUE(runController());
EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f);
@@ -255,14 +226,12 @@ TEST_F(PositionControlBasicTest, IdleThrustInput)
EXPECT_TRUE(runController());
EXPECT_FLOAT_EQ(_output_setpoint.thrust[0], 0.f);
EXPECT_FLOAT_EQ(_output_setpoint.thrust[1], 0.f);
EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f);
EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); // minimum thrust
}
TEST_F(PositionControlBasicTest, InputCombinationsPosition)
{
_input_setpoint.x = .1f;
_input_setpoint.y = .2f;
_input_setpoint.z = .3f;
Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position);
EXPECT_TRUE(runController());
EXPECT_FLOAT_EQ(_output_setpoint.x, .1f);
@@ -278,13 +247,13 @@ TEST_F(PositionControlBasicTest, InputCombinationsPosition)
TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
{
_input_setpoint.vx = .1f;
_input_setpoint.vy = .2f;
_input_setpoint.z = .3f; // altitude
_input_setpoint.velocity[0] = .1f;
_input_setpoint.velocity[1] = .2f;
_input_setpoint.position[2] = .3f; // altitude
EXPECT_TRUE(runController());
// EXPECT_TRUE(isnan(_output_setpoint.x));
// EXPECT_TRUE(isnan(_output_setpoint.y));
EXPECT_TRUE(isnan(_output_setpoint.x));
EXPECT_TRUE(isnan(_output_setpoint.y));
EXPECT_FLOAT_EQ(_output_setpoint.z, .3f);
EXPECT_FLOAT_EQ(_output_setpoint.vx, .1f);
EXPECT_FLOAT_EQ(_output_setpoint.vy, .2f);
@@ -297,9 +266,9 @@ TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
TEST_F(PositionControlBasicTest, SetpointValiditySimple)
{
EXPECT_FALSE(runController());
_input_setpoint.x = .1f;
_input_setpoint.position[0] = .1f;
EXPECT_FALSE(runController());
_input_setpoint.y = .2f;
_input_setpoint.position[1] = .2f;
EXPECT_FALSE(runController());
_input_setpoint.acceleration[2] = .3f;
EXPECT_TRUE(runController());
@@ -308,13 +277,13 @@ TEST_F(PositionControlBasicTest, SetpointValiditySimple)
TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
{
// This test runs any combination of set and unset (NAN) setpoints and checks if it gets accepted or rejected correctly
float *const setpoint_loop_access_map[] = {&_input_setpoint.x, &_input_setpoint.vx, &_input_setpoint.acceleration[0],
&_input_setpoint.y, &_input_setpoint.vy, &_input_setpoint.acceleration[1],
&_input_setpoint.z, &_input_setpoint.vz, &_input_setpoint.acceleration[2]
float *const setpoint_loop_access_map[] = {&_input_setpoint.position[0], &_input_setpoint.velocity[0], &_input_setpoint.acceleration[0],
&_input_setpoint.position[1], &_input_setpoint.velocity[1], &_input_setpoint.acceleration[1],
&_input_setpoint.position[2], &_input_setpoint.velocity[2], &_input_setpoint.acceleration[2]
};
for (int combination = 0; combination < 512; combination++) {
resetInputSetpoint();
_input_setpoint = PositionControl::empty_trajectory_setpoint;
for (int j = 0; j < 9; j++) {
if (combination & (1 << j)) {
@@ -333,8 +302,10 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl
<< "input" << std::endl
<< "position " << _input_setpoint.x << ", " << _input_setpoint.y << ", " << _input_setpoint.z << std::endl
<< "velocity " << _input_setpoint.vx << ", " << _input_setpoint.vy << ", " << _input_setpoint.vz << std::endl
<< "position " << _input_setpoint.position[0] << ", "
<< _input_setpoint.position[1] << ", " << _input_setpoint.position[2] << std::endl
<< "velocity " << _input_setpoint.velocity[0] << ", "
<< _input_setpoint.velocity[1] << ", " << _input_setpoint.velocity[2] << std::endl
<< "acceleration " << _input_setpoint.acceleration[0] << ", "
<< _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl
<< "output" << std::endl
@@ -347,9 +318,7 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
TEST_F(PositionControlBasicTest, InvalidState)
{
_input_setpoint.x = .1f;
_input_setpoint.y = .2f;
_input_setpoint.z = .3f;
Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position);
PositionControlStates states{};
states.position(0) = NAN;
@@ -377,9 +346,7 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust)
const float hover_thrust = 0.6f;
_position_control.setHoverThrust(hover_thrust);
_input_setpoint.vx = 0.f;
_input_setpoint.vy = 0.f;
_input_setpoint.vz = -0.f;
Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity);
// WHEN: we run the controller
EXPECT_TRUE(runController());
@@ -400,16 +367,14 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust)
TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint)
{
// GIVEN: the controller was ran with an invalid setpoint containing some valid values
_input_setpoint.x = .1f;
_input_setpoint.y = .2f;
_input_setpoint.position[0] = .1f;
_input_setpoint.position[1] = .2f;
// all z-axis setpoints stay NAN
EXPECT_FALSE(runController());
// WHEN: we run the controller with a valid setpoint
resetInputSetpoint();
_input_setpoint.vx = 0.f;
_input_setpoint.vy = 0.f;
_input_setpoint.vz = 0.f;
_input_setpoint = PositionControl::empty_trajectory_setpoint;
Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity);
EXPECT_TRUE(runController());
// THEN: the integral did not wind up and produce unexpected deviation