From aaa62eeb2859f468b27371a60e6926a9bc1b1bfc Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 18 Jan 2014 12:00:04 +0900
Subject: [PATCH] Copter: integrated simpler init loiter from WP_Nav

---
 ArduCopter/control_stabilize.pde | 6 +++---
 ArduCopter/navigation.pde        | 7 ++++---
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde
index d75308fe27..b309347fb0 100644
--- a/ArduCopter/control_stabilize.pde
+++ b/ArduCopter/control_stabilize.pde
@@ -239,7 +239,7 @@ static bool loiter_init(bool ignore_checks)
     if (GPS_ok() || ignore_checks) {
         // set target to current position
         // To-Do: supply zero velocity below?
-        wp_nav.init_loiter_target(inertial_nav.get_position(), inertial_nav.get_velocity());
+        wp_nav.init_loiter_target();
         return true;
     }else{
         return false;
@@ -255,7 +255,7 @@ static void loiter_run()
 
     // if not auto armed set throttle to zero and exit immediately
     if(!ap.auto_armed || !inertial_nav.position_ok()) {
-        wp_nav.init_loiter_target(inertial_nav.get_position(), inertial_nav.get_velocity());
+        wp_nav.init_loiter_target();
         attitude_control.init_targets();
         attitude_control.set_throttle_out(0, false);
         return;
@@ -287,7 +287,7 @@ static void loiter_run()
 
     // when landed reset targets and output zero throttle
     if (ap.land_complete) {
-        wp_nav.init_loiter_target(inertial_nav.get_position(), inertial_nav.get_velocity());
+        wp_nav.init_loiter_target();
         attitude_control.init_targets();
         attitude_control.set_throttle_out(0, false);
     }else{
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index 8309dccf44..3ace31ee21 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -99,7 +99,7 @@ static bool set_nav_mode(uint8_t new_nav_mode)
 
         case NAV_LOITER:
             // set target to current position
-            wp_nav.init_loiter_target(inertial_nav.get_position(), inertial_nav.get_velocity());
+            wp_nav.init_loiter_target();
             nav_initialised = true;
             break;
 
@@ -142,7 +142,7 @@ static void update_nav_mode()
         case NAV_LOITER:
             // reset target if we are still on the ground
             if (ap.land_complete) {
-                wp_nav.init_loiter_target(inertial_nav.get_position(),inertial_nav.get_velocity());
+                wp_nav.init_loiter_target();
             }else{
                 // call loiter controller
                 wp_nav.update_loiter();
@@ -228,7 +228,8 @@ circle_set_center(const Vector3f current_position, float heading_in_radians)
     circle_angular_velocity = 0;
 
     // initialise loiter target.  Note: feed forward velocity set to zero
-    wp_nav.init_loiter_target(current_position, Vector3f(0,0,0));
+    // To-Do: modify circle to use position controller and pass in zero velocity.  Vector3f(0,0,0)
+    wp_nav.init_loiter_target();
 }
 
 // update_circle - circle position controller's main call which in turn calls loiter controller with updated target position