AP_Math: cope with large values passed into the wrap functions

this uses modulus if the function would loop too many times

Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>

libraries/AP_Math/examples/location/location.pde

@@ -188,6 +188,64 @@ static void test_accuracy(void)
     }
 }
 
+static const struct {
+    int32_t v, wv;
+} wrap_180_tests[] = {
+    { 32000,            -4000 },
+    { 1500 + 100*36000,  1500 },
+    { -1500 - 100*36000, -1500 },
+};
+
+static const struct {
+    int32_t v, wv;
+} wrap_360_tests[] = {
+    { 32000,            32000 },
+    { 1500 + 100*36000,  1500 },
+    { -1500 - 100*36000, 34500 },
+};
+
+static const struct {
+    float v, wv;
+} wrap_PI_tests[] = {
+    { 0.2f*PI,            0.2f*PI },
+    { 0.2f*PI + 100*PI,  0.2f*PI },
+    { -0.2f*PI - 100*PI,  -0.2f*PI },
+};
+
+static void test_wrap_cd(void)
+{
+    for (uint8_t i=0; i<sizeof(wrap_180_tests)/sizeof(wrap_180_tests[0]); i++) {
+        int32_t r = wrap_180_cd(wrap_180_tests[i].v);
+        if (r != wrap_180_tests[i].wv) {
+            hal.console->printf("wrap_180: v=%ld wv=%ld r=%ld\n",
+                                (long)wrap_180_tests[i].v,
+                                (long)wrap_180_tests[i].wv,
+                                (long)r);
+        }
+    }
+
+    for (uint8_t i=0; i<sizeof(wrap_360_tests)/sizeof(wrap_360_tests[0]); i++) {
+        int32_t r = wrap_360_cd(wrap_360_tests[i].v);
+        if (r != wrap_360_tests[i].wv) {
+            hal.console->printf("wrap_360: v=%ld wv=%ld r=%ld\n",
+                                (long)wrap_360_tests[i].v,
+                                (long)wrap_360_tests[i].wv,
+                                (long)r);
+        }
+    }
+
+    for (uint8_t i=0; i<sizeof(wrap_PI_tests)/sizeof(wrap_PI_tests[0]); i++) {
+        float r = wrap_PI(wrap_PI_tests[i].v);
+        if (fabs(r - wrap_PI_tests[i].wv) > 0.001f) {
+            hal.console->printf("wrap_PI: v=%f wv=%f r=%f\n",
+                                wrap_PI_tests[i].v,
+                                wrap_PI_tests[i].wv,
+                                r);
+        }
+    }
+
+    hal.console->printf("wrap_cd tests done\n");
+}
 
 /*
  *  polygon tests
@@ -197,6 +255,7 @@ void setup(void)
     test_passed_waypoint();
     test_offset();
     test_accuracy();
+    test_wrap_cd();
 }
 
 void loop(void){}

libraries/AP_Math/location.cpp

@@ -20,6 +20,7 @@
 /*
  *  this module deals with calculations involving struct Location
  */
+#include <AP_HAL.h>
 #include <stdlib.h>
 #include "AP_Math.h"
 
@@ -155,8 +156,12 @@ Vector2f location_diff(const struct Location &loc1, const struct Location &loc2)
  */
 int32_t wrap_360_cd(int32_t error)
 {
-    while (error > 36000) error -= 36000;
+    if (error > 360000 || error < -360000) {
-    while (error < 0) error += 36000;
+        // for very large numbers use modulus
+        error = error % 36000;
+    }
+    if (error > 36000) error -= 36000;
+    if (error < 0) error += 36000;
     return error;
 }
 
@@ -165,8 +170,12 @@ int32_t wrap_360_cd(int32_t error)
  */
 int32_t wrap_180_cd(int32_t error)
 {
-    while (error > 18000) error -= 36000;
+    if (error > 360000 || error < -360000) {
-    while (error < -18000) error += 36000;
+        // for very large numbers use modulus
+        error = error % 36000;
+    }
+    if (error > 18000) { error -= 36000; }
+    if (error < -18000) { error += 36000; }
     return error;
 }
 
@@ -175,8 +184,12 @@ int32_t wrap_180_cd(int32_t error)
  */
 float wrap_PI(float angle_in_radians)
 {
-    while (angle_in_radians > PI) angle_in_radians -= 2.0f*PI;
+    if (angle_in_radians > 10*PI || angle_in_radians < -10*PI) {
-    while (angle_in_radians < -PI) angle_in_radians += 2.0f*PI;
+        // for very large numbers use modulus
+        angle_in_radians = fmodf(angle_in_radians, 2*PI);
+    }
+    while (angle_in_radians > PI) angle_in_radians -= 2*PI;
+    while (angle_in_radians < -PI) angle_in_radians += 2*PI;
     return angle_in_radians;
 }