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

381 lines
12 KiB
C++
Raw Blame History

//
// Unit tests for the AP_Math rotations code
//
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_CustomRotations/AP_CustomRotations.h>
AP_CustomRotations cust_rot;
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");
}
// quaternion rotation test
const float q_accuracy = 1.0e-3f;
Quaternion q, qe;
q.from_rotation(rotation);
qe.from_euler(radians(roll), radians(pitch), radians(yaw));
float q_roll, q_pitch, q_yaw, qe_roll, qe_pitch, qe_yaw;
q.to_euler(q_roll, q_pitch, q_yaw);
qe.to_euler(qe_roll, qe_pitch, qe_yaw);
float roll_diff = fabsf(wrap_PI(q_roll - qe_roll));
float pitch_diff = fabsf(wrap_PI(q_pitch - qe_pitch));
float yaw_diff = fabsf(wrap_PI(q_yaw - qe_yaw));
if ((roll_diff > q_accuracy) || (pitch_diff > q_accuracy) || (yaw_diff > q_accuracy)) {
hal.console->printf("quaternion test %u failed : yaw:%f/%f roll:%f/%f pitch:%f/%f\n",
(unsigned)rotation,
(double)q_yaw,(double)qe_yaw,
(double)q_roll,(double)qe_roll,
(double)q_pitch,(double)qe_pitch);
}
// test custom rotations
AP::custom_rotations().set(ROTATION_CUSTOM_1, roll, pitch, yaw);
v1 = v;
v1.rotate(ROTATION_CUSTOM_1);
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("custom rotated: ");
print_vector(v1);
hal.console->printf("correct rotated: ");
print_vector(v2);
hal.console->printf("\n");
}
Quaternion qc;
qc.from_rotation(ROTATION_CUSTOM_1);
float qc_roll, qc_pitch, qc_yaw;
qc.to_euler(qc_roll, qc_pitch, qc_yaw);
roll_diff = fabsf(wrap_PI(qc_roll - qe_roll));
pitch_diff = fabsf(wrap_PI(qc_pitch - qe_pitch));
yaw_diff = fabsf(wrap_PI(qc_yaw - qe_yaw));
if ((roll_diff > q_accuracy) || (pitch_diff > q_accuracy) || (yaw_diff > q_accuracy)) {
hal.console->printf("custom quaternion test %u failed\n", (unsigned)rotation);
}
}
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);
test_euler(ROTATION_ROLL_45,45,0,0);
test_euler(ROTATION_ROLL_315,315,0,0);
test_euler(ROTATION_PITCH_7, 0, 7, 0);
}
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);
}
}
}
static void test_rotate_matrix(void)
{
for (enum Rotation r = ROTATION_NONE;
r < ROTATION_MAX;
r = (enum Rotation)((uint8_t)r+1)) {
//hal.console->printf("\nROTATION(%d)\n", r);
Vector3f vec(1,2,3);
Vector3f vec2 = vec;
vec.rotate(r);
Matrix3f m;
m.from_rotation(r);
vec2 = m * vec2;
//print_vector(vec);
//print_vector(vec2);
if ((vec - vec2).length() > 1e-5) {
hal.console->printf("Rotation Test Failed!!! %.8f\n", (double)(vec - vec2).length());
return;
}
}
hal.console->printf("test_rotate_matrix passed\n");
}
static void test_rotation_duplicates(void)
{
for (enum Rotation r = (enum Rotation)((uint8_t)ROTATION_MAX-1); r > ROTATION_NONE; r = (enum Rotation)((uint8_t)r-1)) {
for (enum Rotation r2 = ROTATION_NONE; r2 < r; r2 = (enum Rotation)((uint8_t)r2+1)) {
if (rotation_equal(r,r2)) {
hal.console->printf("Rotation %i same as %i\n", r, r2);
}
}
}
hal.console->printf("test_rotation_duplicates done\n");
}
/*
* rotation tests
*/
void setup(void)
{
hal.console->begin(115200);
hal.console->printf("rotation unit tests\n\n");
test_rotation_accuracy();
hal.console->printf("\n\n");
test_eulers();
hal.console->printf("\n\n");
missing_rotations();
hal.console->printf("\n\n");
test_rotate_inverse();
hal.console->printf("\n\n");
test_rotate_matrix();
hal.console->printf("\n\n");
test_rotation_duplicates();
hal.console->printf("\n\n");
hal.console->printf("rotation unit tests done\n\n");
return;
}
void loop(void) {}
AP_HAL_MAIN();
Reference in New Issue View Git Blame Copy Permalink