Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
01a504b472259602e13df4cf07df7c0cdbb35993
ardupilot/libraries/AP_Math/tests/test_control.cpp
T

663 lines
26 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>
#include <fenv.h>
#include <signal.h>
#include <setjmp.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);
}
TEST(KinematicLimit, normal_values)
{
// This test's strategy is to select common hand-computable right triangles and show
// that for a variety of 'vertical planes' (defined by the ratio of X and Y) the +z
// 'half' of the permissible region computes the expected result.
// (In a few cases, test the other overload to contribute to test clarity.)
// For the -z 'half', we deliberately pick a different max in order to show that
// some cases are impacted by the z-limit, while others are only impacted by xy-limit.
constexpr float max_xy = 4.0f;
constexpr float max_z_pos = 4.0f;
constexpr float max_z_neg = 8.0f;
Vector3f direction;
float expected;
// Vertical
direction = Vector3f(0.0f, 0.0f, +1.0f);
expected = 4.0f;
EXPECT_FLOAT_EQ(kinematic_limit(direction, max_xy, max_z_neg, max_z_pos), expected);
EXPECT_FLOAT_EQ(kinematic_limit(0.0f, direction.z, max_xy, max_z_neg, max_z_pos), expected);
direction.z *= -1.0f;
expected *= 2.0f;
EXPECT_FLOAT_EQ(kinematic_limit(direction, max_xy, max_z_neg, max_z_pos), expected);
EXPECT_FLOAT_EQ(kinematic_limit(0.0f, direction.z, max_xy, max_z_neg, max_z_pos), expected);
// XZ plane, a (1, 2, sqrt(5)) right triangle
direction = Vector3f(1.0f, 0.0f, +2.0f);
expected = 2.0f * sqrtf(5.0f);
EXPECT_FLOAT_EQ(kinematic_limit(direction, max_xy, max_z_neg, max_z_pos), expected);
direction.z *= -1.0f;
expected *= 2.0f;
EXPECT_FLOAT_EQ(kinematic_limit(direction, max_xy, max_z_neg, max_z_pos), expected);
// Show that flipping the x-direction has no impact
// (once is enough, we don't need to show this in every case)
direction.x *= -1.0f;
EXPECT_FLOAT_EQ(kinematic_limit(direction, max_xy, max_z_neg, max_z_pos), expected);
// YZ plane, a (3, 4, 5) right triangle
direction = Vector3f(0.0f, 3.0f, +4.0f);
expected = 5.0f;
EXPECT_FLOAT_EQ(kinematic_limit(direction, max_xy, max_z_neg, max_z_pos), expected);
direction.z *= -1.0f;
expected *= (4.0f / 3.0f); // With a higher z-mag, this hits the xy-mag limit
EXPECT_FLOAT_EQ(kinematic_limit(direction, max_xy, max_z_neg, max_z_pos), expected);
// X=-Y plane, a (1, 1, sqrt(2)) right triangle
direction = Vector3f(1.0f / sqrtf(2.0f), -1.0f / sqrtf(2.0f), +1.0f);
expected = 4.0f * sqrtf(2.0f);
EXPECT_FLOAT_EQ(kinematic_limit(direction, max_xy, max_z_neg, max_z_pos), expected);
EXPECT_FLOAT_EQ(kinematic_limit(1.0f, direction.z, max_xy, max_z_neg, max_z_pos), expected);
direction.z *= -1.0f;
EXPECT_FLOAT_EQ(kinematic_limit(direction, max_xy, max_z_neg, max_z_pos), expected);
EXPECT_FLOAT_EQ(kinematic_limit(1.0f, direction.z, max_xy, max_z_neg, max_z_pos), expected);
// Horizontal, in an arbitrary XY plane.
direction = Vector3f(-1.23f, -4.56f, 0.0f);
expected = max_xy;
EXPECT_FLOAT_EQ(kinematic_limit(direction, max_xy, max_z_neg, max_z_pos), expected);
direction.z *= -1.0f;
EXPECT_FLOAT_EQ(kinematic_limit(direction, max_xy, max_z_neg, max_z_pos), expected);
}
// This test demonstrates the intended behavior when any of the 3 limits are zero.
TEST(KinematicLimit, zero_max_limit)
{
// If you can select any values here which cause this test to fail,
// that indicates the code has a problem.
constexpr float arbitrary1 = 1.11f;
constexpr float arbitrary2 = 2.22f;
constexpr float arbitrary3 = 3.33f;
constexpr float arbitrary4 = 4.44f;
constexpr float expected_limit = 0.0f;
for (const auto dir_xy : {0.0f, arbitrary1}) {
for (const auto dir_z : {0.0f, arbitrary2, -arbitrary2}) {
// Test 1: max_xy==0 constrains all cases.
// (The case likely to surprise users who don't read the
// documentation is the pure-vertical case.)
float observed_limit = kinematic_limit(dir_xy,
dir_z,
0.0f,
arbitrary3,
arbitrary4);
EXPECT_FLOAT_EQ(observed_limit, expected_limit);
// Test 2: max_z_neg==0 constrains all cases.
// (The cases likely to surprise users who don't read the
// documentation are when the z-component is non-negative.)
observed_limit = kinematic_limit(dir_xy,
dir_z,
arbitrary3,
0.0f,
arbitrary4);
EXPECT_FLOAT_EQ(observed_limit, expected_limit);
// Test 3: max_z_pos==0 constrains all cases.
// (The cases likely to surprise users who don't read the
// documentation are when the z-component is non-positive.)
observed_limit = kinematic_limit(dir_xy,
dir_z,
arbitrary3,
arbitrary4,
0.0f);
EXPECT_FLOAT_EQ(observed_limit, expected_limit);
}
}
}
// catch floating point exceptions
sigjmp_buf avert_your_eyes_children;
static void _tc_sig_fpe(int signum)
{
siglongjmp(avert_your_eyes_children, 1);
}
TEST(Control, test_limit_accel)
{
// reproduction of FPE (https://github.com/ArduPilot/ardupilot/issues/28969)
// FPE will only be raised in SITL HAL, so compiling for linux HAL
// isn't useful.
const Vector2f vel{
0.984285712, 0.176583186
};
Vector2f accel{99.9008408, -557.304077};
const float accel_max = 566.187256;
struct sigaction old_sa_fpe = {};
struct sigaction sa_fpe = {};
sigemptyset(&sa_fpe.sa_mask);
sa_fpe.sa_handler = _tc_sig_fpe;
if (sigaction(SIGFPE, &sa_fpe, &old_sa_fpe) == -1) {
abort();
}
const int excepts = FE_UNDERFLOW | FE_OVERFLOW | FE_INVALID;
fexcept_t old_except_flags;
if (fegetexceptflag(&old_except_flags, excepts) == -1) {
abort();
}
feenableexcept(excepts);
bool signal_caught = false;
if (sigsetjmp(avert_your_eyes_children, 1)) {
// we come through here if an FPE is triggered (via a goto in
// our custom signal handler, _tc_sig_fpe)
signal_caught = true;
} else {
// we come through here normally
EXPECT_TRUE(limit_accel_xy(vel, accel, accel_max));
}
EXPECT_FALSE(signal_caught);
// now restore the original fpe handling
if (fesetexceptflag(&old_except_flags, excepts) == -1) {
abort();
}
sigaction(SIGFPE, &old_sa_fpe, nullptr);
}
TEST(Control, test_limit_accel_reversal_no_lateral_spike)
{
// A saturated braking command aligned with the North axis must not inject a
// lateral (East) acceleration as the cross-track reference rotates through zero
// (the roll wobble at a hard Loiter stop). A small residual perpendicular
// reference component makes the reference direction rotate through the crossing;
// below LIMIT_ACCEL_XY_MIN_REF the fade to the direction-preserving limit
// removes the spike.
const float accel_max = 5.0f;
// braking command pointing South, saturating the limit
const float accel_cmd_n = -1.5f * accel_max;
const float residual_e = 0.02f; // small perpendicular reference residual
float worst_east = 0.0f;
// sweep the reference North component through zero
for (float rn = 1.0f; rn >= -1.0f; rn -= 0.005f) {
const Vector2f vel_norm{rn, residual_e};
Vector2f accel{accel_cmd_n, 0.0f};
limit_accel_xy(vel_norm, accel, accel_max);
// command has zero East; output East is pure injected cross-axis error
worst_east = MAX(worst_east, fabsf(accel.y));
// magnitude must always respect the limit
EXPECT_LE(accel.length(), accel_max * 1.001f);
}
// Without the low-reference fade this reaches ~0.2*accel_max. Assert it stays small.
EXPECT_LT(worst_east, 0.1f * accel_max);
}
AP_GTEST_MAIN()
int hal = 0;
Reference in New Issue View Git Blame Copy Permalink