ArduPlane: fix autofence enable in takeoff mode

Hwurzburg · Jan 1, 2024 · 8134f29 · 8134f29
1 parent 29ec7a9
commit 8134f29
Showing 1 changed file with 17 additions and 7 deletions.
diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp
@@ -127,31 +127,41 @@ 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 = 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 >= 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 {
+    } else if ((altitude >= 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();
         //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);
+        }
+    } 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
         if (plane.long_failsafe_pending) {
         plane.long_failsafe_pending = false;
         plane.failsafe_long_on_event(FAILSAFE_LONG, ModeReason::MODE_TAKEOFF_FAILSAFE);