ArduPlane: fix autofence enable in takeoff mode

ArduPlane/mode_takeoff.cpp

@@ -127,34 +127,42 @@ void ModeTakeoff::update()
     // reset the loiter waypoint target to be correct bearing and dist
     // from starting location in case original yaw used to set it was off due to EKF
     // reset or compass interference from max throttle
+    const float altitude_cm = plane.current_loc.alt - start_loc.alt;
     if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF &&
-        (plane.current_loc.alt - start_loc.alt >= level_alt*100 ||
+        (altitude_cm >= level_alt*100 ||
          start_loc.get_distance(plane.current_loc) >= dist)) {
         // reset the target loiter waypoint using current yaw which should be close to correct starting heading
         const float direction = start_loc.get_bearing_to(plane.current_loc) * 0.01;
         plane.next_WP_loc = start_loc;
         plane.next_WP_loc.offset_bearing(direction, dist);
         plane.next_WP_loc.alt += alt*100.0;
-
         plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
-
-#if AP_FENCE_ENABLED
-        plane.fence.auto_enable_fence_after_takeoff();
-#endif
     }
 
     if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) {
+        //below TAKOFF_LVL_ALT
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100.0);
         plane.takeoff_calc_roll();
         plane.takeoff_calc_pitch();
     } else {
-        plane.calc_nav_roll();
-        plane.calc_nav_pitch();
-        plane.calc_throttle();
+        if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT ,or above and loitering
+#if AP_FENCE_ENABLED
+            plane.fence.auto_enable_fence_after_takeoff();
+#endif
+            plane.calc_nav_roll();
+            plane.calc_nav_pitch();
+            plane.calc_throttle();
+        } else { // still climbing to TAKEOFF_ALT; may be loitering
+            plane.calc_throttle();
+            plane.takeoff_calc_roll();
+            plane.takeoff_calc_pitch();
+        }
+
         //check if in long failsafe, if it is recall long failsafe now to get fs action via events call
+        //this should have already been cleared below, but just in case TKOFF_ALT is mis-configured
         if (plane.long_failsafe_pending) {
-        plane.long_failsafe_pending = false;
-        plane.failsafe_long_on_event(FAILSAFE_LONG, ModeReason::MODE_TAKEOFF_FAILSAFE);
+            plane.long_failsafe_pending = false;
+            plane.failsafe_long_on_event(FAILSAFE_LONG, ModeReason::MODE_TAKEOFF_FAILSAFE);
         }
     }
 }