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

471 lines
17 KiB
C++
Raw Blame History

#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 accel_start = 1.0;
const float dt = 0.01;
// test for update_pos_vel_accel includes update_vel_accel.
// test unlimited behaviour
// 1
float vel = vel_start;
postype_t pos = pos_start;
float accel = accel_start;
update_pos_vel_accel(pos, vel, accel, dt, 0.0, 0.0, 0.0);
EXPECT_FLOAT_EQ(vel, vel_start + accel * dt);
EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt));
// 2
vel = vel_start;
pos = pos_start;
accel = -accel_start;
update_pos_vel_accel(pos, vel, accel, dt, 0.0, 0.0, 0.0);
EXPECT_FLOAT_EQ(vel, vel_start + accel * dt);
EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt));
// error has no impact when not limited
// 3
vel = vel_start;
pos = pos_start;
accel = accel_start;
update_pos_vel_accel(pos, vel, accel, dt, 0.0, 1.0, 1.0);
EXPECT_FLOAT_EQ(vel, vel_start + accel * dt);
EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt));
// 4
vel = vel_start;
pos = pos_start;
accel = accel_start;
update_pos_vel_accel(pos, vel, accel, dt, 0.0, -1.0, -1.0);
EXPECT_FLOAT_EQ(vel, vel_start + accel * dt);
EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt));
// test unlimited behaviour
// zero error should result in normal behaviour
// 5
vel = vel_start;
pos = pos_start;
accel = accel_start;
update_pos_vel_accel(pos, vel, accel, dt, 1.0, 0.0, 0.0);
EXPECT_FLOAT_EQ(vel, vel_start + accel * dt);
EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt));
// 6
vel = vel_start;
pos = pos_start;
accel = -accel_start;
update_pos_vel_accel(pos, vel, accel, dt, 1.0, 0.0, 0.0);
EXPECT_FLOAT_EQ(vel, vel_start + accel * dt);
EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt));
// 7
vel = vel_start;
pos = pos_start;
accel = accel_start;
update_pos_vel_accel(pos, vel, accel, dt, -1.0, 0.0, 0.0);
EXPECT_FLOAT_EQ(vel, vel_start + accel * dt);
EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt));
// 8
vel = vel_start;
pos = pos_start;
accel = -accel_start;
update_pos_vel_accel(pos, vel, accel, dt, -1.0, 0.0, 0.0);
EXPECT_FLOAT_EQ(vel, vel_start + accel * dt);
EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt));
// error sign opposite to limit sign should result in normal behaviour
// 9
vel = vel_start;
pos = pos_start;
accel = accel_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 + vel_start * dt + 0.5 * accel * sq(dt));
// 10
vel = vel_start;
pos = pos_start;
accel = -accel_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 + vel_start * dt + 0.5 * accel * sq(dt));
// 11
vel = vel_start;
pos = pos_start;
accel = accel_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 + vel_start * dt + 0.5 * accel * sq(dt));
// 12
vel = vel_start;
pos = pos_start;
accel = -accel_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 + vel_start * dt + 0.5 * accel * sq(dt));
// error sign same as limit sign should result various limited behaviours
// 13
vel = vel_start;
pos = pos_start;
accel = accel_start;
update_pos_vel_accel(pos, vel, accel, dt, 1.0, 1.0, 1.0);
// vel is not increased
EXPECT_FLOAT_EQ(vel, vel_start);
// pos is not increased
EXPECT_FLOAT_EQ(pos, pos_start);
// 14
vel = vel_start;
pos = pos_start;
accel = -accel_start;
update_pos_vel_accel(pos, vel, accel, dt, 1.0, 1.0, 1.0);
// vel is decreased
EXPECT_FLOAT_EQ(vel, vel_start + accel * dt);
// pos is not increased
EXPECT_FLOAT_EQ(pos, pos_start);
// 15
vel = vel_start;
pos = pos_start;
accel = accel_start;
update_pos_vel_accel(pos, vel, accel, dt, -1.0, -1.0, -1.0);
// vel is increased
EXPECT_FLOAT_EQ(vel, vel_start + accel * dt);
// pos is increased
EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt));
// 16
vel = vel_start;
pos = pos_start;
accel = -accel_start;
update_pos_vel_accel(pos, vel, accel, dt, -1.0, -1.0, -1.0);
// velocity is limited but limit is not applied because velocity is reducing
EXPECT_FLOAT_EQ(vel, vel_start + accel * dt);
// pos is increased
EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt));
// 17
vel = -vel_start;
pos = pos_start;
accel = accel_start;
update_pos_vel_accel(pos, vel, accel, dt, 1.0, 1.0, 1.0);
// velocity is limited but limit is not applied because velocity is reducing
EXPECT_FLOAT_EQ(vel, -vel_start + accel * dt);
// pos is decreased
EXPECT_FLOAT_EQ(pos, pos_start - vel_start * dt + 0.5 * accel * sq(dt));
// 18
vel_start = 0.1 * accel_start * dt;
vel = vel_start;
pos = pos_start;
accel = -accel_start;
update_pos_vel_accel(pos, vel, accel, dt, -1.0, -1.0, -1.0);
// velocity is limited but limit is not applied because velocity is reducing
// final result is zero because velocity would change sign during dt
EXPECT_FLOAT_EQ(vel, 0.0);
// pos is not changed because is_negative(vel_start * dt + 0.5 * accel * sq(t))
EXPECT_FLOAT_EQ(pos, pos_start);
// 19
vel = -vel_start;
pos = pos_start;
accel = accel_start;
update_pos_vel_accel(pos, vel, accel, dt, 1.0, 1.0, 1.0);
// velocity is limited but limit is not applied because velocity is reducing
// final result is zero because velocity would change sign during dt
EXPECT_FLOAT_EQ(vel, 0.0);
// pos is not changed because is_negative(vel_start * dt + 0.5 * accel * sq(t))
EXPECT_FLOAT_EQ(pos, pos_start);
// test for update_pos_vel_accel includes update_vel_accel.
// test unlimited behaviour
// 1
pos_start = 17;
vel_start = 20;
accel_start = 1.0;
Vector2p posxy = Vector2p(pos_start, 0.0);
Vector2f velxy = Vector2f(vel_start, 0.0);
Vector2f accelxy = Vector2f(accel_start, 0.0);
Vector2f limit = Vector2f(0.0, 0.0);
Vector2f pos_error = Vector2f(0.0, 0.0);
Vector2f vel_error = Vector2f(0.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 2
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(-accel_start, 0.0);
limit = Vector2f(0.0, 0.0);
pos_error = Vector2f(0.0, 0.0);
vel_error = Vector2f(0.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// error has no impact when not limited
// 3
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(accel_start, 0.0);
limit = Vector2f(0.0, 0.0);
pos_error = Vector2f(1.0, 0.0);
vel_error = Vector2f(1.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 4
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(accel_start, 0.0);
limit = Vector2f(0.0, 0.0);
pos_error = Vector2f(0.0, 0.0);
vel_error = Vector2f(0.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// test unlimited behaviour
// zero error should result in normal behaviour
// 5
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(accel_start, 0.0);
limit = Vector2f(1.0, 0.0);
pos_error = Vector2f(0.0, 0.0);
vel_error = Vector2f(0.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 6
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(-accel_start, 0.0);
limit = Vector2f(1.0, 0.0);
pos_error = Vector2f(0.0, 0.0);
vel_error = Vector2f(0.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 7
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(accel_start, 0.0);
limit = Vector2f(1.0, 0.0);
pos_error = Vector2f(0.0, 0.0);
vel_error = Vector2f(0.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 8
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(-accel_start, 0.0);
limit = Vector2f(1.0, 0.0);
pos_error = Vector2f(0.0, 0.0);
vel_error = Vector2f(0.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// error sign opposite to limit sign should result in normal behaviour
// 9
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(accel_start, 0.0);
limit = Vector2f(1.0, 0.0);
pos_error = Vector2f(-1.0, 0.0);
vel_error = Vector2f(-1.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 10
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(-accel_start, 0.0);
limit = Vector2f(1.0, 0.0);
pos_error = Vector2f(-1.0, 0.0);
vel_error = Vector2f(-1.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 11
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(accel_start, 0.0);
limit = Vector2f(-1.0, 0.0);
pos_error = Vector2f(1.0, 0.0);
vel_error = Vector2f(1.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 12
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(-accel_start, 0.0);
limit = Vector2f(-1.0, 0.0);
pos_error = Vector2f(1.0, 0.0);
vel_error = Vector2f(1.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// error sign same as limit sign should result various limited behaviours
// 13
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(accel_start, 0.0);
limit = Vector2f(1.0, 0.0);
pos_error = Vector2f(1.0, 0.0);
vel_error = Vector2f(1.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
// vel is not increased
EXPECT_FLOAT_EQ(velxy.x, vel_start);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// pos is not increased
EXPECT_FLOAT_EQ(posxy.x, pos_start);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 14
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(-accel_start, 0.0);
limit = Vector2f(1.0, 0.0);
pos_error = Vector2f(1.0, 0.0);
vel_error = Vector2f(1.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
// vel is decreased
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// pos is not increased
EXPECT_FLOAT_EQ(posxy.x, pos_start);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 15
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(accel_start, 0.0);
limit = Vector2f(-1.0, 0.0);
pos_error = Vector2f(-1.0, 0.0);
vel_error = Vector2f(-1.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
// vel is increased
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// pos is increased
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 16
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(-accel_start, 0.0);
limit = Vector2f(-1.0, 0.0);
pos_error = Vector2f(-1.0, 0.0);
vel_error = Vector2f(-1.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
// velocity is limited but limit is not applied because velocity is reducing
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// pos is increased
EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 17
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(-vel_start, 0.0);
accelxy = Vector2f(accel_start, 0.0);
limit = Vector2f(1.0, 0.0);
pos_error = Vector2f(1.0, 0.0);
vel_error = Vector2f(1.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
// velocity is limited but limit is not applied because velocity is reducing
EXPECT_FLOAT_EQ(velxy.x, -vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// pos is decreased
EXPECT_FLOAT_EQ(posxy.x, pos_start - vel_start * dt + 0.5 * accelxy.x * sq(dt));
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 18
vel_start = 0.1 * accel_start * dt;
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(vel_start, 0.0);
accelxy = Vector2f(-accel_start, 0.0);
limit = Vector2f(-1.0, 0.0);
pos_error = Vector2f(-1.0, 0.0);
vel_error = Vector2f(-1.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
// velocity is limited but limit is not applied because velocity is reducing
// ideally this would be zero but code makes a simplification here
EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// pos is not changed because is_negative(vel_start * dt + 0.5 * accel * sq(t))
EXPECT_FLOAT_EQ(posxy.x, pos_start);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// 19
posxy = Vector2p(pos_start, 0.0);
velxy = Vector2f(-vel_start, 0.0);
accelxy = Vector2f(accel_start, 0.0);
limit = Vector2f(1.0, 0.0);
pos_error = Vector2f(1.0, 0.0);
vel_error = Vector2f(1.0, 0.0);
update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error);
// velocity is limited but limit is not applied because velocity is reducing
// ideally this would be zero but code makes a simplification here
EXPECT_FLOAT_EQ(velxy.x, -vel_start + accelxy.x * dt);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
// pos is not changed because is_negative(vel_start * dt + 0.5 * accel * sq(t))
EXPECT_FLOAT_EQ(posxy.x, pos_start);
EXPECT_FLOAT_EQ(velxy.y, 0.0);
}
AP_GTEST_MAIN()
int hal = 0;
Reference in New Issue View Git Blame Copy Permalink