still unstable

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

libraries/AP_Navigation/Navigation.cpp

@@ -164,11 +164,11 @@ Navigation::wrap_180(int32_t error)
 }
 
 int32_t
-Navigation::wrap_360(int32_t error)
+Navigation::wrap_360(int32_t angle)
 {
-	if (error > 36000)	error -= 36000;
-	if (error < 0)		error += 36000;
-	return error;
+	if (angle > 36000)	angle -= 36000;
+	if (angle < 0)		angle += 36000;
+	return angle;
 }
 
 void

libraries/AP_Navigation/Navigation.h

@@ -5,7 +5,7 @@
 #define XTRACK_GAIN 10 					// Amount to compensate for crosstrack (degrees/100 per meter)
 #define XTRACK_ENTRY_ANGLE 3000			// Max angle used to correct for track following	degrees*100
 #include <GPS.h>						// ArduPilot GPS Library
-#include "Waypoints.h"						// ArduPilot Waypoints Library
+#include "Waypoints.h"					// ArduPilot Waypoints Library
 #include "WProgram.h"
 
 #define T7 10000000
@@ -36,6 +36,9 @@ public:
 	
 	void		set_loiter_vector(int16_t v);
 	void 		update_crosstrack(void);
+
+	int32_t		wrap_180(int32_t error);			// utility
+	int32_t		wrap_360(int32_t angle);			// utility
 	
 	int32_t		bearing;							// deg * 100 : 0 to 360 current desired bearing to navigate
 	int32_t		distance;							// meters : distance plane to next waypoint
@@ -54,8 +57,6 @@ 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
 	GPS			*_gps;

libraries/AP_Navigation/examples/Navigation/Navigation.pde

@@ -11,7 +11,7 @@ This test assumes you are at the LOWl demo Airport
 
 
 AP_GPS_IMU gps;
-Navigation	nav((GPS *) & gps);
+Navigation	nav((GPS *) &gps);
 AP_RC rc;
 
 #define CH_ROLL 0

libraries/Waypoints/Waypoints.cpp

@@ -70,15 +70,6 @@ Waypoints::get_waypoint_with_index(uint8_t i)
 }
 
 
-Waypoints::WP
-Waypoints::get_next_waypoint(void)
-{
-	_index++;
-	if (_index >= _total)
-		_index == 0;
-	return get_waypoint_with_index(_index);
-}
-
 Waypoints::WP
 Waypoints::get_current_waypoint(void)
 {
@@ -91,6 +82,14 @@ Waypoints::get_index(void)
 	return _index;
 }
 
+void
+Waypoints::next_index(void)
+{
+	_index++;
+	if (_index >= _total)
+		_index == 0;
+}
+
 void
 Waypoints::set_index(uint8_t i)
 {

libraries/Waypoints/Waypoints.h

@@ -18,15 +18,15 @@ class Waypoints
 		int32_t lng;				// Longitude * 10**7
 	};
 	
-	WP get_waypoint_with_index(uint8_t i);
-	WP get_current_waypoint(void);
-	WP get_next_waypoint(void);
-	
+	WP			get_waypoint_with_index(uint8_t i);
+	WP			get_current_waypoint(void);
+
 	void 		set_waypoint_with_index(Waypoints::WP wp, uint8_t i);
 
 	void 		set_start_byte(uint16_t start_byte);
 	void 		set_wp_size(uint8_t wp_size);
 	
+	void		next_index(void);
 	uint8_t 	get_index(void);
 	void 		set_index(uint8_t i);