Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: cope with large values passed into the wrap functions

Browse Source 
this uses modulus if the function would loop too many times

Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Andrew Tridgell 2013-10-02 16:08:13 +10:00
parent 34f7f88190
commit fb19dbb404
2 changed files with 78 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/AP_Math/examples/location/location.pde
View File

@ -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){}

25
libraries/AP_Math/location.cpp
View File

@ -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;
while (error < 0) error += 36000;
if (error > 360000 || error < -360000) {
// 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;
while (error < -18000) error += 36000;
if (error > 360000 || error < -360000) {
// 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;
while (angle_in_radians < -PI) angle_in_radians += 2.0f*PI;
if (angle_in_radians > 10*PI || angle_in_radians < -10*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;
}