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

Tools: autotest: add test for guided changing submode after takeoff

Browse Source
This commit is contained in:
Peter Barker 2018-07-30 21:19:14 +10:00 committed by Peter Barker
parent 56a4cd43d4
commit 20c1c4c684
2 changed files with 166 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

147
Tools/autotest/arducopter.py
View File

@ -153,22 +153,70 @@ class AutoTestCopter(AutoTest):
if self.copy_tlog:
shutil.copy(self.logfile, self.buildlog)
def run_cmd(self,
command,
p1,
p2,
p3,
p4,
p5,
p6,
p7,
want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
self.mav.mav.command_long_send(1,
1,
command,
1, # confirmation
p1,
p2,
p3,
p4,
p5,
p6,
p7)
while True:
m = self.mav.recv_match(type='COMMAND_ACK', blocking=True)
print("m: %s" % str(m))
if m.command == command:
if m.result != mavutil.mavlink.MAV_RESULT_ACCEPTED:
raise ValueError()
break
def user_takeoff(self, alt_min=30):
'''takeoff using mavlink takeoff command'''
self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
0, # param1
0, # param2
0, # param3
0, # param4
0, # param5
0, # param6
alt_min # param7
)
self.progress("Ran command")
self.wait_for_alt(alt_min)
def takeoff(self, alt_min=30, takeoff_throttle=1700, arm=False):
"""Takeoff get to 30m altitude."""
self.progress("TAKEOFF")
self.mavproxy.send('switch 6\n') # stabilize mode
self.wait_mode('STABILIZE')
if arm:
self.set_rc(3, 1000)
self.arm_vehicle()
self.set_rc(3, takeoff_throttle)
self.wait_for_alt(alt_min=30)
self.hover()
self.progress("TAKEOFF COMPLETE")
def wait_for_alt(self, alt_min=30):
'''wait for altitude to be reached'''
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
alt = m.relative_alt / 1000.0 # mm -> m
if alt < alt_min:
self.wait_altitude(alt_min,
(alt_min + 5),
relative=True)
self.hover()
self.progress("TAKEOFF COMPLETE")
def land(self, timeout=60):
"""Land the quad."""
@ -1223,6 +1271,98 @@ class AutoTestCopter(AutoTest):
if ex is not None:
raise ex
def guided_achieve_heading(self, heading):
tstart = self.get_sim_time()
self.run_cmd(mavutil.mavlink.MAV_CMD_CONDITION_YAW,
heading, # target angle
10, # degrees/second
1, # -1 is counter-clockwise, 1 clockwise
0, # 1 for relative, 0 for absolute
0, # p5
0, # p6
0, # p7
)
while True:
if self.get_sim_time() - tstart > 200:
raise NotAchievedException()
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
self.progress("heading=%f want=%f" % (m.heading, heading))
if m.heading == heading:
return
def fly_guided_change_submode(self):
'''Ensure we can move around in guided after a takeoff command'''
self.context_push();
ex = None
try:
'''start by disabling GCS failsafe, otherwise we immediately disarm
due to (apparently) not receiving traffic from the GCS for
too long. This is probably a function of --speedup'''
self.set_parameter("FS_GCS_ENABLE", 0)
self.mavproxy.send('mode guided\n') # stabilize mode
self.wait_mode('GUIDED')
self.wait_ready_to_arm()
self.arm_vehicle()
self.user_takeoff(alt_min=10)
startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True)
'''yaw through absolute angles using MAV_CMD_CONDITION_YAW'''
self.guided_achieve_heading(45)
self.guided_achieve_heading(135)
'''move the vehicle using set_position_target_global_int'''
tstart = self.get_sim_time()
while True:
if self.get_sim_time() - tstart > 200:
raise NotAchievedException()
# send a position-control command
self.mav.mav.set_position_target_global_int_send(
0, # timestamp
1, # target system_id
1, # target component id
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
0b1111111111111000, # mask specifying use-only-lat-lon-alt
5, # lat
5, # lon
10, # alt
0, # vx
0, # vy
0, # vz
0, # afx
0, # afy
0, # afz
0, # yaw
0, # yawrate
)
pos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True)
delta = self.get_distance_int(startpos, pos)
self.progress("delta=%f (want >10)" % delta)
if delta > 10:
break
self.progress("Landing")
self.mavproxy.send('switch 2\n') # land mode
self.wait_mode('LAND')
self.mav.motors_disarmed_wait()
except Exception as e:
self.progress("Exception caught")
ex = e
self.context_pop();
self.set_rc(3, 1000)
self.reboot_sitl()
if ex is not None:
raise ex
def autotest(self):
"""Autotest ArduCopter in SITL."""
if not self.hasInit:
@ -1237,6 +1377,9 @@ class AutoTestCopter(AutoTest):
self.progress("Setting up RC parameters")
self.set_rc_default()
self.set_rc(3, 1000)
self.fly_guided_change_submode()
self.progress("Waiting for location")
self.homeloc = self.mav.location()
self.progress("Home location: %s" % self.homeloc)

22
Tools/autotest/common.py
View File

@ -419,7 +419,26 @@ class AutoTest(ABC):
def get_distance(loc1, loc2):
"""Get ground distance between two locations."""
dlat = loc2.lat - loc1.lat
dlong = loc2.lng - loc1.lng
try:
dlong = loc2.lng - loc1.lng
except AttributeError:
dlong = loc2.lon - loc1.lon
return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5
@staticmethod
def get_distance_int(loc1, loc2):
"""Get ground distance between two locations in the normal "int" form
- lat/lon multiplied by 1e7"""
dlat = loc2.lat - loc1.lat
try:
dlong = loc2.lng - loc1.lng
except AttributeError:
dlong = loc2.lon - loc1.lon
dlat /= 10000000.0
dlong /= 10000000.0
return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5
@staticmethod
@ -724,6 +743,7 @@ class AutoTest(ABC):
def wait_ready_to_arm(self, timeout=None):
# wait for EKF checks to pass
self.progress("Waiting reading for arm")
return self.wait_ekf_happy(timeout=timeout)
def wait_ekf_happy(self, timeout=30):