|
|
|
@ -50,6 +50,15 @@
@@ -50,6 +50,15 @@
|
|
|
|
|
#include <string.h> |
|
|
|
|
#include <float.h> |
|
|
|
|
|
|
|
|
|
#ifndef __PX4_QURT |
|
|
|
|
#if defined(__cplusplus) && !defined(__PX4_NUTTX) |
|
|
|
|
#include <cmath> |
|
|
|
|
#define ISFINITE(x) std::isfinite(x) |
|
|
|
|
#else |
|
|
|
|
#define ISFINITE(x) isfinite(x) |
|
|
|
|
#endif |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
|
* |
|
|
|
|
* Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved. |
|
|
|
@ -709,7 +718,7 @@ float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
@@ -709,7 +718,7 @@ float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
|
|
|
|
|
float _wrap_pi(float bearing) |
|
|
|
|
{ |
|
|
|
|
/* value is inf or NaN */ |
|
|
|
|
if (!isfinite(bearing)) { |
|
|
|
|
if (!ISFINITE(bearing)) { |
|
|
|
|
return bearing; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -739,7 +748,7 @@ float _wrap_pi(float bearing)
@@ -739,7 +748,7 @@ float _wrap_pi(float bearing)
|
|
|
|
|
float _wrap_2pi(float bearing) |
|
|
|
|
{ |
|
|
|
|
/* value is inf or NaN */ |
|
|
|
|
if (!isfinite(bearing)) { |
|
|
|
|
if (!ISFINITE(bearing)) { |
|
|
|
|
return bearing; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -769,7 +778,7 @@ float _wrap_2pi(float bearing)
@@ -769,7 +778,7 @@ float _wrap_2pi(float bearing)
|
|
|
|
|
float _wrap_180(float bearing) |
|
|
|
|
{ |
|
|
|
|
/* value is inf or NaN */ |
|
|
|
|
if (!isfinite(bearing)) { |
|
|
|
|
if (!ISFINITE(bearing)) { |
|
|
|
|
return bearing; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -799,7 +808,7 @@ float _wrap_180(float bearing)
@@ -799,7 +808,7 @@ float _wrap_180(float bearing)
|
|
|
|
|
float _wrap_360(float bearing) |
|
|
|
|
{ |
|
|
|
|
/* value is inf or NaN */ |
|
|
|
|
if (!isfinite(bearing)) { |
|
|
|
|
if (!ISFINITE(bearing)) { |
|
|
|
|
return bearing; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|