fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-12 22:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl: acceleration based control strategy

Browse Source
This commit is contained in:
Matthias Grob
2020-01-27 12:03:29 +01:00
parent 1d2ac41edc
commit b3d7445059
4 changed files with 95 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
+56 -21
View File
@@ -188,17 +188,40 @@ TEST_F(PositionControlBasicTest, VelocityLimit)
EXPECT_LE(abs(_output_setpoint.vz), 1.f);
}
TEST_F(PositionControlBasicTest, ThrustLimit)
TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
{
_input_setpoint.x = 10.f;
_input_setpoint.y = 10.f;
_input_setpoint.z = -10.f;
EXPECT_TRUE(runController());
EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f);
EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f);
EXPECT_LT(_attitude.thrust_body[2], -.1f);
EXPECT_GE(_attitude.thrust_body[2], -0.9f);
runController();
Vector3f thrust(_output_setpoint.thrust);
EXPECT_FLOAT_EQ(thrust(0), 0.f);
EXPECT_FLOAT_EQ(thrust(1), 0.f);
EXPECT_FLOAT_EQ(thrust(2), -0.9f);
EXPECT_EQ(_attitude.thrust_body[0], 0.f);
EXPECT_EQ(_attitude.thrust_body[1], 0.f);
EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.9f);
EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f);
}
TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
{
_input_setpoint.x = 10.f;
_input_setpoint.y = 0.f;
_input_setpoint.z = 10.f;
runController();
Vector3f thrust(_output_setpoint.thrust);
EXPECT_FLOAT_EQ(thrust.length(), 0.1f);
EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f);
EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f);
}
TEST_F(PositionControlBasicTest, FailsafeInput)
@@ -216,6 +239,17 @@ TEST_F(PositionControlBasicTest, FailsafeInput)
EXPECT_LE(_attitude.thrust_body[2], -.1f);
}
TEST_F(PositionControlBasicTest, IdleThrustInput)
{
// High downwards acceleration to make sure there's no thrust
Vector3f(0.f, 0.f, 100.f).copyTo(_input_setpoint.acceleration);
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);
}
TEST_F(PositionControlBasicTest, InputCombinationsPosition)
{
_input_setpoint.x = .1f;
@@ -259,16 +293,16 @@ TEST_F(PositionControlBasicTest, SetpointValiditySimple)
EXPECT_FALSE(runController());
_input_setpoint.y = .2f;
EXPECT_FALSE(runController());
_input_setpoint.thrust[2] = .3f;
_input_setpoint.acceleration[2] = .3f;
EXPECT_TRUE(runController());
}
TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
{
// This test runs any position, velocity, thrust setpoint combination and checks if it gets accepted or rejected correctly
float *const setpoint_loop_access_map[] = {&_input_setpoint.x, &_input_setpoint.vx, &_input_setpoint.thrust[0],
&_input_setpoint.y, &_input_setpoint.vy, &_input_setpoint.thrust[1],
&_input_setpoint.z, &_input_setpoint.vz, &_input_setpoint.thrust[2]
// 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]
};
for (int combination = 0; combination < 512; combination++) {
@@ -289,16 +323,17 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
const bool has_xy_pairs = (combination & 7) == ((combination >> 3) & 7);
const bool expected_result = has_x_setpoint && has_y_setpoint && has_z_setpoint && has_xy_pairs;
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
<< "thrust " << _input_setpoint.thrust[0] << ", " << _input_setpoint.thrust[1] << ", " << _input_setpoint.thrust[2]
<< std::endl << "output" << std::endl
<< "position " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl
<< "velocity " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl
<< "thrust " << _output_setpoint.thrust[0] << ", " << _output_setpoint.thrust[1] << ", " << _output_setpoint.thrust[2]
<< std::endl;
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
<< "acceleration " << _input_setpoint.acceleration[0] << ", "
<< _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl
<< "output" << std::endl
<< "position " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl
<< "velocity " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl
<< "acceleration " << _output_setpoint.acceleration[0] << ", "
<< _output_setpoint.acceleration[1] << ", " << _output_setpoint.acceleration[2] << std::endl;
}
}