From 9218cf18ceea10b5a58de66b3f90b2477a13bf7c Mon Sep 17 00:00:00 2001
From: esaldiran <emresaldiran@gmail.com>
Date: Mon, 22 Aug 2022 03:43:41 +0100
Subject: [PATCH] autotest: add test for custom controller

---
 Tools/autotest/arducopter.py | 43 ++++++++++++++++++++++++++++++++++++
 1 file changed, 43 insertions(+)

diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index df072a898f..853b9d5eac 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -902,6 +902,45 @@ class AutoTestCopter(AutoTest):
         self.set_parameter('FS_OPTIONS', 0)
         self.progress("All GCS failsafe tests complete")
 
+    def test_custom_controller(self, timeout=300):
+        self.progress("Configure custom controller parameters")
+        self.set_parameters({
+            'CC_TYPE': 2,
+            'CC_AXIS_MASK': 7,
+            'RC6_OPTION': 109,
+        })
+        self.set_rc(6, 1000)
+        self.reboot_sitl()
+
+        if self.get_parameter("CC_TYPE") != 2 :
+            raise NotAchievedException("Custom controller is not switched to PID backend.")
+
+        # check if we can retrive any param inside PID backend
+        self.get_parameter("CC2_RAT_YAW_P")
+
+        # takeoff in GPS mode and switch to CIRCLE
+        self.takeoff(10, mode="LOITER", takeoff_throttle=2000)
+        self.change_mode("CIRCLE")
+
+        self.context_push()
+        self.context_collect('STATUSTEXT')
+
+        # switch custom controller on
+        self.set_rc(6, 2000)
+        self.wait_statustext("Custom controller is ON", check_context=True)
+
+        # wait 20 second to see if the custom controller destabilize the aircraft
+        if self.wait_altitude(7, 13, relative=True, minimum_duration=20) :
+            raise NotAchievedException("Custom controller is not stable.")
+
+        # switch custom controller off
+        self.set_rc(6, 1000)
+        self.wait_statustext("Custom controller is OFF", check_context=True)
+
+        self.context_pop()
+        self.do_RTL()
+        self.progress("Custom controller test complete")
+
     # Tests all actions and logic behind the battery failsafe
     def fly_battery_failsafe(self, timeout=300):
         self.context_push()
@@ -8784,6 +8823,10 @@ class AutoTestCopter(AutoTest):
              "Test GCS Failsafe",
              self.fly_gcs_failsafe),  # 239s
 
+            ("CustomController",
+             "Test Custom Controller",
+             self.test_custom_controller),
+
             # this group has the smallest runtime right now at around
             #  5mins, so add more tests here, till its around
             #  9-10mins, then make a new group