mirror of https://github.com/ArduPilot/ardupilot
38 lines
936 B
C++
38 lines
936 B
C++
|
#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;
|