Plane:add TKOFF_TIMEOUT to MODE TAKEOFF

Hwurzburg · Jan 19, 2024 · d6e6664 · d6e6664
1 parent 2bc5078
commit d6e6664
Show file tree

Hide file tree

Showing 5 changed files with 43 additions and 23 deletions.
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -1090,6 +1090,7 @@ class Plane : public AP_Vehicle {
     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
@@ -594,22 +594,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
@@ -785,7 +785,7 @@ class ModeTakeoff: public Mode
     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
@@ -62,7 +62,7 @@ ModeTakeoff::ModeTakeoff() :
 
 bool ModeTakeoff::_enter()
 {
-    takeoff_started = false;
+    takeoff_mode_setup = false;
 
     return true;
 }
@@ -79,28 +79,27 @@ 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
         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;
+                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
@@ -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;
+}
+