diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index fe9e9e892b..84c18d207c 100644 --- a/ArduPlane/Plane.h +++ b/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; } diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 854f9306c1..122b3dc361 100644 --- a/ArduPlane/events.cpp +++ b/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"); diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 6ea91d747d..a9af6555b7 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -89,6 +89,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(); diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 834fa93de9..bed8ebb65e 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -1,5 +1,6 @@ #include "mode.h" #include "Plane.h" +#include /* 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); + } } }