aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Tools: autotest: add tests for precision loiter and landing

Browse Source
This commit is contained in:
Peter Barker 2018-07-16 09:54:00 +10:00 committed by Peter Barker
parent a4c532edff
commit a75d678e7f
2 changed files with 231 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

225
Tools/autotest/arducopter.py
View File

@ -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)

7
Tools/autotest/copter_payload_place.txt Normal file
View File

@ -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