TestVelocitySmoothing - Split time synchronization and final state ch…

…ecks into two different test functions

Also improve the comments

src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp

@@ -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,48 +80,65 @@ 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
-	Vector3f a0(0.22f, 0.f, 0.22f);
-	Vector3f v0(2.47f, -5.59e-6f, 2.47f);
+	// 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);
 
-	// Generate the trajectories with constant setpoints and dt
+	// 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);
 
-	// Check that time synchronization actually works
-	ASSERT_LE(fabsf(_trajectories[0].getTotalTime() - _trajectories[1].getTotalTime()), 0.0001);
+	// 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);
+
+	// WHEN: We generate trajectories with constant setpoints and dt
+	Vector3f velocity_setpoints(0.f, 1.f, 0.f);
+	float dt = 0.01f;
 
 	// 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;
 
 	for (int i = 0; i < nb_steps; i++) {
 		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);