Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: fix autofence enable in takeoff mode

This commit is contained in:
Henry Wurzburg 2023-12-19 13:42:24 -06:00 committed by Andrew Tridgell
parent dfc1b84186
commit d91ff440e7
1 changed files with 19 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ArduPlane/mode_takeoff.cpp
View File

@ -134,31 +134,38 @@ 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 {
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();
//check if in long failsafe, if it is recall long failsafe now to get fs action via events call
} 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);