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

PositionControl: replace interfacemapping checks

Browse Source 
Removing the skip_controller and interfaceMapping
concept and replacing it with a single method checking
if the position control update was successful and
return the result in the PositionControl.update().
This commit is contained in:
Matthias Grob
2020-01-23 18:23:05 +01:00
committed by Kabir Mohammed
parent e06fff94bb
commit b75c1308f9
5 changed files with 126 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
+92 -11
View File
@@ -88,6 +88,11 @@ public:
_contraints.speed_up = NAN;
_contraints.speed_down = NAN;
resetInputSetpoint();
}
void resetInputSetpoint()
{
_input_setpoint.x = NAN;
_input_setpoint.y = NAN;
_input_setpoint.z = NAN;
@@ -100,13 +105,14 @@ public:
Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.thrust);
}
void runController()
bool runController()
{
_position_control.setConstraints(_contraints);
_position_control.setInputSetpoint(_input_setpoint);
_position_control.update(.1f);
const bool ret = _position_control.update(.1f);
_position_control.getLocalPositionSetpoint(_output_setpoint);
_position_control.getAttitudeSetpoint(_attitude);
return ret;
}
PositionControl _position_control;
@@ -138,7 +144,7 @@ TEST_F(PositionControlBasicDirectionTest, PositionDirection)
_input_setpoint.x = .1f;
_input_setpoint.y = .1f;
_input_setpoint.z = -.1f;
runController();
EXPECT_TRUE(runController());
checkDirection();
}
@@ -147,7 +153,7 @@ TEST_F(PositionControlBasicDirectionTest, VelocityDirection)
_input_setpoint.vx = .1f;
_input_setpoint.vy = .1f;
_input_setpoint.vz = -.1f;
runController();
EXPECT_TRUE(runController());
checkDirection();
}
@@ -157,14 +163,14 @@ TEST_F(PositionControlBasicTest, TiltLimit)
_input_setpoint.y = 10.f;
_input_setpoint.z = -0.f;
runController();
EXPECT_TRUE(runController());
Vector3f body_z = Quatf(_attitude.q_d).dcm_z();
float angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f)));
EXPECT_GT(angle, 0.f);
EXPECT_LE(angle, 1.f);
_contraints.tilt = .5f;
runController();
EXPECT_TRUE(runController());
body_z = Quatf(_attitude.q_d).dcm_z();
angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f)));
EXPECT_GT(angle, 0.f);
@@ -177,7 +183,7 @@ TEST_F(PositionControlBasicTest, VelocityLimit)
_input_setpoint.y = 10.f;
_input_setpoint.z = -10.f;
runController();
EXPECT_TRUE(runController());
Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy);
EXPECT_LE(velocity_xy.norm(), 1.f);
EXPECT_LE(abs(_output_setpoint.vz), 1.f);
@@ -189,7 +195,7 @@ TEST_F(PositionControlBasicTest, ThrustLimit)
_input_setpoint.y = 10.f;
_input_setpoint.z = -10.f;
runController();
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);
@@ -202,7 +208,7 @@ TEST_F(PositionControlBasicTest, FailsafeInput)
_input_setpoint.thrust[0] = _input_setpoint.thrust[1] = 0.f;
_input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f;
runController();
EXPECT_TRUE(runController());
EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f);
EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f);
EXPECT_LT(_output_setpoint.thrust[2], -.1f);
@@ -217,7 +223,7 @@ TEST_F(PositionControlBasicTest, InputCombinationsPosition)
_input_setpoint.y = .2f;
_input_setpoint.z = .3f;
runController();
EXPECT_TRUE(runController());
EXPECT_FLOAT_EQ(_output_setpoint.x, .1f);
EXPECT_FLOAT_EQ(_output_setpoint.y, .2f);
EXPECT_FLOAT_EQ(_output_setpoint.z, .3f);
@@ -235,7 +241,7 @@ TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
_input_setpoint.vy = .2f;
_input_setpoint.z = .3f; // altitude
runController();
EXPECT_TRUE(runController());
// EXPECT_TRUE(isnan(_output_setpoint.x));
// EXPECT_TRUE(isnan(_output_setpoint.y));
EXPECT_FLOAT_EQ(_output_setpoint.z, .3f);
@@ -246,3 +252,78 @@ TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
EXPECT_FALSE(isnan(_output_setpoint.thrust[1]));
EXPECT_FALSE(isnan(_output_setpoint.thrust[2]));
}
TEST_F(PositionControlBasicTest, SetpointValiditySimple)
{
EXPECT_FALSE(runController());
_input_setpoint.x = .1f;
EXPECT_FALSE(runController());
_input_setpoint.y = .2f;
EXPECT_FALSE(runController());
_input_setpoint.thrust[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]
};
for (int combination = 0; combination < 512; combination++) {
resetInputSetpoint();
for (int j = 0; j < 9; j++) {
if (combination & (1 << j)) {
// Set arbitrary finite value, some values clearly hit the limits to check these corner case combinations
*(setpoint_loop_access_map[j]) = static_cast<float>(combination) / static_cast<float>(j + 1);
}
}
// Expect at least one setpoint per axis
const bool has_x_setpoint = ((combination & 7) != 0);
const bool has_y_setpoint = (((combination >> 3) & 7) != 0);
const bool has_z_setpoint = (((combination >> 6) & 7) != 0);
// Expect xy setpoints to come in pairs
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;
}
}
TEST_F(PositionControlBasicTest, InvalidState)
{
_input_setpoint.x = .1f;
_input_setpoint.y = .2f;
_input_setpoint.z = .3f;
PositionControlStates states{};
states.position(0) = NAN;
_position_control.setState(states);
EXPECT_FALSE(runController());
states.velocity(0) = NAN;
_position_control.setState(states);
EXPECT_FALSE(runController());
states.position(0) = 0.f;
_position_control.setState(states);
EXPECT_FALSE(runController());
states.velocity(0) = 0.f;
states.acceleration(1) = NAN;
_position_control.setState(states);
EXPECT_FALSE(runController());
}