From d91ff440e7d635d95a7478ff2126075ba039d807 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Tue, 19 Dec 2023 13:42:24 -0600 Subject: [PATCH] ArduPlane: fix autofence enable in takeoff mode --- ArduPlane/mode_takeoff.cpp | 31 +++++++++++++++++++------------ 1 file changed, 19 insertions(+), 12 deletions(-) diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 2f70bef071..37d0203fcf 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -134,34 +134,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_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(); - //check if in long failsafe, if it is recall long failsafe now to get fs action via events call + 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 due to being in initial TAKEOFF stage; 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); + plane.long_failsafe_pending = false; + plane.failsafe_long_on_event(FAILSAFE_LONG, ModeReason::MODE_TAKEOFF_FAILSAFE); } } }