You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
271 lines
8.5 KiB
271 lines
8.5 KiB
// |
|
// Unit tests for the AP_Math rotations code |
|
// |
|
|
|
#include <AP_HAL/AP_HAL.h> |
|
#include <AP_Math/AP_Math.h> |
|
|
|
void setup(); |
|
void loop(); |
|
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL(); |
|
|
|
static void print_vector(Vector3f &v) |
|
{ |
|
hal.console->printf("[%.4f %.4f %.4f]\n", |
|
(double)v.x, |
|
(double)v.y, |
|
(double)v.z); |
|
} |
|
|
|
// test rotation method accuracy |
|
static void test_rotation_accuracy(void) |
|
{ |
|
Matrix3f attitude; |
|
Vector3f small_rotation; |
|
float roll, pitch, yaw; |
|
float rot_angle; |
|
|
|
hal.console->printf("\nRotation method accuracy:\n"); |
|
|
|
// test roll |
|
for(int16_t i = 0; i < 90; i++ ) { |
|
|
|
// reset initial attitude |
|
attitude.from_euler(0.0f, 0.0f, 0.0f); |
|
|
|
// calculate small rotation vector |
|
rot_angle = ToRad(i); |
|
small_rotation = Vector3f(rot_angle, 0.0f, 0.0f); |
|
|
|
// apply small rotation |
|
attitude.rotate(small_rotation); |
|
|
|
// get resulting attitude's euler angles |
|
attitude.to_euler(&roll, &pitch, &yaw); |
|
|
|
// now try via from_axis_angle |
|
Matrix3f r2; |
|
r2.from_axis_angle(Vector3f(1.0f, 0.0f, 0.0f), rot_angle); |
|
attitude.from_euler(0.0f, 0.0f, 0.0f); |
|
attitude = r2 * attitude; |
|
|
|
float roll2, pitch2, yaw2; |
|
attitude.to_euler(&roll2, &pitch2, &yaw2); |
|
|
|
// display results |
|
hal.console->printf("actual angle: %d angle1:%4.2f angle2:%4.2f\n", |
|
(int)i, |
|
(double)ToDeg(roll), |
|
(double)ToDeg(roll2)); |
|
} |
|
|
|
// test pitch |
|
for(int16_t i = 0; i < 90; i++ ) { |
|
|
|
// reset initial attitude |
|
attitude.from_euler(0.0f, 0.0f, 0.0f); |
|
|
|
// calculate small rotation vector |
|
rot_angle = ToRad(i); |
|
small_rotation = Vector3f(0.0f ,rot_angle, 0.0f); |
|
|
|
// apply small rotation |
|
attitude.rotate(small_rotation); |
|
|
|
// get resulting attitude's euler angles |
|
attitude.to_euler(&roll, &pitch, &yaw); |
|
|
|
// now try via from_axis_angle |
|
Matrix3f r2; |
|
r2.from_axis_angle(Vector3f(0.0f ,1.0f, 0.0f), rot_angle); |
|
attitude.from_euler(0.0f, 0.0f, 0.0f); |
|
attitude = r2 * attitude; |
|
|
|
float roll2, pitch2, yaw2; |
|
attitude.to_euler(&roll2, &pitch2, &yaw2); |
|
|
|
// display results |
|
hal.console->printf("actual angle: %d angle1:%4.2f angle2:%4.2f\n", |
|
(int)i, |
|
(double)ToDeg(pitch), |
|
(double)ToDeg(pitch2)); |
|
} |
|
|
|
|
|
// test yaw |
|
for(int16_t i = 0; i < 90; i++ ) { |
|
|
|
// reset initial attitude |
|
attitude.from_euler(0.0f, 0.0f, 0.0f); |
|
|
|
// calculate small rotation vector |
|
rot_angle = ToRad(i); |
|
small_rotation = Vector3f(0.0f, 0.0f, rot_angle); |
|
|
|
// apply small rotation |
|
attitude.rotate(small_rotation); |
|
|
|
// get resulting attitude's euler angles |
|
attitude.to_euler(&roll, &pitch, &yaw); |
|
|
|
// now try via from_axis_angle |
|
Matrix3f r2; |
|
r2.from_axis_angle(Vector3f(0.0f, 0.0f, 1.0f), rot_angle); |
|
attitude.from_euler(0.0f, 0.0f, 0.0f); |
|
attitude = r2 * attitude; |
|
|
|
float roll2, pitch2, yaw2; |
|
attitude.to_euler(&roll2, &pitch2, &yaw2); |
|
|
|
// display results |
|
hal.console->printf("actual angle: %d angle1:%4.2f angle2:%4.2f\n", |
|
(int)i, |
|
(double)ToDeg(yaw), |
|
(double)ToDeg(yaw2)); |
|
} |
|
} |
|
|
|
static void test_euler(enum Rotation rotation, float roll, float pitch, float yaw) |
|
{ |
|
Vector3f v, v1, v2, diff; |
|
Matrix3f rotmat; |
|
const float accuracy = 1.0e-6f; |
|
|
|
v.x = 1; |
|
v.y = 2; |
|
v.z = 3; |
|
v1 = v; |
|
|
|
v1.rotate(rotation); |
|
|
|
rotmat.from_euler(radians(roll), radians(pitch), radians(yaw)); |
|
v2 = v; |
|
v2 = rotmat * v2; |
|
|
|
diff = (v2 - v1); |
|
if (diff.length() > accuracy) { |
|
hal.console->printf("euler test %u failed : yaw:%d roll:%d pitch:%d\n", |
|
(unsigned)rotation, |
|
(int)yaw, |
|
(int)roll, |
|
(int)pitch); |
|
hal.console->printf("fast rotated: "); |
|
print_vector(v1); |
|
hal.console->printf("slow rotated: "); |
|
print_vector(v2); |
|
hal.console->printf("\n"); |
|
} |
|
} |
|
|
|
static void test_rotate_inverse(void) |
|
{ |
|
hal.console->printf("\nrotate inverse test(Vector (1,1,1)):\n"); |
|
Vector3f vec(1.0f,1.0f,1.0f), cmp_vec(1.0f, 1.0f, 1.0f); |
|
for (enum Rotation r = ROTATION_NONE; |
|
r < ROTATION_MAX; |
|
r = (enum Rotation)((uint8_t)r+1)) { |
|
hal.console->printf("\nROTATION(%d) ", r); |
|
vec.rotate(r); |
|
print_vector(vec); |
|
|
|
hal.console->printf("INV_ROTATION(%d)", r); |
|
vec.rotate_inverse(r); |
|
print_vector(vec); |
|
if ((vec - cmp_vec).length() > 1e-5) { |
|
hal.console->printf("Rotation Test Failed!!! %.8f\n", (double)(vec - cmp_vec).length()); |
|
return; |
|
} |
|
} |
|
} |
|
static void test_eulers(void) |
|
{ |
|
hal.console->printf("euler tests\n"); |
|
test_euler(ROTATION_NONE, 0, 0, 0); |
|
test_euler(ROTATION_YAW_45, 0, 0, 45); |
|
test_euler(ROTATION_YAW_90, 0, 0, 90); |
|
test_euler(ROTATION_YAW_135, 0, 0, 135); |
|
test_euler(ROTATION_YAW_180, 0, 0, 180); |
|
test_euler(ROTATION_YAW_225, 0, 0, 225); |
|
test_euler(ROTATION_YAW_270, 0, 0, 270); |
|
test_euler(ROTATION_YAW_315, 0, 0, 315); |
|
test_euler(ROTATION_ROLL_180, 180, 0, 0); |
|
test_euler(ROTATION_ROLL_180_YAW_45, 180, 0, 45); |
|
test_euler(ROTATION_ROLL_180_YAW_90, 180, 0, 90); |
|
test_euler(ROTATION_ROLL_180_YAW_135, 180, 0, 135); |
|
test_euler(ROTATION_PITCH_180, 0, 180, 0); |
|
test_euler(ROTATION_ROLL_180_YAW_225, 180, 0, 225); |
|
test_euler(ROTATION_ROLL_180_YAW_270, 180, 0, 270); |
|
test_euler(ROTATION_ROLL_180_YAW_315, 180, 0, 315); |
|
test_euler(ROTATION_ROLL_90, 90, 0, 0); |
|
test_euler(ROTATION_ROLL_90_YAW_45, 90, 0, 45); |
|
test_euler(ROTATION_ROLL_90_YAW_90, 90, 0, 90); |
|
test_euler(ROTATION_ROLL_90_YAW_135, 90, 0, 135); |
|
test_euler(ROTATION_ROLL_270, 270, 0, 0); |
|
test_euler(ROTATION_ROLL_270_YAW_45, 270, 0, 45); |
|
test_euler(ROTATION_ROLL_270_YAW_90, 270, 0, 90); |
|
test_euler(ROTATION_ROLL_270_YAW_135, 270, 0, 135); |
|
test_euler(ROTATION_PITCH_90, 0, 90, 0); |
|
test_euler(ROTATION_PITCH_270, 0, 270, 0); |
|
test_euler(ROTATION_PITCH_180_YAW_90, 0, 180, 90); |
|
test_euler(ROTATION_PITCH_180_YAW_270, 0, 180, 270); |
|
test_euler(ROTATION_ROLL_90_PITCH_90, 90, 90, 0); |
|
test_euler(ROTATION_ROLL_180_PITCH_90,180, 90, 0); |
|
test_euler(ROTATION_ROLL_270_PITCH_90,270, 90, 0); |
|
test_euler(ROTATION_ROLL_90_PITCH_180, 90, 180, 0); |
|
test_euler(ROTATION_ROLL_270_PITCH_180,270,180, 0); |
|
test_euler(ROTATION_ROLL_90_PITCH_270, 90, 270, 0); |
|
test_euler(ROTATION_ROLL_180_PITCH_270,180,270, 0); |
|
test_euler(ROTATION_ROLL_270_PITCH_270,270,270, 0); |
|
test_euler(ROTATION_ROLL_90_PITCH_180_YAW_90, 90, 180, 90); |
|
test_euler(ROTATION_ROLL_90_YAW_270, 90, 0, 270); |
|
test_euler(ROTATION_ROLL_90_PITCH_68_YAW_293,90,68.8,293.3); |
|
} |
|
|
|
static bool have_rotation(const Matrix3f &m) |
|
{ |
|
Matrix3f mt = m.transposed(); |
|
for (enum Rotation r = ROTATION_NONE; |
|
r < ROTATION_MAX; |
|
r = (enum Rotation)((uint8_t)(r + 1))) { |
|
Vector3f v(1.0f, 2.0f, 3.0f); |
|
Vector3f v2 = v; |
|
v2.rotate(r); |
|
v2 = mt * v2; |
|
if ((v2 - v).length() < 0.01f) { |
|
return true; |
|
} |
|
} |
|
return false; |
|
} |
|
|
|
static void missing_rotations(void) |
|
{ |
|
hal.console->printf("testing for missing rotations\n"); |
|
for (uint16_t yaw = 0; yaw < 360; yaw += 90) |
|
for (uint16_t pitch = 0; pitch < 360; pitch += 90) |
|
for (uint16_t roll = 0; roll < 360; roll += 90) { |
|
Matrix3f m; |
|
m.from_euler(ToRad(roll), ToRad(pitch), ToRad(yaw)); |
|
if (!have_rotation(m)) { |
|
hal.console->printf("Missing rotation (%u, %u, %u)\n", roll, pitch, yaw); |
|
} |
|
} |
|
} |
|
|
|
/* |
|
* rotation tests |
|
*/ |
|
void setup(void) |
|
{ |
|
hal.console->printf("rotation unit tests\n\n"); |
|
test_rotation_accuracy(); |
|
test_eulers(); |
|
missing_rotations(); |
|
test_rotate_inverse(); |
|
hal.console->printf("rotation unit tests done\n\n"); |
|
} |
|
|
|
void loop(void) {} |
|
|
|
AP_HAL_MAIN();
|
|
|