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

Math: add wrap_PI

Browse Source
This commit is contained in:
Randy Mackay 2013-06-01 17:18:50 +09:00
parent 23ce35d292
commit 8b87849acd
2 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_Math/AP_Math.h
View File

@ -94,6 +94,11 @@ void location_offset(struct Location *loc, float ofs_north, float ofs_eas
int32_t wrap_360_cd(int32_t error); int32_t wrap_360_cd(int32_t error);
int32_t wrap_180_cd(int32_t error); int32_t wrap_180_cd(int32_t error);
/*
wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
*/
float wrap_PI(float angle_in_radians);
/* /*
print a int32_t lat/long in decimal degrees print a int32_t lat/long in decimal degrees
*/ */

9
libraries/AP_Math/location.cpp
View File

@ -161,6 +161,15 @@ int32_t wrap_180_cd(int32_t error)
return error; return error;
} }
/*
wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
*/
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;
return angle_in_radians;
}
/* /*
print a int32_t lat/long in decimal degrees print a int32_t lat/long in decimal degrees