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) Navigation::calc_bearing_error(void)
{ {
if(_hold_course == -1){ if(_hold_course == -1){
bearing_error = wrap_360(bearing - _gps->ground_course); bearing_error = wrap_180(bearing - _gps->ground_course);
}else{ }else{
bearing_error = _hold_course; bearing_error = _hold_course;
} }
} }
int32_t int32_t
Navigation::wrap_360(int32_t error) Navigation::wrap_180(int32_t error)
{ {
if (error > 18000) error -= 36000; if (error > 18000) error -= 36000;
if (error < -18000) error += 36000; if (error < -18000) error += 36000;
return error; return error;
} }
int32_t
Navigation::wrap_360(int32_t error)
{
if (error > 36000) error -= 36000;
if (error < 0) error += 36000;
return error;
}
void void
Navigation::set_bearing_error(int32_t error) 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_distance_error(void);
void calc_long_scaling(int32_t lat); void calc_long_scaling(int32_t lat);
void reset_crosstrack(void); void reset_crosstrack(void);
int32_t wrap_180(int32_t error); // utility
int32_t wrap_360(int32_t error); // utility int32_t wrap_360(int32_t error); // utility
int16_t _old_bearing; // used to track delta on the bearing int16_t _old_bearing; // used to track delta on the bearing