diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 6e1984a7d9..834fa93de9 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -40,7 +40,7 @@ const AP_Param::GroupInfo ModeTakeoff::var_info[] = { // @Units: m // @User: Standard AP_GROUPINFO("DIST", 4, ModeTakeoff, target_dist, 200), - + // @Param: GND_PITCH // @DisplayName: Takeoff run pitch demand // @Description: Degrees of pitch angle demanded during the takeoff run before speed reaches TKOFF_ROTATE_SPD. For taildraggers set to 3-point ground pitch angle and use TKOFF_TDRAG_ELEV to prevent nose tipover. For nose-wheel steer aircraft set to the ground pitch angle and if a reduction in nose-wheel load is required as speed rises, use a positive offset in TKOFF_GND_PITCH of up to 5 degrees above the angle on ground, starting at the mesured pitch angle and incrementing in 1 degree steps whilst checking for premature rotation and takeoff with each increment. To increase nose-wheel load, use a negative TKOFF_TDRAG_ELEV and refer to notes on TKOFF_TDRAG_ELEV before making adjustments. @@ -68,61 +68,75 @@ bool ModeTakeoff::_enter() void ModeTakeoff::update() { - if (!takeoff_started) { - // see if we will skip takeoff as already flying - if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) { - gcs().send_text(MAV_SEVERITY_INFO, "Takeoff skipped - circling"); - plane.prev_WP_loc = plane.current_loc; - plane.next_WP_loc = plane.current_loc; - takeoff_started = true; - plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); - } + // don't setup waypoints if we dont have a valid position and home! + if (!(plane.current_loc.initialised() && AP::ahrs().home_is_set())) { + plane.calc_nav_roll(); + plane.calc_nav_pitch(); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); + return; } + const float alt = target_alt; + const float dist = target_dist; if (!takeoff_started) { - // setup target location 1.5 times loiter radius from the - // takeoff point, at a height of TKOFF_ALT - const float dist = target_dist; - const float alt = target_alt; + const uint16_t altitude = plane.relative_ground_altitude(false,true); const float direction = degrees(ahrs.yaw); + // see if we will skip takeoff as already flying + if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) { + if (altitude >= alt) { + gcs().send_text(MAV_SEVERITY_INFO, "Above TKOFF alt - loitering"); + plane.next_WP_loc = plane.current_loc; + takeoff_started = true; + plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); + } else { + gcs().send_text(MAV_SEVERITY_INFO, "Climbing to TKOFF alt then loitering"); + plane.next_WP_loc = plane.current_loc; + plane.next_WP_loc.alt += ((alt - altitude) *100); + plane.next_WP_loc.offset_bearing(direction, dist); + takeoff_started = true; + plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); + } + // not flying so do a full takeoff sequence + } else { + // setup target waypoint and alt for loiter at dist and alt from start - start_loc = plane.current_loc; - plane.prev_WP_loc = plane.current_loc; - plane.next_WP_loc = plane.current_loc; - plane.next_WP_loc.alt += alt*100.0; - plane.next_WP_loc.offset_bearing(direction, dist); + start_loc = plane.current_loc; + plane.prev_WP_loc = plane.current_loc; + plane.next_WP_loc = plane.current_loc; + plane.next_WP_loc.alt += alt*100.0; + plane.next_WP_loc.offset_bearing(direction, dist); - plane.crash_state.is_crashed = false; + plane.crash_state.is_crashed = false; - plane.auto_state.takeoff_pitch_cd = level_pitch * 100; + plane.auto_state.takeoff_pitch_cd = level_pitch * 100; - plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF); + plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF); - if (!plane.throttle_suppressed) { - gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm at %.1fm to %.1f deg", - alt, dist, direction); - takeoff_started = true; + if (!plane.throttle_suppressed) { + gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg", + alt, dist, direction); + takeoff_started = true; + } } } // we finish the initial level takeoff if we climb past // TKOFF_LVL_ALT or we pass the target location. The check for // target location prevents us flying forever if we can't climb + // 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 if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF && (plane.current_loc.alt - start_loc.alt >= level_alt*100 || - start_loc.get_distance(plane.current_loc) >= target_dist)) { - // reached level alt, re-calculate bearing to cope with systems with no compass - // or with poor initial compass - float direction = start_loc.get_bearing_to(plane.current_loc) * 0.01; - float dist_done = start_loc.get_distance(plane.current_loc); - const float dist = target_dist; - - plane.next_WP_loc = plane.current_loc; - plane.next_WP_loc.offset_bearing(direction, MAX(dist-dist_done, 0)); - plane.next_WP_loc.alt = start_loc.alt + target_alt*100.0; + 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