From 022148ec0d8f2c547f3cde8e832b937baf66e3d3 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 25 Jun 2013 22:33:47 +0900
Subject: [PATCH] Copter: LAND only control horizontal position if we have GPS
 lock

---
 ArduCopter/commands_logic.pde | 19 ++++++++++++++-----
 ArduCopter/system.pde         |  2 +-
 2 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index bd594a9f1b..c45fcc1d0c 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -323,9 +323,20 @@ static void do_land(const struct Location *cmd)
         // set landing state
         land_state = LAND_STATE_DESCENDING;
 
-        // switch to loiter which restores horizontal control to pilot
-        // To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands
-        set_roll_pitch_mode(ROLL_PITCH_LOITER);
+        // if we have gps lock, attempt to hold horizontal position
+        if( ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D ) {
+            // switch to loiter which restores horizontal control to pilot
+            // To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands
+            set_roll_pitch_mode(ROLL_PITCH_LOITER);
+            // switch into loiter nav mode
+            set_nav_mode(NAV_LOITER);
+        }else{
+            // no gps lock so give horizontal control to pilot
+            // To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands
+            set_roll_pitch_mode(ROLL_PITCH_STABLE);
+            // switch into loiter nav mode
+            set_nav_mode(NAV_NONE);
+        }
 
         // hold yaw while landing
         set_yaw_mode(YAW_HOLD);
@@ -333,8 +344,6 @@ static void do_land(const struct Location *cmd)
         // set throttle mode to land
         set_throttle_mode(THROTTLE_LAND);
 
-        // switch into loiter nav mode
-        set_nav_mode(NAV_LOITER);
     }
 }
 
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 9759405d49..63767f6443 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -434,7 +434,7 @@ static void set_mode(uint8_t mode)
 
     case LAND:
         // To-Do: it is messy to set manual_attitude here because the do_land function is reponsible for setting the roll_pitch_mode
-        if( g_gps->status() == GPS::GPS_OK_FIX_3D ) {
+        if( ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D ) {
             // switch to loiter if we have gps
             ap.manual_attitude = false;
         }else{