ArduPlane: add failsafe protections to Mode Takeoff

ArduPlane/Plane.h

@@ -218,6 +218,9 @@ private:
     // external failsafe boards during baro and airspeed calibration
     bool in_calibration;
 
+    // are we currently in long failsafe but have postponed it in MODE TAKEOFF until min level alt is reached
+    bool long_failsafe_pending;
+
     // GCS selection
     GCS_Plane _gcs; // avoid using this; use gcs()
     GCS_Plane &gcs() { return _gcs; }

ArduPlane/events.cpp

@@ -107,6 +107,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
 
 void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason)
 {
+
     // This is how to handle a long loss of control signal failsafe.
     //  If the GCS is locked up we allow control to revert to RC
     RC_Channels::clear_overrides();
@@ -124,6 +125,14 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
     case Mode::Number::CIRCLE:
     case Mode::Number::LOITER:
     case Mode::Number::THERMAL:
+    case Mode::Number::TAKEOFF:
+        if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF && !(g.fs_action_long == FS_ACTION_LONG_GLIDE || g.fs_action_long == FS_ACTION_LONG_PARACHUTE)) {
+            // don't failsafe if in inital climb of TAKEOFF mode and FS action is not parachute or glide
+            // long failsafe will be re-called if still in fs after initial climb
+            long_failsafe_pending = true;
+            break;
+        }
+
         if(plane.emergency_landing) {
             set_mode(mode_fbwa, reason); // emergency landing switch overrides normal action to allow out of range landing
             break;
@@ -196,7 +205,6 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
     case Mode::Number::QRTL:
     case Mode::Number::LOITER_ALT_QLAND:
 #endif
-    case Mode::Number::TAKEOFF:
     case Mode::Number::INITIALISING:
         break;
     }
@@ -217,6 +225,7 @@ void Plane::failsafe_short_off_event(ModeReason reason)
 
 void Plane::failsafe_long_off_event(ModeReason reason)
 {
+    long_failsafe_pending = false;
     // We're back in radio contact with RC or GCS
     if (reason == ModeReason:: GCS_FAILSAFE) {
         gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Off");

ArduPlane/mode.cpp

@@ -90,6 +90,9 @@ bool Mode::enter()
     // initialize speed variable used in AUTO and GUIDED for DO_CHANGE_SPEED commands
     plane.new_airspeed_cm = -1;
     
+    // clear postponed long failsafe if mode change (from GCS) occurs before recall of long failsafe
+    plane.long_failsafe_pending = false;
+
 #if HAL_QUADPLANE_ENABLED
     quadplane.mode_enter();
 #endif

ArduPlane/mode_takeoff.cpp

@@ -1,5 +1,6 @@
 #include "mode.h"
 #include "Plane.h"
+#include <GCS_MAVLink/GCS.h>
 
 /*
   mode takeoff parameters
@@ -150,6 +151,11 @@ void ModeTakeoff::update()
         plane.calc_nav_roll();
         plane.calc_nav_pitch();
         plane.calc_throttle();
+        //check if in long failsafe, if it is recall long failsafe now to get fs action via events call
+        if (plane.long_failsafe_pending) {
+        plane.long_failsafe_pending = false;
+        plane.failsafe_long_on_event(FAILSAFE_LONG, ModeReason::MODE_TAKEOFF_FAILSAFE);
+        }
     }
 }