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

TestVelocitySmoothing - Split time synchronization and final state checks into two different test functions

Browse Source 
Also improve the comments
This commit is contained in:
bresch 2019-08-28 19:09:04 +02:00 committed by Mathieu Bresciani
parent 94d15931f2
commit fe73cef224
1 changed files with 37 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp
View File

@ -45,13 +45,12 @@ using namespace matrix;
class VelocitySmoothingTest : public ::testing::Test
{
public:
void setConstraints(float j_max, float a_max, float v_max);
void setInitialConditions(Vector3f acc, Vector3f vel, Vector3f pos);
void updateTrajectories(Vector3f velocity_setpoints, float dt);
public:
void setConstraints(float j_max, float a_max, float v_max);
void setInitialConditions(Vector3f acc, Vector3f vel, Vector3f pos);
void updateTrajectories(Vector3f velocity_setpoints, float dt);
VelocitySmoothing _trajectories[3];
VelocitySmoothing _trajectories[3];
};
void VelocitySmoothingTest::setConstraints(float j_max, float a_max, float v_max)
@ -81,39 +80,56 @@ void VelocitySmoothingTest::updateTrajectories(Vector3f velocity_setpoints, floa
VelocitySmoothing::timeSynchronization(_trajectories, 2);
float dummy; // We don't care about the immediate result
for (int i = 0; i < 3; i++) {
_trajectories[i].integrate(dummy, dummy, dummy);
}
}
TEST_F(VelocitySmoothingTest, testConstantSetpoint)
TEST_F(VelocitySmoothingTest, testTimeSynchronization)
{
// Set the constraints
// GIVEN: A set of constraints
const float j_max = 55.2f;
const float a_max = 6.f;
const float v_max = 6.f;
setConstraints(j_max, a_max, v_max);
// Set the initial conditions
// AND: A set of initial conditions
Vector3f a0(0.f, 0.f, 0.f);
Vector3f v0(0.f, 0.f, 0.f);
Vector3f x0(0.f, 0.f, 0.f);
setInitialConditions(a0, v0, x0);
// WHEN: We generate trajectories (time synchronized in XY) with constant setpoints and dt
Vector3f velocity_setpoints(0.f, 1.f, 0.f);
float dt = 0.01f;
updateTrajectories(velocity_setpoints, dt);
// THEN: The X and Y trajectories should have the same total time (= time sunchronized)
EXPECT_LE(fabsf(_trajectories[0].getTotalTime() - _trajectories[1].getTotalTime()), 0.0001);
}
TEST_F(VelocitySmoothingTest, testConstantSetpoint)
{
// GIVEN: A set of initial conditions (same constraints as before)
Vector3f a0(0.22f, 0.f, 0.22f);
Vector3f v0(2.47f, -5.59e-6f, 2.47f);
Vector3f x0(0.f, 0.f, 0.f);
setInitialConditions(a0, v0, x0);
// Generate the trajectories with constant setpoints and dt
// WHEN: We generate trajectories with constant setpoints and dt
Vector3f velocity_setpoints(0.f, 1.f, 0.f);
float dt = 0.01f;
updateTrajectories(velocity_setpoints, dt);
// Check that time synchronization actually works
ASSERT_LE(fabsf(_trajectories[0].getTotalTime() - _trajectories[1].getTotalTime()), 0.0001);
// Compute the number of steps required to reach desired value
// because of known numerical issues, the actual trajectory takes a
// bit more time than the predicted one, this is why we have to add 14 steps
// to the theoretical value
// to the theoretical value.
// The updateTrajectories is fist called once to compute the total time
updateTrajectories(velocity_setpoints, dt);
float t123 = _trajectories[0].getTotalTime();
int nb_steps = ceil(t123 / dt) + 14;
@ -121,8 +137,8 @@ TEST_F(VelocitySmoothingTest, testConstantSetpoint)
updateTrajectories(velocity_setpoints, dt);
}
// Check that all the trajectories reached their desired value
// and that the acceleration is now zero
// THEN: All the trajectories should have reach their
// final state: desired velocity target and zero acceleration
for (int i = 0; i < 3; i++) {
EXPECT_LE(fabsf(_trajectories[i].getCurrentVelocity() - velocity_setpoints(i)), 0.01f);
EXPECT_LE(fabsf(_trajectories[i].getCurrentAcceleration()), 0.0001f);
@ -131,23 +147,23 @@ TEST_F(VelocitySmoothingTest, testConstantSetpoint)
TEST_F(VelocitySmoothingTest, testZeroSetpoint)
{
// Set the initial conditions to zero
// GIVEN: A set of null initial conditions
Vector3f a0(0.f, 0.f, 0.f);
Vector3f v0(0.f, 0.f, 0.f);
Vector3f x0(0.f, 0.f, 0.f);
setInitialConditions(a0, v0, x0);
// Generate the trajectories with zero setpoints
// AND: Null setpoints
Vector3f velocity_setpoints(0.f, 0.f, 0.f);
float dt = 0.01f;
// Run a few times the algorithm
// WHEN: We run a few times the algorithm
for (int i = 0; i < 60; i++) {
updateTrajectories(velocity_setpoints, dt);
}
// Check that all the trajectories are still at zero
// THEN: All the trajectories should still be null
for (int i = 0; i < 3; i++) {
EXPECT_EQ(_trajectories[i].getCurrentJerk(), 0.f);
EXPECT_EQ(_trajectories[i].getCurrentAcceleration(), 0.f);