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_cd() and wrap_180_cd() 

moved from per-vehicle code
This commit is contained in:
Andrew Tridgell 2013-03-29 13:13:37 +11:00
parent a8bd8950c8
commit 091b474a1d
2 changed files with 27 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Math/AP_Math.h
View File

@ -80,6 +80,12 @@ void location_update(struct Location *loc, float bearing, float distance)
// extrapolate latitude/longitude given distances north and east
void location_offset(struct Location *loc, float ofs_north, float ofs_east);
/*
wrap an angle in centi-degrees
*/
int32_t wrap_360_cd(int32_t error);
int32_t wrap_180_cd(int32_t error);
// constrain a value
float constrain(float amt, float low, float high);
int16_t constrain_int16(int16_t amt, int16_t low, int16_t high);

21
libraries/AP_Math/location.cpp
View File

@ -140,3 +140,24 @@ void location_offset(struct Location *loc, float ofs_north, float ofs_east)
loc->lng += dlng;
}
}
/*
wrap an angle in centi-degrees to 0..36000
*/
int32_t wrap_360_cd(int32_t error)
{
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)
{
while (error > 18000) error -= 36000;
while (error < -18000) error += 36000;
return error;
}