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

View File

@ -58,6 +58,7 @@ 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);
float wrap_360(float angle);
/*
wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)

View File

@ -205,6 +205,20 @@ float wrap_180_cd_float(float 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)
*/