autotest: add a test for instantaneous trim switch option

Tools/autotest/arducopter.py

@@ -19845,6 +19845,56 @@ RTL_ALT_M 111
         # we are not at the home location - reboot so the next test starts there
         self.reboot_sitl()
 
+    def AHRSTrim(self):
+        '''check RC stick inputs can be used to save an AHRS trim'''
+        # the Save Trim procedure requires the throttle to be at minimum
+        # ("landed").  The pilot holds the roll and pitch sticks to the
+        # desired trim and then raises the aux switch.  Trim is taken from
+        # the stick positions, not the vehicle's attitude.
+        #
+        # save-trim modifies AHRS_TRIM_X/Y directly rather than via a
+        # parameter set, so we must explicitly set them to a known
+        # (non-zero) starting value here for the harness to record and
+        # revert them after the test.  We use the opposite sign to the
+        # trim the sticks will command, so the save has to overcome them.
+        self.set_parameters({
+            'RC9_OPTION': 5,  # save-trim
+            'AHRS_TRIM_X': -0.05,  # ensure parameter-reversion
+            'AHRS_TRIM_Y': 0.05,  # ensure parameter-reversion
+        })
+        self.change_mode('STABILIZE')
+        self.set_rc_from_map({
+            1: 1900,  # roll full right
+            2: 1100,  # pitch full forward
+            3: 1000,  # throttle to minimum so its control_in is zero
+        })
+        self.context_collect('STATUSTEXT')
+        self.set_rc(9, 2000)
+        self.wait_statustext('Trim saved', check_context=True)
+        self.context_stop_collecting('STATUSTEXT')
+
+        # lower the switch and restore the sticks so the trim can't be
+        # applied a second time (e.g. across the reboot below):
+        self.set_rc(9, 1000)
+        self.set_rc_default()
+
+        # trim is taken from the stick positions, so right-roll/forward-pitch
+        # should overcome the starting trims to give a positive roll trim
+        # and a negative pitch trim:
+        trim_x = self.get_parameter('AHRS_TRIM_X')
+        trim_y = self.get_parameter('AHRS_TRIM_Y')
+        if trim_x < math.radians(2):
+            raise NotAchievedException(f"Expected significant +ve roll trim, got {trim_x}")
+        if trim_y > -math.radians(2):
+            raise NotAchievedException(f"Expected significant -ve pitch trim, got {trim_y}")
+
+        # the trim is saved to storage, so should survive a reboot:
+        self.reboot_sitl()
+        self.assert_parameter_values({
+            "AHRS_TRIM_X": trim_x,
+            "AHRS_TRIM_Y": trim_y,
+        }, epsilon=0.0001)
+
     def RTLStoppingDistanceSpeed(self):
         '''test stopping distance unaffected by RTL speed'''
         self.upload_simple_relhome_mission([
@@ -20892,6 +20942,7 @@ return update, 1000
             self.CompassLearnCopyFromEKFAffinity,
             self.CompassLearnCopyFromSIM,
             self.AHRSAutoTrim,
+            self.AHRSTrim,
             self.Ch6TuningLoitMaxXYSpeed,
             self.IgnorePilotYaw,
             self.TestEKF3CompassFailover,