fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-13 01:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

geo: cleanup of wrap_XXX functions

Browse Source
This commit is contained in:
Anton Babushkin
2013-11-13 23:15:02 +04:00
parent cfaf0ada45
commit e7f4d91022
1 changed files with 36 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/geo/geo.c
+36 -18
View File
@@ -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;