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

AP_Math: added wrap_360()

This commit is contained in:
Andrew Tridgell 2016-05-05 11:21:55 +10:00
parent 9300092840
commit e8142b0b5b
2 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Math/AP_Math.h
View File

@ -58,6 +58,7 @@ int32_t wrap_360_cd(int32_t error);
int32_t wrap_180_cd(int32_t error); int32_t wrap_180_cd(int32_t error);
float wrap_360_cd_float(float angle); float wrap_360_cd_float(float angle);
float wrap_180_cd_float(float angle); float wrap_180_cd_float(float angle);
float wrap_360(float angle);
/* /*
wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees) wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)

14
libraries/AP_Math/location.cpp
View File

@ -205,6 +205,20 @@ float wrap_180_cd_float(float angle)
return angle; return angle;
} }
/*
wrap an angle in degrees to 0..360
*/
float wrap_360(float angle)
{
if (angle >= 720.0f || angle < -360.0f) {
// for larger number use fmodulus
angle = fmod(angle, 360.0f);
}
if (angle >= 360.0f) angle -= 360.0f;
if (angle < 0.0f) angle += 360.0f;
return angle;
}
/* /*
wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees) wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
*/ */