From bb80881c1023ce94b29575d14fff468ce0bb1ba6 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Mon, 1 May 2023 00:30:35 +0100
Subject: [PATCH] Plane: don't trigger RC failsafe until RC has been received
 for the first time

---
 ArduPlane/AP_Arming.cpp | 35 ++++++++++++++++++++++++++++++++++-
 ArduPlane/AP_Arming.h   |  6 ++++++
 ArduPlane/radio.cpp     |  5 ++++-
 3 files changed, 44 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp
index 3848fbe814..215a93d05d 100644
--- a/ArduPlane/AP_Arming.cpp
+++ b/ArduPlane/AP_Arming.cpp
@@ -42,7 +42,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
     }
     //are arming checks disabled?
     if (checks_to_perform == 0) {
-        return true;
+        return mandatory_checks(display_failure);
     }
     if (hal.util->was_watchdog_armed()) {
         // on watchdog reset bypass arming checks to allow for
@@ -91,6 +91,8 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
         ret = false;
     }
 
+    ret &= rc_received_if_enabled_check(display_failure);
+
 #if HAL_QUADPLANE_ENABLED
     ret &= quadplane_checks(display_failure);
 #endif
@@ -124,6 +126,19 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
     return ret;
 }
 
+bool AP_Arming_Plane::mandatory_checks(bool display_failure)
+{
+    bool ret = true;
+
+    ret &= rc_received_if_enabled_check(display_failure);
+
+    // Call parent class checks
+    ret &= AP_Arming::mandatory_checks(display_failure);
+
+    return ret;
+}
+
+
 #if HAL_QUADPLANE_ENABLED
 bool AP_Arming_Plane::quadplane_checks(bool display_failure)
 {
@@ -411,3 +426,21 @@ bool AP_Arming_Plane::mission_checks(bool report)
 #endif
     return ret;
 }
+
+// Checks rc has been received if it is configured to be used
+bool AP_Arming_Plane::rc_received_if_enabled_check(bool display_failure)
+{
+    if (rc().enabled_protocols() == 0) {
+        // No protocols enabled, will never get RC, don't block arming
+        return true;
+    }
+
+    // If RC failsafe is enabled we must receive RC before arming
+    if ((Plane::ThrFailsafe(plane.g.throttle_fs_enabled.get()) == Plane::ThrFailsafe::Enabled) && 
+        !(rc().has_had_rc_receiver() || rc().has_had_rc_override())) {
+        check_failed(display_failure, "Waiting for RC");
+        return false;
+    }
+
+    return true;
+}
diff --git a/ArduPlane/AP_Arming.h b/ArduPlane/AP_Arming.h
index afce68ae11..8d417d6cc3 100644
--- a/ArduPlane/AP_Arming.h
+++ b/ArduPlane/AP_Arming.h
@@ -29,6 +29,9 @@ public:
     void update_soft_armed();
     bool get_delay_arming() const { return delay_arming; };
 
+    // mandatory checks that cannot be bypassed.  This function will only be called if ARMING_CHECK is zero or arming forced
+    bool mandatory_checks(bool display_failure) override;
+
 protected:
     bool ins_checks(bool report) override;
     bool terrain_database_required() const override;
@@ -36,6 +39,9 @@ protected:
     bool quadplane_checks(bool display_failure);
     bool mission_checks(bool report) override;
 
+    // Checks rc has been received if it is configured to be used
+    bool rc_received_if_enabled_check(bool display_failure);
+
 private:
     void change_arm_state(void);
 
diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp
index e4802bfe83..7a28ca11f5 100644
--- a/ArduPlane/radio.cpp
+++ b/ArduPlane/radio.cpp
@@ -278,7 +278,10 @@ void Plane::control_failsafe()
         }
     }
 
-    if (ThrFailsafe(g.throttle_fs_enabled.get()) != ThrFailsafe::Enabled) {
+    const bool allow_failsafe_bypass = !arming.is_armed() && !is_flying() && (rc().enabled_protocols() != 0);
+    const bool has_had_input = rc().has_had_rc_receiver() || rc().has_had_rc_override();
+    if ((ThrFailsafe(g.throttle_fs_enabled.get()) != ThrFailsafe::Enabled) || (allow_failsafe_bypass && !has_had_input)) {
+        // If not flying and disarmed don't trigger failsafe until RC has been received for the fist time
         return;
     }