Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-09 16:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: example fix travis warning

Browse Source 
missing function declaration
implicit cast
some style fix
This commit is contained in:
Pierre Kancir 2017-04-13 13:31:52 +02:00 committed by Francisco Ferreira
parent a2eeab4db7
commit f2812c1efd
4 changed files with 176 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
libraries/AP_Math/examples/eulers/eulers.cpp
View File

@ -5,6 +5,14 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
void setup();
void loop();
void test_matrix_rotate(void);
void test_frame_transforms(void);
void test_conversions(void);
void test_quaternion_eulers(void);
void test_matrix_eulers(void);
const AP_HAL::HAL& hal = AP_HAL::get_HAL(); const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#define SHOW_POLES_BREAKDOWN 0 #define SHOW_POLES_BREAKDOWN 0
@ -13,10 +21,10 @@ static float rad_diff(float rad1, float rad2)
{ {
float diff = rad1 - rad2; float diff = rad1 - rad2;
if (diff > M_PI) { if (diff > M_PI) {
diff -= 2*M_PI; diff -= 2 * M_PI;
} }
if (diff < -M_PI) { if (diff < -M_PI) {
diff += 2*M_PI; diff += 2 * M_PI;
} }
return fabsf(diff); return fabsf(diff);
} }
@ -29,14 +37,17 @@ static void check_result(const char *msg,
isnan(pitch2) || isnan(pitch2) ||
isnan(yaw2)) { isnan(yaw2)) {
hal.console->printf("%s NAN eulers roll=%f pitch=%f yaw=%f\n", hal.console->printf("%s NAN eulers roll=%f pitch=%f yaw=%f\n",
msg, roll, pitch, yaw); msg,
(double)roll,
(double)pitch,
(double)yaw);
} }
if (rad_diff(roll2,roll) > ToRad(179)) { if (rad_diff(roll2,roll) > ToRad(179)) {
// reverse all 3 // reverse all 3
roll2 += fmod(roll2+M_PI, 2*M_PI); roll2 += fmodf(roll2 + M_PI, 2 * M_PI);
pitch2 += fmod(pitch2+M_PI, 2*M_PI); pitch2 += fmodf(pitch2 + M_PI, 2 * M_PI);
yaw2 += fmod(yaw2+M_PI, 2*M_PI); yaw2 += fmodf(yaw2 + M_PI, 2 * M_PI);
} }
if (rad_diff(roll2,roll) > 0.01f || if (rad_diff(roll2,roll) > 0.01f ||
@ -51,17 +62,17 @@ static void check_result(const char *msg,
hal.console->printf( hal.console->printf(
"%s breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", "%s breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
msg, msg,
ToDeg(roll), ToDeg(roll2), (double)ToDeg(roll), (double)ToDeg(roll2),
ToDeg(pitch), ToDeg(pitch2), (double)ToDeg(pitch), (double)ToDeg(pitch2),
ToDeg(yaw), ToDeg(yaw2)); (double)ToDeg(yaw), (double)ToDeg(yaw2));
#endif #endif
} else { } else {
hal.console->printf( hal.console->printf(
"%s incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", "%s incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
msg, msg,
ToDeg(roll), ToDeg(roll2), (double)ToDeg(roll), (double)ToDeg(roll2),
ToDeg(pitch), ToDeg(pitch2), (double)ToDeg(pitch), (double)ToDeg(pitch2),
ToDeg(yaw), ToDeg(yaw2)); (double)ToDeg(yaw), (double)ToDeg(yaw2));
} }
} }
} }
@ -81,14 +92,13 @@ static const float angles[] = { 0, M_PI/8, M_PI/4, M_PI/2, M_PI,
void test_matrix_eulers(void) void test_matrix_eulers(void)
{ {
uint8_t i, j, k;
uint8_t N = ARRAY_SIZE(angles); uint8_t N = ARRAY_SIZE(angles);
hal.console->printf("rotation matrix unit tests\n\n"); hal.console->printf("rotation matrix unit tests\n\n");
for (i=0; i<N; i++) for (uint8_t i = 0; i < N; i++)
for (j=0; j<N; j++) for (uint8_t j = 0; j < N; j++)
for (k=0; k<N; k++) for (uint8_t k = 0; k < N; k++)
test_euler(angles[i], angles[j], angles[k]); test_euler(angles[i], angles[j], angles[k]);
hal.console->printf("tests done\n\n"); hal.console->printf("tests done\n\n");
@ -120,7 +130,6 @@ static void test_quaternion(float roll, float pitch, float yaw)
void test_quaternion_eulers(void) void test_quaternion_eulers(void)
{ {
uint8_t i, j, k;
uint8_t N = ARRAY_SIZE(angles); uint8_t N = ARRAY_SIZE(angles);
hal.console->printf("quaternion unit tests\n\n"); hal.console->printf("quaternion unit tests\n\n");
@ -143,9 +152,9 @@ void test_quaternion_eulers(void)
test_quaternion(0, ToRad(91), 0.1f); test_quaternion(0, ToRad(91), 0.1f);
test_quaternion(0.1f, 0, ToRad(91)); test_quaternion(0.1f, 0, ToRad(91));
for (i=0; i<N; i++) for (uint8_t i = 0; i < N; i++)
for (j=0; j<N; j++) for (uint8_t j = 0; j < N; j++)
for (k=0; k<N; k++) for (uint8_t k = 0; k < N; k++)
test_quaternion(angles[i], angles[j], angles[k]); test_quaternion(angles[i], angles[j], angles[k]);
hal.console->printf("tests done\n\n"); hal.console->printf("tests done\n\n");
@ -171,7 +180,9 @@ static void test_conversion(float roll, float pitch, float yaw)
m2.to_euler(&roll3, &pitch3, &yaw3); m2.to_euler(&roll3, &pitch3, &yaw3);
if (m.is_nan()) { if (m.is_nan()) {
hal.console->printf("NAN matrix roll=%f pitch=%f yaw=%f\n", hal.console->printf("NAN matrix roll=%f pitch=%f yaw=%f\n",
roll, pitch, yaw); (double)roll,
(double)pitch,
(double)yaw);
} }
check_result("test_conversion2", roll, pitch, yaw, roll2, pitch2, yaw2); check_result("test_conversion2", roll, pitch, yaw, roll2, pitch2, yaw2);
@ -180,7 +191,6 @@ static void test_conversion(float roll, float pitch, float yaw)
void test_conversions(void) void test_conversions(void)
{ {
uint8_t i, j, k;
uint8_t N = ARRAY_SIZE(angles); uint8_t N = ARRAY_SIZE(angles);
hal.console->printf("matrix/quaternion tests\n\n"); hal.console->printf("matrix/quaternion tests\n\n");
@ -191,9 +201,9 @@ void test_conversions(void)
test_conversion(-1, 1.1f, -1.2f); test_conversion(-1, 1.1f, -1.2f);
test_conversion(-1, 1.1f, 1.2f); test_conversion(-1, 1.1f, 1.2f);
for (i=0; i<N; i++) for (uint8_t i = 0; i < N; i++)
for (j=0; j<N; j++) for (uint8_t j = 0; j < N; j++)
for (k=0; k<N; k++) for (uint8_t k = 0; k < N; k++)
test_conversion(angles[i], angles[j], angles[k]); test_conversion(angles[i], angles[j], angles[k]);
hal.console->printf("tests done\n\n"); hal.console->printf("tests done\n\n");
@ -211,23 +221,23 @@ void test_frame_transforms(void)
q.normalize(); q.normalize();
m.from_euler(ToRad(45), ToRad(45), ToRad(45)); m.from_euler(ToRad(45), ToRad(45), ToRad(45));
v2 = v = Vector3f(0, 0, 1); v2 = v = Vector3f(0.0f, 0.0f, 1.0f);
q.earth_to_body(v2); q.earth_to_body(v2);
hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z); hal.console->printf("%f %f %f\n", (double)v2.x, (double)v2.y, (double)v2.z);
v2 = m * v; v2 = m * v;
hal.console->printf("%f %f %f\n\n", v2.x, v2.y, v2.z); hal.console->printf("%f %f %f\n\n", (double)v2.x, (double)v2.y, (double)v2.z);
v2 = v = Vector3f(0, 1, 0); v2 = v = Vector3f(0.0f, 1.0f, 0.0f);
q.earth_to_body(v2); q.earth_to_body(v2);
hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z); hal.console->printf("%f %f %f\n", (double)v2.x, (double)v2.y, (double)v2.z);
v2 = m * v; v2 = m * v;
hal.console->printf("%f %f %f\n\n", v2.x, v2.y, v2.z); hal.console->printf("%f %f %f\n\n", (double)v2.x, (double)v2.y, (double)v2.z);
v2 = v = Vector3f(1, 0, 0); v2 = v = Vector3f(1.0f, 0.0f, 0.0f);
q.earth_to_body(v2); q.earth_to_body(v2);
hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z); hal.console->printf("%f %f %f\n", (double)v2.x, (double)v2.y, (double)v2.z);
v2 = m * v; v2 = m * v;
hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z); hal.console->printf("%f %f %f\n", (double)v2.x, (double)v2.y, (double)v2.z);
} }
// generate a random float between -1 and 1 // generate a random float between -1 and 1
@ -252,7 +262,7 @@ void test_matrix_rotate(void)
r.y = rand_num(); r.y = rand_num();
r.z = rand_num(); r.z = rand_num();
for (uint16_t i = 0; i<1000; i++) { for (uint16_t i = 0; i < 1000; i++) {
// old method // old method
Matrix3f temp_matrix; Matrix3f temp_matrix;
temp_matrix.a.x = 0; temp_matrix.a.x = 0;
@ -275,7 +285,7 @@ void test_matrix_rotate(void)
float err = diff.a.length() + diff.b.length() + diff.c.length(); float err = diff.a.length() + diff.b.length() + diff.c.length();
if (err > 0) { if (err > 0) {
hal.console->printf("ERROR: i=%u err=%f\n", (unsigned)i, err); hal.console->printf("ERROR: i=%u err=%f\n", (unsigned)i, (double)err);
} }
} }
} }
@ -296,6 +306,6 @@ void setup(void)
test_matrix_rotate(); test_matrix_rotate();
} }
void loop(void){} void loop(void) {}
AP_HAL_MAIN(); AP_HAL_MAIN();

