From 862454827e299337d6e94bf33856134de4897071 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Sun, 8 Sep 2019 21:08:48 +0200
Subject: [PATCH] VelocitySmoothingTest - Test that the trajectory is always
 within the constraints

---
 src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp
index 6cfce4ba4a..795739704d 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp
@@ -75,6 +75,9 @@ void VelocitySmoothingTest::updateTrajectories(float dt, Vector3f velocity_setpo
 {
 	for (int i = 0; i < 3; i++) {
 		_trajectories[i].updateTraj(dt);
+		EXPECT_LE(fabsf(_trajectories[i].getCurrentJerk()), _trajectories[i].getMaxJerk());
+		EXPECT_LE(fabsf(_trajectories[i].getCurrentAcceleration()), _trajectories[i].getMaxAccel());
+		EXPECT_LE(fabsf(_trajectories[i].getCurrentVelocity()), _trajectories[i].getMaxVel());
 	}
 
 	for (int i = 0; i < 3; i++) {