Tools: autotest: add tests for precision loiter and landing

Tools/autotest/arducopter.py

@@ -6,13 +6,15 @@ import math
 import os
 import shutil
 import time
+import traceback
 
 import pexpect
 from pymavlink import mavutil
 from pymavlink import mavextra
 from pymavlink import quaternion
 
-from pysim import util
+from pysim import util, rotmat
+
 from common import AutoTest
 from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException
 
@@ -1431,6 +1433,54 @@ class AutoTestCopter(AutoTest):
         if ex is not None:
             raise ex
 
+    def fly_precision_sitl(self):
+        """Use SITL PrecLand backend precision messages to land aircraft."""
+
+        self.context_push()
+
+        ex = None
+        try:
+            self.set_parameter("PLND_ENABLED", 1)
+            self.fetch_parameters()
+            self.set_parameter("PLND_TYPE", 4)
+
+            self.set_parameter("RNGFND_TYPE", 1)
+            self.set_parameter("RNGFND_MIN_CM", 0)
+            self.set_parameter("RNGFND_MAX_CM", 4000)
+            self.set_parameter("RNGFND_PIN", 0)
+            self.set_parameter("RNGFND_SCALING", 12.12)
+
+            self.reboot_sitl()
+
+            self.progress("Waiting for location")
+            old_pos = self.mav.location()
+            self.set_rc(3, 1000)
+            self.takeoff(10, 1800)
+            # move away a little
+            self.set_rc(2, 1550)
+            self.wait_distance(5)
+            self.set_rc(2, 1500)
+            self.mavproxy.send('switch 2\n')  # land mode
+            self.mav.motors_disarmed_wait()
+            self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
+            new_pos = self.mav.location()
+            delta = self.get_distance(old_pos, new_pos)
+            if delta > 1:
+                raise NotAchievedException()
+            self.progress("Landed %u metres from original position" % delta)
+
+        except Exception as e:
+            self.progress("Exception caught")
+            ex = e
+
+        self.set_rc(3, 1000)
+        self.context_pop()
+        self.reboot_sitl()
+        self.progress("All done")
+
+        if ex is not None:
+            raise ex
+
     def get_system_clock_utc(self, time_seconds):
         # this is a copy of ArduPilot's AP_RTC function!
         # separate time into ms, sec, min, hour and days but all expressed
@@ -1792,6 +1842,106 @@ class AutoTestCopter(AutoTest):
             if delta > 10:
                 break
 
+    def earth_to_body(self, vector):
+        m = self.mav.messages["ATTITUDE"]
+        x = rotmat.Vector3(m.roll, m.pitch, m.yaw)
+#        print('r=%f p=%f y=%f' % (m.roll, m.pitch, m.yaw))
+        return vector - x
+
+    def loiter_to_ne(self, x, y, z):
+        dest = rotmat.Vector3(x, y, z)
+        while True:
+            m_pos = self.mav.recv_match(type='LOCAL_POSITION_NED',
+                                        blocking=True)
+            pos = rotmat.Vector3(m_pos.x, m_pos.y, m_pos.z)
+            delta_ef = pos - dest
+            dist = math.sqrt(delta_ef.x * delta_ef.x + delta_ef.y * delta_ef.y)
+            self.progress("dist=%f" % (dist,))
+            if dist < 0.1:
+                break
+            delta_bf = self.earth_to_body(delta_ef)
+            angle_x = math.atan2(delta_bf.x, delta_bf.z)
+            angle_y = math.atan2(delta_bf.y, delta_bf.z)
+            distance = math.sqrt(delta_bf.x * delta_bf.x +
+                                 delta_bf.y * delta_bf.y +
+                                 delta_bf.z * delta_bf.z)
+            self.mav.mav.landing_target_send(
+                0, # time_usec
+                1, # target_num
+                mavutil.mavlink.MAV_FRAME_GLOBAL, # frame; AP ignores
+                angle_x, # angle x (radians)
+                angle_y, # angle y (radians)
+                distance, # distance to target
+                0.01, # size of target in radians, X-axis
+                0.01 # size of target in radians, Y-axis
+            )
+
+        tstart = self.get_sim_time()
+        while self.get_sim_time() - tstart < 10:
+            m_pos = self.mav.recv_match(type='LOCAL_POSITION_NED',
+                                        blocking=True)
+            pos = rotmat.Vector3(m_pos.x, m_pos.y, m_pos.z)
+            delta_ef = pos - dest
+            dist = math.sqrt(delta_ef.x * delta_ef.x + delta_ef.y * delta_ef.y)
+            self.progress("dist=%f" % (dist,))
+
+    def fly_payload_place_mission(self):
+        """Test payload placing in auto."""
+        self.context_push()
+
+        ex = None
+        try:
+            self.set_parameter("RNGFND_TYPE", 1)
+            self.set_parameter("RNGFND_MIN_CM", 0)
+            self.set_parameter("RNGFND_MAX_CM", 4000)
+            self.set_parameter("RNGFND_PIN", 0)
+            self.set_parameter("RNGFND_SCALING", 12.12)
+            self.set_parameter("GRIP_ENABLE", 1)
+            self.set_parameter("GRIP_TYPE", 1)
+            self.set_parameter("SIM_GRPS_ENABLE", 1)
+            self.set_parameter("SIM_GRPS_PIN", 8)
+            self.set_parameter("SERVO8_FUNCTION", 28)
+            self.set_parameter("RC9_OPTION", 19)
+            self.reboot_sitl()
+            self.set_rc(9, 2000)
+            # load the mission:
+            global num_wp
+            num_wp = self.load_mission("copter_payload_place.txt")
+            if not num_wp:
+                self.progress("load copter_mission failed")
+                raise NotAchievedException()
+
+            self.progress("Waiting for location")
+            self.mav.location()
+            self.set_rc(3, 1000)
+            self.mavproxy.send('switch 6\n')  # stabilize mode
+            self.mav.wait_heartbeat()
+            self.wait_mode('STABILIZE')
+            self.progress("Waiting reading for arm")
+            self.wait_ready_to_arm()
+
+            self.arm_vehicle()
+
+            self.mavproxy.send('switch 4\n')  # auto mode
+            self.mav.wait_heartbeat()
+            self.wait_mode('AUTO')
+
+            self.set_rc(3, 1500)
+            self.wait_text("Gripper load releas", timeout=90)
+
+            self.mav.motors_disarmed_wait()
+
+        except Exception as e:
+            self.progress("Exception caught")
+            ex = e
+
+        self.context_pop()
+        self.reboot_sitl()
+        self.progress("All done")
+
+        if ex is not None:
+            raise ex
+
     def fly_guided_change_submode(self):
         """"Ensure we can move around in guided after a takeoff command."""
 
