zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
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.
6099 Commits
23 Branches
0 Tags
282 MiB
 
 
 
 
 
 
Tree: 9a653a1a75
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '9a653a1a75'
${ noResults }
ardupilot/libraries/AP_Math/examples/rotations/rotations.pde

243 lines
7.8 KiB
Raw Blame History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
// Unit tests for the AP_Math rotations code
//
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Math.h>
FastSerialPort(Serial, 0);
#ifdef DESKTOP_BUILD
// all of this is needed to build with SITL
#include <SPI.h>
#include <I2C.h>
#include <DataFlash.h>
#include <APM_RC.h>
#include <GCS_MAVLink.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_ADC.h>
#include <AP_Baro.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AP_Semaphore.h>
#include <Filter.h>
#include <AP_Buffer.h>
#include <SITL.h>
Arduino_Mega_ISR_Registry isr_registry;
AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass;
#endif
// standard rotation matrices (these are the originals from the old code)
#define MATRIX_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0,0, 1)
#define MATRIX_ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1)
#define MATRIX_ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)
#define MATRIX_ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1)
#define MATRIX_ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)
#define MATRIX_ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1)
#define MATRIX_ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)
#define MATRIX_ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1)
#define MATRIX_ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1)
#define MATRIX_ROTATION_ROLL_180_YAW_45 Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1)
#define MATRIX_ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1)
#define MATRIX_ROTATION_ROLL_180_YAW_135 Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1)
#define MATRIX_ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1)
#define MATRIX_ROTATION_ROLL_180_YAW_225 Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1)
#define MATRIX_ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1)
#define MATRIX_ROTATION_ROLL_180_YAW_315 Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1)
static void print_matrix(Matrix3f &m)
{
Serial.printf("[%.2f %.2f %.2f] [%.2f %.2f %.2f] [%.2f %.2f %.2f]\n",
m.a.x, m.a.y, m.a.z,
m.b.x, m.b.y, m.b.z,
m.c.x, m.c.y, m.c.z);
}
// test one matrix
static void test_matrix(enum Rotation rotation, Matrix3f m)
{
Matrix3f m2, diff;
const float accuracy = 1.0e-6;
m2.rotation(rotation);
diff = (m - m2);
if (diff.a.length() > accuracy ||
diff.b.length() > accuracy ||
diff.c.length() > accuracy) {
Serial.printf("rotation matrix %u incorrect\n", (unsigned)rotation);
print_matrix(m);
print_matrix(m2);
}
}
// test generation of rotation matrices
static void test_matrices(void)
{
Serial.println("testing rotation matrices\n");
test_matrix(ROTATION_NONE, MATRIX_ROTATION_NONE);
test_matrix(ROTATION_YAW_45, MATRIX_ROTATION_YAW_45);
test_matrix(ROTATION_YAW_90, MATRIX_ROTATION_YAW_90);
test_matrix(ROTATION_YAW_135, MATRIX_ROTATION_YAW_135);
test_matrix(ROTATION_YAW_180, MATRIX_ROTATION_YAW_180);
test_matrix(ROTATION_YAW_225, MATRIX_ROTATION_YAW_225);
test_matrix(ROTATION_YAW_270, MATRIX_ROTATION_YAW_270);
test_matrix(ROTATION_YAW_315, MATRIX_ROTATION_YAW_315);
test_matrix(ROTATION_ROLL_180, MATRIX_ROTATION_ROLL_180);
test_matrix(ROTATION_ROLL_180_YAW_45, MATRIX_ROTATION_ROLL_180_YAW_45);
test_matrix(ROTATION_ROLL_180_YAW_90, MATRIX_ROTATION_ROLL_180_YAW_90);
test_matrix(ROTATION_ROLL_180_YAW_135, MATRIX_ROTATION_ROLL_180_YAW_135);
test_matrix(ROTATION_PITCH_180, MATRIX_ROTATION_PITCH_180);
test_matrix(ROTATION_ROLL_180_YAW_225, MATRIX_ROTATION_ROLL_180_YAW_225);
test_matrix(ROTATION_ROLL_180_YAW_270, MATRIX_ROTATION_ROLL_180_YAW_270);
test_matrix(ROTATION_ROLL_180_YAW_315, MATRIX_ROTATION_ROLL_180_YAW_315);
}
// test rotation of vectors
static void test_vector(enum Rotation rotation, Vector3f v1, bool show=true)
{
Vector3f v2, diff;
Matrix3f m;
v2 = v1;
m.rotation(rotation);
v1.rotate(rotation);
v2 = m * v2;
diff = v1 - v2;
if (diff.length() > 1.0e-6) {
Serial.printf("rotation vector %u incorrect\n", (unsigned)rotation);
Serial.printf("%u %f %f %f\n",
(unsigned)rotation,
v2.x, v2.y, v2.z);
}
if (show) {
Serial.printf("%u %f %f %f\n",
(unsigned)rotation,
v1.x, v1.y, v1.z);
}
}
// generate a random float between -1 and 1
static float rand_num(void)
{
float ret = ((unsigned)random()) % 2000000;
return (ret - 1.0e6) / 1.0e6;
}
// test rotation of vectors
static void test_vector(enum Rotation rotation)
{
uint8_t i;
Vector3f v1;
v1.x = 1;
v1.y = 2;
v1.z = 3;
test_vector(rotation, v1);
for (i=0; i<10; i++) {
v1.x = rand_num();
v1.y = rand_num();
v1.z = rand_num();
test_vector(rotation, v1, false);
}
}
// test rotation of vectors
static void test_vectors(void)
{
Serial.println("testing rotation of vectors\n");
test_vector(ROTATION_NONE);
test_vector(ROTATION_YAW_45);
test_vector(ROTATION_YAW_90);
test_vector(ROTATION_YAW_135);
test_vector(ROTATION_YAW_180);
test_vector(ROTATION_YAW_225);
test_vector(ROTATION_YAW_270);
test_vector(ROTATION_YAW_315);
test_vector(ROTATION_ROLL_180);
test_vector(ROTATION_ROLL_180_YAW_45);
test_vector(ROTATION_ROLL_180_YAW_90);
test_vector(ROTATION_ROLL_180_YAW_135);
test_vector(ROTATION_PITCH_180);
test_vector(ROTATION_ROLL_180_YAW_225);
test_vector(ROTATION_ROLL_180_YAW_270);
test_vector(ROTATION_ROLL_180_YAW_315);
}
// test combinations of rotations
static void test_combinations(void)
{
enum Rotation r1, r2, r3;
bool found;
for (r1=ROTATION_NONE; r1<ROTATION_MAX;
r1 = (enum Rotation)((uint8_t)r1+1)) {
for (r2=ROTATION_NONE; r2<ROTATION_MAX;
r2 = (enum Rotation)((uint8_t)r2+1)) {
r3 = rotation_combination(r1, r2, &found);
if (found) {
Serial.printf("rotation: %u + %u -> %u\n",
(unsigned)r1, (unsigned)r2, (unsigned)r3);
} else {
Serial.printf("ERROR rotation: no combination for %u + %u\n",
(unsigned)r1, (unsigned)r2);
}
}
}
}
// 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;
Serial.println("\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
Serial.printf_P(PSTR("actual angle: %d\tcalculated angle:%4.2f\n"),(int)i,ToDeg(yaw));
}
}
/*
* rotation tests
*/
void setup(void)
{
Serial.begin(115200);
Serial.println("rotation unit tests\n");
test_matrices();
test_vectors();
test_combinations();
test_rotation_accuracy();
Serial.println("rotation unit tests done\n");
}
void
loop(void)
{
}