From 20cd9e523cc179f0335f48864825f48c955bc867 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sun, 15 Sep 2024 16:09:08 +0100
Subject: [PATCH] Copter: add and use new `afs_mode` mode method

---
 ArduCopter/afs_copter.cpp | 12 +-----------
 ArduCopter/mode.h         | 29 +++++++++++++++++++++++++++++
 2 files changed, 30 insertions(+), 11 deletions(-)

diff --git a/ArduCopter/afs_copter.cpp b/ArduCopter/afs_copter.cpp
index 0d64d99e005f1..a0b83b36cad3a 100644
--- a/ArduCopter/afs_copter.cpp
+++ b/ArduCopter/afs_copter.cpp
@@ -55,17 +55,7 @@ void AP_AdvancedFailsafe_Copter::setup_IO_failsafe(void)
  */
 AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void)
 {
-    switch (copter.flightmode->mode_number()) {
-    case Mode::Number::AUTO:
-    case Mode::Number::AUTO_RTL:
-    case Mode::Number::GUIDED:
-    case Mode::Number::RTL:
-    case Mode::Number::LAND:
-        return AP_AdvancedFailsafe::AFS_AUTO;
-    default:
-        break;
-    }
-    return AP_AdvancedFailsafe::AFS_STABILIZED;
+    return copter.flightmode->afs_mode();
 }
 
 //to force entering auto mode when datalink loss 
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 9f451ad0ad545..d6a83fbf2a98b 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -4,6 +4,10 @@
 #include <AP_Math/chirp.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h> // TODO why is this needed if Copter.h includes this
 
+#if ADVANCED_FAILSAFE
+#include "afs_copter.h"
+#endif
+
 class Parameters;
 class ParametersG2;
 
@@ -130,6 +134,11 @@ class Mode {
     virtual bool allows_flip() const { return false; }
     virtual bool crash_check_enabled() const { return true; }
 
+#if ADVANCED_FAILSAFE
+    // Return the type of this mode for use by advanced failsafe
+    virtual AP_AdvancedFailsafe_Copter::control_mode afs_mode() const { return AP_AdvancedFailsafe_Copter::control_mode::AFS_STABILIZED; }
+#endif
+
 #if FRAME_CONFIG == HELI_FRAME
     virtual bool allows_inverted() const { return false; };
 #endif
@@ -509,6 +518,11 @@ class ModeAuto : public Mode {
     bool allows_inverted() const override { return true; };
 #endif
 
+#if ADVANCED_FAILSAFE
+    // Return the type of this mode for use by advanced failsafe
+    AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
+#endif
+
     // Auto modes
     enum class SubMode : uint8_t {
         TAKEOFF,
@@ -1047,6 +1061,11 @@ class ModeGuided : public Mode {
 
     bool requires_terrain_failsafe() const override { return true; }
 
+#if ADVANCED_FAILSAFE
+    // Return the type of this mode for use by advanced failsafe
+    AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
+#endif
+
     // Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
     // attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes
     //                IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity
@@ -1214,6 +1233,11 @@ class ModeLand : public Mode {
 
     bool is_landing() const override { return true; };
 
+#if ADVANCED_FAILSAFE
+    // Return the type of this mode for use by advanced failsafe
+    AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
+#endif
+
     void do_not_use_GPS();
 
     // returns true if LAND mode is trying to control X/Y position
@@ -1391,6 +1415,11 @@ class ModeRTL : public Mode {
 
     bool requires_terrain_failsafe() const override { return true; }
 
+#if ADVANCED_FAILSAFE
+    // Return the type of this mode for use by advanced failsafe
+    AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
+#endif
+
     // for reporting to GCS
     bool get_wp(Location &loc) const override;