Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: float versions of wrap_360 and wrap_180

This commit is contained in:
Randy Mackay 2014-02-03 13:28:48 +09:00 committed by Andrew Tridgell
parent 364946cb7f
commit 37cfbc9ad5
2 changed files with 31 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Math/AP_Math.h
View File

@ -105,6 +105,8 @@ Vector2f location_diff(const struct Location &loc1, const struct Location &loc2)
*/
int32_t wrap_360_cd(int32_t error);
int32_t wrap_180_cd(int32_t error);
float wrap_360_cd_float(float angle);
float wrap_180_cd_float(float angle);
/*
wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)

30
libraries/AP_Math/location.cpp
View File

@ -152,7 +152,7 @@ Vector2f location_diff(const struct Location &loc1, const struct Location &loc2)
}
/*
wrap an angle in centi-degrees to 0..36000
wrap an angle in centi-degrees to 0..35999
*/
int32_t wrap_360_cd(int32_t error)
{
@ -179,6 +179,34 @@ int32_t wrap_180_cd(int32_t error)
return error;
}
/*
wrap an angle in centi-degrees to 0..35999
*/
float wrap_360_cd_float(float angle)
{
if (angle >= 72000.0f || angle < -36000.0f) {
// for larger number use fmodulus
angle = fmod(angle, 36000.0f);
}
if (angle >= 36000.0f) angle -= 36000.0f;
if (angle < 0.0f) angle += 36000.0f;
return angle;
}
/*
wrap an angle in centi-degrees to -18000..18000
*/
float wrap_180_cd_float(float angle)
{
if (angle > 54000.0f || angle < -54000.0f) {
// for large numbers use modulus
angle = fmod(angle,36000.0f);
}
if (angle > 18000.0f) { angle -= 36000.0f; }
if (angle < -18000.0f) { angle += 36000.0f; }
return angle;
}
/*
wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
*/