Copter: bug fix for take-off in loiter

ArduCopter/ArduCopter.pde

@@ -1752,8 +1752,8 @@ void update_roll_pitch_mode(void)
 
         // if landed simply keep the copter level
         if (ap.land_complete) {
-            nav_roll = 0;
-            nav_pitch = 0;
+            nav_roll = ahrs.roll_sensor;
+            nav_pitch = ahrs.pitch_sensor;
         }else{
             // update loiter target from user controls
             wp_nav.move_loiter_target(control_roll, control_pitch,0.01f);

ArduCopter/navigation.pde

@@ -162,9 +162,10 @@ static void update_nav_mode()
             // 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());
+            }else{
+                // call loiter controller
+                wp_nav.update_loiter();
             }
-            // call loiter controller
-            wp_nav.update_loiter();
             break;
 
         case NAV_WP: