From a75d678e7fd3c91164cc5a11bf91ef0a8c3c7618 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 16 Jul 2018 09:54:00 +1000 Subject: [PATCH] Tools: autotest: add tests for precision loiter and landing --- Tools/autotest/arducopter.py | 225 +++++++++++++++++++++++- Tools/autotest/copter_payload_place.txt | 7 + 2 files changed, 231 insertions(+), 1 deletion(-) create mode 100644 Tools/autotest/copter_payload_place.txt diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index e6c264268c..fcc1429488 100644 --- a/Tools/autotest/arducopter.py +++ b/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) diff --git a/Tools/autotest/copter_payload_place.txt b/Tools/autotest/copter_payload_place.txt new file mode 100644 index 0000000000..a3bc594e2f --- /dev/null +++ b/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