From dff31e7ddef00ec55a09d7faefd5b4c9dd16be7f Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Thu, 9 Sep 2010 06:35:21 +0000
Subject: [PATCH] git-svn-id: https://arducopter.googlecode.com/svn/trunk@444
 f9c3cf11-9bcb-44bc-f272-b75c42450872

---
 libraries/AP_Navigation/Navigation.cpp | 31 ++++++++++++++++++++++++--
 libraries/AP_Navigation/Navigation.h   | 12 +++++++---
 2 files changed, 38 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_Navigation/Navigation.cpp b/libraries/AP_Navigation/Navigation.cpp
index 1848406b8a..51fb2f74b0 100644
--- a/libraries/AP_Navigation/Navigation.cpp
+++ b/libraries/AP_Navigation/Navigation.cpp
@@ -51,7 +51,15 @@ Navigation::load_first_wp(void)
 void
 Navigation::load_home(void)
 {
-	set_home(_wp->get_waypoint_with_index(0));
+	home = _wp->get_waypoint_with_index(0);
+}
+
+void
+Navigation::return_to_home_with_alt(uint32_t alt)
+{
+	Waypoints::WP loc = _wp->get_waypoint_with_index(0);
+	loc.alt += alt;
+	set_next_wp(loc);
 }
 
 void
@@ -66,11 +74,17 @@ Navigation::load_wp_index(uint8_t i)
 	_wp->set_index(i);
 	set_next_wp(_wp->get_current_waypoint());
 }
-
+void
+Navigation::hold_location()
+{
+	
+	set_next_wp()
+}
 
 void
 Navigation::set_home(Waypoints::WP loc)
 {
+	Waypoints::WP loc
 	_wp->set_waypoint_with_index(loc, i);
 	home 		= loc;
 	//location 	= home;
@@ -150,6 +164,13 @@ Navigation::wrap_360(int32_t error)
 	return error;
 }
 
+void
+Navigation::set_bearing_error(int32_t error)
+{
+	bearing_error = wrap_360(error);
+}
+
+
 /*****************************************
  * Altitude error with Airspeed correction
  *****************************************/
@@ -166,6 +187,12 @@ Navigation::calc_altitude_error(void)
 	altitude_error 	= _target_altitude - location.alt;
 }
 
+void
+Navigation::set_loiter_vector(int16_t v)
+{
+	_vector = constrain(v, -18000, 18000);
+}
+
 void
 Navigation::update_crosstrack(void)
 {
diff --git a/libraries/AP_Navigation/Navigation.h b/libraries/AP_Navigation/Navigation.h
index 876cf4ebb5..0b30c3f5c3 100644
--- a/libraries/AP_Navigation/Navigation.h
+++ b/libraries/AP_Navigation/Navigation.h
@@ -13,14 +13,20 @@ public:
 	void 		update_gps(void);					// called 50 Hz
 	void		set_home(Waypoints::WP loc);
 	void 		load_first_wp(void);
-	void 		load_wp_index(uint8_t i);
+	void 		load_wp_with_index(uint8_t i);
 	void 		load_home(void);
+	void		return_to_home(void);
+
 	void		reload_wp(void);
 	void		set_wp(Waypoints::WP loc);
-	void		set_hold_course(int16_t b);				// 1 = hold a current course, 0 disables course hold
-	int16_t 	get_hold_course(void);				// 1 = hold a current course, 0 disables course hold
+
+	void		set_hold_course(int16_t b);				// -1 deisables, 0-36000 enables
+	int16_t 	get_hold_course(void);
+
 	int32_t 	get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2);
 	int32_t 	get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2);
+	void		set_bearing_error(int32_t error);
+	
 	void 		update_crosstrack(void);
 	
 	int32_t		bearing;							// deg * 100 : 0 to 360 current desired bearing to navigate