From a3fc49981f5aa8cd458423289142bc74a496837b Mon Sep 17 00:00:00 2001 From: Pascal Brisset Date: Tue, 26 Feb 2008 10:36:27 +0000 Subject: [PATCH] terminating angle normalization --- sw/airborne/snav.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/snav.c b/sw/airborne/snav.c index d964536f2f..9da2f76245 100644 --- a/sw/airborne/snav.c +++ b/sw/airborne/snav.c @@ -8,7 +8,7 @@ #include "gps.h" #define Sign(_x) ((_x) > 0 ? 1 : (-1)) -#define Norm2Pi(x) ({ float _x = x; while (_x < 0.) _x += 2*M_PI; while (_x > 2*M_PI) _x -= 2*M_PI; _x; }) +#define Norm2Pi(x) ({ uint8_t _i=1; float _x = x; while (_i && _x < 0.) { _i++;_x += 2*M_PI; } while (_i && _x > 2*M_PI) { _i++; _x -= 2*M_PI; } _x; }) static struct point wp_cd, wp_td, wp_ca, wp_ta; static float d_radius, a_radius;