Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: added a control.cpp test suite

This commit is contained in:
Andrew Tridgell 2022-12-22 14:28:33 +11:00 committed by Randy Mackay
parent a99748a13d
commit f0d8a383d1
1 changed files with 37 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_Math/tests/test_control.cpp Normal file
View File

@ -0,0 +1,37 @@
#include <AP_gtest.h>
#include <AP_Math/AP_Math.h>
#include <AP_Math/vector2.h>
#include <AP_Math/vector3.h>
#include <AP_Math/control.h>
TEST(Control, test_control)
{
postype_t pos_start = 17;
float vel_start = 20;
float vel = vel_start;
postype_t pos = pos_start;
const float dt = 0.01;
const float accel = 1.0;
update_pos_vel_accel(pos, vel, accel, dt, 0, 0, 0);
EXPECT_FLOAT_EQ(vel, vel_start + accel*dt);
EXPECT_FLOAT_EQ(pos, pos_start + 0.5*(vel+vel_start)*dt);
vel = vel_start;
pos = pos_start;
update_pos_vel_accel(pos, vel, accel, dt, 1.0, 1.0, 1.0);
EXPECT_FLOAT_EQ(vel, vel_start);
EXPECT_FLOAT_EQ(pos, pos_start);
vel = vel_start;
pos = pos_start;
update_pos_vel_accel(pos, vel, accel, dt, -1.0, 1.0, 1.0);
EXPECT_FLOAT_EQ(vel, vel_start + accel*dt);
EXPECT_FLOAT_EQ(pos, pos_start + 0.5*(vel+vel_start)*dt);
}
AP_GTEST_MAIN()
int hal = 0;