118
libraries/AP_Math/examples/location/location.cpp
View File

@ -5,6 +5,9 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL(); const AP_HAL::HAL& hal = AP_HAL::get_HAL();
static const struct { static const struct {
@ -17,21 +20,21 @@ static const struct {
{ Vector2f(-35.36438601157189f, 149.16613916088568f), { Vector2f(-35.36438601157189f, 149.16613916088568f),
Vector2f(-35.364432558610254f, 149.16287313113048f), Vector2f(-35.364432558610254f, 149.16287313113048f),
Vector2f(-35.36491510034746f, 149.16365837225004f), false }, Vector2f(-35.36491510034746f, 149.16365837225004f), false },
{ Vector2f(0, 0), { Vector2f(0.0f, 0.0f),
Vector2f(0, 1), Vector2f(0.0f, 1.0f),
Vector2f(0, 2), true }, Vector2f(0.0f, 2.0f), true },
{ Vector2f(0, 0), { Vector2f(0.0f, 0.0f),
Vector2f(0, 2), Vector2f(0.0f, 2.0f),
Vector2f(0, 1), false }, Vector2f(0.0f, 1.0f), false },
{ Vector2f(0, 0), { Vector2f(0.0f, 0.0f),
Vector2f(1, 0), Vector2f(1.0f, 0.0f),
Vector2f(2, 0), true }, Vector2f(2.0f, 0.0f), true },
{ Vector2f(0, 0), { Vector2f(0.0f, 0.0f),
Vector2f(2, 0), Vector2f(2.0f, 0.0f),
Vector2f(1, 0), false }, Vector2f(1.0f, 0.0f), false },
{ Vector2f(0, 0), { Vector2f(0.0f, 0.0f),
Vector2f(-1, 1), Vector2f(-1.0f, 1.0f),
Vector2f(-2, 2), true }, Vector2f(-2.0f, 2.0f), true },
}; };
static struct Location location_from_point(Vector2f pt) static struct Location location_from_point(Vector2f pt)
@ -45,7 +48,7 @@ static struct Location location_from_point(Vector2f pt)
static void test_passed_waypoint(void) static void test_passed_waypoint(void)
{ {
hal.console->printf("waypoint tests starting\n"); hal.console->printf("waypoint tests starting\n");
for (uint8_t i=0; i<ARRAY_SIZE(test_points); i++) { for (uint8_t i = 0; i < ARRAY_SIZE(test_points); i++) {
struct Location loc = location_from_point(test_points[i].location); struct Location loc = location_from_point(test_points[i].location);
struct Location wp1 = location_from_point(test_points[i].wp1); struct Location wp1 = location_from_point(test_points[i].wp1);
struct Location wp2 = location_from_point(test_points[i].wp2); struct Location wp2 = location_from_point(test_points[i].wp2);
@ -81,27 +84,27 @@ static void test_one_offset(const struct Location &loc,
if (fabsf(dist - dist2) > 1.0f || if (fabsf(dist - dist2) > 1.0f ||
brg_error > 1.0f) { brg_error > 1.0f) {
hal.console->printf("Failed offset test brg_error=%f dist_error=%f\n", hal.console->printf("Failed offset test brg_error=%f dist_error=%f\n",
brg_error, dist-dist2); (double)brg_error, (double)(dist - dist2));
} }
} }
static const struct { static const struct {
float ofs_north, ofs_east, distance, bearing; float ofs_north, ofs_east, distance, bearing;
} test_offsets[] = { } test_offsets[] = {
{ 1000, 1000, sqrt(2.0f)*1000, 45 }, { 1000.0f, 1000.0f, sqrtf(2.0f) * 1000.0f, 45.0f },
{ 1000, -1000, sqrt(2.0f)*1000, -45 }, { 1000.0f, -1000.0f, sqrtf(2.0f) * 1000.0f, -45.0f },
{ 1000, 0, 1000, 0 }, { 1000.0f, 0.0f, 1000.0f, 0.0f },
{ 0, 1000, 1000, 90 }, { 0.0f, 1000.0f, 1000.0f, 90.0f },
}; };
static void test_offset(void) static void test_offset(void)
{ {
struct Location loc {}; struct Location loc {};
loc.lat = -35*1.0e7f; loc.lat = -35 * 1.0e7f;
loc.lng = 149*1.0e7f; loc.lng = 149 * 1.0e7f;
for (uint8_t i=0; i<ARRAY_SIZE(test_offsets); i++) { for (uint8_t i = 0; i < ARRAY_SIZE(test_offsets); i++) {
test_one_offset(loc, test_one_offset(loc,
test_offsets[i].ofs_north, test_offsets[i].ofs_north,
test_offsets[i].ofs_east, test_offsets[i].ofs_east,
@ -122,53 +125,53 @@ static void test_accuracy(void)
loc.lng = -120.0e7f; loc.lng = -120.0e7f;
struct Location loc2 = loc; struct Location loc2 = loc;
Vector2f v((loc.lat*1.0e-7f), (loc.lng*1.0e-7f)); Vector2f v((loc.lat * 1.0e-7f), (loc.lng* 1.0e-7f));
Vector2f v2; Vector2f v2;
loc2 = loc; loc2 = loc;
loc2.lat += 10000000; loc2.lat += 10000000;
v2 = Vector2f(loc2.lat*1.0e-7f, loc2.lng*1.0e-7f); v2 = Vector2f(loc2.lat * 1.0e-7f, loc2.lng * 1.0e-7f);
hal.console->printf("1 degree lat dist=%.4f\n", get_distance(loc, loc2)); hal.console->printf("1 degree lat dist=%.4f\n", (double)get_distance(loc, loc2));
loc2 = loc; loc2 = loc;
loc2.lng += 10000000; loc2.lng += 10000000;
v2 = Vector2f(loc2.lat*1.0e-7f, loc2.lng*1.0e-7f); v2 = Vector2f(loc2.lat * 1.0e-7f, loc2.lng * 1.0e-7f);
hal.console->printf("1 degree lng dist=%.4f\n", get_distance(loc, loc2)); hal.console->printf("1 degree lng dist=%.4f\n", (double)get_distance(loc, loc2));
for (int32_t i=0; i<100; i++) { for (int32_t i = 0; i < 100; i++) {
loc2 = loc; loc2 = loc;
loc2.lat += i; loc2.lat += i;
v2 = Vector2f((loc.lat+i)*1.0e-7f, loc.lng*1.0e-7f); v2 = Vector2f((loc.lat + i) * 1.0e-7f, loc.lng * 1.0e-7f);
if (v2.x != v.x || v2.y != v.y) { if (v2 != v) {
hal.console->printf("lat v2 != v at i=%d dist=%.4f\n", (int)i, get_distance(loc, loc2)); hal.console->printf("lat v2 != v at i=%d dist=%.4f\n", (int)i, (double)get_distance(loc, loc2));
break; break;
} }
} }
for (int32_t i=0; i<100; i++) { for (int32_t i = 0; i < 100; i++) {
loc2 = loc; loc2 = loc;
loc2.lng += i; loc2.lng += i;
v2 = Vector2f(loc.lat*1.0e-7f, (loc.lng+i)*1.0e-7f); v2 = Vector2f(loc.lat * 1.0e-7f, (loc.lng + i) * 1.0e-7f);
if (v2.x != v.x || v2.y != v.y) { if (v2 != v) {
hal.console->printf("lng v2 != v at i=%d dist=%.4f\n", (int)i, get_distance(loc, loc2)); hal.console->printf("lng v2 != v at i=%d dist=%.4f\n", (int)i, (double)get_distance(loc, loc2));
break; break;
} }
} }
for (int32_t i=0; i<100; i++) { for (int32_t i = 0; i < 100; i++) {
loc2 = loc; loc2 = loc;
loc2.lat -= i; loc2.lat -= i;
v2 = Vector2f((loc.lat-i)*1.0e-7f, loc.lng*1.0e-7f); v2 = Vector2f((loc.lat - i) * 1.0e-7f, loc.lng * 1.0e-7f);
if (v2.x != v.x || v2.y != v.y) { if (v2 != v) {
hal.console->printf("-lat v2 != v at i=%d dist=%.4f\n", (int)i, get_distance(loc, loc2)); hal.console->printf("-lat v2 != v at i=%d dist=%.4f\n", (int)i, (double)get_distance(loc, loc2));
break; break;
} }
} }
for (int32_t i=0; i<100; i++) { for (int32_t i = 0; i < 100; i++) {
loc2 = loc; loc2 = loc;
loc2.lng -= i; loc2.lng -= i;
v2 = Vector2f(loc.lat*1.0e-7f, (loc.lng-i)*1.0e-7f); v2 = Vector2f(loc.lat * 1.0e-7f, (loc.lng - i) * 1.0e-7f);
if (v2.x != v.x || v2.y != v.y) { if (v2 != v) {
hal.console->printf("-lng v2 != v at i=%d dist=%.4f\n", (int)i, get_distance(loc, loc2)); hal.console->printf("-lng v2 != v at i=%d dist=%.4f\n", (int)i, (double)get_distance(loc, loc2));
break; break;
} }
} }
@ -200,7 +203,7 @@ static const struct {
static void test_wrap_cd(void) static void test_wrap_cd(void)
{ {
for (uint8_t i=0; i < ARRAY_SIZE(wrap_180_tests); i++) { for (uint8_t i = 0; i < ARRAY_SIZE(wrap_180_tests); i++) {
int32_t r = wrap_180_cd(wrap_180_tests[i].v); int32_t r = wrap_180_cd(wrap_180_tests[i].v);
if (r != wrap_180_tests[i].wv) { if (r != wrap_180_tests[i].wv) {
hal.console->printf("wrap_180: v=%ld wv=%ld r=%ld\n", hal.console->printf("wrap_180: v=%ld wv=%ld r=%ld\n",
@ -210,7 +213,7 @@ static void test_wrap_cd(void)
} }
} }
for (uint8_t i=0; i < ARRAY_SIZE(wrap_360_tests); i++) { for (uint8_t i = 0; i < ARRAY_SIZE(wrap_360_tests); i++) {
int32_t r = wrap_360_cd(wrap_360_tests[i].v); int32_t r = wrap_360_cd(wrap_360_tests[i].v);
if (r != wrap_360_tests[i].wv) { if (r != wrap_360_tests[i].wv) {
hal.console->printf("wrap_360: v=%ld wv=%ld r=%ld\n", hal.console->printf("wrap_360: v=%ld wv=%ld r=%ld\n",
@ -220,13 +223,13 @@ static void test_wrap_cd(void)
} }
} }
for (uint8_t i=0; i < ARRAY_SIZE(wrap_PI_tests); i++) { for (uint8_t i = 0; i < ARRAY_SIZE(wrap_PI_tests); i++) {
float r = wrap_PI(wrap_PI_tests[i].v); float r = wrap_PI(wrap_PI_tests[i].v);
if (fabsf(r - wrap_PI_tests[i].wv) > 0.001f) { if (fabsf(r - wrap_PI_tests[i].wv) > 0.001f) {
hal.console->printf("wrap_PI: v=%f wv=%f r=%f\n", hal.console->printf("wrap_PI: v=%f wv=%f r=%f\n",
wrap_PI_tests[i].v, (double)wrap_PI_tests[i].v,
wrap_PI_tests[i].wv, (double)wrap_PI_tests[i].wv,
r); (double)r);
} }
} }
@ -239,16 +242,16 @@ static void test_wgs_conversion_functions(void)
#define D2R DEG_TO_RAD_DOUBLE #define D2R DEG_TO_RAD_DOUBLE
/* Maximum allowable error in quantities with units of length (in meters). */ /* Maximum allowable error in quantities with units of length (in meters). */
#define MAX_DIST_ERROR_M 1e-6 static const double MAX_DIST_ERROR_M = 1e-6;
/* Maximum allowable error in quantities with units of angle (in sec of arc). /* Maximum allowable error in quantities with units of angle (in sec of arc).
* 1 second of arc on the equator is ~31 meters. */ * 1 second of arc on the equator is ~31 meters. */
#define MAX_ANGLE_ERROR_SEC 1e-7 static const double MAX_ANGLE_ERROR_SEC = 1e-7;
#define MAX_ANGLE_ERROR_RAD (MAX_ANGLE_ERROR_SEC*(D2R/3600.0)) static const double MAX_ANGLE_ERROR_RAD = (MAX_ANGLE_ERROR_SEC * (D2R / (double)3600.0));
/* Semi-major axis. */ /* Semi-major axis. */
#define EARTH_A 6378137.0 static const double EARTH_A = 6378137.0;
/* Semi-minor axis. */ /* Semi-minor axis. */
#define EARTH_B 6356752.31424517929553985595703125 static const double EARTH_B = 6356752.31424517929553985595703125;
#define NUM_COORDS 10 #define NUM_COORDS 10
@ -291,7 +294,8 @@ static void test_wgs_conversion_functions(void)
hal.console->printf("passing llh to ecef test %d\n", i); hal.console->printf("passing llh to ecef test %d\n", i);
} else { } else {
hal.console->printf("failed llh to ecef test %d: ", i); hal.console->printf("failed llh to ecef test %d: ", i);
hal.console->printf("(%f - %f) (%f - %f) (%f - %f) => %.10f %.10f %.10f\n", ecef[0], ecefs[i][0], ecef[1], ecefs[i][1], ecef[2], ecefs[i][2], x_err, y_err, z_err); hal.console->printf("(%f - %f) (%f - %f) (%f - %f) => %.10f %.10f %.10f\n",
ecef[0], ecefs[i][0], ecef[1], ecefs[i][1], ecef[2], ecefs[i][2], x_err, y_err, z_err);
} }
} }

23
libraries/AP_Math/examples/polygon/polygon.cpp
View File

@ -5,6 +5,9 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL(); const AP_HAL::HAL& hal = AP_HAL::get_HAL();
/* /*
@ -59,7 +62,7 @@ static const struct {
*/ */
void setup(void) void setup(void)
{ {
unsigned i, count; uint32_t count;
bool all_passed = true; bool all_passed = true;
uint32_t start_time; uint32_t start_time;
@ -75,13 +78,12 @@ void setup(void)
all_passed = false; all_passed = false;
} }
for (i=0; i<ARRAY_SIZE(test_points); i++) { for (uint32_t i = 0; i < ARRAY_SIZE(test_points); i++) {
bool result; bool result = Polygon_outside(test_points[i].point,
result = Polygon_outside(test_points[i].point,
OBC_boundary, ARRAY_SIZE(OBC_boundary)); OBC_boundary, ARRAY_SIZE(OBC_boundary));
hal.console->printf("%10f,%10f %s %s\n", hal.console->printf("%10f,%10f %s %s\n",
1.0e-7f*test_points[i].point.x, (double)(1.0e-7f * test_points[i].point.x),
1.0e-7f*test_points[i].point.y, (double)(1.0e-7f * test_points[i].point.y),
result ? "OUTSIDE" : "INSIDE ", result ? "OUTSIDE" : "INSIDE ",
result == test_points[i].outside ? "PASS" : "FAIL"); result == test_points[i].outside ? "PASS" : "FAIL");
if (result != test_points[i].outside) { if (result != test_points[i].outside) {
@ -92,10 +94,9 @@ void setup(void)
hal.console->printf("Speed test:\n"); hal.console->printf("Speed test:\n");
start_time = AP_HAL::micros(); start_time = AP_HAL::micros();
for (count=0; count<1000; count++) { for (count = 0; count < 1000; count++) {
for (i=0; i<ARRAY_SIZE(test_points); i++) { for (uint32_t i = 0; i < ARRAY_SIZE(test_points); i++) {
bool result; bool result = Polygon_outside(test_points[i].point,
result = Polygon_outside(test_points[i].point,
OBC_boundary, ARRAY_SIZE(OBC_boundary)); OBC_boundary, ARRAY_SIZE(OBC_boundary));
if (result != test_points[i].outside) { if (result != test_points[i].outside) {
all_passed = false; all_passed = false;
@ -103,7 +104,7 @@ void setup(void)
} }
} }
hal.console->printf("%u usec/call\n", (unsigned)((AP_HAL::micros() hal.console->printf("%u usec/call\n", (unsigned)((AP_HAL::micros()
- start_time)/(count*ARRAY_SIZE(test_points)))); - start_time)/(count * ARRAY_SIZE(test_points))));
hal.console->printf("%s\n", all_passed ? "ALL TESTS PASSED" : "TEST FAILED"); hal.console->printf("%s\n", all_passed ? "ALL TESTS PASSED" : "TEST FAILED");
} }

103
libraries/AP_Math/examples/rotations/rotations.cpp
View File

@ -5,12 +5,17 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL(); const AP_HAL::HAL& hal = AP_HAL::get_HAL();
static void print_vector(Vector3f &v) static void print_vector(Vector3f &v)
{ {
hal.console->printf("[%.4f %.4f %.4f]\n", hal.console->printf("[%.4f %.4f %.4f]\n",
v.x, v.y, v.z); (double)v.x,
(double)v.y,
(double)v.z);
} }
// test rotation method accuracy // test rotation method accuracy
@ -19,20 +24,19 @@ static void test_rotation_accuracy(void)
Matrix3f attitude; Matrix3f attitude;
Vector3f small_rotation; Vector3f small_rotation;
float roll, pitch, yaw; float roll, pitch, yaw;
int16_t i;
float rot_angle; float rot_angle;
hal.console->printf("\nRotation method accuracy:\n"); hal.console->printf("\nRotation method accuracy:\n");
// test roll // test roll
for( i=0; i<90; i++ ) { for(int16_t i = 0; i < 90; i++ ) {
// reset initial attitude // reset initial attitude
attitude.from_euler(0,0,0); attitude.from_euler(0.0f, 0.0f, 0.0f);
// calculate small rotation vector // calculate small rotation vector
rot_angle = ToRad(i); rot_angle = ToRad(i);
small_rotation = Vector3f(rot_angle,0,0); small_rotation = Vector3f(rot_angle, 0.0f, 0.0f);
// apply small rotation // apply small rotation
attitude.rotate(small_rotation); attitude.rotate(small_rotation);
@ -42,8 +46,8 @@ static void test_rotation_accuracy(void)
// now try via from_axis_angle // now try via from_axis_angle
Matrix3f r2; Matrix3f r2;
r2.from_axis_angle(Vector3f(1,0,0), rot_angle); r2.from_axis_angle(Vector3f(1.0f, 0.0f, 0.0f), rot_angle);
attitude.from_euler(0,0,0); attitude.from_euler(0.0f, 0.0f, 0.0f);
attitude = r2 * attitude; attitude = r2 * attitude;
float roll2, pitch2, yaw2; float roll2, pitch2, yaw2;
@ -51,18 +55,20 @@ static void test_rotation_accuracy(void)
// display results // display results
hal.console->printf("actual angle: %d angle1:%4.2f angle2:%4.2f\n", hal.console->printf("actual angle: %d angle1:%4.2f angle2:%4.2f\n",
(int)i,ToDeg(roll), ToDeg(roll2)); (int)i,
(double)ToDeg(roll),
(double)ToDeg(roll2));
} }
// test pitch // test pitch
for( i=0; i<90; i++ ) { for(int16_t i = 0; i < 90; i++ ) {
// reset initial attitude // reset initial attitude
attitude.from_euler(0,0,0); attitude.from_euler(0.0f, 0.0f, 0.0f);
// calculate small rotation vector // calculate small rotation vector
rot_angle = ToRad(i); rot_angle = ToRad(i);
small_rotation = Vector3f(0,rot_angle,0); small_rotation = Vector3f(0.0f ,rot_angle, 0.0f);
// apply small rotation // apply small rotation
attitude.rotate(small_rotation); attitude.rotate(small_rotation);
@ -72,8 +78,8 @@ static void test_rotation_accuracy(void)
// now try via from_axis_angle // now try via from_axis_angle
Matrix3f r2; Matrix3f r2;
r2.from_axis_angle(Vector3f(0,1,0), rot_angle); r2.from_axis_angle(Vector3f(0.0f ,1.0f, 0.0f), rot_angle);
attitude.from_euler(0,0,0); attitude.from_euler(0.0f, 0.0f, 0.0f);
attitude = r2 * attitude; attitude = r2 * attitude;
float roll2, pitch2, yaw2; float roll2, pitch2, yaw2;
@ -81,19 +87,21 @@ static void test_rotation_accuracy(void)
// display results // display results
hal.console->printf("actual angle: %d angle1:%4.2f angle2:%4.2f\n", hal.console->printf("actual angle: %d angle1:%4.2f angle2:%4.2f\n",
(int)i,ToDeg(pitch), ToDeg(pitch2)); (int)i,
(double)ToDeg(pitch),
(double)ToDeg(pitch2));
} }
// test yaw // test yaw
for( i=0; i<90; i++ ) { for(int16_t i = 0; i < 90; i++ ) {
// reset initial attitude // reset initial attitude
attitude.from_euler(0,0,0); attitude.from_euler(0.0f, 0.0f, 0.0f);
// calculate small rotation vector // calculate small rotation vector
rot_angle = ToRad(i); rot_angle = ToRad(i);
small_rotation = Vector3f(0,0,rot_angle); small_rotation = Vector3f(0.0f, 0.0f, rot_angle);
// apply small rotation // apply small rotation
attitude.rotate(small_rotation); attitude.rotate(small_rotation);
@ -103,8 +111,8 @@ static void test_rotation_accuracy(void)
// now try via from_axis_angle // now try via from_axis_angle
Matrix3f r2; Matrix3f r2;
r2.from_axis_angle(Vector3f(0,0,1), rot_angle); r2.from_axis_angle(Vector3f(0.0f, 0.0f, 1.0f), rot_angle);
attitude.from_euler(0,0,0); attitude.from_euler(0.0f, 0.0f, 0.0f);
attitude = r2 * attitude; attitude = r2 * attitude;
float roll2, pitch2, yaw2; float roll2, pitch2, yaw2;
@ -112,7 +120,9 @@ static void test_rotation_accuracy(void)
// display results // display results
hal.console->printf("actual angle: %d angle1:%4.2f angle2:%4.2f\n", hal.console->printf("actual angle: %d angle1:%4.2f angle2:%4.2f\n",
(int)i,ToDeg(yaw), ToDeg(yaw2)); (int)i,
(double)ToDeg(yaw),
(double)ToDeg(yaw2));
} }
} }
@ -151,19 +161,19 @@ static void test_euler(enum Rotation rotation, float roll, float pitch, float ya
static void test_rotate_inverse(void) static void test_rotate_inverse(void)
{ {
hal.console->printf("\nrotate inverse test(Vector (1,1,1)):\n"); 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); Vector3f vec(1.0f,1.0f,1.0f), cmp_vec(1.0f, 1.0f, 1.0f);
for (enum Rotation r=ROTATION_NONE; for (enum Rotation r = ROTATION_NONE;
r<ROTATION_MAX; r < ROTATION_MAX;
r = (enum Rotation)((uint8_t)r+1)) { r = (enum Rotation)((uint8_t)r+1)) {
hal.console->printf("\nROTATION(%d) ",r); hal.console->printf("\nROTATION(%d) ", r);
vec.rotate(r); vec.rotate(r);
print_vector(vec); print_vector(vec);
hal.console->printf("INV_ROTATION(%d)",r); hal.console->printf("INV_ROTATION(%d)", r);
vec.rotate_inverse(r); vec.rotate_inverse(r);
print_vector(vec); print_vector(vec);
if((vec - cmp_vec).length() > 1e-5) { if ((vec - cmp_vec).length() > 1e-5) {
hal.console->printf("Rotation Test Failed!!! %.8f\n",(vec - cmp_vec).length()); hal.console->printf("Rotation Test Failed!!! %.8f\n", (double)(vec - cmp_vec).length());
return; return;
} }
} }
@ -196,18 +206,18 @@ static void test_eulers(void)
test_euler(ROTATION_ROLL_270_YAW_90, 270, 0, 90); test_euler(ROTATION_ROLL_270_YAW_90, 270, 0, 90);
test_euler(ROTATION_ROLL_270_YAW_135, 270, 0, 135); test_euler(ROTATION_ROLL_270_YAW_135, 270, 0, 135);
test_euler(ROTATION_PITCH_90, 0, 90, 0); test_euler(ROTATION_PITCH_90, 0, 90, 0);
test_euler(ROTATION_PITCH_270, 0, 270, 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_90, 0, 180, 90);
test_euler(ROTATION_PITCH_180_YAW_270, 0, 180, 270); test_euler(ROTATION_PITCH_180_YAW_270, 0, 180, 270);
test_euler(ROTATION_ROLL_90_PITCH_90, 90, 90, 0); test_euler(ROTATION_ROLL_90_PITCH_90, 90, 90, 0);
test_euler(ROTATION_ROLL_180_PITCH_90,180, 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_270_PITCH_90,270, 90, 0);
test_euler(ROTATION_ROLL_90_PITCH_180, 90, 180, 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_270_PITCH_180,270,180, 0);
test_euler(ROTATION_ROLL_90_PITCH_270, 90, 270, 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_180_PITCH_270,180,270, 0);
test_euler(ROTATION_ROLL_270_PITCH_270,270,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_PITCH_180_YAW_90, 90, 180, 90);
test_euler(ROTATION_ROLL_90_YAW_270, 90, 0, 270); 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_90_PITCH_68_YAW_293,90,68.8,293.3);
} }
@ -215,10 +225,10 @@ static void test_eulers(void)
static bool have_rotation(const Matrix3f &m) static bool have_rotation(const Matrix3f &m)
{ {
Matrix3f mt = m.transposed(); Matrix3f mt = m.transposed();
for (enum Rotation r=ROTATION_NONE; for (enum Rotation r = ROTATION_NONE;
r<ROTATION_MAX; r < ROTATION_MAX;
r = (enum Rotation)((uint8_t)r+1)) { r = (enum Rotation)((uint8_t)(r + 1))) {
Vector3f v(1,2,3); Vector3f v(1.0f, 2.0f, 3.0f);
Vector3f v2 = v; Vector3f v2 = v;
v2.rotate(r); v2.rotate(r);
v2 = mt * v2; v2 = mt * v2;
@ -232,10 +242,9 @@ static bool have_rotation(const Matrix3f &m)
static void missing_rotations(void) static void missing_rotations(void)
{ {
hal.console->printf("testing for missing rotations\n"); hal.console->printf("testing for missing rotations\n");
uint16_t roll, pitch, yaw; for (uint16_t yaw = 0; yaw < 360; yaw += 90)
for (yaw=0; yaw<360; yaw += 90) for (uint16_t pitch = 0; pitch < 360; pitch += 90)
for (pitch=0; pitch<360; pitch += 90) for (uint16_t roll = 0; roll < 360; roll += 90) {
for (roll=0; roll<360; roll += 90) {
Matrix3f m; Matrix3f m;
m.from_euler(ToRad(roll), ToRad(pitch), ToRad(yaw)); m.from_euler(ToRad(roll), ToRad(pitch), ToRad(yaw));
if (!have_rotation(m)) { if (!have_rotation(m)) {