/C-OSD/arducam-osd/libraries/AP_Math/examples/eulers/Makefile |
---|
0,0 → 1,4 |
include ../../../AP_Common/Arduino.mk |
sitl: |
make -f ../../../../libraries/Desktop/Desktop.mk |
/C-OSD/arducam-osd/libraries/AP_Math/examples/eulers/eulers.pde |
---|
0,0 → 1,281 |
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
// |
// Unit tests for the AP_Math euler code |
// |
#include <FastSerial.h> |
#include <AP_Common.h> |
#include <AP_Math.h> |
FastSerialPort(Serial, 0); |
#ifdef DESKTOP_BUILD |
// all of this is needed to build with SITL |
#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 <SPI.h> |
#include <I2C.h> |
#include <AP_Baro.h> |
#include <AP_Compass.h> |
#include <AP_GPS.h> |
#include <Filter.h> |
Arduino_Mega_ISR_Registry isr_registry; |
AP_Baro_BMP085_HIL barometer; |
AP_Compass_HIL compass; |
#endif |
#include <AP_Declination.h> |
static float rad_diff(float rad1, float rad2) |
{ |
float diff = rad1 - rad2; |
if (diff > PI) { |
diff -= 2*PI; |
} |
if (diff < -PI) { |
diff += 2*PI; |
} |
return fabs(diff); |
} |
static void check_result(float roll, float pitch, float yaw, |
float roll2, float pitch2, float yaw2) |
{ |
if (isnan(roll2) || |
isnan(pitch2) || |
isnan(yaw2)) { |
Serial.printf("NAN eulers roll=%f pitch=%f yaw=%f\n", |
roll, pitch, yaw); |
} |
if (rad_diff(roll2,roll) > ToRad(179)) { |
// reverse all 3 |
roll2 += fmod(roll2+PI, 2*PI); |
pitch2 += fmod(pitch2+PI, 2*PI); |
yaw2 += fmod(yaw2+PI, 2*PI); |
} |
if (rad_diff(roll2,roll) > 0.01 || |
rad_diff(pitch2, pitch) > 0.01 || |
rad_diff(yaw2, yaw) > 0.01) { |
if (pitch >= PI/2 || |
pitch <= -PI/2 || |
ToDeg(rad_diff(pitch, PI/2)) < 1 || |
ToDeg(rad_diff(pitch, -PI/2)) < 1) { |
// we expect breakdown at these poles |
Serial.printf("breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", |
ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2)); |
} else { |
Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", |
ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2)); |
} |
} |
} |
static void test_euler(float roll, float pitch, float yaw) |
{ |
Matrix3f m; |
float roll2, pitch2, yaw2; |
m.from_euler(roll, pitch, yaw); |
m.to_euler(&roll2, &pitch2, &yaw2); |
check_result(roll, pitch, yaw, roll2, pitch2, yaw2); |
} |
#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) |
static const float angles[] = { 0, PI/8, PI/4, PI/2, PI, |
-PI/8, -PI/4, -PI/2, -PI}; |
void test_matrix_eulers(void) |
{ |
uint8_t i, j, k; |
uint8_t N = ARRAY_LENGTH(angles); |
Serial.println("rotation matrix unit tests\n"); |
for (i=0; i<N; i++) |
for (j=0; j<N; j++) |
for (k=0; k<N; k++) |
test_euler(angles[i], angles[j], angles[k]); |
Serial.println("tests done\n"); |
} |
static void test_quaternion(float roll, float pitch, float yaw) |
{ |
Quaternion q; |
float roll2, pitch2, yaw2; |
q.from_euler(roll, pitch, yaw); |
q.to_euler(&roll2, &pitch2, &yaw2); |
check_result(roll, pitch, yaw, roll2, pitch2, yaw2); |
} |
void test_quaternion_eulers(void) |
{ |
uint8_t i, j, k; |
uint8_t N = ARRAY_LENGTH(angles); |
Serial.println("quaternion unit tests\n"); |
test_quaternion(PI/4, 0, 0); |
test_quaternion(0, PI/4, 0); |
test_quaternion(0, 0, PI/4); |
test_quaternion(-PI/4, 0, 0); |
test_quaternion(0, -PI/4, 0); |
test_quaternion(0, 0, -PI/4); |
test_quaternion(-PI/4, 1, 1); |
test_quaternion(1, -PI/4, 1); |
test_quaternion(1, 1, -PI/4); |
test_quaternion(ToRad(89), 0, 0.1); |
test_quaternion(0, ToRad(89), 0.1); |
test_quaternion(0.1, 0, ToRad(89)); |
test_quaternion(ToRad(91), 0, 0.1); |
test_quaternion(0, ToRad(91), 0.1); |
test_quaternion(0.1, 0, ToRad(91)); |
for (i=0; i<N; i++) |
for (j=0; j<N; j++) |
for (k=0; k<N; k++) |
test_quaternion(angles[i], angles[j], angles[k]); |
Serial.println("tests done\n"); |
} |
static void test_conversion(float roll, float pitch, float yaw) |
{ |
Quaternion q; |
Matrix3f m, m2; |
float roll2, pitch2, yaw2; |
float roll3, pitch3, yaw3; |
q.from_euler(roll, pitch, yaw); |
q.to_euler(&roll2, &pitch2, &yaw2); |
check_result(roll, pitch, yaw, roll2, pitch2, yaw2); |
q.rotation_matrix(m); |
m.to_euler(&roll2, &pitch2, &yaw2); |
m2.from_euler(roll, pitch, yaw); |
m2.to_euler(&roll3, &pitch3, &yaw3); |
if (m.is_nan()) { |
Serial.printf("NAN matrix roll=%f pitch=%f yaw=%f\n", |
roll, pitch, yaw); |
} |
check_result(roll, pitch, yaw, roll2, pitch2, yaw2); |
check_result(roll, pitch, yaw, roll3, pitch3, yaw3); |
} |
void test_conversions(void) |
{ |
uint8_t i, j, k; |
uint8_t N = ARRAY_LENGTH(angles); |
Serial.println("matrix/quaternion tests\n"); |
test_conversion(1, 1.1, 1.2); |
test_conversion(1, -1.1, 1.2); |
test_conversion(1, -1.1, -1.2); |
test_conversion(-1, 1.1, -1.2); |
test_conversion(-1, 1.1, 1.2); |
for (i=0; i<N; i++) |
for (j=0; j<N; j++) |
for (k=0; k<N; k++) |
test_conversion(angles[i], angles[j], angles[k]); |
Serial.println("tests done\n"); |
} |
void test_frame_transforms(void) |
{ |
Vector3f v, v2; |
Quaternion q; |
Matrix3f m; |
Serial.println("frame transform tests\n"); |
q.from_euler(ToRad(90), 0, 0); |
v2 = v = Vector3f(0, 0, 1); |
q.earth_to_body(v2); |
printf("%f %f %f\n", v2.x, v2.y, v2.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; |
} |
void test_matrix_rotate(void) |
{ |
Matrix3f m1, m2, diff; |
Vector3f r; |
m1.identity(); |
m2.identity(); |
r.x = rand_num(); |
r.y = rand_num(); |
r.z = rand_num(); |
for (uint16_t i = 0; i<1000; i++) { |
// old method |
Matrix3f temp_matrix; |
temp_matrix.a.x = 0; |
temp_matrix.a.y = -r.z; |
temp_matrix.a.z = r.y; |
temp_matrix.b.x = r.z; |
temp_matrix.b.y = 0; |
temp_matrix.b.z = -r.x; |
temp_matrix.c.x = -r.y; |
temp_matrix.c.y = r.x; |
temp_matrix.c.z = 0; |
temp_matrix = m1 * temp_matrix; |
m1 += temp_matrix; |
// new method |
m2.rotate(r); |
// check they behave in the same way |
diff = m1 - m2; |
float err = diff.a.length() + diff.b.length() + diff.c.length(); |
if (err > 0) { |
Serial.printf("ERROR: i=%u err=%f\n", (unsigned)i, err); |
} |
} |
} |
/* |
euler angle tests |
*/ |
void setup(void) |
{ |
Serial.begin(115200); |
Serial.println("euler unit tests\n"); |
test_conversion(0, PI, 0); |
test_frame_transforms(); |
test_conversions(); |
test_quaternion_eulers(); |
test_matrix_eulers(); |
test_matrix_rotate(); |
} |
void |
loop(void) |
{ |
} |
/C-OSD/arducam-osd/libraries/AP_Math/examples/polygon/Makefile |
---|
0,0 → 1,0 |
include ../../../AP_Common/Arduino.mk |
/C-OSD/arducam-osd/libraries/AP_Math/examples/polygon/polygon.pde |
---|
0,0 → 1,115 |
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
// |
// Unit tests for the AP_Math polygon code |
// |
#include <FastSerial.h> |
#include <AP_Common.h> |
#include <AP_Math.h> |
FastSerialPort(Serial, 0); |
/* |
this is the boundary of the 2010 outback challenge |
Note that the last point must be the same as the first for the |
Polygon_outside() algorithm |
*/ |
static const Vector2l OBC_boundary[] = { |
Vector2l(-265695640, 1518373730), |
Vector2l(-265699560, 1518394050), |
Vector2l(-265768230, 1518411420), |
Vector2l(-265773080, 1518403440), |
Vector2l(-265815110, 1518419500), |
Vector2l(-265784860, 1518474690), |
Vector2l(-265994890, 1518528860), |
Vector2l(-266092110, 1518747420), |
Vector2l(-266454780, 1518820530), |
Vector2l(-266435720, 1518303500), |
Vector2l(-265875990, 1518344050), |
Vector2l(-265695640, 1518373730) |
}; |
static const struct { |
Vector2l point; |
bool outside; |
} test_points[] = { |
{ Vector2l(-266398870, 1518220000), true }, |
{ Vector2l(-266418700, 1518709260), false }, |
{ Vector2l(-350000000, 1490000000), true }, |
{ Vector2l(0, 0), true }, |
{ Vector2l(-265768150, 1518408250), false }, |
{ Vector2l(-265774060, 1518405860), true }, |
{ Vector2l(-266435630, 1518303440), true }, |
{ Vector2l(-266435650, 1518313540), false }, |
{ Vector2l(-266435690, 1518303530), false }, |
{ Vector2l(-266435690, 1518303490), true }, |
{ Vector2l(-265875990, 1518344049), true }, |
{ Vector2l(-265875990, 1518344051), false }, |
{ Vector2l(-266454781, 1518820530), true }, |
{ Vector2l(-266454779, 1518820530), true }, |
{ Vector2l(-266092109, 1518747420), true }, |
{ Vector2l(-266092111, 1518747420), false }, |
{ Vector2l(-266092110, 1518747421), true }, |
{ Vector2l(-266092110, 1518747419), false }, |
{ Vector2l(-266092111, 1518747421), true }, |
{ Vector2l(-266092109, 1518747421), true }, |
{ Vector2l(-266092111, 1518747419), false }, |
}; |
#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) |
/* |
polygon tests |
*/ |
void setup(void) |
{ |
unsigned i, count; |
bool all_passed = true; |
uint32_t start_time; |
Serial.begin(115200); |
Serial.println("polygon unit tests\n"); |
if (!Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary))) { |
Serial.println("OBC boundary is not complete!"); |
all_passed = false; |
} |
if (Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary)-1)) { |
Serial.println("Polygon_complete test failed"); |
all_passed = false; |
} |
for (i=0; i<ARRAY_LENGTH(test_points); i++) { |
bool result; |
result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary)); |
Serial.printf_P(PSTR("%10f,%10f %s %s\n"), |
1.0e-7*test_points[i].point.x, |
1.0e-7*test_points[i].point.y, |
result?"OUTSIDE":"INSIDE ", |
result == test_points[i].outside?"PASS":"FAIL"); |
if (result != test_points[i].outside) { |
all_passed = false; |
} |
} |
Serial.println(all_passed?"TEST PASSED":"TEST FAILED"); |
Serial.println("Speed test:"); |
start_time = micros(); |
for (count=0; count<1000; count++) { |
for (i=0; i<ARRAY_LENGTH(test_points); i++) { |
bool result; |
result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary)); |
if (result != test_points[i].outside) { |
all_passed = false; |
} |
} |
} |
Serial.printf("%u usec/call\n", (unsigned)((micros() - start_time)/(count*ARRAY_LENGTH(test_points)))); |
Serial.println(all_passed?"ALL TESTS PASSED":"TEST FAILED"); |
} |
void |
loop(void) |
{ |
} |
/C-OSD/arducam-osd/libraries/AP_Math/examples/rotations/Makefile |
---|
0,0 → 1,4 |
include ../../../AP_Common/Arduino.mk |
sitl: |
make -f ../../../../libraries/Desktop/Desktop.mk |
/C-OSD/arducam-osd/libraries/AP_Math/examples/rotations/rotations.pde |
---|
0,0 → 1,205 |
/// -*- 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_Math.h> |
FastSerialPort(Serial, 0); |
#ifdef DESKTOP_BUILD |
// all of this is needed to build with SITL |
#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> |
Arduino_Mega_ISR_Registry isr_registry; |
AP_Baro_BMP085_HIL barometer; |
AP_Compass_HIL compass; |
#endif |
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library |
// 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); |
} |
} |
} |
} |
/* |
rotation tests |
*/ |
void setup(void) |
{ |
Serial.begin(115200); |
Serial.println("rotation unit tests\n"); |
test_matrices(); |
test_vectors(); |
test_combinations(); |
Serial.println("rotation unit tests done\n"); |
} |
void |
loop(void) |
{ |
} |