diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index d2d35a7b14..df180b6f74 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -453,7 +453,7 @@ private: float throttle_lim_max; float throttle_lim_min; uint32_t throttle_max_timer_ms; - // Good candidate for keeping the initial time for TKOFF_THR_MAX_T. + uint32_t level_off_start_time_ms; } takeoff_state; // ground steering controller state @@ -1147,6 +1147,7 @@ private: int16_t get_takeoff_pitch_min_cd(void); void landing_gear_update(void); bool check_takeoff_timeout(void); + bool check_takeoff_timeout_level_off(void); // avoidance_adsb.cpp void avoidance_adsb_update(void); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 2110b1455d..624dc2749c 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -589,7 +589,10 @@ bool Plane::verify_takeoff() // see if we have reached takeoff altitude int32_t relative_alt_cm = adjusted_relative_altitude_cm(); - if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) { + if ( + relative_alt_cm > auto_state.takeoff_altitude_rel_cm || // altitude reached + plane.check_takeoff_timeout_level_off() // pitch level-off maneuver has timed out + ) { gcs().send_text(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm", (double)(relative_alt_cm*0.01f)); steer_state.hold_course_cd = -1; diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 5f93cc4729..d8e3c11f75 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -119,6 +119,7 @@ void ModeTakeoff::update() gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg", alt, dist, direction); plane.takeoff_state.start_time_ms = millis(); + plane.takeoff_state.level_off_start_time_ms = 0; plane.takeoff_state.throttle_max_timer_ms = millis(); takeoff_mode_setup = true; plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function. @@ -157,6 +158,12 @@ void ModeTakeoff::update() plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); } + // If we have timed-out on the attempt to close the final few meters + // during pitch level-off, continue to NORMAL flight stage. + if (plane.check_takeoff_timeout_level_off()) { + plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); + } + if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { //below TKOFF_ALT plane.takeoff_calc_roll(); diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index dda412d5ee..d7617e2f56 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -121,6 +121,7 @@ bool Plane::auto_takeoff_check(void) takeoff_state.launchTimerStarted = false; takeoff_state.last_tkoff_arm_time = 0; takeoff_state.start_time_ms = now; + takeoff_state.level_off_start_time_ms = 0; takeoff_state.throttle_max_timer_ms = now; steer_state.locked_course_err = 0; // use current heading without any error offset return true; @@ -316,6 +317,7 @@ int16_t Plane::get_takeoff_pitch_min_cd(void) // make a note of that altitude to use it as a start height for scaling gcs().send_text(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", int(remaining_height_to_target_cm/100)); auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm; + takeoff_state.level_off_start_time_ms = AP_HAL::millis(); } } } @@ -376,9 +378,8 @@ void Plane::landing_gear_update(void) #endif /* - check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred and disarms on timeout + check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred */ - bool Plane::check_takeoff_timeout(void) { if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) { @@ -400,3 +401,17 @@ bool Plane::check_takeoff_timeout(void) return false; } +/* + check if the pitch level-off time has expired; returns true if timeout has occurred +*/ +bool Plane::check_takeoff_timeout_level_off(void) +{ + if (takeoff_state.level_off_start_time_ms > 0) { + // A takeoff is in progress. + uint32_t now = AP_HAL::millis(); + if ((now - takeoff_state.level_off_start_time_ms) > (uint32_t)(1000U * g.takeoff_pitch_limit_reduction_sec)) { + return true; + } + } + return false; +}