better error wrapping

git-svn-id: https://arducopter.googlecode.com/svn/trunk@519 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/AP_Navigation/Navigation.cpp

@@ -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);
 }
 
 

libraries/AP_Navigation/Navigation.h

@@ -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