@@ -2171,6 +2321,72 @@ class AutoTestCopter(AutoTest):
 
         self.reboot_sitl() # to handle MNT_TYPE changing
 
+    def fly_precision_companion(self):
+        """Use Companion PrecLand backend precision messages to loiter."""
+
+        self.context_push()
+
+        ex = None
+        try:
+            self.set_parameter("PLND_ENABLED", 1)
+            self.fetch_parameters()
+            # enable companion backend:
+            self.set_parameter("PLND_TYPE", 1)
+
+            self.set_parameter("RNGFND_TYPE", 1)
+            self.set_parameter("RNGFND_MIN_CM", 0)
+            self.set_parameter("RNGFND_MAX_CM", 4000)
+            self.set_parameter("RNGFND_PIN", 0)
+            self.set_parameter("RNGFND_SCALING", 12.12)
+
+            # set up a channel switch to enable precision loiter:
+            self.set_parameter("RC7_OPTION", 39)
+
+            self.reboot_sitl()
+
+            self.progress("Waiting for location")
+            self.mav.location()
+            self.set_rc(3, 1000)
+            self.mavproxy.send('switch 6\n')  # stabilize mode
+            self.mav.wait_heartbeat()
+            self.wait_mode('STABILIZE')
+            self.progress("Waiting reading for arm")
+            self.wait_ready_to_arm()
+
+            # we should be doing precision loiter at this point
+            start = self.mav.recv_match(type='LOCAL_POSITION_NED',
+                                        blocking=True)
+
+            self.arm_vehicle()
+            self.set_rc(3, 1800)
+            alt_min = 10
+            self.wait_altitude(alt_min,
+                               (alt_min + 5),
+                               relative=True)
+            self.set_rc(3, 1500)
+            # move away a little
+            self.set_rc(2, 1550)
+            self.wait_distance(5)
+            self.set_rc(2, 1500)
+            self.mavproxy.send('mode loiter\n')
+            self.wait_mode('LOITER')
+
+            # turn precision loiter on:
+            self.set_rc(7, 2000)
+
+            # try to drag aircraft to a position 5 metres north-east-east:
+            self.loiter_to_ne(start.x + 5, start.y + 10, start.z + 10)
+            self.loiter_to_ne(start.x + 5, start.y - 10, start.z + 10)
+
+        except Exception as e:
+            self.progress("Exception caught: %s" % traceback.format_exc(e))
+            ex = e
+
+        self.context_pop()
+        self.set_rc(3, 1000)
+        self.reboot_sitl()
+        self.progress("All done")
+
         if ex is not None:
             raise ex
 
@@ -2203,6 +2419,13 @@ class AutoTestCopter(AutoTest):
 
             self.run_test("Loiter-To-Alt", self.fly_loiter_to_alt)
 
+            self.run_test("Payload Place Mission",
+                          self.fly_payload_place_mission)
+            self.run_test("Precision Loiter (Companion)",
+                          self.fly_precision_companion)
+            self.run_test("Precision Landing (SITL)",
+                          self.fly_precision_sitl)
+
             self.progress("Waiting for location")
             self.homeloc = self.mav.location()
             self.progress("Home location: %s" % self.homeloc)

Tools/autotest/copter_payload_place.txt

@@ -0,0 +1,7 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.000000	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	-35.363007	149.165070	20.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.362869	149.165222	20.000000	1
+3	0	3	94	0.000000	0.000000	0.000000	0.000000	-35.363106	149.165436	20.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.363220	149.165512	20.000000	1
+5	0	3	21	0.000000	0.000000	0.000000	1.000000	-35.363228	149.165695	20.000000	1