mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-09 16:13:56 -03:00
AP_Math: example fix travis warning
missing function declaration implicit cast some style fix
This commit is contained in:
Pierre Kancir
Francisco Ferreira
parent
a2eeab4db7
commit
f2812c1efd
@ -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)
|
||||
{
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
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)
|
||||
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)
|
||||
|
||||
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)
|
||||
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)
|
||||
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)
|
||||
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)
|
||||
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)
|
||||
test_matrix_rotate();
|
||||
}
|
||||
|
||||
void loop(void){}
|
||||
void loop(void) {}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
||||
@ -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 {
|
||||
{ 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)
|
||||
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,
|
||||
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)
|
||||
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 {
|
||||
|
||||
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)
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
#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)
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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 {
|
||||
*/
|
||||
void setup(void)
|
||||
{
|
||||
unsigned i, count;
|
||||
uint32_t count;
|
||||
bool all_passed = true;
|
||||
uint32_t start_time;
|
||||
|
||||
@ -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)
|
||||
|
||||
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)
|
||||
}
|
||||
}
|
||||
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");
|
||||
}
|
||||
|
||||
|
||||
@ -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)
|
||||
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)
|
||||
|
||||
// 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)
|
||||
|
||||
// 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)
|
||||
|
||||
// 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)
|
||||
|
||||
// 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)
|
||||
|
||||
// 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)
|
||||
|
||||
// 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
|
||||
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)
|
||||
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)
|
||||
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)
|
||||
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)) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user