From 3973c28f156d88ee83b9a1ea712e37ca28e20367 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Wed, 17 Jan 2024 11:39:20 -0600 Subject: [PATCH] Plane:add TKOFF_TIMEOUT to MODE TAKEOFF --- ArduPlane/Plane.h | 1 + ArduPlane/commands_logic.cpp | 18 ++---------------- ArduPlane/mode.h | 2 +- ArduPlane/mode_takeoff.cpp | 19 +++++++++++++------ ArduPlane/takeoff.cpp | 26 ++++++++++++++++++++++++++ 5 files changed, 43 insertions(+), 23 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 649da30fe9..e410043d8d 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1090,6 +1090,7 @@ private: int8_t takeoff_tail_hold(void); int16_t get_takeoff_pitch_min_cd(void); void landing_gear_update(void); + bool check_takeoff_timeout(void); // avoidance_adsb.cpp void avoidance_adsb_update(void); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 33ebd0c451..7c50d9ab49 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -595,22 +595,8 @@ bool Plane::verify_takeoff() } // check for optional takeoff timeout - if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) { - const float ground_speed = gps.ground_speed(); - const float takeoff_min_ground_speed = 4; - if (!arming.is_armed_and_safety_off()) { - return false; - } - if (ground_speed >= takeoff_min_ground_speed) { - takeoff_state.start_time_ms = 0; - } else { - uint32_t now = AP_HAL::millis(); - if (now - takeoff_state.start_time_ms > (uint32_t)(1000U * g2.takeoff_timeout)) { - gcs().send_text(MAV_SEVERITY_INFO, "Takeoff timeout at %.1f m/s", ground_speed); - plane.arming.disarm(AP_Arming::Method::TAKEOFFTIMEOUT); - mission.reset(); - } - } + if (plane.check_takeoff_timeout()) { + mission.reset(); } // see if we have reached takeoff altitude diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index d2a4d3d11e..2c10d2a94e 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -788,7 +788,7 @@ protected: AP_Int16 target_dist; AP_Int8 level_pitch; - bool takeoff_started; + bool takeoff_mode_setup; Location start_loc; bool _enter() override; diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index b807c64777..2f70bef071 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -62,7 +62,7 @@ ModeTakeoff::ModeTakeoff() : bool ModeTakeoff::_enter() { - takeoff_started = false; + takeoff_mode_setup = false; return true; } @@ -79,7 +79,7 @@ void ModeTakeoff::update() const float alt = target_alt; const float dist = target_dist; - if (!takeoff_started) { + if (!takeoff_mode_setup) { const uint16_t altitude = plane.relative_ground_altitude(false,true); const float direction = degrees(ahrs.get_yaw()); // see if we will skip takeoff as already flying @@ -87,20 +87,19 @@ void ModeTakeoff::update() if (altitude >= alt) { gcs().send_text(MAV_SEVERITY_INFO, "Above TKOFF alt - loitering"); plane.next_WP_loc = plane.current_loc; - takeoff_started = true; + takeoff_mode_setup = 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; + takeoff_mode_setup = 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; @@ -116,11 +115,19 @@ void ModeTakeoff::update() if (!plane.throttle_suppressed) { gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg", alt, dist, direction); - takeoff_started = true; + plane.takeoff_state.start_time_ms = millis(); + takeoff_mode_setup = true; + } } } + // check for optional takeoff timeout + if (plane.check_takeoff_timeout()) { + plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); + takeoff_mode_setup = false; + } + // 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 diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 5d8a428d34..faa220412f 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -305,3 +305,29 @@ void Plane::landing_gear_update(void) g2.landing_gear.update(relative_ground_altitude(g.rangefinder_landing)); } #endif + +/* + check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred and disarms on timeout +*/ + +bool Plane::check_takeoff_timeout(void) +{ + if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) { + const float ground_speed = AP::gps().ground_speed(); + const float takeoff_min_ground_speed = 4; + if (ground_speed >= takeoff_min_ground_speed) { + takeoff_state.start_time_ms = 0; + return false; + } else { + uint32_t now = AP_HAL::millis(); + if (now - takeoff_state.start_time_ms > (uint32_t)(1000U * g2.takeoff_timeout)) { + gcs().send_text(MAV_SEVERITY_INFO, "Takeoff timeout: %.1f m/s speed < 4m/s", ground_speed); + arming.disarm(AP_Arming::Method::TAKEOFFTIMEOUT); + takeoff_state.start_time_ms = 0; + return true; + } + } + } + return false; +} +