dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'wrap_pi_fixes' into beta

This commit is contained in:
Anton Babushkin 2014-02-01 11:06:57 +01:00
parent 8660ea914a c31d065340
commit 542ec2d91d
1 changed files with 36 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
src/lib/geo/geo.c
View File

@ -436,22 +436,22 @@ __EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, flo
__EXPORT float _wrap_pi(float bearing) __EXPORT float _wrap_pi(float bearing)
{ {
/* value is inf or NaN */ /* value is inf or NaN */
if (!isfinite(bearing) || bearing == 0) { if (!isfinite(bearing)) {
return bearing; return bearing;
} }
int c = 0; int c = 0;
while (bearing > M_PI_F) {
while (bearing > M_PI_F && c < 30) {
bearing -= M_TWOPI_F; bearing -= M_TWOPI_F;
c++; if (c++ > 3)
return NAN;
} }
c = 0; c = 0;
while (bearing <= -M_PI_F) {
while (bearing <= -M_PI_F && c < 30) {
bearing += M_TWOPI_F; bearing += M_TWOPI_F;
c++; if (c++ > 3)
return NAN;
} }
return bearing; return bearing;
@ -464,12 +464,18 @@ __EXPORT float _wrap_2pi(float bearing)
return bearing; return bearing;
} }
while (bearing >= M_TWOPI_F) { int c = 0;
bearing = bearing - M_TWOPI_F; while (bearing > M_TWOPI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3)
return NAN;
} }
while (bearing < 0.0f) { c = 0;
bearing = bearing + M_TWOPI_F; while (bearing <= 0.0f) {
bearing += M_TWOPI_F;
if (c++ > 3)
return NAN;
} }
return bearing; return bearing;
@ -482,12 +488,18 @@ __EXPORT float _wrap_180(float bearing)
return bearing; return bearing;
} }
int c = 0;
while (bearing > 180.0f) { while (bearing > 180.0f) {
bearing = bearing - 360.0f; bearing -= 360.0f;
if (c++ > 3)
return NAN;
} }
while (bearing <= -180.0f) { c = 0;
bearing = bearing + 360.0f; while (bearing <= -180.0f) {
bearing += 360.0f;
if (c++ > 3)
return NAN;
} }
return bearing; return bearing;
@ -500,12 +512,18 @@ __EXPORT float _wrap_360(float bearing)
return bearing; return bearing;
} }
while (bearing >= 360.0f) { int c = 0;
bearing = bearing - 360.0f; while (bearing > 360.0f) {
bearing -= 360.0f;
if (c++ > 3)
return NAN;
} }
while (bearing < 0.0f) { c = 0;
bearing = bearing + 360.0f; while (bearing <= 0.0f) {
bearing += 360.0f;
if (c++ > 3)
return NAN;
} }
return bearing; return bearing;