Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: Replace wrap_* functions with template versions

Browse Source
This commit is contained in:
dgrat
2016-05-10 11:41:26 -03:00
committed by Lucas De Marchi
parent 49cfd6fd9b
commit 76362caee0
12 changed files with 161 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_Math/location.cpp
-98
View File
@@ -149,104 +149,6 @@ Vector2f location_diff(const struct Location &loc1, const struct Location &loc2)
(loc2.lng - loc1.lng) * LOCATION_SCALING_FACTOR * longitude_scale(loc1));
}
/*
wrap an angle in centi-degrees to 0..35999
*/
int32_t wrap_360_cd(int32_t error)
{
if (error > 360000 || error < -360000) {
// for very large numbers use modulus
error = error % 36000;
}
while (error >= 36000) error -= 36000;
while (error < 0) error += 36000;
return error;
}
/*
wrap an angle in centi-degrees to -18000..18000
*/
int32_t wrap_180_cd(int32_t error)
{
if (error > 360000 || error < -360000) {
// for very large numbers use modulus
error = error % 36000;
}
while (error > 18000) { error -= 36000; }
while (error < -18000) { error += 36000; }
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 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)
*/
float wrap_PI(float angle_in_radians)
{
if (angle_in_radians > 10*M_PI || angle_in_radians < -10*M_PI) {
// for very large numbers use modulus
angle_in_radians = fmodf(angle_in_radians, 2*M_PI);
}
while (angle_in_radians > M_PI) angle_in_radians -= 2*M_PI;
while (angle_in_radians < -M_PI) angle_in_radians += 2*M_PI;
return angle_in_radians;
}
/*
* wrap an angle in radians to 0..2PI
*/
float wrap_2PI(float angle)
{
if (angle > 10*M_PI || angle < -10*M_PI) {
// for very large numbers use modulus
angle = fmodf(angle, 2*M_PI);
}
while (angle > 2*M_PI) angle -= 2*M_PI;
while (angle < 0) angle += 2*M_PI;
return angle;
}
/*
return true if lat and lng match. Ignores altitude and options
*/