diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-11-13 23:15:02 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-11-13 23:15:02 +0400 |
| commit | e7f4d91022af363905b758ab39bde1addcb69517 (patch) | |
| tree | 0a9c101890f76e4a8e74091aac74fa3f87f900be /src/lib/geo | |
| parent | cfaf0ada45a9b99a83221c6c51a50090c55a1483 (diff) | |
| download | px4-firmware-e7f4d91022af363905b758ab39bde1addcb69517.tar.gz px4-firmware-e7f4d91022af363905b758ab39bde1addcb69517.tar.bz2 px4-firmware-e7f4d91022af363905b758ab39bde1addcb69517.zip | |
geo: cleanup of wrap_XXX functions
Diffstat (limited to 'src/lib/geo')
| -rw-r--r-- | src/lib/geo/geo.c | 54 |
1 files changed, 36 insertions, 18 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 43105fdba..9e533b92d 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -390,22 +390,22 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d __EXPORT float _wrap_pi(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing) || bearing == 0) { + if (!isfinite(bearing)) { return bearing; } int c = 0; - - while (bearing > M_PI_F && c < 30) { + while (bearing > M_PI_F) { bearing -= M_TWOPI_F; - c++; + if (c++ > 3) + return NAN; } c = 0; - - while (bearing <= -M_PI_F && c < 30) { + while (bearing <= -M_PI_F) { bearing += M_TWOPI_F; - c++; + if (c++ > 3) + return NAN; } return bearing; @@ -418,12 +418,18 @@ __EXPORT float _wrap_2pi(float bearing) return bearing; } - while (bearing >= M_TWOPI_F) { - bearing = bearing - M_TWOPI_F; + int c = 0; + while (bearing > M_TWOPI_F) { + bearing -= M_TWOPI_F; + if (c++ > 3) + return NAN; } - while (bearing < 0.0f) { - bearing = bearing + M_TWOPI_F; + c = 0; + while (bearing <= 0.0f) { + bearing += M_TWOPI_F; + if (c++ > 3) + return NAN; } return bearing; @@ -436,12 +442,18 @@ __EXPORT float _wrap_180(float bearing) return bearing; } + int c = 0; while (bearing > 180.0f) { - bearing = bearing - 360.0f; + bearing -= 360.0f; + if (c++ > 3) + return NAN; } - while (bearing <= -180.0f) { - bearing = bearing + 360.0f; + c = 0; + while (bearing <= -180.0f) { + bearing += 360.0f; + if (c++ > 3) + return NAN; } return bearing; @@ -454,12 +466,18 @@ __EXPORT float _wrap_360(float bearing) return bearing; } - while (bearing >= 360.0f) { - bearing = bearing - 360.0f; + int c = 0; + while (bearing > 360.0f) { + bearing -= 360.0f; + if (c++ > 3) + return NAN; } - while (bearing < 0.0f) { - bearing = bearing + 360.0f; + c = 0; + while (bearing <= 0.0f) { + bearing += 360.0f; + if (c++ > 3) + return NAN; } return bearing; |