Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-11 20:59:22 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
99619ec9bca7196327ea25dda0a293f87de00db3
ardupilot/libraries/AP_Math/tests/test_control.cpp
T
ChrisRosserandClaude Opus 4.8 99619ec9bc AP_Math: fade cross-track accel prioritisation to zero near zero speed 
limit_accel_xy() splits the acceleration command into along-track and
cross-track components using the normalised velocity, and prioritises the
cross-track component when the command saturates the maximum lean angle.

As the velocity vector rotates through zero (e.g. a hard stick reversal in
Loiter, where forward speed passes through zero before reversing) the unit
velocity spins rapidly. With any small residual lateral velocity (wind crab,
residual velocity from prior manoeuvring) this re-projects the saturated
longitudinal braking command onto a rapidly rotating axis and emits a large
lateral acceleration spike, seen as a roll wobble at the moment of the stop
and as a PSCE.TAE spike at each PSCN.VN zero crossing.

Fade the cross-track prioritisation out to a direction-preserving magnitude
limit below LIMIT_ACCEL_XY_MIN_SPEED_MS (1 m/s). Above that speed behaviour is
unchanged, so path/corner tracking is unaffected. The limiter only acts when
the command saturates the lean-angle limit, and stationary hold already used
the isotropic branch, so precision position hold is unaffected.

Adds a regression test sweeping velocity through zero and asserting no lateral
acceleration is injected.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
2026-07-27 12:10:17 +09:00

661 lines
25 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 velocity vector rotates through zero
// during a hard reversal (roll wobble at the Loiter stop). A small residual
// lateral velocity is what makes vel_unit rotate through the crossing.
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.05f; // small lateral residual velocity (m/s)
float worst_east = 0.0f;
// sweep the North velocity through zero
for (float vn = 2.0f; vn >= -2.0f; vn -= 0.01f) {
const Vector2f vel{vn, residual_e};
Vector2f accel{accel_cmd_n, 0.0f};
limit_accel_xy(vel, 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-speed 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