From 5a178984c4fe9357180da95510f85ff02129ca29 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Tue, 7 Mar 2023 12:11:50 +1030
Subject: [PATCH] Copter: Add Radio Failsafe Brake option

---
 ArduCopter/Copter.h       |  4 +++-
 ArduCopter/Parameters.cpp |  2 +-
 ArduCopter/defines.h      |  1 +
 ArduCopter/events.cpp     | 21 +++++++++++++++++++++
 4 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 77b11873f4..8322ad66e6 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -605,7 +605,8 @@ private:
         SMARTRTL           = 3,
         SMARTRTL_LAND      = 4,
         TERMINATE          = 5,
-        AUTO_DO_LAND_START = 6
+        AUTO_DO_LAND_START = 6,
+        BRAKE_LAND         = 7
     };
 
     enum class FailsafeOption {
@@ -774,6 +775,7 @@ private:
     void set_mode_SmartRTL_or_RTL(ModeReason reason);
     void set_mode_SmartRTL_or_land_with_pause(ModeReason reason);
     void set_mode_auto_do_land_start_or_RTL(ModeReason reason);
+    void set_mode_brake_or_land_with_pause(ModeReason reason);
     bool should_disarm_on_failsafe();
     void do_failsafe_action(FailsafeAction action, ModeReason reason);
     void announce_failsafe(const char *type, const char *action_undertaken=nullptr);
diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp
index b15f9e42ff..7f77a53783 100644
--- a/ArduCopter/Parameters.cpp
+++ b/ArduCopter/Parameters.cpp
@@ -226,7 +226,7 @@ const AP_Param::Info Copter::var_info[] = {
     // @Param: FS_THR_ENABLE
     // @DisplayName: Throttle Failsafe Enable
     // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel
-    // @Values:  0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Removed in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land,6:Enabled Auto DO_LAND_START or RTL
+    // @Values:  0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Removed in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land,6:Enabled Auto DO_LAND_START or RTL,7:Enabled always Brake or Land
     // @User: Standard
     GSCALAR(failsafe_throttle,  "FS_THR_ENABLE",   FS_THR_ENABLED_ALWAYS_RTL),
 
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 384fb8d97e..b3cd317f76 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -141,6 +141,7 @@ enum LoggingParameters {
 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL      4
 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND     5
 #define FS_THR_ENABLED_AUTO_RTL_OR_RTL             6
+#define FS_THR_ENABLED_BRAKE_OR_LAND               7
 
 // GCS failsafe definitions (FS_GCS_ENABLE parameter)
 #define FS_GCS_DISABLED                        0
diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp
index 6d7e8f563c..69e4edb3b1 100644
--- a/ArduCopter/events.cpp
+++ b/ArduCopter/events.cpp
@@ -36,6 +36,9 @@ void Copter::failsafe_radio_on_event()
         case FS_THR_ENABLED_AUTO_RTL_OR_RTL:
             desired_action = FailsafeAction::AUTO_DO_LAND_START;
             break;
+        case FS_THR_ENABLED_BRAKE_OR_LAND:
+            desired_action = FailsafeAction::BRAKE_LAND;
+            break;
         default:
             desired_action = FailsafeAction::LAND;
     }
@@ -425,6 +428,21 @@ void Copter::set_mode_auto_do_land_start_or_RTL(ModeReason reason)
     set_mode_RTL_or_land_with_pause(reason);
 }
 
+// Sets mode to Brake or LAND with 4 second delay before descent starts
+// This can come from failsafe or RC option
+void Copter::set_mode_brake_or_land_with_pause(ModeReason reason)
+{
+#if MODE_BRAKE_ENABLED == ENABLED
+    if (set_mode(Mode::Number::BRAKE, reason)) {
+        AP_Notify::events.failsafe_mode_change = 1;
+        return;
+    }
+#endif
+
+    gcs().send_text(MAV_SEVERITY_WARNING, "Trying Land Mode");
+    set_mode_land_with_pause(reason);
+}
+
 bool Copter::should_disarm_on_failsafe() {
     if (ap.in_arming_delay) {
         return true;
@@ -476,6 +494,9 @@ void Copter::do_failsafe_action(FailsafeAction action, ModeReason reason){
         case FailsafeAction::AUTO_DO_LAND_START:
             set_mode_auto_do_land_start_or_RTL(reason);
             break;
+        case FailsafeAction::BRAKE_LAND:
+            set_mode_brake_or_land_with_pause(reason);
+            break;
     }
 
 #if AP_GRIPPER_ENABLED