Skip to content

Commit

Permalink
AP_Math: example fix travis warning
Browse files Browse the repository at this point in the history 
missing function declaration
implicit cast
some style fix
  • Loading branch information
@khancyr @OXINARF
khancyr authored and OXINARF committed Apr 13, 2017
1 parent a2eeab4 commit f2812c1
Show file tree
Hide file tree
Showing 4 changed files with 176 additions and 152 deletions.
84 changes: 47 additions & 37 deletions libraries/AP_Math/examples/eulers/eulers.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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);
}
Expand All @@ -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 ||
Expand All @@ -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));
}
}
}
Expand All @@ -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");
Expand Down Expand Up @@ -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");
Expand All @@ -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");
Expand All @@ -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);
Expand All @@ -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");
Expand All @@ -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");
Expand All @@ -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
Expand All @@ -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;
Expand All @@ -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);
}
}
}
Expand All @@ -296,6 +306,6 @@ void setup(void)
test_matrix_rotate();
}

void loop(void){}
void loop(void) {}

AP_HAL_MAIN();
Loading

0 comments on commit f2812c1

Please sign in to comment.