diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-02-01 11:06:57 +0100 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-02-01 11:06:57 +0100 |
| commit | 542ec2d91d8b77a968de3b2474268126467b71a3 (patch) | |
| tree | 6f2ca8f61b094bfedc568473377fe81b29748bc0 /src/lib/geo | |
| parent | 8660ea914a0410a3503653767efb09adc1cedff2 (diff) | |
| parent | c31d065340a44ef121069b5b82395526b910d3e8 (diff) | |
| download | px4-firmware-542ec2d91d8b77a968de3b2474268126467b71a3.tar.gz px4-firmware-542ec2d91d8b77a968de3b2474268126467b71a3.tar.bz2 px4-firmware-542ec2d91d8b77a968de3b2474268126467b71a3.zip | |
Merge branch 'wrap_pi_fixes' into beta
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 446cd5bee..9b3e202e6 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -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) { /* 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; @@ -464,12 +464,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; @@ -482,12 +488,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; @@ -500,12 +512,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; |