Copter: LAND only control horizontal position if we have GPS lock

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

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{