ardupilot/libraries/AP_Math/examples/rotations/rotations.pde

181 lines
5.5 KiB
Plaintext

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
// Unit tests for the AP_Math rotations code
//
#include <AP_HAL.h>
#include <stdlib.h>
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Param.h>
#include <AP_HAL_AVR.h>
#include <AP_HAL_AVR_SITL.h>
#include <AP_HAL_Empty.h>
#include <AP_HAL_PX4.h>
#include <AP_Math.h>
#include <Filter.h>
#include <AP_ADC.h>
#include <SITL.h>
#include <AP_Compass.h>
#include <AP_Baro.h>
#include <GCS_MAVLink.h>
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
static void print_vector(Vector3f &v)
{
hal.console->printf("[%.2f %.2f %.2f]\n",
v.x, v.y, v.z);
}
// test rotation method accuracy
static void test_rotation_accuracy(void)
{
Matrix3f attitude;
Vector3f small_rotation;
float roll, pitch, yaw;
int16_t i;
float rot_angle;
hal.console->println_P(PSTR("\nRotation method accuracy:"));
for( i=0; i<90; i++ ) {
// reset initial attitude
attitude.from_euler(0,0,0);
// calculate small rotation vector
rot_angle = ToRad(i);
small_rotation = Vector3f(0,0,rot_angle);
// apply small rotation
attitude.rotate(small_rotation);
// get resulting attitude's euler angles
attitude.to_euler(&roll, &pitch, &yaw);
// display results
hal.console->printf_P(
PSTR("actual angle: %d\tcalculated angle:%4.2f\n"),
(int)i,ToDeg(yaw));
}
}
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-6;
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 incorrect\n", (unsigned)rotation);
print_vector(v);
print_vector(v1);
print_vector(v2);
}
}
static void test_eulers(void)
{
hal.console->println("euler tests");
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);
}
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,2,3);
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->println("testing for missing rotations");
uint16_t roll, pitch, yaw;
for (yaw=0; yaw<360; yaw += 90)
for (pitch=0; pitch<360; pitch += 90)
for (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->println("rotation unit tests\n");
test_rotation_accuracy();
test_eulers();
missing_rotations();
hal.console->println("rotation unit tests done\n");
}
void loop(void) {}
AP_HAL_MAIN();