mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-26 18:48:30 -04:00
AP_Math: fix compile warnings re float constants
This commit is contained in:
parent
a4b8575432
commit
a8dda9f2ed
@ -6,7 +6,7 @@
|
||||
float safe_asin(float v)
|
||||
{
|
||||
if (isnan(v)) {
|
||||
return 0.0;
|
||||
return 0.0f;
|
||||
}
|
||||
if (v >= 1.0f) {
|
||||
return PI/2;
|
||||
|
@ -70,9 +70,9 @@ static void check_result(const char *msg,
|
||||
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 (rad_diff(roll2,roll) > 0.01f ||
|
||||
rad_diff(pitch2, pitch) > 0.01f ||
|
||||
rad_diff(yaw2, yaw) > 0.01f) {
|
||||
if (pitch >= PI/2 ||
|
||||
pitch <= -PI/2 ||
|
||||
ToDeg(rad_diff(pitch, PI/2)) < 1 ||
|
||||
@ -168,13 +168,13 @@ void test_quaternion_eulers(void)
|
||||
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(89), 0, 0.1f);
|
||||
test_quaternion(0, ToRad(89), 0.1f);
|
||||
test_quaternion(0.1f, 0, ToRad(89));
|
||||
|
||||
test_quaternion(ToRad(91), 0, 0.1);
|
||||
test_quaternion(0, ToRad(91), 0.1);
|
||||
test_quaternion(0.1, 0, ToRad(91));
|
||||
test_quaternion(ToRad(91), 0, 0.1f);
|
||||
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++)
|
||||
@ -218,11 +218,11 @@ void test_conversions(void)
|
||||
|
||||
hal.console->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);
|
||||
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);
|
||||
test_conversion(-1, 1.1f, 1.2f);
|
||||
|
||||
for (i=0; i<N; i++)
|
||||
for (j=0; j<N; j++)
|
||||
|
@ -40,12 +40,12 @@ static const struct {
|
||||
Vector2f wp1, wp2, location;
|
||||
bool passed;
|
||||
} test_points[] = {
|
||||
{ Vector2f(-35.3647759314918, 149.16265692810987),
|
||||
Vector2f(-35.36279922658029, 149.16352169591426),
|
||||
Vector2f(-35.36214956969903, 149.16461410046492), true },
|
||||
{ Vector2f(-35.36438601157189, 149.16613916088568),
|
||||
Vector2f(-35.364432558610254, 149.16287313113048),
|
||||
Vector2f(-35.36491510034746, 149.16365837225004), false },
|
||||
{ Vector2f(-35.3647759314918f, 149.16265692810987f),
|
||||
Vector2f(-35.36279922658029f, 149.16352169591426f),
|
||||
Vector2f(-35.36214956969903f, 149.16461410046492f), true },
|
||||
{ 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 },
|
||||
@ -101,7 +101,7 @@ static void test_one_offset(const struct Location &loc,
|
||||
hal.console->printf("location_offset took %u usec\n",
|
||||
(unsigned)(hal.scheduler->micros() - t1));
|
||||
dist2 = get_distance(loc, loc2);
|
||||
bearing2 = get_bearing_cd(loc, loc2) * 0.01;
|
||||
bearing2 = get_bearing_cd(loc, loc2) * 0.01f;
|
||||
float brg_error = bearing2-bearing;
|
||||
if (brg_error > 180) {
|
||||
brg_error -= 360;
|
||||
@ -109,8 +109,8 @@ static void test_one_offset(const struct Location &loc,
|
||||
brg_error += 360;
|
||||
}
|
||||
|
||||
if (fabsf(dist - dist2) > 1.0 ||
|
||||
brg_error > 1.0) {
|
||||
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);
|
||||
}
|
||||
@ -119,8 +119,8 @@ static void test_one_offset(const struct Location &loc,
|
||||
static const struct {
|
||||
float ofs_north, ofs_east, distance, bearing;
|
||||
} test_offsets[] = {
|
||||
{ 1000, 1000, sqrt(2.0)*1000, 45 },
|
||||
{ 1000, -1000, sqrt(2.0)*1000, -45 },
|
||||
{ 1000, 1000, sqrt(2.0f)*1000, 45 },
|
||||
{ 1000, -1000, sqrt(2.0f)*1000, -45 },
|
||||
{ 1000, 0, 1000, 0 },
|
||||
{ 0, 1000, 1000, 90 },
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user