uncrustify libraries/AP_Math/examples/rotations/rotations.pde

This commit is contained in:
uncrustify 2012-08-16 23:20:14 -07:00 committed by Pat Hickey
parent baa74ae360
commit a40691e874
1 changed files with 43 additions and 43 deletions

View File

@ -11,25 +11,25 @@ FastSerialPort(Serial, 0);
#ifdef DESKTOP_BUILD #ifdef DESKTOP_BUILD
// all of this is needed to build with SITL // all of this is needed to build with SITL
#include <DataFlash.h> #include <DataFlash.h>
#include <APM_RC.h> #include <APM_RC.h>
#include <GCS_MAVLink.h> #include <GCS_MAVLink.h>
#include <Arduino_Mega_ISR_Registry.h> #include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h> #include <AP_PeriodicProcess.h>
#include <AP_ADC.h> #include <AP_ADC.h>
#include <AP_Baro.h> #include <AP_Baro.h>
#include <AP_Compass.h> #include <AP_Compass.h>
#include <AP_GPS.h> #include <AP_GPS.h>
Arduino_Mega_ISR_Registry isr_registry; Arduino_Mega_ISR_Registry isr_registry;
AP_Baro_BMP085_HIL barometer; AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass; AP_Compass_HIL compass;
#endif #endif
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library #include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
// standard rotation matrices (these are the originals from the old code) // 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_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_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_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_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1)
@ -57,17 +57,17 @@ static void print_matrix(Matrix3f &m)
// test one matrix // test one matrix
static void test_matrix(enum Rotation rotation, Matrix3f m) static void test_matrix(enum Rotation rotation, Matrix3f m)
{ {
Matrix3f m2, diff; Matrix3f m2, diff;
const float accuracy = 1.0e-6; const float accuracy = 1.0e-6;
m2.rotation(rotation); m2.rotation(rotation);
diff = (m - m2); diff = (m - m2);
if (diff.a.length() > accuracy || if (diff.a.length() > accuracy ||
diff.b.length() > accuracy || diff.b.length() > accuracy ||
diff.c.length() > accuracy) { diff.c.length() > accuracy) {
Serial.printf("rotation matrix %u incorrect\n", (unsigned)rotation); Serial.printf("rotation matrix %u incorrect\n", (unsigned)rotation);
print_matrix(m); print_matrix(m);
print_matrix(m2); print_matrix(m2);
} }
} }
// test generation of rotation matrices // test generation of rotation matrices
@ -95,19 +95,19 @@ static void test_matrices(void)
// test rotation of vectors // test rotation of vectors
static void test_vector(enum Rotation rotation, Vector3f v1, bool show=true) static void test_vector(enum Rotation rotation, Vector3f v1, bool show=true)
{ {
Vector3f v2, diff; Vector3f v2, diff;
Matrix3f m; Matrix3f m;
v2 = v1; v2 = v1;
m.rotation(rotation); m.rotation(rotation);
v1.rotate(rotation); v1.rotate(rotation);
v2 = m * v2; v2 = m * v2;
diff = v1 - v2; diff = v1 - v2;
if (diff.length() > 1.0e-6) { if (diff.length() > 1.0e-6) {
Serial.printf("rotation vector %u incorrect\n", (unsigned)rotation); Serial.printf("rotation vector %u incorrect\n", (unsigned)rotation);
Serial.printf("%u %f %f %f\n", Serial.printf("%u %f %f %f\n",
(unsigned)rotation, (unsigned)rotation,
v2.x, v2.y, v2.z); v2.x, v2.y, v2.z);
} }
if (show) { if (show) {
Serial.printf("%u %f %f %f\n", Serial.printf("%u %f %f %f\n",
(unsigned)rotation, (unsigned)rotation,
@ -118,8 +118,8 @@ static void test_vector(enum Rotation rotation, Vector3f v1, bool show=true)
// generate a random float between -1 and 1 // generate a random float between -1 and 1
static float rand_num(void) static float rand_num(void)
{ {
float ret = ((unsigned)random()) % 2000000; float ret = ((unsigned)random()) % 2000000;
return (ret - 1.0e6) / 1.0e6; return (ret - 1.0e6) / 1.0e6;
} }
// test rotation of vectors // test rotation of vectors
@ -127,10 +127,10 @@ static void test_vector(enum Rotation rotation)
{ {
uint8_t i; uint8_t i;
Vector3f v1; Vector3f v1;
v1.x = 1; v1.x = 1;
v1.y = 2; v1.y = 2;
v1.z = 3; v1.z = 3;
test_vector(rotation, v1); test_vector(rotation, v1);
for (i=0; i<10; i++) { for (i=0; i<10; i++) {
@ -170,8 +170,8 @@ static void test_combinations(void)
enum Rotation r1, r2, r3; enum Rotation r1, r2, r3;
bool found; bool found;
for (r1=ROTATION_NONE; r1<ROTATION_MAX; for (r1=ROTATION_NONE; r1<ROTATION_MAX;
r1 = (enum Rotation)((uint8_t)r1+1)) { r1 = (enum Rotation)((uint8_t)r1+1)) {
for (r2=ROTATION_NONE; r2<ROTATION_MAX; for (r2=ROTATION_NONE; r2<ROTATION_MAX;
r2 = (enum Rotation)((uint8_t)r2+1)) { r2 = (enum Rotation)((uint8_t)r2+1)) {
r3 = rotation_combination(r1, r2, &found); r3 = rotation_combination(r1, r2, &found);
@ -187,7 +187,7 @@ static void test_combinations(void)
} }
/* /*
rotation tests * rotation tests
*/ */
void setup(void) void setup(void)
{ {