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

better error wrapping 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@519 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-09-15 01:45:25 +00:00
parent b9f8b6727d
commit 6cf2d1e86e
2 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Navigation/Navigation.cpp
View File

@ -149,24 +149,32 @@ void
Navigation::calc_bearing_error(void)
{
if(_hold_course == -1){
bearing_error = wrap_360(bearing - _gps->ground_course);
bearing_error = wrap_180(bearing - _gps->ground_course);
}else{
bearing_error = _hold_course;
}
}
int32_t
Navigation::wrap_360(int32_t error)
Navigation::wrap_180(int32_t error)
{
if (error > 18000) error -= 36000;
if (error < -18000) error += 36000;
return error;
}
int32_t
Navigation::wrap_360(int32_t error)
{
if (error > 36000) error -= 36000;
if (error < 0) error += 36000;
return error;
}
void
Navigation::set_bearing_error(int32_t error)
{
bearing_error = wrap_360(error);
bearing_error = wrap_180(error);
}

1
libraries/AP_Navigation/Navigation.h
View File

@ -54,6 +54,7 @@ private:
void calc_distance_error(void);
void calc_long_scaling(int32_t lat);
void reset_crosstrack(void);
int32_t wrap_180(int32_t error); // utility
int32_t wrap_360(int32_t error); // utility
int16_t _old_bearing; // used to track delta on the bearing