autotest: add test for AP_Motors6DOF

This test uses a normal, non-6dof frame, and validates that it is able to do a simple mission

Author: Williangalvani

Tools/autotest/arducopter.py

@@ -14895,6 +14895,35 @@ def ScriptingFlyVelocity(self):
         # Test done
         self.land_and_disarm()
 
+    def Scripting6DoFMotors(self):
+        '''test 6DoF scripting motor matrix'''
+        self.context_collect('STATUSTEXT')
+
+        self.set_parameters({
+            "FRAME_CLASS": 16,  # MOTOR_FRAME_6DOF_SCRIPTING
+            "SCR_ENABLE": 1,
+        })
+
+        self.install_example_script_context("Copter_Motors_6DoF.lua")
+        self.reboot_sitl()
+
+        self.wait_statustext("6DoF Copter quad scripting", timeout=30, check_context=True)
+        self.wait_ready_to_arm()
+
+        self.upload_simple_relhome_mission([
+            (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 30, 0, 20),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 30, 30, 20),
+            (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
+        ])
+
+        num_wp = self.get_mission_count()
+        self.set_parameter("AUTO_OPTIONS", 3)
+        self.change_mode('AUTO')
+        self.arm_vehicle()
+        self.wait_waypoint(num_wp-1, num_wp-1, timeout=120)
+        self.wait_disarmed(timeout=60)
+
     def RTLYaw(self):
         '''test that vehicle yaws to original heading on RTL'''
         # 0 is WP_YAW_BEHAVIOR_NONE
@@ -16016,6 +16045,7 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.TestTetherStuck,
             self.ScriptingFlipMode,
             self.ScriptingFlyVelocity,
+            self.Scripting6DoFMotors,
             self.EK3_EXT_NAV_vel_without_vert,
             self.CompassLearnCopyFromEKF,
             self.AHRSAutoTrim,

libraries/AP_Scripting/examples/Copter_Motors_6DoF.lua

@@ -0,0 +1,18 @@
+-- Replicate the standard Quad X frame via 6DoF scripting motor matrix
+-- Raw factors derived from angle-based Quad X definition:
+--   cos(angle+90) for roll, cos(angle) for pitch
+-- Motor #  Roll     Pitch    Yaw  Throttle Forward Lateral Reversible TestOrder
+
+Motors_6DoF:add_motor(0, -0.7071,  0.7071,  1.0, 1.0, 0, 0, false, 1)
+Motors_6DoF:add_motor(1,  0.7071, -0.7071,  1.0, 1.0, 0, 0, false, 3)
+Motors_6DoF:add_motor(2,  0.7071,  0.7071, -1.0, 1.0, 0, 0, false, 4)
+Motors_6DoF:add_motor(3, -0.7071, -0.7071, -1.0, 1.0, 0, 0, false, 2)
+
+assert(Motors_6DoF:init(4), 'unable to setup 4 motors')
+
+-- no dedicated forward/lateral thrusters, use tilt like a normal copter
+attitude_control:set_forward_enable(false)
+attitude_control:set_lateral_enable(false)
+
+motors:set_frame_string("6DoF Copter quad scripting")
+gcs:send_text(6, "6DoF Copter quad scripting")