zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Math: example fix travis warning 

missing function declaration
implicit cast
some style fix
master
Pierre Kancir 8 years ago committed by Francisco Ferreira
parent
a2eeab4db7
commit
f2812c1efd
4 changed files with 176 additions and 152 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 84
      libraries/AP_Math/examples/eulers/eulers.cpp
  2. 118
      libraries/AP_Math/examples/location/location.cpp
  3. 23
      libraries/AP_Math/examples/polygon/polygon.cpp
  4. 103
      libraries/AP_Math/examples/rotations/rotations.cpp

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

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

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

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

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

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

@ -5,12 +5,17 @@ @@ -5,12 +5,17 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
static void print_vector(Vector3f &v)
{
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
@ -19,20 +24,19 @@ static void test_rotation_accuracy(void) @@ -19,20 +24,19 @@ static void test_rotation_accuracy(void)
Matrix3f attitude;
Vector3f small_rotation;
float roll, pitch, yaw;
int16_t i;
float rot_angle;
hal.console->printf("\nRotation method accuracy:\n");
// test roll
for( i=0; i<90; i++ ) {
for(int16_t i = 0; i < 90; i++ ) {
// reset initial attitude
attitude.from_euler(0,0,0);
attitude.from_euler(0.0f, 0.0f, 0.0f);
// calculate small rotation vector
rot_angle = ToRad(i);
small_rotation = Vector3f(rot_angle,0,0);
small_rotation = Vector3f(rot_angle, 0.0f, 0.0f);
// apply small rotation
attitude.rotate(small_rotation);
@ -42,8 +46,8 @@ static void test_rotation_accuracy(void) @@ -42,8 +46,8 @@ static void test_rotation_accuracy(void)
// now try via from_axis_angle
Matrix3f r2;
r2.from_axis_angle(Vector3f(1,0,0), rot_angle);
attitude.from_euler(0,0,0);
r2.from_axis_angle(Vector3f(1.0f, 0.0f, 0.0f), rot_angle);
attitude.from_euler(0.0f, 0.0f, 0.0f);
attitude = r2 * attitude;
float roll2, pitch2, yaw2;
@ -51,18 +55,20 @@ static void test_rotation_accuracy(void) @@ -51,18 +55,20 @@ static void test_rotation_accuracy(void)
// display results
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
for( i=0; i<90; i++ ) {
for(int16_t i = 0; i < 90; i++ ) {
// reset initial attitude
attitude.from_euler(0,0,0);
attitude.from_euler(0.0f, 0.0f, 0.0f);
// calculate small rotation vector
rot_angle = ToRad(i);
small_rotation = Vector3f(0,rot_angle,0);
small_rotation = Vector3f(0.0f ,rot_angle, 0.0f);
// apply small rotation
attitude.rotate(small_rotation);
@ -72,8 +78,8 @@ static void test_rotation_accuracy(void) @@ -72,8 +78,8 @@ static void test_rotation_accuracy(void)
// now try via from_axis_angle
Matrix3f r2;
r2.from_axis_angle(Vector3f(0,1,0), rot_angle);
attitude.from_euler(0,0,0);
r2.from_axis_angle(Vector3f(0.0f ,1.0f, 0.0f), rot_angle);
attitude.from_euler(0.0f, 0.0f, 0.0f);
attitude = r2 * attitude;
float roll2, pitch2, yaw2;
@ -81,19 +87,21 @@ static void test_rotation_accuracy(void) @@ -81,19 +87,21 @@ static void test_rotation_accuracy(void)
// display results
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
for( i=0; i<90; i++ ) {
for(int16_t i = 0; i < 90; i++ ) {
// reset initial attitude
attitude.from_euler(0,0,0);
attitude.from_euler(0.0f, 0.0f, 0.0f);
// calculate small rotation vector
rot_angle = ToRad(i);
small_rotation = Vector3f(0,0,rot_angle);
small_rotation = Vector3f(0.0f, 0.0f, rot_angle);
// apply small rotation
attitude.rotate(small_rotation);
@ -103,8 +111,8 @@ static void test_rotation_accuracy(void) @@ -103,8 +111,8 @@ static void test_rotation_accuracy(void)
// now try via from_axis_angle
Matrix3f r2;
r2.from_axis_angle(Vector3f(0,0,1), rot_angle);
attitude.from_euler(0,0,0);
r2.from_axis_angle(Vector3f(0.0f, 0.0f, 1.0f), rot_angle);
attitude.from_euler(0.0f, 0.0f, 0.0f);
attitude = r2 * attitude;
float roll2, pitch2, yaw2;
@ -112,7 +120,9 @@ static void test_rotation_accuracy(void) @@ -112,7 +120,9 @@ static void test_rotation_accuracy(void)
// display results
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 @@ -151,19 +161,19 @@ static void test_euler(enum Rotation rotation, float roll, float pitch, float ya
static void test_rotate_inverse(void)
{
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);
for (enum Rotation r=ROTATION_NONE;
r<ROTATION_MAX;
Vector3f vec(1.0f,1.0f,1.0f), cmp_vec(1.0f, 1.0f, 1.0f);
for (enum Rotation r = ROTATION_NONE;
r < ROTATION_MAX;
r = (enum Rotation)((uint8_t)r+1)) {
hal.console->printf("\nROTATION(%d) ",r);
hal.console->printf("\nROTATION(%d) ", r);
vec.rotate(r);
print_vector(vec);
hal.console->printf("INV_ROTATION(%d)",r);
hal.console->printf("INV_ROTATION(%d)", r);
vec.rotate_inverse(r);
print_vector(vec);
if((vec - cmp_vec).length() > 1e-5) {
hal.console->printf("Rotation Test Failed!!! %.8f\n",(vec - cmp_vec).length());
if ((vec - cmp_vec).length() > 1e-5) {
hal.console->printf("Rotation Test Failed!!! %.8f\n", (double)(vec - cmp_vec).length());
return;
}
}
@ -196,18 +206,18 @@ static void test_eulers(void) @@ -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_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_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);
test_euler(ROTATION_ROLL_90_PITCH_68_YAW_293,90,68.8,293.3);
}
@ -215,10 +225,10 @@ static void test_eulers(void) @@ -215,10 +225,10 @@ static void test_eulers(void)
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);
for (enum Rotation r = ROTATION_NONE;
r < ROTATION_MAX;
r = (enum Rotation)((uint8_t)(r + 1))) {
Vector3f v(1.0f, 2.0f, 3.0f);
Vector3f v2 = v;
v2.rotate(r);
v2 = mt * v2;
@ -232,10 +242,9 @@ static bool have_rotation(const Matrix3f &m) @@ -232,10 +242,9 @@ static bool have_rotation(const Matrix3f &m)
static void missing_rotations(void)
{
hal.console->printf("testing for missing rotations\n");
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) {
for (uint16_t yaw = 0; yaw < 360; yaw += 90)
for (uint16_t pitch = 0; pitch < 360; pitch += 90)
for (uint16_t roll = 0; roll < 360; roll += 90) {
Matrix3f m;
m.from_euler(ToRad(roll), ToRad(pitch), ToRad(yaw));
if (!have_rotation(m)) {

Write Preview
Loading…
Cancel
Save