Plane:add TKOFF_TIMEOUT to MODE TAKEOFF

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);

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) {
+    if (plane.check_takeoff_timeout()) {
-        const float ground_speed = gps.ground_speed();
+        mission.reset();
-        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();
-            }
-        }
     }
 
     // see if we have reached takeoff altitude

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;

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

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;
+}
+