2017-02-21 13:32:26 -04:00
|
|
|
#!/usr/bin/env python
|
|
|
|
|
|
|
|
# Fly ArduCopter in SITL
|
2016-11-08 07:06:05 -04:00
|
|
|
from __future__ import print_function
|
2016-07-31 07:22:06 -03:00
|
|
|
import math
|
|
|
|
import os
|
|
|
|
import shutil
|
2018-06-29 07:38:10 -03:00
|
|
|
import time
|
2018-07-15 20:54:00 -03:00
|
|
|
import traceback
|
2016-07-31 07:22:06 -03:00
|
|
|
|
|
|
|
import pexpect
|
2018-03-14 08:08:53 -03:00
|
|
|
from pymavlink import mavutil
|
2018-10-12 18:20:13 -03:00
|
|
|
from pymavlink import mavextra
|
2016-07-31 07:22:06 -03:00
|
|
|
|
2018-07-15 20:54:00 -03:00
|
|
|
from pysim import util, rotmat
|
|
|
|
|
2018-07-31 06:50:02 -03:00
|
|
|
from common import AutoTest
|
|
|
|
from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException
|
2018-03-14 08:08:53 -03:00
|
|
|
|
2011-10-31 04:29:52 -03:00
|
|
|
# get location of scripts
|
2016-07-31 07:22:06 -03:00
|
|
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
2019-02-15 20:46:33 -04:00
|
|
|
SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 584, 270)
|
|
|
|
SITL_START_LOCATION_AVC = mavutil.location(40.072842, -105.230575, 1586, 0)
|
2011-10-30 23:50:34 -03:00
|
|
|
|
2018-07-31 06:50:02 -03:00
|
|
|
|
2017-08-16 10:55:21 -03:00
|
|
|
# Flight mode switch positions are set-up in arducopter.param to be
|
|
|
|
# switch 1 = Circle
|
|
|
|
# switch 2 = Land
|
|
|
|
# switch 3 = RTL
|
|
|
|
# switch 4 = Auto
|
|
|
|
# switch 5 = Loiter
|
|
|
|
# switch 6 = Stabilize
|
2011-11-09 01:43:18 -04:00
|
|
|
|
2011-10-30 23:50:34 -03:00
|
|
|
|
2018-03-14 08:08:53 -03:00
|
|
|
class AutoTestCopter(AutoTest):
|
2018-10-10 10:06:01 -03:00
|
|
|
@staticmethod
|
|
|
|
def get_not_armable_mode_list():
|
|
|
|
return ["AUTO", "AUTOTUNE", "BRAKE", "CIRCLE", "FLIP", "LAND", "RTL", "SMART_RTL", "AVOID_ADSB", "FOLLOW"]
|
|
|
|
|
|
|
|
@staticmethod
|
|
|
|
def get_not_disarmed_settable_modes_list():
|
|
|
|
return ["FLIP", "AUTOTUNE"]
|
|
|
|
|
|
|
|
@staticmethod
|
|
|
|
def get_no_position_not_settable_modes_list():
|
|
|
|
return []
|
|
|
|
|
|
|
|
@staticmethod
|
|
|
|
def get_position_armable_modes_list():
|
|
|
|
return ["DRIFT", "GUIDED", "LOITER", "POSHOLD", "THROW"]
|
|
|
|
|
|
|
|
@staticmethod
|
|
|
|
def get_normal_armable_modes_list():
|
|
|
|
return ["ACRO", "ALT_HOLD", "SPORT", "STABILIZE", "GUIDED_NOGPS"]
|
2019-03-09 00:20:36 -04:00
|
|
|
|
|
|
|
def log_name(self):
|
|
|
|
return "ArduCopter"
|
|
|
|
|
|
|
|
def test_filepath(self):
|
|
|
|
return os.path.realpath(__file__)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2019-02-15 20:46:33 -04:00
|
|
|
def sitl_start_location(self):
|
|
|
|
return SITL_START_LOCATION
|
|
|
|
|
2018-03-15 08:31:19 -03:00
|
|
|
def mavproxy_options(self):
|
|
|
|
ret = super(AutoTestCopter, self).mavproxy_options()
|
|
|
|
if self.frame != 'heli':
|
|
|
|
ret.append('--quadcopter')
|
|
|
|
return ret
|
|
|
|
|
|
|
|
def sitl_streamrate(self):
|
|
|
|
return 5
|
|
|
|
|
2018-03-15 19:56:37 -03:00
|
|
|
def vehicleinfo_key(self):
|
|
|
|
return 'ArduCopter'
|
|
|
|
|
2019-03-09 00:20:36 -04:00
|
|
|
def default_frame(self):
|
|
|
|
return "+"
|
2018-12-13 00:45:43 -04:00
|
|
|
|
2019-03-09 00:20:36 -04:00
|
|
|
def uses_vicon(self):
|
|
|
|
return True
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
def close(self):
|
2018-03-15 08:54:34 -03:00
|
|
|
super(AutoTestCopter, self).close()
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-03-14 08:08:53 -03:00
|
|
|
# [2014/05/07] FC Because I'm doing a cross machine build
|
|
|
|
# (source is on host, build is on guest VM) I cannot hard link
|
2018-03-05 11:14:34 -04:00
|
|
|
# This flag tells me that I need to copy the data out
|
|
|
|
if self.copy_tlog:
|
|
|
|
shutil.copy(self.logfile, self.buildlog)
|
|
|
|
|
2018-10-10 10:07:21 -03:00
|
|
|
def is_copter(self):
|
|
|
|
return True
|
|
|
|
|
2019-03-26 09:17:11 -03:00
|
|
|
def get_stick_arming_channel(self):
|
2018-10-10 10:07:21 -03:00
|
|
|
return int(self.get_parameter("RCMAP_YAW"))
|
|
|
|
|
|
|
|
def get_disarm_delay(self):
|
|
|
|
return int(self.get_parameter("DISARM_DELAY"))
|
|
|
|
|
|
|
|
def set_autodisarm_delay(self, delay):
|
|
|
|
self.set_parameter("DISARM_DELAY", delay)
|
|
|
|
|
2018-07-30 08:19:14 -03:00
|
|
|
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
|
2018-11-22 23:51:58 -04:00
|
|
|
)
|
2018-07-30 08:19:14 -03:00
|
|
|
self.progress("Ran command")
|
|
|
|
self.wait_for_alt(alt_min)
|
|
|
|
|
2018-10-02 19:28:21 -03:00
|
|
|
def takeoff(self,
|
|
|
|
alt_min=30,
|
|
|
|
takeoff_throttle=1700,
|
2018-12-17 02:42:21 -04:00
|
|
|
require_absolute=True,
|
2019-02-02 00:19:18 -04:00
|
|
|
mode="STABILIZE",
|
|
|
|
timeout=30):
|
2018-03-05 11:14:34 -04:00
|
|
|
"""Takeoff get to 30m altitude."""
|
2018-07-30 08:19:14 -03:00
|
|
|
self.progress("TAKEOFF")
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode(mode)
|
2018-08-23 07:24:34 -03:00
|
|
|
if not self.armed():
|
2018-10-02 19:28:21 -03:00
|
|
|
self.wait_ready_to_arm(require_absolute=require_absolute)
|
2018-10-10 10:07:21 -03:00
|
|
|
self.zero_throttle()
|
2018-05-10 04:15:38 -03:00
|
|
|
self.arm_vehicle()
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(3, takeoff_throttle)
|
2019-02-02 00:19:18 -04:00
|
|
|
self.wait_for_alt(alt_min=alt_min, timeout=timeout)
|
2018-07-30 08:19:14 -03:00
|
|
|
self.hover()
|
|
|
|
self.progress("TAKEOFF COMPLETE")
|
|
|
|
|
2019-02-02 00:19:18 -04:00
|
|
|
def wait_for_alt(self, alt_min=30, timeout=30):
|
2018-07-31 06:50:02 -03:00
|
|
|
"""Wait for altitude to be reached."""
|
2018-05-29 05:05:56 -03:00
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
alt = m.relative_alt / 1000.0 # mm -> m
|
|
|
|
if alt < alt_min:
|
2018-07-31 06:53:36 -03:00
|
|
|
self.wait_altitude(alt_min - 1,
|
2018-05-29 05:05:56 -03:00
|
|
|
(alt_min + 5),
|
2019-02-02 00:19:18 -04:00
|
|
|
relative=True,
|
|
|
|
timeout=timeout)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2019-04-08 23:26:18 -03:00
|
|
|
def land_and_disarm(self, timeout=60):
|
2018-03-05 11:14:34 -04:00
|
|
|
"""Land the quad."""
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("STARTING LANDING")
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode("LAND")
|
2018-07-31 06:47:49 -03:00
|
|
|
self.wait_altitude(-5, 1, relative=True, timeout=timeout)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.progress("LANDING: ok!")
|
2019-04-08 23:26:18 -03:00
|
|
|
self.mav.motors_disarmed_wait()
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
def hover(self, hover_throttle=1500):
|
|
|
|
self.set_rc(3, hover_throttle)
|
|
|
|
|
2018-03-14 08:08:53 -03:00
|
|
|
# loiter - fly south west, then loiter within 5m position and altitude
|
2018-03-05 11:14:34 -04:00
|
|
|
def loiter(self, holdtime=10, maxaltchange=5, maxdistchange=5):
|
|
|
|
"""Hold loiter position."""
|
2018-12-17 02:42:21 -04:00
|
|
|
self.takeoff(10, mode="LOITER")
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# first aim south east
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("turn south east")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1580)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_heading(170)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1500)
|
|
|
|
|
|
|
|
# fly south east 50m
|
|
|
|
self.set_rc(2, 1100)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_distance(50)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1500)
|
|
|
|
|
|
|
|
# wait for copter to slow moving
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_groundspeed(0, 2)
|
2013-05-02 23:22:15 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
start_altitude = m.alt
|
|
|
|
start = self.mav.location()
|
|
|
|
tstart = self.get_sim_time()
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Holding loiter at %u meters for %u seconds" %
|
|
|
|
(start_altitude, holdtime))
|
2019-03-07 18:34:09 -04:00
|
|
|
while self.get_sim_time_cached() < tstart + holdtime:
|
2018-03-05 11:14:34 -04:00
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
pos = self.mav.location()
|
|
|
|
delta = self.get_distance(start, pos)
|
|
|
|
alt_delta = math.fabs(m.alt - start_altitude)
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
|
2018-03-05 11:14:34 -04:00
|
|
|
if alt_delta > maxaltchange:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise NotAchievedException(
|
|
|
|
"Loiter alt shifted %u meters (> limit %u)" %
|
|
|
|
(alt_delta, maxaltchange))
|
2018-03-05 11:14:34 -04:00
|
|
|
if delta > maxdistchange:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise NotAchievedException(
|
|
|
|
"Loiter shifted %u meters (> limit of %u)" %
|
2018-11-22 23:51:58 -04:00
|
|
|
(delta, maxdistchange))
|
2018-04-27 15:21:53 -03:00
|
|
|
self.progress("Loiter OK for %u seconds" % holdtime)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
self.progress("Climb to 30m")
|
|
|
|
self.change_alt(30)
|
|
|
|
|
|
|
|
self.progress("Descend to 20m")
|
|
|
|
self.change_alt(20)
|
|
|
|
|
|
|
|
self.do_RTL()
|
|
|
|
|
2019-03-05 21:29:32 -04:00
|
|
|
def watch_altitude_maintained(self, min_alt, max_alt, timeout=10):
|
|
|
|
'''watch alt, relative alt must remain between min_alt and max_alt'''
|
|
|
|
tstart = self.get_sim_time_cached()
|
|
|
|
while True:
|
|
|
|
if self.get_sim_time_cached() - tstart > timeout:
|
|
|
|
return
|
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
if m.alt <= min_alt:
|
|
|
|
raise NotAchievedException("Altitude not maintained: want >%f got=%f" % (min_alt, m.alt))
|
|
|
|
|
|
|
|
def test_mode_ALT_HOLD(self):
|
|
|
|
self.takeoff(10, mode="ALT_HOLD")
|
|
|
|
self.watch_altitude_maintained(9, 11, timeout=5)
|
|
|
|
# feed in full elevator and aileron input and make sure we
|
|
|
|
# retain altitude:
|
|
|
|
self.set_rc(1, 1000)
|
|
|
|
self.set_rc(2, 1000)
|
|
|
|
self.watch_altitude_maintained(9, 11, timeout=5)
|
|
|
|
self.set_rc(1, 1500)
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
self.do_RTL()
|
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
def change_alt(self, alt_min, climb_throttle=1920, descend_throttle=1080):
|
|
|
|
"""Change altitude."""
|
2018-05-29 05:05:56 -03:00
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
alt = m.relative_alt / 1000.0 # mm -> m
|
|
|
|
if alt < alt_min:
|
|
|
|
self.progress("Rise to alt:%u from %u" % (alt_min, alt))
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(3, climb_throttle)
|
2018-05-29 05:05:56 -03:00
|
|
|
self.wait_altitude(alt_min, (alt_min + 5), relative=True)
|
2018-03-05 11:14:34 -04:00
|
|
|
else:
|
2018-05-29 05:05:56 -03:00
|
|
|
self.progress("Lower to alt:%u from %u" % (alt_min, alt))
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(3, descend_throttle)
|
2018-05-29 05:05:56 -03:00
|
|
|
self.wait_altitude((alt_min - 5), alt_min, relative=True)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.hover()
|
|
|
|
|
|
|
|
#################################################
|
|
|
|
# TESTS FLY
|
|
|
|
#################################################
|
|
|
|
|
2018-03-15 08:31:19 -03:00
|
|
|
# fly a square in alt_hold mode
|
2018-03-05 11:14:34 -04:00
|
|
|
def fly_square(self, side=50, timeout=300):
|
2018-12-17 02:42:21 -04:00
|
|
|
|
|
|
|
self.clear_mission()
|
|
|
|
|
|
|
|
self.takeoff(10)
|
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
"""Fly a square, flying N then E ."""
|
|
|
|
tstart = self.get_sim_time()
|
2013-05-02 23:22:15 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# ensure all sticks in the middle
|
|
|
|
self.set_rc(1, 1500)
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
self.set_rc(4, 1500)
|
|
|
|
|
|
|
|
# switch to loiter mode temporarily to stop us from rising
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode('LOITER')
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# first aim north
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("turn right towards north")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1580)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_heading(10)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1500)
|
|
|
|
|
|
|
|
# save bottom left corner of box as waypoint
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Save WP 1 & 2")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.save_wp()
|
|
|
|
|
|
|
|
# switch back to stabilize mode
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode('STABILIZE')
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-07-17 00:36:46 -03:00
|
|
|
# increase throttle a bit because we're about to pitch:
|
|
|
|
self.set_rc(3, 1525)
|
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# pitch forward to fly north
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Going north %u meters" % side)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1300)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_distance(side)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1500)
|
2013-10-02 05:45:10 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# save top left corner of square as waypoint
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Save WP 3")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.save_wp()
|
2016-07-31 07:22:06 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# roll right to fly east
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Going east %u meters" % side)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(1, 1700)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_distance(side)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(1, 1500)
|
2013-05-19 03:32:54 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# save top right corner of square as waypoint
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Save WP 4")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.save_wp()
|
2013-05-19 03:32:54 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# pitch back to fly south
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Going south %u meters" % side)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1700)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_distance(side)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1500)
|
2013-05-19 03:32:54 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# save bottom right corner of square as waypoint
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Save WP 5")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.save_wp()
|
2013-05-19 03:32:54 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# roll left to fly west
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Going west %u meters" % side)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(1, 1300)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_distance(side)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(1, 1500)
|
|
|
|
|
|
|
|
# save bottom left corner of square (should be near home) as waypoint
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Save WP 6")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.save_wp()
|
|
|
|
|
2018-07-17 00:36:46 -03:00
|
|
|
# reduce throttle again
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# descend to 10m
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Descend to 10m in Loiter")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.mavproxy.send('switch 5\n') # loiter mode
|
|
|
|
self.wait_mode('LOITER')
|
|
|
|
self.set_rc(3, 1300)
|
|
|
|
time_left = timeout - (self.get_sim_time() - tstart)
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("timeleft = %u" % time_left)
|
2018-03-05 11:14:34 -04:00
|
|
|
if time_left < 20:
|
|
|
|
time_left = 20
|
2018-05-29 05:05:56 -03:00
|
|
|
self.wait_altitude(-10, 10, time_left, relative=True)
|
2018-08-20 12:47:58 -03:00
|
|
|
self.set_rc(3, 1500)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.save_wp()
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
# save the stored mission to file
|
|
|
|
num_wp = self.save_mission_to_file(os.path.join(testdir,
|
|
|
|
"ch7_mission.txt"))
|
|
|
|
if not num_wp:
|
|
|
|
self.fail_list.append("save_mission_to_file")
|
|
|
|
self.progress("save_mission_to_file failed")
|
|
|
|
|
|
|
|
self.progress("test: Fly a mission from 1 to %u" % num_wp)
|
|
|
|
self.mavproxy.send('wp set 1\n')
|
|
|
|
self.change_mode('AUTO')
|
|
|
|
self.wait_waypoint(0, num_wp-1, timeout=500)
|
|
|
|
self.progress("test: MISSION COMPLETE: passed!")
|
|
|
|
self.change_mode('LAND')
|
|
|
|
self.mav.motors_disarmed_wait()
|
|
|
|
|
2018-05-10 04:15:38 -03:00
|
|
|
# enter RTL mode and wait for the vehicle to disarm
|
2018-12-17 02:42:21 -04:00
|
|
|
def do_RTL(self, timeout=250):
|
2018-03-05 11:14:34 -04:00
|
|
|
"""Return, land."""
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode("RTL")
|
2018-05-15 03:41:15 -03:00
|
|
|
self.set_rc(3, 1500)
|
2018-03-05 11:14:34 -04:00
|
|
|
tstart = self.get_sim_time()
|
2019-03-07 18:34:09 -04:00
|
|
|
while self.get_sim_time_cached() < tstart + timeout:
|
2018-05-29 05:05:56 -03:00
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
alt = m.relative_alt / 1000.0 # mm -> m
|
2019-03-07 18:28:03 -04:00
|
|
|
home_distance = self.distance_to_home(use_cached_home=True)
|
2018-05-10 04:15:38 -03:00
|
|
|
home = ""
|
2018-05-29 05:05:56 -03:00
|
|
|
if alt <= 1 and home_distance < 10:
|
2018-05-10 04:15:38 -03:00
|
|
|
home = "HOME"
|
2018-07-02 00:22:54 -03:00
|
|
|
self.progress("Alt: %.02f HomeDist: %.02f %s" %
|
2018-05-29 05:05:56 -03:00
|
|
|
(alt, home_distance, home))
|
2018-05-10 04:15:38 -03:00
|
|
|
# our post-condition is that we are disarmed:
|
|
|
|
if not self.armed():
|
2018-10-17 23:55:16 -03:00
|
|
|
if home == "":
|
|
|
|
raise NotAchievedException("Did not get home")
|
|
|
|
# success!
|
2018-04-27 15:21:53 -03:00
|
|
|
return
|
2018-10-17 23:55:16 -03:00
|
|
|
raise AutoTestTimeoutException("Did not get home and disarm")
|
2017-12-08 01:41:17 -04:00
|
|
|
|
2018-07-24 23:57:08 -03:00
|
|
|
def fly_loiter_to_alt(self):
|
|
|
|
"""loiter to alt"""
|
|
|
|
|
|
|
|
self.context_push()
|
|
|
|
|
|
|
|
ex = None
|
|
|
|
try:
|
|
|
|
|
|
|
|
self.set_parameter("PLND_ENABLED", 1)
|
|
|
|
self.fetch_parameters()
|
|
|
|
self.set_parameter("PLND_TYPE", 4)
|
|
|
|
|
2019-07-10 10:57:46 -03:00
|
|
|
self.set_analog_rangefinder_parameters()
|
2018-07-24 23:57:08 -03:00
|
|
|
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
2018-12-20 08:05:30 -04:00
|
|
|
self.load_mission("copter_loiter_to_alt.txt")
|
2018-07-24 23:57:08 -03:00
|
|
|
|
|
|
|
self.mavproxy.send('switch 5\n')
|
|
|
|
self.wait_mode('LOITER')
|
|
|
|
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
|
|
|
|
self.arm_vehicle()
|
|
|
|
|
|
|
|
self.mavproxy.send('mode auto\n')
|
|
|
|
self.wait_mode('AUTO')
|
|
|
|
|
|
|
|
self.set_rc(3, 1550)
|
|
|
|
|
|
|
|
self.wait_current_waypoint(2)
|
|
|
|
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
|
|
|
|
self.mav.motors_disarmed_wait()
|
|
|
|
except Exception as e:
|
|
|
|
ex = e
|
|
|
|
|
|
|
|
self.context_pop()
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
def fly_throttle_failsafe(self, side=60, timeout=180):
|
|
|
|
"""Fly east, Failsafe, return, land."""
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
self.takeoff(10)
|
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# switch to loiter mode temporarily to stop us from rising
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode('LOITER')
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# first aim east
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("turn east")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1580)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_heading(135)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1500)
|
|
|
|
|
|
|
|
# raise throttle slightly to avoid hitting the ground
|
2018-08-29 08:44:32 -03:00
|
|
|
pos = self.mav.location(relative_alt=True)
|
|
|
|
if pos.alt > 25:
|
|
|
|
self.set_rc(3, 1300)
|
|
|
|
self.wait_altitude(20, 25, relative=True)
|
|
|
|
if pos.alt < 20:
|
|
|
|
self.set_rc(3, 1800)
|
|
|
|
self.wait_altitude(20, 25, relative=True)
|
|
|
|
self.hover()
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode("STABILIZE")
|
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
self.hover()
|
|
|
|
|
|
|
|
# fly east 60 meters
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("# Going forward %u meters" % side)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1350)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_distance(side, 5, 60)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1500)
|
|
|
|
|
|
|
|
# pull throttle low
|
2018-12-17 02:42:21 -04:00
|
|
|
self.progress("# Enter Failsafe by setting very low throttle")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(3, 900)
|
|
|
|
|
|
|
|
tstart = self.get_sim_time()
|
2018-12-17 02:42:21 -04:00
|
|
|
homeloc = self.poll_home_position()
|
|
|
|
home = mavutil.location(homeloc.latitude/1e7,
|
|
|
|
homeloc.longitude/1e7,
|
|
|
|
homeloc.altitude/1e3,
|
|
|
|
0)
|
2019-03-07 18:34:09 -04:00
|
|
|
while self.get_sim_time_cached() < tstart + timeout:
|
2018-05-29 05:05:56 -03:00
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
2018-08-20 12:47:58 -03:00
|
|
|
alt = m.relative_alt / 1000.0 # mm -> m
|
2018-03-05 11:14:34 -04:00
|
|
|
pos = self.mav.location()
|
2018-12-17 02:42:21 -04:00
|
|
|
home_distance = self.get_distance(home, pos )
|
2018-07-02 00:22:54 -03:00
|
|
|
self.progress("Alt: %.02f HomeDist: %.0f" % (alt, home_distance))
|
2018-03-05 11:14:34 -04:00
|
|
|
# check if we've reached home
|
2018-05-29 05:05:56 -03:00
|
|
|
if alt <= 1 and home_distance < 10:
|
2018-03-05 11:14:34 -04:00
|
|
|
# reduce throttle
|
|
|
|
self.set_rc(3, 1100)
|
|
|
|
# switch back to stabilize
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode("LAND")
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Waiting for disarm")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.mav.motors_disarmed_wait()
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Reached failsafe home OK")
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode('STABILIZE')
|
2018-10-10 10:07:21 -03:00
|
|
|
self.zero_throttle()
|
2018-04-27 15:21:53 -03:00
|
|
|
return
|
2018-12-17 02:42:21 -04:00
|
|
|
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
2018-10-17 23:55:16 -03:00
|
|
|
raise AutoTestTimeoutException(
|
2018-12-17 02:42:21 -04:00
|
|
|
("Failed to land and disarm on failsafe RTL - "
|
2018-10-17 23:55:16 -03:00
|
|
|
"timed out after %u seconds" % timeout))
|
2017-12-08 01:41:17 -04:00
|
|
|
|
2018-07-31 06:47:49 -03:00
|
|
|
def fly_battery_failsafe(self, timeout=300):
|
2018-12-17 02:42:21 -04:00
|
|
|
self.takeoff(10, mode='LOITER')
|
2013-06-01 06:20:57 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# enable battery failsafe
|
2018-03-20 01:22:06 -03:00
|
|
|
self.set_parameter('BATT_FS_LOW_ACT', 1)
|
2013-06-01 06:20:57 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# trigger low voltage
|
|
|
|
self.set_parameter('SIM_BATT_VOLTAGE', 10)
|
2013-06-01 06:20:57 -03:00
|
|
|
|
2018-04-27 15:21:53 -03:00
|
|
|
# wait for LAND mode. If unsuccessful an exception will be raised
|
2018-07-31 06:47:49 -03:00
|
|
|
self.wait_mode('LAND', timeout=timeout)
|
2017-08-16 10:55:21 -03:00
|
|
|
|
2018-04-27 15:21:53 -03:00
|
|
|
self.progress("Successfully entered LAND after battery failsafe")
|
2019-02-26 00:29:12 -04:00
|
|
|
self.mav.motors_disarmed_wait()
|
2018-08-20 12:47:58 -03:00
|
|
|
self.reboot_sitl()
|
2016-07-31 07:22:06 -03:00
|
|
|
|
2018-03-14 08:08:53 -03:00
|
|
|
# fly_stability_patch - fly south, then hold loiter within 5m
|
|
|
|
# position and altitude and reduce 1 motor to 60% efficiency
|
|
|
|
def fly_stability_patch(self,
|
|
|
|
holdtime=30,
|
|
|
|
maxaltchange=5,
|
|
|
|
maxdistchange=10):
|
2018-12-17 02:42:21 -04:00
|
|
|
|
|
|
|
self.takeoff(10, mode="LOITER")
|
2014-04-07 10:25:38 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# first south
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("turn south")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1580)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_heading(180)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1500)
|
2014-04-07 10:25:38 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# fly west 80m
|
|
|
|
self.set_rc(2, 1100)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_distance(80)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1500)
|
2014-04-07 10:25:38 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# wait for copter to slow moving
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_groundspeed(0, 2)
|
2014-04-07 10:25:38 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
start_altitude = m.alt
|
|
|
|
start = self.mav.location()
|
|
|
|
tstart = self.get_sim_time()
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Holding loiter at %u meters for %u seconds" %
|
|
|
|
(start_altitude, holdtime))
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# cut motor 1 to 55% efficiency
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Cutting motor 1 to 60% efficiency")
|
2018-08-06 09:30:42 -03:00
|
|
|
self.set_parameter("SIM_ENGINE_MUL", 0.60)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2019-03-07 18:34:09 -04:00
|
|
|
while self.get_sim_time_cached() < tstart + holdtime:
|
2018-03-05 11:14:34 -04:00
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
pos = self.mav.location()
|
|
|
|
delta = self.get_distance(start, pos)
|
|
|
|
alt_delta = math.fabs(m.alt - start_altitude)
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
|
2018-03-05 11:14:34 -04:00
|
|
|
if alt_delta > maxaltchange:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise NotAchievedException(
|
|
|
|
"Loiter alt shifted %u meters (> limit %u)" %
|
|
|
|
(alt_delta, maxaltchange))
|
2018-03-05 11:14:34 -04:00
|
|
|
if delta > maxdistchange:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise NotAchievedException(
|
|
|
|
("Loiter shifted %u meters (> limit of %u)" %
|
|
|
|
(delta, maxdistchange)))
|
2016-07-31 07:22:06 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# restore motor 1 to 100% efficiency
|
2018-08-06 09:30:42 -03:00
|
|
|
self.set_parameter("SIM_ENGINE_MUL", 1.0)
|
2016-07-31 07:22:06 -03:00
|
|
|
|
2018-05-09 00:32:23 -03:00
|
|
|
self.progress("Stability patch and Loiter OK for %us" % holdtime)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
self.progress("RTL after stab patch")
|
|
|
|
self.do_RTL()
|
|
|
|
|
2019-03-05 23:04:46 -04:00
|
|
|
def debug_arming_issue(self):
|
|
|
|
while True:
|
|
|
|
self.send_mavlink_arm_command()
|
|
|
|
m = self.mav.recv_match(blocking=True, timeout=1)
|
|
|
|
if m is None:
|
|
|
|
continue
|
|
|
|
if m.get_type() in ["STATUSTEXT", "COMMAND_ACK"]:
|
|
|
|
print("Got: %s" % str(m))
|
|
|
|
if self.mav.motors_armed():
|
|
|
|
self.progress("Armed")
|
|
|
|
return
|
|
|
|
|
2019-06-06 21:57:13 -03:00
|
|
|
def wait_distance_from_home(self, distance_min, distance_max, timeout=10):
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while True:
|
|
|
|
if self.get_sim_time() - tstart > timeout:
|
|
|
|
raise NotAchievedException("Did not achieve distance from home")
|
|
|
|
distance = self.distance_to_home(use_cached_home=True)
|
|
|
|
self.progress("Distance from home: now=%f %f<want<%f" %
|
|
|
|
(distance, distance_min, distance_max))
|
|
|
|
if distance >= distance_min and distance <= distance_max:
|
|
|
|
return
|
|
|
|
|
|
|
|
# fly_fence_test - fly east until you hit the horizontal circular fence
|
|
|
|
avoid_behave_slide = 0
|
|
|
|
def fly_fence_avoid_test_radius_check(self, timeout=180, avoid_behave=avoid_behave_slide):
|
|
|
|
using_mode = "LOITER" # must be something which adjusts velocity!
|
|
|
|
self.change_mode(using_mode)
|
|
|
|
self.set_parameter("FENCE_ENABLE", 1) # fence
|
|
|
|
self.set_parameter("FENCE_TYPE", 2) # circle
|
|
|
|
fence_radius = 15
|
|
|
|
self.set_parameter("FENCE_RADIUS", fence_radius)
|
|
|
|
fence_margin = 3
|
|
|
|
self.set_parameter("FENCE_MARGIN", fence_margin)
|
|
|
|
self.set_parameter("AVOID_ENABLE", 1)
|
|
|
|
self.set_parameter("AVOID_BEHAVE", avoid_behave)
|
|
|
|
self.set_parameter("RC10_OPTION", 40) # avoid-enable
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.set_rc(10, 2000)
|
|
|
|
home_distance = self.distance_to_home(use_cached_home=True)
|
|
|
|
if home_distance > 5:
|
|
|
|
raise PreconditionFailedException("Expected to be within 5m of home")
|
|
|
|
self.zero_throttle()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.set_rc(3, 1700)
|
|
|
|
self.wait_altitude(10, 100, relative=True)
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
self.set_rc(2, 1400)
|
|
|
|
self.wait_distance_from_home(12, 20)
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
push_time = 70 # push against barrier for 60 seconds
|
|
|
|
failed_max = False
|
|
|
|
failed_min = False
|
|
|
|
while True:
|
|
|
|
if self.get_sim_time() - tstart > push_time:
|
|
|
|
self.progress("Push time up")
|
|
|
|
break
|
|
|
|
# make sure we don't RTL:
|
|
|
|
if not self.mode_is(using_mode):
|
|
|
|
raise NotAchievedException("Changed mode away from %s" % using_mode)
|
|
|
|
distance = self.distance_to_home(use_cached_home=True)
|
|
|
|
inner_radius = fence_radius - fence_margin
|
|
|
|
want_min = inner_radius - 1 # allow 1m either way
|
|
|
|
want_max = inner_radius + 1 # allow 1m either way
|
|
|
|
self.progress("Push: distance=%f %f<want<%f" %
|
|
|
|
(distance, want_min, want_max))
|
|
|
|
if distance < want_min:
|
|
|
|
if failed_min is False:
|
|
|
|
self.progress("Failed min")
|
|
|
|
failed_min = True
|
|
|
|
if distance > want_max:
|
|
|
|
if failed_max is False:
|
|
|
|
self.progress("Failed max")
|
|
|
|
failed_max = True
|
|
|
|
if failed_min and failed_max:
|
|
|
|
raise NotAchievedException("Failed both min and max checks. Clever")
|
|
|
|
if failed_min:
|
|
|
|
raise NotAchievedException("Failed min")
|
|
|
|
if failed_max:
|
|
|
|
raise NotAchievedException("Failed max")
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
self.do_RTL()
|
|
|
|
|
|
|
|
def fly_fence_avoid_test(self, timeout=180):
|
|
|
|
self.fly_fence_avoid_test_radius_check(avoid_behave=1, timeout=timeout)
|
|
|
|
self.fly_fence_avoid_test_radius_check(avoid_behave=0, timeout=timeout)
|
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# fly_fence_test - fly east until you hit the horizontal circular fence
|
|
|
|
def fly_fence_test(self, timeout=180):
|
|
|
|
# enable fence, disable avoidance
|
2018-08-06 09:30:42 -03:00
|
|
|
self.set_parameter("FENCE_ENABLE", 1)
|
|
|
|
self.set_parameter("AVOID_ENABLE", 0)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2019-03-05 23:04:46 -04:00
|
|
|
self.change_mode("LOITER")
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
|
|
|
|
# fence requires home to be set:
|
|
|
|
m = self.poll_home_position()
|
|
|
|
if m is None:
|
|
|
|
raise NotAchievedException("Did not receive HOME_POSITION")
|
|
|
|
self.progress("home: %s" % str(m))
|
|
|
|
|
|
|
|
self.start_subtest("ensure we can't arm if ouside fence")
|
|
|
|
fence = os.path.join(self.mission_directory(),
|
|
|
|
"fence-in-middle-of-nowhere.txt")
|
|
|
|
self.mavproxy.send('fence load %s\n' % fence)
|
|
|
|
self.mavproxy.expect("Loaded 6 geo-fence")
|
|
|
|
self.delay_sim_time(5) # let fence check run so it loads-from-eeprom
|
|
|
|
seen_statustext = False
|
|
|
|
seen_command_ack = False
|
|
|
|
|
|
|
|
tstart = self.get_sim_time_cached()
|
|
|
|
while True:
|
|
|
|
self.send_mavlink_arm_command()
|
|
|
|
if seen_command_ack and seen_statustext:
|
|
|
|
break
|
|
|
|
if self.get_sim_time_cached() - tstart > 5:
|
|
|
|
raise NotAchievedException("Did not see failure-to-arm messages (statustext=%s command_ack=%s" % (seen_statustext, seen_command_ack))
|
|
|
|
m = self.mav.recv_match(blocking=True, timeout=1)
|
|
|
|
if m is None:
|
|
|
|
continue
|
|
|
|
if m.get_type() == "STATUSTEXT":
|
|
|
|
if "vehicle outside fence" in m.text:
|
|
|
|
self.progress("Got: %s" % str(m))
|
|
|
|
seen_statustext = True
|
|
|
|
elif "PreArm" in m.text:
|
|
|
|
self.progress("Got: %s" % str(m))
|
|
|
|
raise NotAchievedException("Unexpected prearm failure (%s)" % m.text)
|
|
|
|
|
|
|
|
if m.get_type() == "COMMAND_ACK":
|
|
|
|
print("Got: %s" % str(m))
|
|
|
|
if m.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM:
|
|
|
|
if m.result != 4:
|
|
|
|
raise NotAchievedException("command-ack says we didn't fail to arm")
|
|
|
|
self.progress("Got: %s" % str(m))
|
|
|
|
seen_command_ack = True
|
|
|
|
if self.mav.motors_armed():
|
|
|
|
raise NotAchievedException("Armed when we shouldn't have")
|
|
|
|
|
|
|
|
self.progress("Failed to arm outside fence (good!)")
|
|
|
|
self.drain_mav()
|
|
|
|
self.mavproxy.send('fence clear\n')
|
|
|
|
self.delay_sim_time(2)
|
|
|
|
self.mavproxy.send('fence list\n')
|
|
|
|
self.mavproxy.expect("No geo-fence points")
|
|
|
|
|
|
|
|
self.start_subtest("Check breach-fence behaviour")
|
|
|
|
self.set_parameter("FENCE_TYPE", 2)
|
|
|
|
self.takeoff(10, mode="LOITER")
|
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# first east
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("turn east")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1580)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_heading(160)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1500)
|
|
|
|
|
2019-03-07 18:28:46 -04:00
|
|
|
fence_radius = self.get_parameter("FENCE_RADIUS")
|
|
|
|
|
|
|
|
self.progress("flying forward (east) until we hit fence")
|
2018-03-05 11:14:34 -04:00
|
|
|
pitching_forward = True
|
|
|
|
self.set_rc(2, 1100)
|
|
|
|
|
2019-03-07 18:28:46 -04:00
|
|
|
self.progress("Waiting for fence breach")
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while not self.mode_is("RTL"):
|
|
|
|
if self.get_sim_time_cached() - tstart > 30:
|
|
|
|
self.NotAchievedException("Did not breach fence")
|
|
|
|
|
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
alt = m.relative_alt / 1000.0 # mm -> m
|
|
|
|
home_distance = self.distance_to_home(use_cached_home=True)
|
|
|
|
self.progress("Alt: %.02f HomeDistance: %.02f (fence radius=%f)" %
|
|
|
|
(alt, home_distance, fence_radius))
|
|
|
|
|
|
|
|
self.progress("Waiting until we get home and disarm")
|
2018-03-05 11:14:34 -04:00
|
|
|
tstart = self.get_sim_time()
|
2019-03-07 18:34:09 -04:00
|
|
|
while self.get_sim_time_cached() < tstart + timeout:
|
2018-05-29 05:05:56 -03:00
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
alt = m.relative_alt / 1000.0 # mm -> m
|
2019-03-07 18:28:03 -04:00
|
|
|
home_distance = self.distance_to_home(use_cached_home=True)
|
2018-07-02 00:22:54 -03:00
|
|
|
self.progress("Alt: %.02f HomeDistance: %.02f" %
|
2018-05-29 05:05:56 -03:00
|
|
|
(alt, home_distance))
|
2018-03-14 08:08:53 -03:00
|
|
|
# recenter pitch sticks once we're home so we don't fly off again
|
2019-03-07 18:28:46 -04:00
|
|
|
if pitching_forward and home_distance < 50:
|
2018-03-05 11:14:34 -04:00
|
|
|
pitching_forward = False
|
2018-09-03 23:53:53 -03:00
|
|
|
self.set_rc(2, 1475)
|
2018-03-05 11:14:34 -04:00
|
|
|
# disable fence
|
2018-08-06 09:30:42 -03:00
|
|
|
self.set_parameter("FENCE_ENABLE", 0)
|
2018-08-29 09:53:55 -03:00
|
|
|
if (alt <= 1 and home_distance < 10) or (not self.armed() and home_distance < 10):
|
2018-03-05 11:14:34 -04:00
|
|
|
# reduce throttle
|
2018-10-10 10:07:21 -03:00
|
|
|
self.zero_throttle()
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode("LAND")
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Waiting for disarm")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.mav.motors_disarmed_wait()
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Reached home OK")
|
2018-10-10 10:07:21 -03:00
|
|
|
self.zero_throttle()
|
2018-04-27 15:21:53 -03:00
|
|
|
return
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
# give we're testing RTL, doing one here probably doesn't make sense
|
2019-03-07 18:28:03 -04:00
|
|
|
home_distance = self.distance_to_home(use_cached_home=True)
|
2018-10-17 23:55:16 -03:00
|
|
|
raise AutoTestTimeoutException(
|
2019-03-07 18:28:46 -04:00
|
|
|
"Fence test failed to reach home (%fm distance) - "
|
|
|
|
"timed out after %u seconds" % (home_distance, timeout,))
|
2017-08-16 10:55:21 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# fly_alt_fence_test - fly up until you hit the fence
|
2018-07-31 06:47:49 -03:00
|
|
|
def fly_alt_max_fence_test(self):
|
2018-12-17 02:42:21 -04:00
|
|
|
self.takeoff(10, mode="LOITER")
|
2018-03-05 11:14:34 -04:00
|
|
|
"""Hold loiter position."""
|
|
|
|
self.mavproxy.send('switch 5\n') # loiter mode
|
|
|
|
self.wait_mode('LOITER')
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# enable fence, disable avoidance
|
2018-08-06 09:30:42 -03:00
|
|
|
self.set_parameter("FENCE_ENABLE", 1)
|
|
|
|
self.set_parameter("AVOID_ENABLE", 0)
|
|
|
|
self.set_parameter("FENCE_TYPE", 1)
|
2013-09-19 03:51:18 -03:00
|
|
|
|
2018-04-27 15:21:53 -03:00
|
|
|
self.change_alt(10)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# first east
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("turn east")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1580)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_heading(160)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1500)
|
|
|
|
|
|
|
|
# fly forward (east) at least 20m
|
|
|
|
self.set_rc(2, 1100)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_distance(20)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# stop flying forward and start flying up:
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
self.set_rc(3, 1800)
|
|
|
|
|
|
|
|
# wait for fence to trigger
|
2018-04-14 08:31:22 -03:00
|
|
|
self.wait_mode('RTL', timeout=120)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Waiting for disarm")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.mav.motors_disarmed_wait()
|
|
|
|
|
2018-10-10 10:07:21 -03:00
|
|
|
self.zero_throttle()
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
def fly_gps_glitch_loiter_test(self, timeout=30, max_distance=20):
|
2018-05-09 00:32:23 -03:00
|
|
|
"""fly_gps_glitch_loiter_test. Fly south east in loiter and test
|
|
|
|
reaction to gps glitch."""
|
2018-12-17 02:42:21 -04:00
|
|
|
self.takeoff(10, mode="LOITER")
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# turn on simulator display of gps and actual position
|
|
|
|
if self.use_map:
|
|
|
|
self.show_gps_and_sim_positions(True)
|
|
|
|
|
|
|
|
# set-up gps glitch array
|
2018-03-14 08:08:53 -03:00
|
|
|
glitch_lat = [0.0002996,
|
|
|
|
0.0006958,
|
|
|
|
0.0009431,
|
|
|
|
0.0009991,
|
|
|
|
0.0009444,
|
|
|
|
0.0007716,
|
|
|
|
0.0006221]
|
|
|
|
glitch_lon = [0.0000717,
|
|
|
|
0.0000912,
|
|
|
|
0.0002761,
|
|
|
|
0.0002626,
|
|
|
|
0.0002807,
|
|
|
|
0.0002049,
|
|
|
|
0.0001304]
|
2018-03-05 11:14:34 -04:00
|
|
|
glitch_num = len(glitch_lat)
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("GPS Glitches:")
|
2018-03-05 11:14:34 -04:00
|
|
|
for i in range(1, glitch_num):
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("glitch %d %.7f %.7f" %
|
|
|
|
(i, glitch_lat[i], glitch_lon[i]))
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# turn south east
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("turn south east")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1580)
|
2018-04-27 15:21:53 -03:00
|
|
|
try:
|
|
|
|
self.wait_heading(150)
|
|
|
|
self.set_rc(4, 1500)
|
|
|
|
# fly forward (south east) at least 60m
|
|
|
|
self.set_rc(2, 1100)
|
|
|
|
self.wait_distance(60)
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
# wait for copter to slow down
|
|
|
|
except Exception as e:
|
2018-03-05 11:14:34 -04:00
|
|
|
if self.use_map:
|
|
|
|
self.show_gps_and_sim_positions(False)
|
2018-04-27 15:21:53 -03:00
|
|
|
raise e
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# record time and position
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
tnow = tstart
|
|
|
|
start_pos = self.sim_location()
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# initialise current glitch
|
|
|
|
glitch_current = 0
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Apply first glitch")
|
2019-06-20 22:05:54 -03:00
|
|
|
self.set_parameter("SIM_GPS_GLITCH_X", glitch_lat[glitch_current])
|
|
|
|
self.set_parameter("SIM_GPS_GLITCH_Y", glitch_lon[glitch_current])
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# record position for 30 seconds
|
|
|
|
while tnow < tstart + timeout:
|
2019-03-07 18:34:09 -04:00
|
|
|
tnow = self.get_sim_time_cached()
|
2018-03-05 11:14:34 -04:00
|
|
|
desired_glitch_num = int((tnow - tstart) * 2.2)
|
|
|
|
if desired_glitch_num > glitch_current and glitch_current != -1:
|
|
|
|
glitch_current = desired_glitch_num
|
2018-03-14 08:08:53 -03:00
|
|
|
# turn off glitching if we've reached the end of glitch list
|
2018-03-05 11:14:34 -04:00
|
|
|
if glitch_current >= glitch_num:
|
|
|
|
glitch_current = -1
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Completed Glitches")
|
2018-08-06 09:30:42 -03:00
|
|
|
self.set_parameter("SIM_GPS_GLITCH_X", 0)
|
|
|
|
self.set_parameter("SIM_GPS_GLITCH_Y", 0)
|
2018-03-05 11:14:34 -04:00
|
|
|
else:
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Applying glitch %u" % glitch_current)
|
2018-03-05 11:14:34 -04:00
|
|
|
# move onto the next glitch
|
2019-06-20 22:05:54 -03:00
|
|
|
self.set_parameter("SIM_GPS_GLITCH_X", glitch_lat[glitch_current])
|
|
|
|
self.set_parameter("SIM_GPS_GLITCH_Y", glitch_lon[glitch_current])
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# start displaying distance moved after all glitches applied
|
|
|
|
if glitch_current == -1:
|
2018-05-29 05:05:56 -03:00
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT',
|
|
|
|
blocking=True)
|
|
|
|
alt = m.alt/1000.0 # mm -> m
|
2018-03-05 11:14:34 -04:00
|
|
|
curr_pos = self.sim_location()
|
|
|
|
moved_distance = self.get_distance(curr_pos, start_pos)
|
2018-07-02 00:22:54 -03:00
|
|
|
self.progress("Alt: %.02f Moved: %.0f" %
|
|
|
|
(alt, moved_distance))
|
2018-03-05 11:14:34 -04:00
|
|
|
if moved_distance > max_distance:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise NotAchievedException(
|
|
|
|
"Moved over %u meters, Failed!" % max_distance)
|
2019-03-07 18:34:09 -04:00
|
|
|
else:
|
|
|
|
self.drain_mav()
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# disable gps glitch
|
|
|
|
if glitch_current != -1:
|
2018-08-06 09:30:42 -03:00
|
|
|
self.set_parameter("SIM_GPS_GLITCH_X", 0)
|
|
|
|
self.set_parameter("SIM_GPS_GLITCH_Y", 0)
|
2018-03-05 11:14:34 -04:00
|
|
|
if self.use_map:
|
|
|
|
self.show_gps_and_sim_positions(False)
|
|
|
|
|
2018-04-27 15:21:53 -03:00
|
|
|
self.progress("GPS glitch test passed!"
|
|
|
|
" stayed within %u meters for %u seconds" %
|
|
|
|
(max_distance, timeout))
|
2019-02-26 00:29:12 -04:00
|
|
|
self.do_RTL()
|
2019-03-10 21:59:12 -03:00
|
|
|
# re-arming is problematic because the GPS is glitching!
|
|
|
|
self.reboot_sitl()
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# fly_gps_glitch_auto_test - fly mission and test reaction to gps glitch
|
2019-03-13 00:09:59 -03:00
|
|
|
def fly_gps_glitch_auto_test(self, timeout=180):
|
2018-03-05 11:14:34 -04:00
|
|
|
# set-up gps glitch array
|
2018-03-14 08:08:53 -03:00
|
|
|
glitch_lat = [0.0002996,
|
|
|
|
0.0006958,
|
|
|
|
0.0009431,
|
|
|
|
0.0009991,
|
|
|
|
0.0009444,
|
|
|
|
0.0007716,
|
|
|
|
0.0006221]
|
|
|
|
glitch_lon = [0.0000717,
|
|
|
|
0.0000912,
|
|
|
|
0.0002761,
|
|
|
|
0.0002626,
|
|
|
|
0.0002807,
|
|
|
|
0.0002049,
|
|
|
|
0.0001304]
|
2018-03-05 11:14:34 -04:00
|
|
|
glitch_num = len(glitch_lat)
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("GPS Glitches:")
|
2018-03-05 11:14:34 -04:00
|
|
|
for i in range(1, glitch_num):
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("glitch %d %.7f %.7f" %
|
|
|
|
(i, glitch_lat[i], glitch_lon[i]))
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# Fly mission #1
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("# Load copter_glitch_mission")
|
2018-03-05 11:14:34 -04:00
|
|
|
# load the waypoint count
|
2018-03-14 08:08:53 -03:00
|
|
|
num_wp = self.load_mission("copter_glitch_mission.txt")
|
2017-08-16 10:55:21 -03:00
|
|
|
if not num_wp:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise NotAchievedException("load copter_glitch_mission failed")
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# turn on simulator display of gps and actual position
|
|
|
|
if self.use_map:
|
|
|
|
self.show_gps_and_sim_positions(True)
|
|
|
|
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("test: Fly a mission from 1 to %u" % num_wp)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.mavproxy.send('wp set 1\n')
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode("STABILIZE")
|
|
|
|
self.wait_ready_to_arm()
|
2018-10-10 10:07:21 -03:00
|
|
|
self.zero_throttle()
|
2018-12-17 02:42:21 -04:00
|
|
|
self.arm_vehicle()
|
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# switch into AUTO mode and raise throttle
|
|
|
|
self.mavproxy.send('switch 4\n') # auto mode
|
|
|
|
self.wait_mode('AUTO')
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
|
|
|
|
# wait until 100m from home
|
2018-04-27 15:21:53 -03:00
|
|
|
try:
|
2018-10-16 23:56:06 -03:00
|
|
|
self.wait_distance(100, 5, 90)
|
2018-04-27 15:21:53 -03:00
|
|
|
except Exception as e:
|
2018-03-05 11:14:34 -04:00
|
|
|
if self.use_map:
|
|
|
|
self.show_gps_and_sim_positions(False)
|
2018-04-27 15:21:53 -03:00
|
|
|
raise e
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# record time and position
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
|
|
|
|
# initialise current glitch
|
|
|
|
glitch_current = 0
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Apply first glitch")
|
2019-06-20 22:05:54 -03:00
|
|
|
self.set_parameter("SIM_GPS_GLITCH_X", glitch_lat[glitch_current])
|
|
|
|
self.set_parameter("SIM_GPS_GLITCH_Y", glitch_lon[glitch_current])
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# record position for 30 seconds
|
|
|
|
while glitch_current < glitch_num:
|
|
|
|
tnow = self.get_sim_time()
|
|
|
|
desired_glitch_num = int((tnow - tstart) * 2.2)
|
|
|
|
if desired_glitch_num > glitch_current and glitch_current != -1:
|
|
|
|
glitch_current = desired_glitch_num
|
|
|
|
# apply next glitch
|
|
|
|
if glitch_current < glitch_num:
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Applying glitch %u" % glitch_current)
|
2019-06-20 22:05:54 -03:00
|
|
|
self.set_parameter("SIM_GPS_GLITCH_X",
|
2018-03-14 08:08:53 -03:00
|
|
|
glitch_lat[glitch_current])
|
2019-06-20 22:05:54 -03:00
|
|
|
self.set_parameter("SIM_GPS_GLITCH_Y",
|
2018-03-14 08:08:53 -03:00
|
|
|
glitch_lon[glitch_current])
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# turn off glitching
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Completed Glitches")
|
2018-08-06 09:30:42 -03:00
|
|
|
self.set_parameter("SIM_GPS_GLITCH_X", 0)
|
|
|
|
self.set_parameter("SIM_GPS_GLITCH_Y", 0)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# continue with the mission
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_waypoint(0, num_wp-1, timeout=500)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# wait for arrival back home
|
2018-03-14 08:08:53 -03:00
|
|
|
self.mav.recv_match(type='VFR_HUD', blocking=True)
|
2019-03-07 18:28:03 -04:00
|
|
|
while self.distance_to_home(use_cached_home=True) > 5:
|
2019-03-07 18:34:09 -04:00
|
|
|
if self.get_sim_time_cached() > (tstart + timeout):
|
2018-10-17 23:55:16 -03:00
|
|
|
raise AutoTestTimeoutException(
|
|
|
|
("GPS Glitch testing failed"
|
|
|
|
"- exceeded timeout %u seconds" % timeout))
|
2018-04-27 15:21:53 -03:00
|
|
|
|
2018-03-14 08:08:53 -03:00
|
|
|
self.mav.recv_match(type='VFR_HUD', blocking=True)
|
2019-03-07 18:28:03 -04:00
|
|
|
self.progress("Dist from home: %.02f" % self.distance_to_home(use_cached_home=True))
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# turn off simulator display of gps and actual position
|
|
|
|
if self.use_map:
|
|
|
|
self.show_gps_and_sim_positions(False)
|
|
|
|
|
2018-04-27 15:21:53 -03:00
|
|
|
self.progress("GPS Glitch test Auto completed: passed!")
|
2019-02-26 00:29:12 -04:00
|
|
|
self.mav.motors_disarmed_wait()
|
2019-03-10 21:59:12 -03:00
|
|
|
# re-arming is problematic because the GPS is glitching!
|
|
|
|
self.reboot_sitl()
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-03-14 08:08:53 -03:00
|
|
|
# fly_simple - assumes the simple bearing is initialised to be
|
|
|
|
# directly north flies a box with 100m west, 15 seconds north,
|
|
|
|
# 50 seconds east, 15 seconds south
|
2018-07-31 06:47:49 -03:00
|
|
|
def fly_simple(self, side=50):
|
2018-12-17 02:42:21 -04:00
|
|
|
self.takeoff(10, mode="LOITER")
|
2018-03-05 11:14:34 -04:00
|
|
|
# hold position in loiter
|
|
|
|
self.mavproxy.send('switch 5\n') # loiter mode
|
|
|
|
self.wait_mode('LOITER')
|
|
|
|
|
|
|
|
# set SIMPLE mode for all flight modes
|
2018-08-06 09:30:42 -03:00
|
|
|
self.set_parameter("SIMPLE", 63)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# switch to stabilize mode
|
|
|
|
self.mavproxy.send('switch 6\n')
|
|
|
|
self.wait_mode('STABILIZE')
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
|
|
|
|
# fly south 50m
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("# Flying south %u meters" % side)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(1, 1300)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_distance(side, 5, 60)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(1, 1500)
|
|
|
|
|
|
|
|
# fly west 8 seconds
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("# Flying west for 8 seconds")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1300)
|
|
|
|
tstart = self.get_sim_time()
|
2019-03-07 18:34:09 -04:00
|
|
|
while self.get_sim_time_cached() < (tstart + 8):
|
2018-03-14 08:08:53 -03:00
|
|
|
self.mav.recv_match(type='VFR_HUD', blocking=True)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1500)
|
|
|
|
|
|
|
|
# fly north 25 meters
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("# Flying north %u meters" % (side/2.0))
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(1, 1700)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_distance(side/2, 5, 60)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(1, 1500)
|
|
|
|
|
|
|
|
# fly east 8 seconds
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("# Flying east for 8 seconds")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1700)
|
|
|
|
tstart = self.get_sim_time()
|
2019-03-07 18:34:09 -04:00
|
|
|
while self.get_sim_time_cached() < (tstart + 8):
|
2018-03-14 08:08:53 -03:00
|
|
|
self.mav.recv_match(type='VFR_HUD', blocking=True)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1500)
|
|
|
|
|
|
|
|
# hover in place
|
|
|
|
self.hover()
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
self.do_RTL()
|
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# fly_super_simple - flies a circle around home for 45 seconds
|
|
|
|
def fly_super_simple(self, timeout=45):
|
2018-12-17 02:42:21 -04:00
|
|
|
self.takeoff(10, mode="LOITER")
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# fly forward 20m
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("# Flying forward 20 meters")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1300)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_distance(20, 5, 60)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(2, 1500)
|
|
|
|
|
|
|
|
# set SUPER SIMPLE mode for all flight modes
|
2018-08-06 09:30:42 -03:00
|
|
|
self.set_parameter("SUPER_SIMPLE", 63)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# switch to stabilize mode
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode("STABILIZE")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(3, 1500)
|
|
|
|
|
|
|
|
# start copter yawing slowly
|
|
|
|
self.set_rc(4, 1550)
|
|
|
|
|
|
|
|
# roll left for timeout seconds
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("# rolling left from pilot's POV for %u seconds"
|
|
|
|
% timeout)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(1, 1300)
|
|
|
|
tstart = self.get_sim_time()
|
2019-03-07 18:34:09 -04:00
|
|
|
while self.get_sim_time_cached() < (tstart + timeout):
|
2018-03-14 08:08:53 -03:00
|
|
|
self.mav.recv_match(type='VFR_HUD', blocking=True)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# stop rolling and yawing
|
|
|
|
self.set_rc(1, 1500)
|
|
|
|
self.set_rc(4, 1500)
|
|
|
|
|
|
|
|
# restore simple mode parameters to default
|
2018-08-06 09:30:42 -03:00
|
|
|
self.set_parameter("SUPER_SIMPLE", 0)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# hover in place
|
|
|
|
self.hover()
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
self.do_RTL()
|
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# fly_circle - flies a circle with 20m radius
|
2018-07-31 06:47:49 -03:00
|
|
|
def fly_circle(self, holdtime=36):
|
2018-12-17 02:42:21 -04:00
|
|
|
self.takeoff(10, mode="LOITER")
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# face west
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("turn west")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1580)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_heading(270)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(4, 1500)
|
|
|
|
|
|
|
|
# set CIRCLE radius
|
2018-08-06 09:30:42 -03:00
|
|
|
self.set_parameter("CIRCLE_RADIUS", 3000)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
|
|
|
# fly forward (east) at least 100m
|
|
|
|
self.set_rc(2, 1100)
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_distance(100)
|
2018-03-05 11:14:34 -04:00
|
|
|
# return pitch stick back to middle
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
|
|
|
|
# set CIRCLE mode
|
|
|
|
self.mavproxy.send('switch 1\n') # circle mode
|
|
|
|
self.wait_mode('CIRCLE')
|
|
|
|
|
|
|
|
# wait
|
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
start_altitude = m.alt
|
|
|
|
tstart = self.get_sim_time()
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Circle at %u meters for %u seconds" %
|
|
|
|
(start_altitude, holdtime))
|
2019-03-07 18:34:09 -04:00
|
|
|
while self.get_sim_time_cached() < tstart + holdtime:
|
2018-03-05 11:14:34 -04:00
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
2018-07-02 00:22:54 -03:00
|
|
|
self.progress("heading %d" % m.heading)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("CIRCLE OK for %u seconds" % holdtime)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
self.do_RTL()
|
|
|
|
|
2019-02-02 00:19:18 -04:00
|
|
|
def wait_attitude(self, desroll=None, despitch=None, timeout=2, tolerance=10):
|
|
|
|
'''wait for an attitude (degrees)'''
|
|
|
|
if desroll is None and despitch is None:
|
|
|
|
raise ValueError("despitch or desroll must be supplied")
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while True:
|
|
|
|
if self.get_sim_time_cached() - tstart > 2:
|
|
|
|
raise AutoTestTimeoutException("Failed to achieve attitude")
|
|
|
|
m = self.mav.recv_match(type='ATTITUDE', blocking=True)
|
|
|
|
roll_deg = math.degrees(m.roll)
|
|
|
|
pitch_deg = math.degrees(m.pitch)
|
|
|
|
self.progress("wait_att: roll=%f desroll=%s pitch=%f despitch=%s" %
|
|
|
|
(roll_deg, desroll, pitch_deg, despitch))
|
|
|
|
if desroll is not None and abs(roll_deg - desroll) > tolerance:
|
|
|
|
continue
|
|
|
|
if despitch is not None and abs(pitch_deg - despitch) > tolerance:
|
|
|
|
continue
|
|
|
|
return
|
|
|
|
|
|
|
|
def fly_flip(self):
|
|
|
|
ex = None
|
|
|
|
try:
|
|
|
|
self.mavproxy.send("set streamrate -1\n")
|
|
|
|
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, 100)
|
|
|
|
|
|
|
|
self.takeoff(20)
|
|
|
|
self.hover()
|
|
|
|
old_speedup = self.get_parameter("SIM_SPEEDUP")
|
|
|
|
self.set_parameter('SIM_SPEEDUP', 1)
|
|
|
|
self.progress("Flipping in roll")
|
|
|
|
self.set_rc(1, 1700)
|
|
|
|
self.mavproxy.send('mode FLIP\n') # don't wait for heartbeat!
|
|
|
|
self.wait_attitude(despitch=0, desroll=45, tolerance=30)
|
|
|
|
self.wait_attitude(despitch=0, desroll=90, tolerance=30)
|
|
|
|
self.wait_attitude(despitch=0, desroll=-45, tolerance=30)
|
|
|
|
self.progress("Waiting for level")
|
|
|
|
self.set_rc(1, 1500) # can't change quickly enough!
|
|
|
|
self.wait_attitude(despitch=0, desroll=0, tolerance=5)
|
|
|
|
|
|
|
|
self.progress("Regaining altitude")
|
|
|
|
self.change_mode('STABILIZE')
|
|
|
|
self.set_rc(3, 1800)
|
|
|
|
self.wait_for_alt(20)
|
|
|
|
self.hover()
|
|
|
|
|
|
|
|
self.progress("Flipping in pitch")
|
|
|
|
self.set_rc(2, 1700)
|
|
|
|
self.mavproxy.send('mode FLIP\n') # don't wait for heartbeat!
|
|
|
|
self.wait_attitude(despitch=45, desroll=0, tolerance=30)
|
|
|
|
# can't check roll here as it flips from 0 to -180..
|
|
|
|
self.wait_attitude(despitch=90, tolerance=30)
|
|
|
|
self.wait_attitude(despitch=-45, tolerance=30)
|
|
|
|
self.progress("Waiting for level")
|
|
|
|
self.set_rc(1, 1500) # can't change quickly enough!
|
|
|
|
self.wait_attitude(despitch=0, desroll=0, tolerance=5)
|
|
|
|
self.set_parameter('SIM_SPEEDUP', old_speedup)
|
|
|
|
self.change_mode('RTL')
|
|
|
|
self.mav.motors_disarmed_wait()
|
|
|
|
except Exception as e:
|
|
|
|
ex = e
|
|
|
|
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, 0)
|
|
|
|
sr = self.sitl_streamrate()
|
|
|
|
self.mavproxy.send("set streamrate %u\n" % sr)
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2018-10-02 19:28:21 -03:00
|
|
|
# fly_optical_flow_limits - test EKF navigation limiting
|
|
|
|
def fly_optical_flow_limits(self):
|
|
|
|
ex = None
|
|
|
|
self.context_push()
|
|
|
|
try:
|
2018-12-17 02:42:21 -04:00
|
|
|
|
2018-10-02 19:28:21 -03:00
|
|
|
self.set_parameter("SIM_FLOW_ENABLE", 1)
|
2019-04-04 02:11:14 -03:00
|
|
|
self.set_parameter("FLOW_TYPE", 10)
|
2018-10-02 19:28:21 -03:00
|
|
|
|
2019-07-10 10:57:46 -03:00
|
|
|
self.set_analog_rangefinder_parameters()
|
2018-10-02 19:28:21 -03:00
|
|
|
|
|
|
|
self.set_parameter("SIM_GPS_DISABLE", 1)
|
2018-11-15 04:05:24 -04:00
|
|
|
self.set_parameter("SIM_TERRAIN", 0)
|
2018-10-02 19:28:21 -03:00
|
|
|
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
|
|
|
self.takeoff(alt_min=2, require_absolute=False)
|
|
|
|
|
|
|
|
self.mavproxy.send('mode loiter\n')
|
|
|
|
self.wait_mode('LOITER')
|
|
|
|
|
|
|
|
# speed should be limited to <10m/s
|
|
|
|
self.set_rc(2, 1000)
|
|
|
|
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
timeout = 60
|
|
|
|
while self.get_sim_time_cached() - tstart < timeout:
|
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
spd = m.groundspeed
|
2018-11-06 04:55:55 -04:00
|
|
|
max_speed = 8
|
|
|
|
self.progress("%0.1f: Low Speed: %f (want <= %u)" %
|
|
|
|
(self.get_sim_time_cached() - tstart,
|
|
|
|
spd,
|
|
|
|
max_speed))
|
|
|
|
if spd > max_speed:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise NotAchievedException(("Speed should be limited by"
|
|
|
|
"EKF optical flow limits"))
|
2018-10-02 19:28:21 -03:00
|
|
|
|
|
|
|
self.progress("Moving higher")
|
|
|
|
self.change_alt(60)
|
|
|
|
|
|
|
|
self.wait_groundspeed(10, 100, timeout=60)
|
|
|
|
except Exception as e:
|
|
|
|
ex = e
|
|
|
|
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
self.context_pop()
|
2019-02-26 00:29:12 -04:00
|
|
|
self.disarm_vehicle(force=True)
|
2018-10-02 19:28:21 -03:00
|
|
|
self.reboot_sitl()
|
|
|
|
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2018-04-17 23:47:31 -03:00
|
|
|
# fly_autotune - autotune the virtual vehicle
|
|
|
|
def fly_autotune(self):
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
self.takeoff(10)
|
|
|
|
|
2018-04-17 23:47:31 -03:00
|
|
|
# hold position in loiter
|
|
|
|
self.mavproxy.send('mode autotune\n')
|
|
|
|
self.wait_mode('AUTOTUNE')
|
|
|
|
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
sim_time_expected = 5000
|
|
|
|
deadline = tstart + sim_time_expected
|
|
|
|
while self.get_sim_time_cached() < deadline:
|
|
|
|
now = self.get_sim_time_cached()
|
|
|
|
m = self.mav.recv_match(type='STATUSTEXT',
|
|
|
|
blocking=True,
|
|
|
|
timeout=1)
|
|
|
|
if m is None:
|
|
|
|
continue
|
|
|
|
self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text))
|
|
|
|
if "AutoTune: Success" in m.text:
|
|
|
|
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
|
|
|
# near enough for now:
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode('LAND')
|
|
|
|
self.mav.motors_disarmed_wait()
|
2018-04-17 23:47:31 -03:00
|
|
|
return
|
|
|
|
|
2018-10-17 23:55:16 -03:00
|
|
|
raise NotAchievedException("AUTOTUNE failed (%u seconds)" %
|
|
|
|
(self.get_sim_time() - tstart))
|
2018-04-17 23:47:31 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# fly_auto_test - fly mission which tests a significant number of commands
|
|
|
|
def fly_auto_test(self):
|
|
|
|
# Fly mission #1
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("# Load copter_mission")
|
2018-03-05 11:14:34 -04:00
|
|
|
# load the waypoint count
|
2018-03-14 08:08:53 -03:00
|
|
|
num_wp = self.load_mission("copter_mission.txt")
|
2018-03-05 11:14:34 -04:00
|
|
|
if not num_wp:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise NotAchievedException("load copter_mission failed")
|
2013-05-19 04:19:59 -03:00
|
|
|
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("test: Fly a mission from 1 to %u" % num_wp)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.mavproxy.send('wp set 1\n')
|
2013-09-22 23:45:30 -03:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode("LOITER")
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# switch into AUTO mode and raise throttle
|
2018-12-17 02:42:21 -04:00
|
|
|
self.change_mode("AUTO")
|
2018-03-05 11:14:34 -04:00
|
|
|
self.set_rc(3, 1500)
|
2013-09-22 23:45:30 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# fly the mission
|
2018-05-09 01:22:13 -03:00
|
|
|
self.wait_waypoint(0, num_wp-1, timeout=500)
|
2013-09-22 23:45:30 -03:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# set throttle to minimum
|
2018-10-10 10:07:21 -03:00
|
|
|
self.zero_throttle()
|
2011-12-16 00:39:05 -04:00
|
|
|
|
2018-03-05 11:14:34 -04:00
|
|
|
# wait for disarm
|
|
|
|
self.mav.motors_disarmed_wait()
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("MOTORS DISARMED OK")
|
2011-12-16 00:39:05 -04:00
|
|
|
|
2018-04-27 15:21:53 -03:00
|
|
|
self.progress("Auto mission completed: passed!")
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2017-04-10 14:48:48 -03:00
|
|
|
def fly_motor_fail(self, fail_servo=0, fail_mul=0.0, holdtime=30):
|
|
|
|
"""Test flight with reduced motor efficiency"""
|
|
|
|
|
|
|
|
# we only expect an octocopter to survive ATM:
|
|
|
|
servo_counts = {
|
2018-07-31 06:50:02 -03:00
|
|
|
# 2: 6, # hexa
|
|
|
|
3: 8, # octa
|
|
|
|
# 5: 6, # Y6
|
2017-04-10 14:48:48 -03:00
|
|
|
}
|
|
|
|
frame_class = int(self.get_parameter("FRAME_CLASS"))
|
|
|
|
if frame_class not in servo_counts:
|
|
|
|
self.progress("Test not relevant for frame_class %u" % frame_class)
|
|
|
|
return
|
|
|
|
|
|
|
|
servo_count = servo_counts[frame_class]
|
|
|
|
|
|
|
|
if fail_servo < 0 or fail_servo > servo_count:
|
|
|
|
raise ValueError('fail_servo outside range for frame class')
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
self.takeoff(10, mode="LOITER")
|
2017-04-10 14:48:48 -03:00
|
|
|
|
|
|
|
self.change_alt(alt_min=50)
|
|
|
|
|
|
|
|
# Get initial values
|
|
|
|
start_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
start_attitude = self.mav.recv_match(type='ATTITUDE', blocking=True)
|
|
|
|
|
|
|
|
hover_time = 5
|
|
|
|
try:
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
int_error_alt = 0
|
|
|
|
int_error_yaw_rate = 0
|
|
|
|
int_error_yaw = 0
|
|
|
|
self.progress("Hovering for %u seconds" % hover_time)
|
|
|
|
failed = False
|
2019-03-07 18:34:09 -04:00
|
|
|
while True:
|
|
|
|
now = self.get_sim_time_cached()
|
|
|
|
if now - tstart > holdtime + hover_time:
|
|
|
|
break
|
2017-04-10 14:48:48 -03:00
|
|
|
|
|
|
|
servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW',
|
|
|
|
blocking=True)
|
|
|
|
hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
attitude = self.mav.recv_match(type='ATTITUDE', blocking=True)
|
|
|
|
|
2019-03-07 18:34:09 -04:00
|
|
|
if not failed and now - tstart > hover_time:
|
2017-04-10 14:48:48 -03:00
|
|
|
self.progress("Killing motor %u (%u%%)" %
|
|
|
|
(fail_servo+1, fail_mul))
|
|
|
|
self.set_parameter("SIM_ENGINE_FAIL", fail_servo)
|
|
|
|
self.set_parameter("SIM_ENGINE_MUL", fail_mul)
|
|
|
|
failed = True
|
|
|
|
|
|
|
|
if failed:
|
2019-03-07 18:34:09 -04:00
|
|
|
self.progress("Hold Time: %f/%f" % (now-tstart, holdtime))
|
2017-04-10 14:48:48 -03:00
|
|
|
|
|
|
|
servo_pwm = [servo.servo1_raw,
|
|
|
|
servo.servo2_raw,
|
|
|
|
servo.servo3_raw,
|
|
|
|
servo.servo4_raw,
|
|
|
|
servo.servo5_raw,
|
|
|
|
servo.servo6_raw,
|
|
|
|
servo.servo7_raw,
|
|
|
|
servo.servo8_raw]
|
|
|
|
|
|
|
|
self.progress("PWM output per motor")
|
|
|
|
for i, pwm in enumerate(servo_pwm[0:servo_count]):
|
|
|
|
if pwm > 1900:
|
|
|
|
state = "oversaturated"
|
|
|
|
elif pwm < 1200:
|
|
|
|
state = "undersaturated"
|
|
|
|
else:
|
|
|
|
state = "OK"
|
|
|
|
|
2018-07-31 06:50:02 -03:00
|
|
|
if failed and i == fail_servo:
|
2017-04-10 14:48:48 -03:00
|
|
|
state += " (failed)"
|
|
|
|
|
|
|
|
self.progress("servo %u [pwm=%u] [%s]" % (i+1, pwm, state))
|
|
|
|
|
|
|
|
alt_delta = hud.alt - start_hud.alt
|
|
|
|
yawrate_delta = attitude.yawspeed - start_attitude.yawspeed
|
|
|
|
yaw_delta = attitude.yaw - start_attitude.yaw
|
|
|
|
|
|
|
|
self.progress("Alt=%fm (delta=%fm)" % (hud.alt, alt_delta))
|
|
|
|
self.progress("Yaw rate=%f (delta=%f) (rad/s)" %
|
|
|
|
(attitude.yawspeed, yawrate_delta))
|
|
|
|
self.progress("Yaw=%f (delta=%f) (deg)" %
|
|
|
|
(attitude.yaw, yaw_delta))
|
|
|
|
|
2019-03-07 18:34:09 -04:00
|
|
|
dt = self.get_sim_time() - now
|
2017-04-10 14:48:48 -03:00
|
|
|
int_error_alt += abs(alt_delta/dt)
|
|
|
|
int_error_yaw_rate += abs(yawrate_delta/dt)
|
|
|
|
int_error_yaw += abs(yaw_delta/dt)
|
|
|
|
self.progress("## Error Integration ##")
|
|
|
|
self.progress(" Altitude: %fm" % int_error_alt)
|
|
|
|
self.progress(" Yaw rate: %f rad/s" % int_error_yaw_rate)
|
|
|
|
self.progress(" Yaw: %f deg" % int_error_yaw)
|
|
|
|
self.progress("----")
|
|
|
|
|
2019-03-25 00:56:02 -03:00
|
|
|
if int_error_yaw_rate > 0.1:
|
|
|
|
raise NotAchievedException("Vehicle is spinning")
|
|
|
|
|
2017-04-10 14:48:48 -03:00
|
|
|
if alt_delta < -20:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise NotAchievedException("Vehicle is descending")
|
2017-04-10 14:48:48 -03:00
|
|
|
|
|
|
|
self.set_parameter("SIM_ENGINE_FAIL", 0)
|
|
|
|
self.set_parameter("SIM_ENGINE_MUL", 1.0)
|
|
|
|
except Exception as e:
|
|
|
|
self.set_parameter("SIM_ENGINE_FAIL", 0)
|
|
|
|
self.set_parameter("SIM_ENGINE_MUL", 1.0)
|
|
|
|
raise e
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
self.do_RTL()
|
2017-04-10 14:48:48 -03:00
|
|
|
|
2018-06-29 07:38:10 -03:00
|
|
|
def fly_vision_position(self):
|
2018-07-31 06:50:02 -03:00
|
|
|
"""Disable GPS navigation, enable Vicon input."""
|
2018-06-29 07:38:10 -03:00
|
|
|
# scribble down a location we can set origin to:
|
|
|
|
|
|
|
|
self.progress("Waiting for location")
|
|
|
|
self.mavproxy.send('switch 6\n') # stabilize mode
|
2018-12-13 18:17:02 -04:00
|
|
|
self.wait_heartbeat()
|
2018-06-29 07:38:10 -03:00
|
|
|
self.wait_mode('STABILIZE')
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
|
|
|
|
old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
print("old_pos=%s" % str(old_pos))
|
|
|
|
|
2018-07-31 06:50:02 -03:00
|
|
|
self.context_push()
|
2018-06-29 07:38:10 -03:00
|
|
|
|
|
|
|
ex = None
|
|
|
|
try:
|
|
|
|
self.set_parameter("GPS_TYPE", 0)
|
|
|
|
self.set_parameter("EK2_GPS_TYPE", 3)
|
|
|
|
self.set_parameter("SERIAL5_PROTOCOL", 1)
|
|
|
|
self.reboot_sitl()
|
|
|
|
# without a GPS or some sort of external prompting, AP
|
|
|
|
# doesn't send system_time messages. So prompt it:
|
2019-03-13 03:12:28 -03:00
|
|
|
self.mav.mav.system_time_send(int(time.time() * 1000000), 0)
|
2018-06-29 07:38:10 -03:00
|
|
|
self.progress("Waiting for non-zero-lat")
|
2018-07-12 06:44:03 -03:00
|
|
|
tstart = self.get_sim_time()
|
2018-06-29 07:38:10 -03:00
|
|
|
while True:
|
2019-06-28 05:51:24 -03:00
|
|
|
self.mav.mav.set_gps_global_origin_send(1,
|
|
|
|
old_pos.lat,
|
|
|
|
old_pos.lon,
|
|
|
|
old_pos.alt)
|
2018-06-29 07:38:10 -03:00
|
|
|
gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT',
|
|
|
|
blocking=True)
|
2019-06-28 05:51:24 -03:00
|
|
|
self.progress("gpi=%s" % str(gpi))
|
2018-06-29 07:38:10 -03:00
|
|
|
if gpi.lat != 0:
|
|
|
|
break
|
|
|
|
|
2019-06-28 05:51:24 -03:00
|
|
|
if self.get_sim_time_cached() - tstart > 60:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise AutoTestTimeoutException("Did not get non-zero lat")
|
2018-06-29 07:38:10 -03:00
|
|
|
|
2018-08-23 07:24:34 -03:00
|
|
|
self.takeoff()
|
2018-06-29 07:38:10 -03:00
|
|
|
self.set_rc(1, 1600)
|
2018-07-12 06:44:03 -03:00
|
|
|
tstart = self.get_sim_time()
|
2018-06-29 07:38:10 -03:00
|
|
|
while True:
|
|
|
|
vicon_pos = self.mav.recv_match(type='VICON_POSITION_ESTIMATE',
|
|
|
|
blocking=True)
|
2018-07-31 06:50:02 -03:00
|
|
|
# print("vpe=%s" % str(vicon_pos))
|
|
|
|
self.mav.recv_match(type='GLOBAL_POSITION_INT',
|
|
|
|
blocking=True)
|
|
|
|
# self.progress("gpi=%s" % str(gpi))
|
2018-06-29 07:38:10 -03:00
|
|
|
if vicon_pos.x > 40:
|
|
|
|
break
|
|
|
|
|
2019-03-07 18:34:09 -04:00
|
|
|
if self.get_sim_time_cached() - tstart > 100:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise AutoTestTimeoutException("Vicon showed no movement")
|
2018-06-29 07:38:10 -03:00
|
|
|
|
|
|
|
# recenter controls:
|
|
|
|
self.set_rc(1, 1500)
|
|
|
|
self.progress("# Enter RTL")
|
|
|
|
self.mavproxy.send('switch 3\n')
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while True:
|
2019-03-07 18:34:09 -04:00
|
|
|
if self.get_sim_time_cached() - tstart > 200:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise NotAchievedException("Did not disarm")
|
2018-07-31 06:50:02 -03:00
|
|
|
self.mav.recv_match(type='GLOBAL_POSITION_INT',
|
|
|
|
blocking=True)
|
|
|
|
# print("gpi=%s" % str(gpi))
|
|
|
|
self.mav.recv_match(type='SIMSTATE',
|
|
|
|
blocking=True)
|
|
|
|
# print("ss=%s" % str(ss))
|
2018-06-29 07:38:10 -03:00
|
|
|
# wait for RTL disarm:
|
|
|
|
if not self.armed():
|
|
|
|
break
|
|
|
|
|
|
|
|
except Exception as e:
|
2019-06-18 00:13:49 -03:00
|
|
|
self.progress("Exception caught: %s" % (
|
|
|
|
self.get_exception_stacktrace(e)))
|
2018-06-29 07:38:10 -03:00
|
|
|
ex = e
|
|
|
|
|
2018-07-31 06:50:02 -03:00
|
|
|
self.context_pop()
|
2018-10-10 10:07:21 -03:00
|
|
|
self.zero_throttle()
|
2018-06-29 07:38:10 -03:00
|
|
|
self.reboot_sitl()
|
|
|
|
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2019-01-31 01:01:33 -04:00
|
|
|
def fly_rtl_speed(self):
|
|
|
|
"""Test RTL Speed parameters"""
|
|
|
|
rtl_speed_ms = 7
|
|
|
|
wpnav_speed_ms = 4
|
|
|
|
wpnav_accel_mss = 3
|
|
|
|
tolerance = 0.5
|
|
|
|
self.load_mission("copter_rtl_speed.txt")
|
|
|
|
self.set_parameter('WPNAV_ACCEL', wpnav_accel_mss * 100)
|
|
|
|
self.set_parameter('RTL_SPEED', rtl_speed_ms * 100)
|
|
|
|
self.set_parameter('WPNAV_SPEED', wpnav_speed_ms * 100)
|
|
|
|
self.change_mode('LOITER')
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.change_mode('AUTO')
|
|
|
|
self.set_rc(3, 1600)
|
|
|
|
self.wait_altitude(19, 25, relative=True)
|
|
|
|
self.wait_groundspeed(wpnav_speed_ms-tolerance, wpnav_speed_ms+tolerance)
|
|
|
|
self.monitor_groundspeed(wpnav_speed_ms, timeout=20)
|
|
|
|
self.change_mode('RTL')
|
|
|
|
self.wait_groundspeed(rtl_speed_ms-tolerance, rtl_speed_ms+tolerance)
|
|
|
|
self.monitor_groundspeed(rtl_speed_ms, timeout=5)
|
|
|
|
self.change_mode('AUTO')
|
|
|
|
self.wait_groundspeed(0-tolerance, 0+tolerance)
|
|
|
|
self.wait_groundspeed(wpnav_speed_ms-tolerance, wpnav_speed_ms+tolerance)
|
|
|
|
self.monitor_groundspeed(wpnav_speed_ms, timeout=5)
|
|
|
|
self.change_mode('RTL')
|
|
|
|
self.mav.motors_disarmed_wait()
|
|
|
|
|
2018-08-06 22:25:23 -03:00
|
|
|
def fly_nav_delay(self):
|
2018-07-31 06:50:02 -03:00
|
|
|
"""Fly a simple mission that has a delay in it."""
|
2018-08-06 22:25:23 -03:00
|
|
|
|
2018-07-31 06:50:02 -03:00
|
|
|
self.load_mission("copter_nav_delay.txt")
|
2018-08-06 22:25:23 -03:00
|
|
|
|
2019-03-07 18:30:09 -04:00
|
|
|
self.set_parameter("DISARM_DELAY", 0)
|
2018-08-06 22:25:23 -03:00
|
|
|
|
2019-03-07 18:30:09 -04:00
|
|
|
self.change_mode("LOITER")
|
|
|
|
self.wait_ready_to_arm()
|
2018-08-06 22:25:23 -03:00
|
|
|
|
2019-03-07 18:30:09 -04:00
|
|
|
self.arm_vehicle()
|
|
|
|
self.change_mode("AUTO")
|
|
|
|
self.set_rc(3, 1600)
|
|
|
|
count_start = -1
|
|
|
|
count_stop = -1
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
last_mission_current_msg = 0
|
|
|
|
last_seq = None
|
|
|
|
while self.armed(): # we RTL at end of mission
|
|
|
|
now = self.get_sim_time_cached()
|
|
|
|
if now - tstart > 120:
|
|
|
|
raise AutoTestTimeoutException("Did not disarm as expected")
|
|
|
|
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
|
|
|
|
at_delay_item = ""
|
|
|
|
if m.seq == 3:
|
|
|
|
at_delay_item = "(At delay item)"
|
|
|
|
if count_start == -1:
|
|
|
|
count_start = now
|
|
|
|
if ((now - last_mission_current_msg) > 1 or m.seq != last_seq):
|
|
|
|
dist = None
|
|
|
|
x = self.mav.messages.get("NAV_CONTROLLER_OUTPUT", None)
|
|
|
|
if x is not None:
|
|
|
|
dist = x.wp_dist
|
|
|
|
self.progress("MISSION_CURRENT.seq=%u dist=%s %s" %
|
|
|
|
(m.seq, dist, at_delay_item))
|
|
|
|
last_mission_current_msg = self.get_sim_time_cached()
|
|
|
|
last_seq = m.seq
|
|
|
|
if m.seq > 3:
|
|
|
|
if count_stop == -1:
|
|
|
|
count_stop = now
|
|
|
|
calculated_delay = count_stop - count_start
|
|
|
|
want_delay = 59 # should reflect what's in the mission file
|
|
|
|
self.progress("Stopped for %u seconds (want >=%u seconds)" %
|
|
|
|
(calculated_delay, want_delay))
|
|
|
|
if calculated_delay < want_delay:
|
|
|
|
raise NotAchievedException("Did not delay for long enough")
|
2018-08-06 22:25:23 -03:00
|
|
|
|
2019-01-07 18:51:36 -04:00
|
|
|
def test_rangefinder(self):
|
|
|
|
ex = None
|
|
|
|
self.context_push()
|
|
|
|
self.progress("Making sure we don't ordinarily get RANGEFINDER")
|
|
|
|
try:
|
|
|
|
m = self.mav.recv_match(type='RANGEFINDER',
|
|
|
|
blocking=True,
|
|
|
|
timeout=5)
|
|
|
|
except Exception as e:
|
|
|
|
print("Caught exception %s" % str(e))
|
|
|
|
|
|
|
|
if m is not None:
|
|
|
|
raise NotAchievedException("Received unexpected RANGEFINDER msg")
|
|
|
|
|
2019-04-08 23:26:18 -03:00
|
|
|
# may need to force a rotation if some other test has used the
|
|
|
|
# rangefinder...
|
|
|
|
self.progress("Ensure no RFND messages in log")
|
|
|
|
if self.current_onboard_log_contains_message("RFND"):
|
|
|
|
raise NotAchievedException("Found unexpected RFND message")
|
|
|
|
|
2019-01-07 18:51:36 -04:00
|
|
|
try:
|
2019-07-10 10:57:46 -03:00
|
|
|
self.set_analog_rangefinder_parameters()
|
2019-01-07 18:51:36 -04:00
|
|
|
self.set_parameter("RC9_OPTION", 10) # rangefinder
|
|
|
|
self.set_rc(9, 2000)
|
|
|
|
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
|
|
|
self.progress("Making sure we now get RANGEFINDER messages")
|
|
|
|
m = self.mav.recv_match(type='RANGEFINDER',
|
|
|
|
blocking=True,
|
|
|
|
timeout=10)
|
|
|
|
if m is None:
|
|
|
|
raise NotAchievedException("Did not get expected RANGEFINDER msg")
|
|
|
|
|
|
|
|
self.progress("Checking RangeFinder is marked as enabled in mavlink")
|
|
|
|
m = self.mav.recv_match(type='SYS_STATUS',
|
|
|
|
blocking=True,
|
|
|
|
timeout=10)
|
|
|
|
flags = m.onboard_control_sensors_enabled
|
|
|
|
if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION:
|
|
|
|
raise NotAchievedException("Laser not enabled in SYS_STATUS")
|
|
|
|
self.progress("Disabling laser using switch")
|
|
|
|
self.set_rc(9, 1000)
|
|
|
|
self.delay_sim_time(1)
|
|
|
|
self.progress("Checking RangeFinder is marked as disabled in mavlink")
|
|
|
|
m = self.mav.recv_match(type='SYS_STATUS',
|
|
|
|
blocking=True,
|
|
|
|
timeout=10)
|
|
|
|
flags = m.onboard_control_sensors_enabled
|
|
|
|
if flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION:
|
|
|
|
raise NotAchievedException("Laser enabled in SYS_STATUS")
|
|
|
|
|
2018-06-05 22:31:14 -03:00
|
|
|
self.progress("Re-enabling rangefinder")
|
|
|
|
self.set_rc(9, 2000)
|
|
|
|
self.delay_sim_time(1)
|
|
|
|
m = self.mav.recv_match(type='SYS_STATUS',
|
|
|
|
blocking=True,
|
|
|
|
timeout=10)
|
|
|
|
flags = m.onboard_control_sensors_enabled
|
|
|
|
if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION:
|
|
|
|
raise NotAchievedException("Laser not enabled in SYS_STATUS")
|
|
|
|
|
|
|
|
self.takeoff(10, mode="LOITER")
|
|
|
|
|
|
|
|
m_r = self.mav.recv_match(type='RANGEFINDER',
|
|
|
|
blocking=True)
|
|
|
|
m_p = self.mav.recv_match(type='GLOBAL_POSITION_INT',
|
|
|
|
blocking=True)
|
|
|
|
|
|
|
|
if abs(m_r.distance - m_p.relative_alt/1000) > 1:
|
|
|
|
raise NotAchievedException("rangefinder/global position int mismatch")
|
|
|
|
|
2019-04-08 23:26:18 -03:00
|
|
|
self.land_and_disarm()
|
|
|
|
|
|
|
|
if not self.current_onboard_log_contains_message("RFND"):
|
|
|
|
raise NotAchievedException("Did not see expected RFND message")
|
2018-06-05 22:31:14 -03:00
|
|
|
|
2019-01-07 18:51:36 -04:00
|
|
|
except Exception as e:
|
2019-06-18 00:13:49 -03:00
|
|
|
self.progress("Exception caught: %s" % (
|
|
|
|
self.get_exception_stacktrace(e)))
|
|
|
|
ex = e
|
|
|
|
self.context_pop()
|
|
|
|
self.reboot_sitl()
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
|
|
|
def test_surface_tracking(self):
|
|
|
|
ex = None
|
|
|
|
self.context_push()
|
|
|
|
|
|
|
|
try:
|
2019-07-10 10:57:46 -03:00
|
|
|
self.set_analog_rangefinder_parameters()
|
2019-06-18 00:13:49 -03:00
|
|
|
self.set_parameter("RC9_OPTION", 10) # rangefinder
|
|
|
|
self.set_rc(9, 2000)
|
|
|
|
|
|
|
|
self.reboot_sitl() # needed for both rangefinder and initial position
|
|
|
|
self.assert_vehicle_location_is_at_startup_location()
|
|
|
|
|
|
|
|
self.takeoff(10, mode="LOITER")
|
|
|
|
lower_surface_pos = mavutil.location(-35.362421, 149.164534, 584, 270)
|
|
|
|
here = self.mav.location()
|
|
|
|
bearing = self.get_bearing(here, lower_surface_pos)
|
|
|
|
|
|
|
|
self.change_mode("GUIDED")
|
|
|
|
self.guided_achieve_heading(bearing)
|
|
|
|
self.change_mode("LOITER")
|
|
|
|
self.delay_sim_time(2)
|
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
orig_absolute_alt_mm = m.alt
|
|
|
|
|
|
|
|
self.progress("Original alt: absolute=%f" % orig_absolute_alt_mm)
|
|
|
|
|
|
|
|
self.progress("Flying somewhere which surface is known lower compared to takeoff point")
|
|
|
|
self.set_rc(2, 1450)
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while True:
|
|
|
|
if self.get_sim_time() - tstart > 200:
|
|
|
|
raise NotAchievedException("Did not reach lower point")
|
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
x = mavutil.location(m.lat/1e7, m.lon/1e7, m.alt/1e3, 0)
|
|
|
|
dist = self.get_distance(x, lower_surface_pos)
|
|
|
|
delta = (orig_absolute_alt_mm - m.alt)/1000.0
|
|
|
|
|
|
|
|
self.progress("Distance: %fm abs-alt-delta: %fm" %
|
|
|
|
(dist, delta))
|
2019-07-01 05:55:40 -03:00
|
|
|
if dist < 15:
|
2019-06-18 00:13:49 -03:00
|
|
|
if delta < 0.8:
|
|
|
|
raise NotAchievedException("Did not dip in altitude as expected")
|
|
|
|
break
|
|
|
|
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
self.do_RTL()
|
|
|
|
|
|
|
|
except Exception as e:
|
|
|
|
self.progress("Exception caught: %s" % (
|
|
|
|
self.get_exception_stacktrace(e)))
|
|
|
|
self.disarm_vehicle(force=True)
|
2019-01-07 18:51:36 -04:00
|
|
|
ex = e
|
|
|
|
self.context_pop()
|
|
|
|
self.reboot_sitl()
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2019-01-08 21:40:49 -04:00
|
|
|
def test_parachute(self):
|
|
|
|
|
|
|
|
self.set_rc(9, 1000)
|
|
|
|
self.set_parameter("CHUTE_ENABLED", 1)
|
|
|
|
self.set_parameter("CHUTE_TYPE", 10)
|
|
|
|
self.set_parameter("SERVO9_FUNCTION", 27)
|
|
|
|
self.set_parameter("SIM_PARA_ENABLE", 1)
|
|
|
|
self.set_parameter("SIM_PARA_PIN", 9)
|
|
|
|
|
2019-01-31 19:11:25 -04:00
|
|
|
self.progress("Test triggering parachute in mission")
|
|
|
|
self.load_mission("copter_parachute_mission.txt")
|
|
|
|
self.change_mode('LOITER')
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.change_mode('AUTO')
|
|
|
|
self.set_rc(3, 1600)
|
|
|
|
self.mavproxy.expect('BANG')
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
|
|
|
self.progress("Test triggering with mavlink message")
|
|
|
|
self.takeoff(20)
|
|
|
|
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PARACHUTE,
|
|
|
|
2, # release
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0)
|
|
|
|
self.mavproxy.expect('BANG')
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
2019-01-08 21:40:49 -04:00
|
|
|
self.progress("Testing three-position switch")
|
|
|
|
self.set_parameter("RC9_OPTION", 23) # parachute 3pos
|
|
|
|
|
|
|
|
self.progress("Test manual triggering")
|
|
|
|
self.takeoff(20)
|
|
|
|
self.set_rc(9, 2000)
|
|
|
|
self.mavproxy.expect('BANG')
|
|
|
|
self.set_rc(9, 1000)
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
|
|
|
self.context_push()
|
|
|
|
self.progress("Crashing with 3pos switch in enable position")
|
|
|
|
self.takeoff(40)
|
|
|
|
self.set_rc(9, 1500)
|
|
|
|
self.set_parameter("SIM_ENGINE_MUL", 0)
|
|
|
|
self.set_parameter("SIM_ENGINE_FAIL", 1)
|
|
|
|
self.mavproxy.expect('BANG')
|
|
|
|
self.set_rc(9, 1000)
|
|
|
|
self.reboot_sitl()
|
2019-07-08 12:59:02 -03:00
|
|
|
self.context_pop()
|
2019-01-08 21:40:49 -04:00
|
|
|
|
|
|
|
self.progress("Crashing with 3pos switch in disable position")
|
|
|
|
loiter_alt = 10
|
|
|
|
self.takeoff(loiter_alt, mode='LOITER')
|
|
|
|
self.set_rc(9, 1100)
|
|
|
|
self.set_parameter("SIM_ENGINE_MUL", 0)
|
|
|
|
self.set_parameter("SIM_ENGINE_FAIL", 1)
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while self.get_sim_time_cached() < tstart + 5:
|
|
|
|
m = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=1)
|
|
|
|
if m is None:
|
|
|
|
continue
|
|
|
|
if "BANG" in m.text:
|
|
|
|
self.set_rc(9, 1000)
|
|
|
|
self.reboot_sitl()
|
|
|
|
raise NotAchievedException("Parachute deployed when disabled")
|
|
|
|
self.set_rc(9, 1000)
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
2018-07-15 20:54:00 -03:00
|
|
|
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)
|
|
|
|
|
2019-07-10 10:57:46 -03:00
|
|
|
self.set_analog_rangefinder_parameters()
|
2019-04-04 04:37:07 -03:00
|
|
|
self.set_parameter("SIM_SONAR_SCALE", 12)
|
|
|
|
|
|
|
|
start = self.mav.location()
|
|
|
|
target = start
|
|
|
|
(target.lat, target.lng) = mavextra.gps_offset(start.lat, start.lng, 4, -4)
|
|
|
|
self.progress("Setting target to %f %f" % (target.lat, target.lng))
|
|
|
|
|
|
|
|
self.set_parameter("SIM_PLD_ENABLE", 1)
|
|
|
|
self.set_parameter("SIM_PLD_LAT", target.lat)
|
|
|
|
self.set_parameter("SIM_PLD_LON", target.lng)
|
|
|
|
self.set_parameter("SIM_PLD_HEIGHT", 0)
|
|
|
|
self.set_parameter("SIM_PLD_ALT_LMT", 15)
|
|
|
|
self.set_parameter("SIM_PLD_DIST_LMT", 10)
|
2018-07-15 20:54:00 -03:00
|
|
|
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
|
|
|
self.progress("Waiting for location")
|
|
|
|
old_pos = self.mav.location()
|
2018-10-10 10:07:21 -03:00
|
|
|
self.zero_throttle()
|
2018-07-15 20:54:00 -03:00
|
|
|
self.takeoff(10, 1800)
|
2019-03-06 20:23:58 -04:00
|
|
|
self.change_mode("LAND")
|
2018-07-15 20:54:00 -03:00
|
|
|
self.mav.motors_disarmed_wait()
|
|
|
|
self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
new_pos = self.mav.location()
|
2019-04-04 04:37:07 -03:00
|
|
|
delta = self.get_distance(target, new_pos)
|
|
|
|
self.progress("Landed %f metres from target position" % delta)
|
2019-03-06 20:23:58 -04:00
|
|
|
max_delta = 1
|
|
|
|
if delta > max_delta:
|
2019-04-04 04:37:07 -03:00
|
|
|
raise NotAchievedException("Did not land close enough to target position (%fm > %fm" % (delta, max_delta))
|
2018-07-15 20:54:00 -03:00
|
|
|
|
2019-04-02 23:53:56 -03:00
|
|
|
if not self.current_onboard_log_contains_message("PL"):
|
|
|
|
raise NotAchievedException("Did not see expected PL message")
|
|
|
|
|
2018-07-15 20:54:00 -03:00
|
|
|
except Exception as e:
|
2019-04-02 23:53:56 -03:00
|
|
|
self.progress("Exception caught (%s)" % str(e))
|
2018-07-15 20:54:00 -03:00
|
|
|
ex = e
|
|
|
|
|
2018-10-10 10:07:21 -03:00
|
|
|
self.zero_throttle()
|
2018-07-15 20:54:00 -03:00
|
|
|
self.context_pop()
|
|
|
|
self.reboot_sitl()
|
|
|
|
self.progress("All done")
|
|
|
|
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2018-07-31 06:50:02 -03:00
|
|
|
def get_system_clock_utc(self, time_seconds):
|
2018-08-07 06:58:40 -03:00
|
|
|
# this is a copy of ArduPilot's AP_RTC function!
|
|
|
|
# separate time into ms, sec, min, hour and days but all expressed
|
|
|
|
# in milliseconds
|
2018-07-31 06:50:02 -03:00
|
|
|
time_ms = time_seconds * 1000
|
|
|
|
ms = time_ms % 1000
|
|
|
|
sec_ms = (time_ms % (60 * 1000)) - ms
|
|
|
|
min_ms = (time_ms % (60 * 60 * 1000)) - sec_ms - ms
|
|
|
|
hour_ms = (time_ms % (24 * 60 * 60 * 1000)) - min_ms - sec_ms - ms
|
2018-08-07 06:58:40 -03:00
|
|
|
|
|
|
|
# convert times as milliseconds into appropriate units
|
2018-07-31 06:50:02 -03:00
|
|
|
secs = sec_ms / 1000
|
|
|
|
mins = min_ms / (60 * 1000)
|
|
|
|
hours = hour_ms / (60 * 60 * 1000)
|
|
|
|
return (hours, mins, secs, 0)
|
2018-08-07 06:58:40 -03:00
|
|
|
|
|
|
|
def calc_delay(self, seconds):
|
|
|
|
# delay-for-seconds has to be long enough that we're at the
|
|
|
|
# waypoint before that time. Otherwise we'll try to wait a
|
|
|
|
# day....
|
|
|
|
(hours,
|
|
|
|
mins,
|
|
|
|
secs,
|
|
|
|
ms) = self.get_system_clock_utc(seconds)
|
|
|
|
self.progress("Now is %uh %um %us" % (hours, mins, secs))
|
|
|
|
secs += 17 # add seventeen seconds
|
|
|
|
if secs >= 60:
|
|
|
|
secs %= 60
|
|
|
|
mins += 1 # add sixty seconds
|
|
|
|
mins += 1
|
|
|
|
if mins >= 60:
|
|
|
|
mins %= 60
|
|
|
|
hours += 1
|
|
|
|
if hours >= 24:
|
|
|
|
hours %= 24
|
|
|
|
return (hours, mins, secs, 0)
|
|
|
|
|
2018-08-08 06:09:48 -03:00
|
|
|
def reset_delay_item_seventyseven(self, seq):
|
2018-08-07 06:58:40 -03:00
|
|
|
while True:
|
|
|
|
self.progress("Requesting request for seq %u" % (seq,))
|
|
|
|
self.mav.mav.mission_write_partial_list_send(1, # target system
|
|
|
|
1, # target component
|
|
|
|
seq, # start index
|
|
|
|
seq)
|
|
|
|
req = self.mav.recv_match(type='MISSION_REQUEST',
|
|
|
|
blocking=True,
|
|
|
|
timeout=1)
|
|
|
|
if req is not None and req.seq == seq:
|
|
|
|
if req.get_srcSystem() == 255:
|
|
|
|
self.progress("Shutup MAVProxy")
|
|
|
|
continue
|
|
|
|
# notionally this might be in the message cache before
|
|
|
|
# we prompt for it... *shrug*
|
|
|
|
break
|
|
|
|
|
|
|
|
# we have received a request for the item. Supply it:
|
|
|
|
|
|
|
|
frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
|
|
|
|
command = mavutil.mavlink.MAV_CMD_NAV_DELAY
|
|
|
|
# retrieve mission item and check it:
|
|
|
|
tried_set = False
|
2018-11-22 23:51:58 -04:00
|
|
|
hours = None
|
|
|
|
mins = None
|
|
|
|
secs = None
|
2018-08-07 06:58:40 -03:00
|
|
|
while True:
|
2018-08-15 20:44:32 -03:00
|
|
|
self.progress("Requesting item")
|
|
|
|
self.mav.mav.mission_request_send(1,
|
|
|
|
1,
|
2018-11-22 23:51:58 -04:00
|
|
|
seq)
|
2018-08-07 06:58:40 -03:00
|
|
|
st = self.mav.recv_match(type='MISSION_ITEM',
|
|
|
|
blocking=True,
|
|
|
|
timeout=1)
|
2018-08-15 20:44:32 -03:00
|
|
|
if st is None:
|
|
|
|
continue
|
2018-08-07 06:58:40 -03:00
|
|
|
|
2018-08-15 20:44:32 -03:00
|
|
|
print("Item: %s" % str(st))
|
2018-11-22 23:51:58 -04:00
|
|
|
have_match = (tried_set and
|
|
|
|
st.seq == seq and
|
|
|
|
st.command == command and
|
|
|
|
st.param2 == hours and
|
|
|
|
st.param3 == mins and
|
|
|
|
st.param4 == secs)
|
|
|
|
if have_match:
|
2018-08-15 20:44:32 -03:00
|
|
|
return
|
2018-11-22 23:51:58 -04:00
|
|
|
|
2018-08-15 20:44:32 -03:00
|
|
|
self.progress("Mission mismatch")
|
|
|
|
|
|
|
|
m = None
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while True:
|
|
|
|
if self.get_sim_time_cached() - tstart > 3:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise NotAchievedException(
|
|
|
|
"Did not receive MISSION_REQUEST")
|
2018-08-07 06:58:40 -03:00
|
|
|
self.mav.mav.mission_write_partial_list_send(1,
|
|
|
|
1,
|
2018-08-15 20:44:32 -03:00
|
|
|
seq,
|
|
|
|
seq)
|
|
|
|
m = self.mav.recv_match(type='MISSION_REQUEST',
|
|
|
|
blocking=True,
|
|
|
|
timeout=1)
|
|
|
|
if m is None:
|
|
|
|
continue
|
|
|
|
if m.seq != st.seq:
|
|
|
|
continue
|
|
|
|
break
|
|
|
|
|
|
|
|
self.progress("Sending absolute-time mission item")
|
|
|
|
|
|
|
|
# we have to change out the delay time...
|
|
|
|
now = self.mav.messages["SYSTEM_TIME"]
|
|
|
|
if now is None:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise PreconditionFailedException("Never got SYSTEM_TIME")
|
2018-08-15 20:44:32 -03:00
|
|
|
if now.time_unix_usec == 0:
|
2018-10-17 23:55:16 -03:00
|
|
|
raise PreconditionFailedException("system time is zero")
|
2018-08-15 20:44:32 -03:00
|
|
|
(hours, mins, secs, ms) = self.calc_delay(
|
|
|
|
now.time_unix_usec/1000000)
|
|
|
|
|
|
|
|
self.progress("Delay until %uh %um %us" %
|
|
|
|
(hours, mins, secs))
|
|
|
|
|
|
|
|
self.mav.mav.mission_item_send(
|
|
|
|
1, # target system
|
|
|
|
1, # target component
|
|
|
|
seq, # seq
|
|
|
|
frame, # frame
|
|
|
|
command, # command
|
|
|
|
0, # current
|
|
|
|
1, # autocontinue
|
|
|
|
0, # p1 (relative seconds)
|
|
|
|
hours, # p2
|
|
|
|
mins, # p3
|
|
|
|
secs, # p4
|
|
|
|
0, # p5
|
|
|
|
0, # p6
|
|
|
|
0) # p7
|
|
|
|
tried_set = True
|
|
|
|
ack = self.mav.recv_match(type='MISSION_ACK',
|
|
|
|
blocking=True,
|
|
|
|
timeout=1)
|
|
|
|
self.progress("Received ack: %s" % str(ack))
|
2018-08-07 06:58:40 -03:00
|
|
|
|
|
|
|
def fly_nav_delay_abstime(self):
|
2018-08-08 06:09:48 -03:00
|
|
|
"""fly a simple mission that has a delay in it"""
|
2018-08-07 06:58:40 -03:00
|
|
|
|
2018-07-31 06:50:02 -03:00
|
|
|
self.load_mission("copter_nav_delay.txt")
|
2018-08-07 06:58:40 -03:00
|
|
|
|
2019-03-07 18:30:09 -04:00
|
|
|
self.change_mode("LOITER")
|
2018-08-07 06:58:40 -03:00
|
|
|
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
|
2018-08-08 06:09:48 -03:00
|
|
|
delay_item_seq = 3
|
|
|
|
self.reset_delay_item_seventyseven(delay_item_seq)
|
2018-08-07 06:58:40 -03:00
|
|
|
delay_for_seconds = 77
|
|
|
|
reset_at_m = self.mav.recv_match(type='SYSTEM_TIME', blocking=True)
|
|
|
|
reset_at = reset_at_m.time_unix_usec/1000000
|
|
|
|
|
2019-03-07 18:30:09 -04:00
|
|
|
self.arm_vehicle()
|
|
|
|
self.change_mode("AUTO")
|
|
|
|
self.set_rc(3, 1600)
|
|
|
|
count_stop = -1
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while self.armed(): # we RTL at end of mission
|
2019-03-07 18:34:09 -04:00
|
|
|
now = self.get_sim_time_cached()
|
2019-03-07 18:30:09 -04:00
|
|
|
if now - tstart > 120:
|
|
|
|
raise AutoTestTimeoutException("Did not disarm as expected")
|
|
|
|
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
|
|
|
|
at_delay_item = ""
|
|
|
|
if m.seq == delay_item_seq:
|
|
|
|
at_delay_item = "(delay item)"
|
|
|
|
self.progress("MISSION_CURRENT.seq=%u %s" % (m.seq, at_delay_item))
|
|
|
|
if m.seq > delay_item_seq:
|
|
|
|
if count_stop == -1:
|
|
|
|
count_stop_m = self.mav.recv_match(type='SYSTEM_TIME',
|
|
|
|
blocking=True)
|
|
|
|
count_stop = count_stop_m.time_unix_usec/1000000
|
|
|
|
calculated_delay = count_stop - reset_at
|
|
|
|
error = abs(calculated_delay - delay_for_seconds)
|
|
|
|
self.progress("Stopped for %u seconds (want >=%u seconds)" %
|
|
|
|
(calculated_delay, delay_for_seconds))
|
|
|
|
if error > 2:
|
|
|
|
raise NotAchievedException("delay outside expectations")
|
2018-08-07 06:58:40 -03:00
|
|
|
|
2018-08-08 06:09:48 -03:00
|
|
|
def fly_nav_takeoff_delay_abstime(self):
|
|
|
|
"""make sure taking off at a specific time works"""
|
2018-12-20 08:05:30 -04:00
|
|
|
self.load_mission("copter_nav_delay_takeoff.txt")
|
2018-08-08 06:09:48 -03:00
|
|
|
|
2019-03-07 18:30:09 -04:00
|
|
|
self.change_mode("LOITER")
|
2018-08-08 06:09:48 -03:00
|
|
|
self.wait_ready_to_arm()
|
|
|
|
|
|
|
|
delay_item_seq = 2
|
|
|
|
self.reset_delay_item_seventyseven(delay_item_seq)
|
|
|
|
delay_for_seconds = 77
|
2018-11-22 23:51:58 -04:00
|
|
|
reset_at = self.get_sim_time_cached()
|
2018-08-08 06:09:48 -03:00
|
|
|
|
2019-03-07 18:30:09 -04:00
|
|
|
self.arm_vehicle()
|
|
|
|
self.change_mode("AUTO")
|
2018-08-08 06:09:48 -03:00
|
|
|
|
2019-03-07 18:30:09 -04:00
|
|
|
self.set_rc(3, 1600)
|
2018-08-08 06:09:48 -03:00
|
|
|
|
2019-03-07 18:30:09 -04:00
|
|
|
# should not take off for about least 77 seconds
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
took_off = False
|
|
|
|
while self.armed():
|
|
|
|
now = self.get_sim_time_cached()
|
|
|
|
if now - tstart > 200:
|
|
|
|
# timeout
|
|
|
|
break
|
|
|
|
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
|
|
|
|
now = self.get_sim_time_cached()
|
|
|
|
self.progress("%s" % str(m))
|
|
|
|
if m.seq > delay_item_seq:
|
|
|
|
if not took_off:
|
|
|
|
took_off = True
|
|
|
|
delta_time = now - reset_at
|
|
|
|
if abs(delta_time - delay_for_seconds) > 2:
|
|
|
|
raise NotAchievedException((
|
|
|
|
"Did not take off on time "
|
|
|
|
"measured=%f want=%f" %
|
|
|
|
(delta_time, delay_for_seconds)))
|
|
|
|
|
|
|
|
if not took_off:
|
|
|
|
raise NotAchievedException("Did not take off")
|
2018-08-08 06:09:48 -03:00
|
|
|
|
2018-09-07 01:20:23 -03:00
|
|
|
def fly_zigzag_mode(self):
|
|
|
|
'''test zigzag mode'''
|
|
|
|
|
|
|
|
# set channel 8 for zigzag savewp and recentre it
|
|
|
|
self.set_parameter("RC8_OPTION", 61)
|
|
|
|
|
|
|
|
self.takeoff(alt_min=5, mode='LOITER')
|
|
|
|
|
|
|
|
ZIGZAG = 24
|
|
|
|
j=0
|
|
|
|
while j<4: # conduct test for all 4 directions
|
|
|
|
self.set_rc(8, 1500)
|
|
|
|
self.set_rc(4, 1420)
|
|
|
|
self.wait_heading(352-j*90) # align heading with the run-way
|
|
|
|
self.set_rc(4, 1500)
|
|
|
|
self.change_mode(ZIGZAG)
|
|
|
|
self.set_rc(8, 1100) # record point A
|
|
|
|
self.set_rc(1, 1700) # fly side-way for 20m
|
|
|
|
self.wait_distance(20)
|
|
|
|
self.set_rc(1, 1500)
|
|
|
|
self.wait_groundspeed(0, 0.20) # wait until the copter slows down
|
|
|
|
self.set_rc(8, 1500) # pilot always have to cross mid position when changing for low to high position
|
|
|
|
self.set_rc(8, 1900) # record point B
|
|
|
|
|
|
|
|
i=1
|
|
|
|
while i<2: # run zigzag A->B and B->A for 5 times
|
|
|
|
self.set_rc(2, 1300) # fly forward for 10 meter
|
|
|
|
self.wait_distance(10)
|
|
|
|
self.set_rc(2, 1500) # re-centre pitch rc control
|
|
|
|
self.wait_groundspeed(0, 0.2) #wait until the copter slows down
|
|
|
|
self.set_rc(8, 1500) # switch to mid position
|
|
|
|
self.set_rc(8, 1100) # auto execute vector BA
|
|
|
|
self.wait_distance(17) # wait for it to finish
|
|
|
|
self.wait_groundspeed(0, 0.2) #wait until the copter slows down
|
|
|
|
|
|
|
|
self.set_rc(2, 1300) # fly forward for 10 meter
|
|
|
|
self.wait_distance(10)
|
|
|
|
self.set_rc(2, 1500) # re-centre pitch rc control
|
|
|
|
self.wait_groundspeed(0, 0.2) #wait until the copter slows down
|
|
|
|
self.set_rc(8, 1500) # switch to mid position
|
|
|
|
self.set_rc(8, 1900) # auto execute vector AB
|
|
|
|
self.wait_distance(17) # wait for it to finish
|
|
|
|
self.wait_groundspeed(0, 0.2) #wait until the copter slows down
|
|
|
|
i=i+1
|
|
|
|
# test the case when pilot switch to manual control during the auto flight
|
|
|
|
self.set_rc(2, 1300) # fly forward for 10 meter
|
|
|
|
self.wait_distance(10)
|
|
|
|
self.set_rc(2, 1500) # re-centre pitch rc control
|
|
|
|
self.wait_groundspeed(0, 0.2) #wait until the copter slows down
|
|
|
|
self.set_rc(8, 1500) # switch to mid position
|
|
|
|
self.set_rc(8, 1100) # switch to low position, auto execute vector BA
|
|
|
|
self.wait_distance(8) # purposely switch to manual halfway
|
|
|
|
self.set_rc(8, 1500)
|
|
|
|
self.wait_groundspeed(0, 0.2) # copter should slow down here
|
|
|
|
self.set_rc(2, 1300) # manual control to fly forward
|
|
|
|
self.wait_distance(8)
|
|
|
|
self.set_rc(2, 1500) # re-centre pitch rc control
|
|
|
|
self.wait_groundspeed(0, 0.2) # wait until the copter slows down
|
|
|
|
self.set_rc(8, 1100) # copter should continue mission here
|
|
|
|
self.wait_distance(8) # wait for it to finish rest of BA
|
|
|
|
self.wait_groundspeed(0, 0.2) #wait until the copter slows down
|
|
|
|
self.set_rc(8, 1500) # switch to mid position
|
|
|
|
self.set_rc(8, 1900) # switch to execute AB again
|
|
|
|
self.wait_distance(17) # wait for it to finish
|
|
|
|
self.wait_groundspeed(0, 0.2) # wait until the copter slows down
|
|
|
|
self.change_mode('LOITER')
|
|
|
|
j=j+1
|
|
|
|
|
|
|
|
self.do_RTL()
|
|
|
|
|
|
|
|
|
2018-04-14 08:31:22 -03:00
|
|
|
def test_setting_modes_via_modeswitch(self):
|
2018-07-31 06:50:02 -03:00
|
|
|
self.context_push()
|
2018-04-14 08:31:22 -03:00
|
|
|
ex = None
|
|
|
|
try:
|
|
|
|
fltmode_ch = 5
|
|
|
|
self.set_parameter("FLTMODE_CH", fltmode_ch)
|
|
|
|
self.set_rc(fltmode_ch, 1000) # PWM for mode1
|
|
|
|
testmodes = [("FLTMODE1", 4, "GUIDED", 1165),
|
|
|
|
("FLTMODE2", 13, "SPORT", 1295),
|
|
|
|
("FLTMODE3", 6, "RTL", 1425),
|
|
|
|
("FLTMODE4", 7, "CIRCLE", 1555),
|
|
|
|
("FLTMODE5", 1, "ACRO", 1685),
|
|
|
|
("FLTMODE6", 17, "BRAKE", 1815),
|
2018-07-31 06:50:02 -03:00
|
|
|
]
|
2018-04-14 08:31:22 -03:00
|
|
|
for mode in testmodes:
|
|
|
|
(parm, parm_value, name, pwm) = mode
|
|
|
|
self.set_parameter(parm, parm_value)
|
|
|
|
|
|
|
|
for mode in reversed(testmodes):
|
|
|
|
(parm, parm_value, name, pwm) = mode
|
|
|
|
self.set_rc(fltmode_ch, pwm)
|
|
|
|
self.wait_mode(name)
|
|
|
|
|
|
|
|
for mode in testmodes:
|
|
|
|
(parm, parm_value, name, pwm) = mode
|
|
|
|
self.set_rc(fltmode_ch, pwm)
|
|
|
|
self.wait_mode(name)
|
|
|
|
|
|
|
|
for mode in reversed(testmodes):
|
|
|
|
(parm, parm_value, name, pwm) = mode
|
|
|
|
self.set_rc(fltmode_ch, pwm)
|
|
|
|
self.wait_mode(name)
|
|
|
|
|
|
|
|
self.mavproxy.send('switch 6\n')
|
|
|
|
self.wait_mode("BRAKE")
|
|
|
|
self.mavproxy.send('switch 5\n')
|
|
|
|
self.wait_mode("ACRO")
|
|
|
|
|
|
|
|
except Exception as e:
|
2019-06-18 00:13:49 -03:00
|
|
|
self.progress("Exception caught: %s" % (
|
|
|
|
self.get_exception_stacktrace(e)))
|
2018-04-14 08:31:22 -03:00
|
|
|
ex = e
|
|
|
|
|
|
|
|
self.context_pop()
|
|
|
|
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
|
|
|
def test_setting_modes_via_auxswitch(self):
|
2018-07-31 06:50:02 -03:00
|
|
|
self.context_push()
|
2018-04-14 08:31:22 -03:00
|
|
|
ex = None
|
|
|
|
try:
|
|
|
|
fltmode_ch = int(self.get_parameter("FLTMODE_CH"))
|
|
|
|
self.set_rc(fltmode_ch, 1000)
|
|
|
|
self.wait_mode("CIRCLE")
|
|
|
|
self.set_rc(9, 1000)
|
|
|
|
self.set_rc(10, 1000)
|
|
|
|
self.set_parameter("RC9_OPTION", 18) # land
|
|
|
|
self.set_parameter("RC10_OPTION", 55) # guided
|
|
|
|
self.set_rc(9, 1900)
|
|
|
|
self.wait_mode("LAND")
|
|
|
|
self.set_rc(10, 1900)
|
|
|
|
self.wait_mode("GUIDED")
|
|
|
|
self.set_rc(10, 1000) # this re-polls the mode switch
|
|
|
|
self.wait_mode("CIRCLE")
|
|
|
|
except Exception as e:
|
2019-06-18 00:13:49 -03:00
|
|
|
self.progress("Exception caught: %s" % (
|
|
|
|
self.get_exception_stacktrace(e)))
|
2018-04-14 08:31:22 -03:00
|
|
|
ex = e
|
|
|
|
|
2018-07-31 06:50:02 -03:00
|
|
|
self.context_pop()
|
2018-04-14 08:31:22 -03:00
|
|
|
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2019-02-22 01:29:13 -04:00
|
|
|
def fly_guided_stop(self,
|
|
|
|
timeout=20,
|
|
|
|
groundspeed_tolerance=0.05,
|
|
|
|
climb_tolerance=0.001):
|
|
|
|
"""stop the vehicle moving in guided mode"""
|
|
|
|
self.progress("Stopping vehicle")
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while True:
|
|
|
|
if self.get_sim_time_cached() - tstart > timeout:
|
|
|
|
raise NotAchievedException("Vehicle did not stop")
|
|
|
|
# send a position-control command
|
|
|
|
self.mav.mav.set_position_target_local_ned_send(
|
|
|
|
0, # timestamp
|
|
|
|
1, # target system_id
|
|
|
|
1, # target component id
|
|
|
|
mavutil.mavlink.MAV_FRAME_BODY_NED,
|
|
|
|
0b1111111111111000, # mask specifying use-only-x-y-z
|
|
|
|
0, # x
|
|
|
|
0, # y
|
|
|
|
0, # z
|
|
|
|
0, # vx
|
|
|
|
0, # vy
|
|
|
|
0, # vz
|
|
|
|
0, # afx
|
|
|
|
0, # afy
|
|
|
|
0, # afz
|
|
|
|
0, # yaw
|
|
|
|
0, # yawrate
|
|
|
|
)
|
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
print("%s" % str(m))
|
|
|
|
if (m.groundspeed < groundspeed_tolerance and
|
|
|
|
m.climb < climb_tolerance):
|
|
|
|
break
|
|
|
|
|
2019-03-14 22:00:34 -03:00
|
|
|
def fly_guided_move_global_relative_alt(self, lat, lon, alt):
|
2018-10-12 18:20:13 -03:00
|
|
|
startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
|
|
|
|
blocking=True)
|
|
|
|
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while True:
|
2019-03-07 18:34:09 -04:00
|
|
|
if self.get_sim_time_cached() - tstart > 200:
|
2018-10-12 18:20:13 -03:00
|
|
|
raise NotAchievedException("Did not move far enough")
|
|
|
|
# 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
|
|
|
|
lat, # lat
|
|
|
|
lon, # lon
|
|
|
|
alt, # 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
|
|
|
|
|
2019-02-22 01:29:13 -04:00
|
|
|
def fly_guided_move_local(self, x, y, z_up, timeout=100):
|
|
|
|
"""move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED"""
|
|
|
|
startpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)
|
|
|
|
self.progress("startpos=%s" % str(startpos))
|
|
|
|
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while True:
|
2019-03-07 18:34:09 -04:00
|
|
|
if self.get_sim_time_cached() - tstart > timeout:
|
2019-02-22 01:29:13 -04:00
|
|
|
raise NotAchievedException("Did not start to move")
|
|
|
|
# send a position-control command
|
|
|
|
self.mav.mav.set_position_target_local_ned_send(
|
|
|
|
0, # timestamp
|
|
|
|
1, # target system_id
|
|
|
|
1, # target component id
|
|
|
|
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
|
|
|
|
0b1111111111111000, # mask specifying use-only-x-y-z
|
|
|
|
x, # x
|
|
|
|
y, # y
|
|
|
|
-z_up,# z
|
|
|
|
0, # vx
|
|
|
|
0, # vy
|
|
|
|
0, # vz
|
|
|
|
0, # afx
|
|
|
|
0, # afy
|
|
|
|
0, # afz
|
|
|
|
0, # yaw
|
|
|
|
0, # yawrate
|
|
|
|
)
|
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
print("%s" % m)
|
|
|
|
if m.groundspeed > 0.5:
|
|
|
|
break
|
|
|
|
|
|
|
|
self.progress("Waiting for vehicle to stop...")
|
|
|
|
self.wait_groundspeed(1, 100, timeout=timeout)
|
|
|
|
|
|
|
|
stoppos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)
|
|
|
|
self.progress("stop_pos=%s" % str(stoppos))
|
|
|
|
|
|
|
|
x_achieved = stoppos.x - startpos.x
|
|
|
|
if x_achieved - x > 1:
|
|
|
|
raise NotAchievedException("Did not achieve x position: want=%f got=%f" % (x, x_achieved))
|
|
|
|
|
|
|
|
y_achieved = stoppos.y - startpos.y
|
|
|
|
if y_achieved - y > 1:
|
|
|
|
raise NotAchievedException("Did not achieve y position: want=%f got=%f" % (y, y_achieved))
|
|
|
|
|
|
|
|
z_achieved = stoppos.z - startpos.z
|
|
|
|
if z_achieved - z_up > 1:
|
|
|
|
raise NotAchievedException("Did not achieve z position: want=%f got=%f" % (z_up, z_achieved))
|
|
|
|
|
2019-03-30 21:57:32 -03:00
|
|
|
def test_guided_local_position_target(self, x, y, z_up):
|
|
|
|
""" Check target position being received by vehicle """
|
|
|
|
# set POSITION_TARGET_LOCAL_NED message rate using SET_MESSAGE_INTERVAL
|
|
|
|
self.progress("Setting local target in NED: (%f, %f, %f)" % (x, y, -z_up))
|
|
|
|
self.progress("Setting rate to 1 Hz")
|
|
|
|
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 1)
|
|
|
|
# set position target
|
|
|
|
self.mav.mav.set_position_target_local_ned_send(
|
|
|
|
0, # timestamp
|
|
|
|
1, # target system_id
|
|
|
|
1, # target component id
|
|
|
|
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
|
|
|
|
0b1111111111111000, # mask specifying use only xyz
|
|
|
|
x, # x
|
|
|
|
y, # y
|
|
|
|
-z_up, # z
|
|
|
|
0, # vx
|
|
|
|
0, # vy
|
|
|
|
0, # vz
|
|
|
|
0, # afx
|
|
|
|
0, # afy
|
|
|
|
0, # afz
|
|
|
|
0, # yaw
|
|
|
|
0, # yawrate
|
|
|
|
)
|
|
|
|
m = self.mav.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=2)
|
|
|
|
self.progress("Received local target: %s" % str(m))
|
|
|
|
|
|
|
|
if not (m.type_mask == 0xFFF8 or m.type_mask == 0x0FF8):
|
|
|
|
raise NotAchievedException("Did not receive proper mask: expected=65528 or 4088, got=%u" % m.type_mask)
|
|
|
|
|
|
|
|
if x - m.x > 0.1:
|
|
|
|
raise NotAchievedException("Did not receive proper target position x: wanted=%f got=%f" % (x, m.x))
|
|
|
|
|
|
|
|
if y - m.y > 0.1:
|
|
|
|
raise NotAchievedException("Did not receive proper target position y: wanted=%f got=%f" % (y, m.y))
|
|
|
|
|
|
|
|
if z_up - (-m.z) > 0.1:
|
|
|
|
raise NotAchievedException("Did not receive proper target position z: wanted=%f got=%f" % (z_up, -m.z))
|
|
|
|
|
|
|
|
def test_guided_local_velocity_target(self, vx, vy, vz_up, timeout=3):
|
|
|
|
" Check local target velocity being recieved by vehicle "
|
|
|
|
self.progress("Setting local NED velocity target: (%f, %f, %f)" %(vx, vy, -vz_up))
|
|
|
|
self.progress("Setting POSITION_TARGET_LOCAL_NED message rate to 10Hz")
|
|
|
|
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 10)
|
|
|
|
|
|
|
|
# Drain old messages and ignore the ramp-up to the required target velocity
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while self.get_sim_time_cached() - tstart < timeout:
|
|
|
|
# send velocity-control command
|
|
|
|
self.mav.mav.set_position_target_local_ned_send(
|
|
|
|
0, # timestamp
|
|
|
|
1, # target system_id
|
|
|
|
1, # target component id
|
|
|
|
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
|
|
|
|
0b1111111111000111, # mask specifying use only vx,vy,vz
|
|
|
|
0, # x
|
|
|
|
0, # y
|
|
|
|
0, # z
|
|
|
|
vx, # vx
|
|
|
|
vy, # vy
|
|
|
|
-vz_up, # vz
|
|
|
|
0, # afx
|
|
|
|
0, # afy
|
|
|
|
0, # afz
|
|
|
|
0, # yaw
|
|
|
|
0, # yawrate
|
|
|
|
)
|
|
|
|
m = self.mav.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=1)
|
|
|
|
|
|
|
|
if m is None:
|
|
|
|
raise NotAchievedException("Did not receive any message for 1 sec")
|
|
|
|
|
|
|
|
self.progress("Received local target: %s" % str(m))
|
|
|
|
|
|
|
|
# Check the last received message
|
|
|
|
if not (m.type_mask == 0xFFC7 or m.type_mask == 0x0FC7):
|
|
|
|
raise NotAchievedException("Did not receive proper mask: expected=65479 or 4039, got=%u" % m.type_mask)
|
|
|
|
|
|
|
|
if vx - m.vx > 0.1:
|
|
|
|
raise NotAchievedException("Did not receive proper target velocity vx: wanted=%f got=%f" % (vx, m.vx))
|
|
|
|
|
|
|
|
if vy - m.vy > 0.1:
|
|
|
|
raise NotAchievedException("Did not receive proper target velocity vy: wanted=%f got=%f" % (vy, m.vy))
|
|
|
|
|
|
|
|
if vz_up - (-m.vz) > 0.1:
|
|
|
|
raise NotAchievedException("Did not receive proper target velocity vz: wanted=%f got=%f" % (vz_up, -m.vz))
|
|
|
|
|
|
|
|
self.progress("Received proper target velocity commands")
|
|
|
|
|
|
|
|
def test_position_target_message_mode(self):
|
|
|
|
" Ensure that POSITION_TARGET_LOCAL_NED messages are sent in Guided Mode only "
|
2019-04-09 02:35:19 -03:00
|
|
|
self.hover()
|
2019-03-30 21:57:32 -03:00
|
|
|
self.change_mode('LOITER')
|
|
|
|
self.progress("Setting POSITION_TARGET_LOCAL_NED message rate to 10Hz")
|
|
|
|
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 10)
|
|
|
|
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while self.get_sim_time_cached() < tstart + 5:
|
|
|
|
m = self.mav.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=1)
|
|
|
|
if m is None:
|
|
|
|
continue
|
|
|
|
|
|
|
|
raise NotAchievedException("Received POSITION_TARGET message in LOITER mode: %s" % str(m))
|
|
|
|
|
|
|
|
self.progress("Did not receive any POSITION_TARGET_LOCAL_NED message in LOITER mode. Success")
|
|
|
|
|
2018-07-15 20:54:00 -03:00
|
|
|
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
|
|
|
|
|
2019-03-07 18:34:09 -04:00
|
|
|
def loiter_to_ne(self, x, y, z, timeout=30):
|
2018-07-15 20:54:00 -03:00
|
|
|
dest = rotmat.Vector3(x, y, z)
|
2019-03-07 18:34:09 -04:00
|
|
|
tstart = self.get_sim_time()
|
2018-07-15 20:54:00 -03:00
|
|
|
while True:
|
2019-03-07 18:34:09 -04:00
|
|
|
if self.get_sim_time_cached() - tstart > timeout:
|
|
|
|
raise NotAchievedException("Did not loiter to ne!")
|
2018-07-15 20:54:00 -03:00
|
|
|
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)
|
2019-03-12 23:53:33 -03:00
|
|
|
dist_max = 2
|
|
|
|
self.progress("dist=%f want <%f" % (dist, dist_max))
|
|
|
|
if dist < dist_max:
|
2018-07-15 20:54:00 -03:00
|
|
|
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()
|
2019-03-07 18:34:09 -04:00
|
|
|
while self.get_sim_time_cached() - tstart < 10:
|
2018-07-15 20:54:00 -03:00
|
|
|
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:
|
2019-07-10 10:57:46 -03:00
|
|
|
self.set_analog_rangefinder_parameters()
|
2018-07-15 20:54:00 -03:00
|
|
|
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:
|
2018-12-20 08:05:30 -04:00
|
|
|
self.load_mission("copter_payload_place.txt")
|
2018-07-15 20:54:00 -03:00
|
|
|
|
|
|
|
self.progress("Waiting for location")
|
|
|
|
self.mav.location()
|
2018-10-10 10:07:21 -03:00
|
|
|
self.zero_throttle()
|
2018-07-15 20:54:00 -03:00
|
|
|
self.mavproxy.send('switch 6\n') # stabilize mode
|
2018-12-13 18:17:02 -04:00
|
|
|
self.wait_heartbeat()
|
2018-07-15 20:54:00 -03:00
|
|
|
self.wait_mode('STABILIZE')
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
|
|
|
|
self.arm_vehicle()
|
|
|
|
|
|
|
|
self.mavproxy.send('switch 4\n') # auto mode
|
2018-12-13 18:17:02 -04:00
|
|
|
self.wait_heartbeat()
|
2018-07-15 20:54:00 -03:00
|
|
|
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:
|
2019-06-18 00:13:49 -03:00
|
|
|
self.progress("Exception caught: %s" % (
|
|
|
|
self.get_exception_stacktrace(e)))
|
2018-07-15 20:54:00 -03:00
|
|
|
ex = e
|
|
|
|
|
|
|
|
self.context_pop()
|
|
|
|
self.reboot_sitl()
|
|
|
|
self.progress("All done")
|
|
|
|
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2018-07-30 08:19:14 -03:00
|
|
|
def fly_guided_change_submode(self):
|
2018-07-31 06:50:02 -03:00
|
|
|
""""Ensure we can move around in guided after a takeoff command."""
|
2018-07-30 08:19:14 -03:00
|
|
|
|
2019-03-10 23:58:26 -03:00
|
|
|
'''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.set_parameter("DISARM_DELAY", 0) # until traffic problems are fixed
|
|
|
|
self.change_mode("GUIDED")
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
2018-07-30 08:19:14 -03:00
|
|
|
|
2019-03-10 23:58:26 -03:00
|
|
|
self.user_takeoff(alt_min=10)
|
2019-02-22 01:29:13 -04:00
|
|
|
|
2019-03-10 23:58:26 -03:00
|
|
|
"""yaw through absolute angles using MAV_CMD_CONDITION_YAW"""
|
|
|
|
self.guided_achieve_heading(45)
|
|
|
|
self.guided_achieve_heading(135)
|
2018-07-30 08:19:14 -03:00
|
|
|
|
2019-03-10 23:58:26 -03:00
|
|
|
"""move the vehicle using set_position_target_global_int"""
|
2019-03-14 22:00:34 -03:00
|
|
|
self.fly_guided_move_global_relative_alt(5, 5, 10)
|
2018-07-30 08:19:14 -03:00
|
|
|
|
2019-03-10 23:58:26 -03:00
|
|
|
"""move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED"""
|
|
|
|
self.fly_guided_stop()
|
|
|
|
self.fly_guided_move_local(5, 5, 10)
|
2018-07-30 08:19:14 -03:00
|
|
|
|
2019-03-30 21:57:32 -03:00
|
|
|
""" Check target position received by vehicle using SET_MESSAGE_INTERVAL """
|
|
|
|
self.test_guided_local_position_target(5, 5, 10)
|
|
|
|
self.test_guided_local_velocity_target(2, 2, 1)
|
|
|
|
self.test_position_target_message_mode()
|
|
|
|
|
2019-03-10 23:58:26 -03:00
|
|
|
self.do_RTL()
|
2018-07-30 08:19:14 -03:00
|
|
|
|
2018-10-23 01:56:36 -03:00
|
|
|
def test_gripper_mission(self):
|
|
|
|
self.context_push()
|
|
|
|
ex = None
|
|
|
|
try:
|
|
|
|
self.load_mission("copter-gripper-mission.txt")
|
|
|
|
self.mavproxy.send('mode loiter\n')
|
|
|
|
self.wait_ready_to_arm()
|
2019-06-18 00:13:49 -03:00
|
|
|
self.assert_vehicle_location_is_at_startup_location()
|
2018-10-23 01:56:36 -03:00
|
|
|
self.arm_vehicle()
|
|
|
|
self.mavproxy.send('mode auto\n')
|
|
|
|
self.wait_mode('AUTO')
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
self.mavproxy.expect("Gripper Grabbed")
|
|
|
|
self.mavproxy.expect("Gripper Released")
|
|
|
|
except Exception as e:
|
2019-06-18 00:13:49 -03:00
|
|
|
self.progress("Exception caught: %s" % (
|
|
|
|
self.get_exception_stacktrace(e)))
|
2018-10-23 01:56:36 -03:00
|
|
|
self.mavproxy.send('mode land\n')
|
|
|
|
ex = e
|
|
|
|
self.context_pop()
|
|
|
|
self.mav.motors_disarmed_wait()
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2019-03-06 02:37:26 -04:00
|
|
|
def fly_manual_throttle_mode_change(self):
|
|
|
|
self.set_parameter("FS_GCS_ENABLE", 0) # avoid GUIDED instant disarm
|
|
|
|
self.change_mode("STABILIZE")
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.change_mode("ACRO")
|
|
|
|
self.change_mode("STABILIZE")
|
|
|
|
self.change_mode("GUIDED")
|
|
|
|
self.set_rc(3, 1700)
|
|
|
|
self.watch_altitude_maintained(-1, 0.2) # should not take off in guided
|
|
|
|
self.run_cmd_do_set_mode(
|
|
|
|
"ACRO",
|
|
|
|
want_result=mavutil.mavlink.MAV_RESULT_UNSUPPORTED) # should fix this result code!
|
2019-03-07 05:08:28 -04:00
|
|
|
self.run_cmd_do_set_mode(
|
|
|
|
"STABILIZE",
|
|
|
|
want_result=mavutil.mavlink.MAV_RESULT_UNSUPPORTED) # should fix this result code!
|
|
|
|
self.run_cmd_do_set_mode(
|
|
|
|
"DRIFT",
|
|
|
|
want_result=mavutil.mavlink.MAV_RESULT_UNSUPPORTED) # should fix this result code!
|
2019-03-06 02:37:26 -04:00
|
|
|
self.set_rc(3, 1000)
|
|
|
|
self.run_cmd_do_set_mode("ACRO")
|
|
|
|
self.mav.motors_disarmed_wait()
|
|
|
|
|
2019-03-10 21:59:12 -03:00
|
|
|
def test_mount_pitch(self, despitch, despitch_tolerance, timeout=10):
|
2018-10-12 18:20:13 -03:00
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while True:
|
2019-03-07 18:34:09 -04:00
|
|
|
if self.get_sim_time_cached() - tstart > timeout:
|
2019-03-05 21:02:15 -04:00
|
|
|
raise NotAchievedException("Mount pitch not achieved")
|
2018-10-12 18:20:13 -03:00
|
|
|
|
|
|
|
m = self.mav.recv_match(type='MOUNT_STATUS',
|
|
|
|
blocking=True,
|
|
|
|
timeout=5)
|
|
|
|
# self.progress("pitch=%f roll=%f yaw=%f" %
|
|
|
|
# (m.pointing_a, m.pointing_b, m.pointing_c))
|
|
|
|
mount_pitch = m.pointing_a/100.0 # centidegrees to degrees
|
|
|
|
if abs(despitch - mount_pitch) > despitch_tolerance:
|
|
|
|
self.progress("Mount pitch incorrect: %f != %f" %
|
|
|
|
(mount_pitch, despitch))
|
|
|
|
continue
|
|
|
|
self.progress("Mount pitch correct: %f degrees == %f" %
|
|
|
|
(mount_pitch, despitch))
|
|
|
|
return
|
|
|
|
|
|
|
|
def do_pitch(self, pitch):
|
|
|
|
'''pitch aircraft in guided/angle mode'''
|
|
|
|
self.mav.mav.set_attitude_target_send(
|
|
|
|
0, # time_boot_ms
|
|
|
|
1, # target sysid
|
|
|
|
1, # target compid
|
|
|
|
0, # bitmask of things to ignore
|
|
|
|
mavextra.euler_to_quat([0, math.radians(pitch), 0]), # att
|
|
|
|
0, # roll rate (rad/s)
|
|
|
|
1, # pitch rate
|
|
|
|
0, # yaw rate
|
|
|
|
0.5) # thrust, 0 to 1, translated to a climb/descent rate
|
|
|
|
|
|
|
|
def test_mount(self):
|
|
|
|
ex = None
|
|
|
|
self.context_push()
|
|
|
|
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.progress("Setting up servo mount")
|
|
|
|
roll_servo = 5
|
|
|
|
pitch_servo = 6
|
|
|
|
yaw_servo = 7
|
|
|
|
self.set_parameter("MNT_TYPE", 1)
|
|
|
|
self.set_parameter("SERVO%u_FUNCTION" % roll_servo, 8) # roll
|
|
|
|
self.set_parameter("SERVO%u_FUNCTION" % pitch_servo, 7) # pitch
|
|
|
|
self.set_parameter("SERVO%u_FUNCTION" % yaw_servo, 6) # yaw
|
|
|
|
self.reboot_sitl() # to handle MNT_TYPE changing
|
|
|
|
|
|
|
|
# make sure we're getting mount status and gimbal reports
|
|
|
|
self.mav.recv_match(type='MOUNT_STATUS',
|
|
|
|
blocking=True,
|
|
|
|
timeout=5)
|
|
|
|
self.mav.recv_match(type='GIMBAL_REPORT',
|
|
|
|
blocking=True,
|
|
|
|
timeout=5)
|
|
|
|
|
|
|
|
# test pitch isn't stabilising:
|
|
|
|
m = self.mav.recv_match(type='MOUNT_STATUS',
|
|
|
|
blocking=True,
|
|
|
|
timeout=5)
|
|
|
|
if m.pointing_a != 0 or m.pointing_b != 0 or m.pointing_c != 0:
|
2019-03-05 21:02:15 -04:00
|
|
|
raise NotAchievedException("Mount stabilising when not requested")
|
2018-10-12 18:20:13 -03:00
|
|
|
|
|
|
|
self.mavproxy.send('mode guided\n')
|
|
|
|
self.wait_mode('GUIDED')
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
|
|
|
|
self.user_takeoff()
|
|
|
|
|
|
|
|
despitch = 10
|
|
|
|
despitch_tolerance = 3
|
|
|
|
|
|
|
|
self.progress("Pitching vehicle")
|
|
|
|
self.do_pitch(despitch) # will time out!
|
|
|
|
|
|
|
|
self.wait_pitch(despitch, despitch_tolerance)
|
|
|
|
|
|
|
|
# check we haven't modified:
|
|
|
|
m = self.mav.recv_match(type='MOUNT_STATUS',
|
|
|
|
blocking=True,
|
|
|
|
timeout=5)
|
|
|
|
if m.pointing_a != 0 or m.pointing_b != 0 or m.pointing_c != 0:
|
2019-03-05 21:02:15 -04:00
|
|
|
raise NotAchievedException("Mount stabilising when not requested")
|
2018-10-12 18:20:13 -03:00
|
|
|
|
|
|
|
self.progress("Enable pitch stabilization using MOUNT_CONFIGURE")
|
|
|
|
self.mav.mav.mount_configure_send(
|
|
|
|
1, # target system
|
|
|
|
1, # target component
|
|
|
|
mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING,
|
|
|
|
0, # stab-roll
|
|
|
|
1, # stab-pitch
|
|
|
|
0)
|
|
|
|
|
2019-03-10 21:59:12 -03:00
|
|
|
self.do_pitch(despitch)
|
2018-10-12 18:20:13 -03:00
|
|
|
self.test_mount_pitch(-despitch, 1)
|
|
|
|
|
|
|
|
self.progress("Disable pitch using MAV_CMD_DO_MOUNT_CONFIGURE")
|
|
|
|
self.do_pitch(despitch)
|
|
|
|
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,
|
|
|
|
mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
)
|
|
|
|
self.test_mount_pitch(0, 0)
|
|
|
|
|
|
|
|
self.progress("Point somewhere using MOUNT_CONTROL (ANGLE)")
|
|
|
|
self.do_pitch(despitch)
|
|
|
|
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,
|
|
|
|
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
)
|
|
|
|
self.mav.mav.mount_control_send(
|
|
|
|
1, # target system
|
|
|
|
1, # target component
|
2018-11-22 23:51:58 -04:00
|
|
|
20 * 100, # pitch
|
|
|
|
20 * 100, # roll (centidegrees)
|
2018-10-12 18:20:13 -03:00
|
|
|
0, # yaw
|
|
|
|
0 # save position
|
|
|
|
)
|
|
|
|
self.test_mount_pitch(20, 1)
|
|
|
|
|
|
|
|
self.progress("Point somewhere using MOUNT_CONTROL (GPS)")
|
|
|
|
self.do_pitch(despitch)
|
|
|
|
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,
|
|
|
|
mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
)
|
|
|
|
start = self.mav.location()
|
|
|
|
self.progress("start=%s" % str(start))
|
|
|
|
(t_lat, t_lon) = mavextra.gps_offset(start.lat, start.lng, 10, 20)
|
|
|
|
t_alt = 0
|
|
|
|
|
|
|
|
self.progress("loc %f %f %f" % (start.lat, start.lng, start.alt))
|
|
|
|
self.progress("targetting %f %f %f" % (t_lat, t_lon, t_alt))
|
|
|
|
self.do_pitch(despitch)
|
|
|
|
self.mav.mav.mount_control_send(
|
|
|
|
1, # target system
|
|
|
|
1, # target component
|
2019-03-13 03:12:28 -03:00
|
|
|
int(t_lat * 1e7), # lat
|
|
|
|
int(t_lon * 1e7), # lon
|
2018-10-12 18:20:13 -03:00
|
|
|
t_alt * 100, # alt
|
|
|
|
0 # save position
|
|
|
|
)
|
|
|
|
self.test_mount_pitch(-52, 5)
|
|
|
|
|
|
|
|
# now test RC targetting
|
|
|
|
self.progress("Testing mount RC targetting")
|
|
|
|
|
|
|
|
# this is a one-off; ArduCopter *will* time out this directive!
|
|
|
|
self.progress("Levelling aircraft")
|
|
|
|
self.mav.mav.set_attitude_target_send(
|
|
|
|
0, # time_boot_ms
|
|
|
|
1, # target sysid
|
|
|
|
1, # target compid
|
|
|
|
0, # bitmask of things to ignore
|
|
|
|
mavextra.euler_to_quat([0, 0, 0]), # att
|
|
|
|
1, # roll rate (rad/s)
|
|
|
|
1, # pitch rate
|
|
|
|
1, # yaw rate
|
|
|
|
0.5) # thrust, 0 to 1, translated to a climb/descent rate
|
|
|
|
|
|
|
|
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,
|
|
|
|
mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
)
|
|
|
|
try:
|
|
|
|
self.context_push()
|
|
|
|
self.set_parameter('MNT_RC_IN_ROLL', 11)
|
|
|
|
self.set_parameter('MNT_RC_IN_TILT', 12)
|
|
|
|
self.set_parameter('MNT_RC_IN_PAN', 13)
|
|
|
|
self.progress("Testing RC angular control")
|
|
|
|
self.set_rc(11, 1500)
|
|
|
|
self.set_rc(12, 1500)
|
|
|
|
self.set_rc(13, 1500)
|
|
|
|
self.test_mount_pitch(0, 1)
|
|
|
|
self.set_rc(12, 1400)
|
|
|
|
self.test_mount_pitch(-11.25, 0.01)
|
|
|
|
self.set_rc(12, 1800)
|
|
|
|
self.test_mount_pitch(33.75, 0.01)
|
|
|
|
self.set_rc(11, 1500)
|
|
|
|
self.set_rc(12, 1500)
|
|
|
|
self.set_rc(13, 1500)
|
|
|
|
|
|
|
|
self.progress("Testing RC rate control")
|
|
|
|
self.set_parameter('MNT_JSTICK_SPD', 10)
|
|
|
|
self.test_mount_pitch(0, 1)
|
|
|
|
self.set_rc(12, 1300)
|
|
|
|
self.test_mount_pitch(-5, 1)
|
|
|
|
self.test_mount_pitch(-10, 1)
|
|
|
|
self.test_mount_pitch(-15, 1)
|
|
|
|
self.test_mount_pitch(-20, 1)
|
|
|
|
self.set_rc(12, 1700)
|
|
|
|
self.test_mount_pitch(-15, 1)
|
|
|
|
self.test_mount_pitch(-10, 1)
|
|
|
|
self.test_mount_pitch(-5, 1)
|
|
|
|
self.test_mount_pitch(0, 1)
|
|
|
|
self.test_mount_pitch(5, 1)
|
|
|
|
|
|
|
|
self.progress("Reverting to angle mode")
|
|
|
|
self.set_parameter('MNT_JSTICK_SPD', 0)
|
|
|
|
self.set_rc(12, 1500)
|
|
|
|
self.test_mount_pitch(0, 0.1)
|
|
|
|
|
|
|
|
self.context_pop()
|
|
|
|
|
|
|
|
except Exception:
|
|
|
|
self.context_pop()
|
|
|
|
raise
|
|
|
|
|
|
|
|
self.progress("Testing mount ROI behaviour")
|
|
|
|
self.test_mount_pitch(0, 0.1)
|
|
|
|
start = self.mav.location()
|
|
|
|
self.progress("start=%s" % str(start))
|
|
|
|
(roi_lat, roi_lon) = mavextra.gps_offset(start.lat,
|
|
|
|
start.lng,
|
|
|
|
10,
|
|
|
|
20)
|
|
|
|
roi_alt = 0
|
|
|
|
self.progress("Using MAV_CMD_DO_SET_ROI_LOCATION")
|
|
|
|
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
roi_lat,
|
|
|
|
roi_lon,
|
|
|
|
roi_alt,
|
|
|
|
)
|
|
|
|
self.test_mount_pitch(-52, 5)
|
|
|
|
|
|
|
|
start = self.mav.location()
|
|
|
|
(roi_lat, roi_lon) = mavextra.gps_offset(start.lat,
|
|
|
|
start.lng,
|
|
|
|
-100,
|
|
|
|
-200)
|
|
|
|
roi_alt = 0
|
|
|
|
self.progress("Using MAV_CMD_DO_SET_ROI")
|
|
|
|
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
roi_lat,
|
|
|
|
roi_lon,
|
|
|
|
roi_alt,
|
|
|
|
)
|
|
|
|
self.test_mount_pitch(-7.5, 1)
|
|
|
|
|
|
|
|
self.progress("checking ArduCopter yaw-aircraft-for-roi")
|
|
|
|
try:
|
|
|
|
self.context_push()
|
|
|
|
|
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
self.progress("current heading %u" % m.heading)
|
|
|
|
self.set_parameter("SERVO%u_FUNCTION" % yaw_servo, 0) # yaw
|
|
|
|
self.progress("Waiting for check_servo_map to do its job")
|
|
|
|
self.wait_seconds(5)
|
|
|
|
start = self.mav.location()
|
|
|
|
self.progress("Moving to guided/position controller")
|
2019-03-14 22:00:34 -03:00
|
|
|
self.fly_guided_move_global_relative_alt(1, 0, 0)
|
2018-10-12 18:20:13 -03:00
|
|
|
self.guided_achieve_heading(0)
|
|
|
|
(roi_lat, roi_lon) = mavextra.gps_offset(start.lat,
|
|
|
|
start.lng,
|
|
|
|
-100,
|
|
|
|
-200)
|
|
|
|
roi_alt = 0
|
|
|
|
self.progress("Using MAV_CMD_DO_SET_ROI")
|
|
|
|
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
roi_lat,
|
|
|
|
roi_lon,
|
|
|
|
roi_alt,
|
|
|
|
)
|
|
|
|
|
|
|
|
self.wait_heading(110, timeout=600)
|
|
|
|
|
|
|
|
self.context_pop()
|
|
|
|
except Exception:
|
|
|
|
self.context_pop()
|
|
|
|
raise
|
|
|
|
|
|
|
|
except Exception as e:
|
|
|
|
ex = e
|
|
|
|
self.context_pop()
|
|
|
|
|
2019-02-26 00:29:12 -04:00
|
|
|
self.disarm_vehicle(force=True)
|
2018-10-12 18:20:13 -03:00
|
|
|
self.reboot_sitl() # to handle MNT_TYPE changing
|
|
|
|
|
2018-11-22 23:51:58 -04:00
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2018-03-22 22:31:09 -03:00
|
|
|
def fly_throw_mode(self):
|
|
|
|
# test boomerang mode:
|
|
|
|
self.progress("Throwing vehicle away")
|
|
|
|
self.set_parameter("THROW_NEXTMODE", 6)
|
|
|
|
self.set_parameter("SIM_SHOVE_Z", -30)
|
|
|
|
self.set_parameter("SIM_SHOVE_X", -20)
|
|
|
|
self.change_mode('THROW')
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
2019-03-11 22:29:42 -03:00
|
|
|
try:
|
|
|
|
self.set_parameter("SIM_SHOVE_TIME", 500)
|
|
|
|
except ValueError as e:
|
|
|
|
# the shove resets this to zero
|
|
|
|
pass
|
|
|
|
|
2018-03-22 22:31:09 -03:00
|
|
|
tstart = self.get_sim_time()
|
|
|
|
self.wait_mode('RTL')
|
2019-03-11 22:29:42 -03:00
|
|
|
max_good_tdelta = 15
|
2018-03-22 22:31:09 -03:00
|
|
|
tdelta = self.get_sim_time() - tstart
|
|
|
|
self.progress("Vehicle in RTL")
|
|
|
|
self.mav.motors_disarmed_wait()
|
|
|
|
self.progress("Vehicle disarmed")
|
|
|
|
if tdelta > max_good_tdelta:
|
2019-03-11 22:29:42 -03:00
|
|
|
raise NotAchievedException("Took too long to enter RTL: %fs > %fs" %
|
|
|
|
(tdelta, max_good_tdelta))
|
2018-03-22 22:31:09 -03:00
|
|
|
self.progress("Vehicle returned")
|
|
|
|
|
2019-04-10 22:53:35 -03:00
|
|
|
def test_onboard_compass_calibration(self, timeout=240):
|
|
|
|
twist_x = 2.1
|
|
|
|
twist_y = 2.2
|
|
|
|
twist_z = 2.3
|
|
|
|
ex = None
|
|
|
|
self.context_push()
|
|
|
|
try:
|
|
|
|
self.set_parameter("SIM_GND_BEHAV", 0)
|
|
|
|
self.set_parameter("AHRS_EKF_TYPE", 10)
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
|
|
|
report = self.mav.messages.get("MAG_CAL_REPORT", None)
|
|
|
|
if report is not None:
|
|
|
|
raise PreconditionFailedException("MAG_CAL_REPORT found")
|
|
|
|
|
|
|
|
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_START_MAG_CAL,
|
|
|
|
1, # bitmask of compasses to calibrate
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
timeout=1)
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
last_twist_time = 0
|
|
|
|
while True:
|
|
|
|
now = self.get_sim_time_cached()
|
|
|
|
if now - tstart > timeout:
|
|
|
|
raise NotAchievedException("timeout before cal complete")
|
|
|
|
report = self.mav.messages.get("MAG_CAL_REPORT", None)
|
|
|
|
if report is not None:
|
|
|
|
print("Report: %s" % str(report))
|
|
|
|
break
|
|
|
|
if now - last_twist_time > 5:
|
|
|
|
last_twist_time = now
|
|
|
|
twist_x *= 1.1
|
|
|
|
twist_y *= 1.2
|
|
|
|
twist_z *= 1.3
|
|
|
|
if abs(twist_x) > 10:
|
|
|
|
twist_x /= -2
|
|
|
|
if abs(twist_y) > 10:
|
|
|
|
twist_y /= -2
|
|
|
|
if abs(twist_z) > 10:
|
|
|
|
twist_z /= -2
|
|
|
|
self.set_parameter("SIM_TWIST_X", twist_x)
|
|
|
|
self.set_parameter("SIM_TWIST_Y", twist_y)
|
|
|
|
self.set_parameter("SIM_TWIST_Z", twist_z)
|
|
|
|
try:
|
|
|
|
self.set_parameter("SIM_TWIST_TIME", 100)
|
|
|
|
except ValueError as e:
|
|
|
|
# the shove resets this to zero
|
|
|
|
pass
|
|
|
|
|
|
|
|
m = self.mav.recv_match(type="MAG_CAL_PROGRESS", timeout=1)
|
|
|
|
self.progress("progress: %s" % str(m))
|
|
|
|
if m is None:
|
|
|
|
continue
|
|
|
|
att = self.mav.messages.get("ATTITUDE", None)
|
|
|
|
self.progress("Attitude: %f %f %f" %
|
|
|
|
(math.degrees(att.roll), math.degrees(att.pitch), math.degrees(att.yaw)))
|
|
|
|
except Exception as e:
|
2019-06-18 00:13:49 -03:00
|
|
|
self.progress("Exception caught: %s" % (
|
|
|
|
self.get_exception_stacktrace(e)))
|
2019-04-10 22:53:35 -03:00
|
|
|
ex = e
|
|
|
|
|
|
|
|
self.context_pop()
|
|
|
|
self.reboot_sitl()
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2019-03-23 01:30:34 -03:00
|
|
|
def fly_brake_mode(self):
|
|
|
|
# test brake mode
|
|
|
|
self.progress("Testing brake mode")
|
|
|
|
self.takeoff(10, mode="LOITER")
|
|
|
|
|
|
|
|
self.progress("Ensuring RC inputs have no effect in brake mode")
|
|
|
|
self.change_mode("STABILIZE")
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
self.set_rc(2, 1200)
|
|
|
|
self.wait_groundspeed(5, 1000)
|
|
|
|
|
|
|
|
self.change_mode("BRAKE")
|
|
|
|
self.wait_groundspeed(0, 1)
|
|
|
|
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
|
|
|
|
self.do_RTL()
|
|
|
|
self.progress("Ran brake mode")
|
|
|
|
|
2018-07-15 20:54:00 -03:00
|
|
|
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)
|
|
|
|
|
2019-07-10 10:57:46 -03:00
|
|
|
self.set_analog_rangefinder_parameters()
|
2018-07-15 20:54:00 -03:00
|
|
|
|
|
|
|
# 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()
|
2018-10-10 10:07:21 -03:00
|
|
|
self.zero_throttle()
|
2018-07-15 20:54:00 -03:00
|
|
|
self.mavproxy.send('switch 6\n') # stabilize mode
|
2018-12-13 18:17:02 -04:00
|
|
|
self.wait_heartbeat()
|
2018-07-15 20:54:00 -03:00
|
|
|
self.wait_mode('STABILIZE')
|
|
|
|
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:
|
2019-06-18 00:13:49 -03:00
|
|
|
self.progress("Exception caught: %s" % (
|
|
|
|
self.get_exception_stacktrace(e)))
|
2018-07-15 20:54:00 -03:00
|
|
|
ex = e
|
|
|
|
|
|
|
|
self.context_pop()
|
2018-10-10 10:07:21 -03:00
|
|
|
self.zero_throttle()
|
2019-02-26 00:29:12 -04:00
|
|
|
self.disarm_vehicle(force=True)
|
2018-07-15 20:54:00 -03:00
|
|
|
self.reboot_sitl()
|
|
|
|
self.progress("All done")
|
|
|
|
|
2018-10-12 18:20:13 -03:00
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2019-03-10 08:17:21 -03:00
|
|
|
def loiter_requires_position(self):
|
2019-02-28 20:46:59 -04:00
|
|
|
# ensure we can't switch to LOITER without position
|
|
|
|
self.progress("Ensure we can't enter LOITER without position")
|
|
|
|
self.context_push()
|
|
|
|
self.set_parameter("GPS_TYPE", 0)
|
|
|
|
self.reboot_sitl()
|
|
|
|
self.change_mode('STABILIZE')
|
2019-03-20 21:15:11 -03:00
|
|
|
self.wait_ekf_flags((mavutil.mavlink.ESTIMATOR_ATTITUDE |
|
|
|
|
mavutil.mavlink.ESTIMATOR_VELOCITY_VERT |
|
|
|
|
mavutil.mavlink.ESTIMATOR_POS_VERT_ABS |
|
|
|
|
mavutil.mavlink.ESTIMATOR_CONST_POS_MODE |
|
|
|
|
mavutil.mavlink.ESTIMATOR_PRED_POS_HORIZ_REL),
|
|
|
|
0,
|
|
|
|
timeout=120)
|
2019-02-28 20:46:59 -04:00
|
|
|
self.arm_vehicle()
|
|
|
|
self.mavproxy.send("mode LOITER\n")
|
|
|
|
self.mavproxy.expect("requires position")
|
|
|
|
self.disarm_vehicle()
|
|
|
|
self.context_pop()
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
2019-03-10 08:17:21 -03:00
|
|
|
def test_arm_feature(self):
|
|
|
|
self.loiter_requires_position()
|
|
|
|
|
2019-02-28 20:46:59 -04:00
|
|
|
super(AutoTestCopter, self).test_arm_feature()
|
|
|
|
|
2019-01-09 22:26:36 -04:00
|
|
|
def test_parameter_checks(self):
|
2019-03-04 23:52:24 -04:00
|
|
|
self.test_parameter_checks_poscontrol("PSC")
|
2019-01-09 22:26:36 -04:00
|
|
|
|
2018-12-31 00:23:14 -04:00
|
|
|
def fly_poshold_takeoff(self):
|
|
|
|
"""ensure vehicle stays put until it is ready to fly"""
|
|
|
|
self.context_push()
|
|
|
|
|
|
|
|
ex = None
|
|
|
|
try:
|
|
|
|
self.set_parameter("PILOT_TKOFF_ALT", 700)
|
|
|
|
self.mavproxy.send('mode POSHOLD\n')
|
|
|
|
self.wait_mode('POSHOLD')
|
|
|
|
self.set_rc(3, 1000)
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.wait_seconds(2)
|
|
|
|
# check we are still on the ground...
|
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
if abs(m.relative_alt) > 100:
|
|
|
|
raise NotAchievedException("Took off prematurely")
|
|
|
|
|
|
|
|
self.progress("Pushing throttle up")
|
|
|
|
self.set_rc(3, 1710)
|
|
|
|
self.wait_seconds(0.5)
|
|
|
|
self.progress("Bringing back to hover throttle")
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
|
|
|
|
# make sure we haven't already reached alt:
|
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
max_initial_alt = 500
|
|
|
|
if abs(m.relative_alt) > max_initial_alt:
|
|
|
|
raise NotAchievedException("Took off too fast (%f > %f" %
|
|
|
|
(abs(m.relative_alt), max_initial_alt))
|
|
|
|
|
|
|
|
self.progress("Monitoring takeoff-to-alt")
|
|
|
|
self.wait_altitude(6.9, 8, relative=True)
|
|
|
|
|
|
|
|
self.progress("Making sure we stop at our takeoff altitude")
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while self.get_sim_time() - tstart < 5:
|
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
delta = abs(7000 - m.relative_alt)
|
|
|
|
self.progress("alt=%f delta=%f" % (m.relative_alt/1000,
|
|
|
|
delta/1000))
|
|
|
|
if delta > 1000:
|
|
|
|
raise NotAchievedException("Failed to maintain takeoff alt")
|
|
|
|
self.progress("takeoff OK")
|
|
|
|
except Exception as e:
|
|
|
|
ex = e
|
|
|
|
|
|
|
|
self.mavproxy.send('mode LAND\n')
|
|
|
|
self.wait_mode('LAND')
|
|
|
|
self.mav.motors_disarmed_wait()
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
|
|
|
|
self.context_pop()
|
|
|
|
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2018-12-26 20:16:30 -04:00
|
|
|
def initial_mode(self):
|
|
|
|
return "STABILIZE"
|
|
|
|
|
|
|
|
def initial_mode_switch_mode(self):
|
2019-02-03 07:28:42 -04:00
|
|
|
return "STABILIZE"
|
2018-12-26 20:16:30 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
def default_mode(self):
|
|
|
|
return "STABILIZE"
|
2018-04-14 08:31:22 -03:00
|
|
|
|
2019-02-03 06:43:07 -04:00
|
|
|
def rc_defaults(self):
|
|
|
|
ret = super(AutoTestCopter, self).rc_defaults()
|
|
|
|
ret[3] = 1000
|
2019-02-03 07:28:42 -04:00
|
|
|
ret[5] = 1800 # mode switch
|
2019-02-03 06:43:07 -04:00
|
|
|
return ret
|
2018-08-23 07:24:34 -03:00
|
|
|
|
2019-03-26 22:31:06 -03:00
|
|
|
def test_manual_control(self):
|
|
|
|
'''test manual_control mavlink message'''
|
|
|
|
self.change_mode('STABILIZE')
|
|
|
|
self.takeoff(10)
|
|
|
|
|
|
|
|
tstart = self.get_sim_time_cached()
|
|
|
|
want_pitch_degrees = -20
|
|
|
|
while True:
|
|
|
|
if self.get_sim_time_cached() - tstart > 10:
|
|
|
|
raise AutoTestTimeoutException("Did not reach pitch")
|
|
|
|
self.progress("Sending pitch-forward")
|
|
|
|
self.mav.mav.manual_control_send(
|
|
|
|
1, # target system
|
|
|
|
500, # x (pitch)
|
|
|
|
32767, # y (roll)
|
|
|
|
32767, # z (thrust)
|
|
|
|
32767, # r (yaw)
|
|
|
|
0) # button mask
|
|
|
|
m = self.mav.recv_match(type='ATTITUDE', blocking=True, timeout=1)
|
|
|
|
print("m=%s" % str(m))
|
|
|
|
if m is None:
|
|
|
|
continue
|
|
|
|
p = math.degrees(m.pitch)
|
|
|
|
self.progress("pitch=%f want<=%f" % (p, want_pitch_degrees))
|
|
|
|
if p <= want_pitch_degrees:
|
|
|
|
break
|
|
|
|
self.mav.mav.manual_control_send(
|
|
|
|
1, # target system
|
|
|
|
32767, # x (pitch)
|
|
|
|
32767, # y (roll)
|
|
|
|
32767, # z (thrust)
|
|
|
|
32767, # r (yaw)
|
|
|
|
0) # button mask
|
|
|
|
self.do_RTL()
|
|
|
|
|
2019-05-29 05:32:22 -03:00
|
|
|
def check_avoidance_corners(self):
|
|
|
|
self.takeoff(10, mode="LOITER")
|
|
|
|
self.set_rc(2, 1400)
|
|
|
|
west_loc = mavutil.location(-35.363007,
|
|
|
|
149.164911,
|
|
|
|
0,
|
|
|
|
0)
|
|
|
|
self.wait_location(west_loc, accuracy=6)
|
|
|
|
north_loc = mavutil.location(-35.362908,
|
|
|
|
149.165051,
|
|
|
|
0,
|
|
|
|
0)
|
|
|
|
self.reach_heading_manual(0);
|
|
|
|
self.wait_location(north_loc, accuracy=6)
|
|
|
|
self.reach_heading_manual(90);
|
|
|
|
east_loc = mavutil.location(-35.363013,
|
|
|
|
149.165194,
|
|
|
|
0,
|
|
|
|
0)
|
|
|
|
self.wait_location(east_loc, accuracy=6)
|
|
|
|
self.reach_heading_manual(225);
|
|
|
|
self.wait_location(west_loc, accuracy=6)
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
self.do_RTL()
|
|
|
|
|
|
|
|
def fly_proximity_avoidance_test(self):
|
|
|
|
self.context_push()
|
|
|
|
ex = None
|
|
|
|
try:
|
|
|
|
avoid_filepath = os.path.join(self.mission_directory(),
|
|
|
|
"copter-avoidance-fence.txt")
|
|
|
|
self.mavproxy.send("fence load %s\n" % avoid_filepath)
|
|
|
|
self.mavproxy.expect("Loaded 5 geo-fence")
|
|
|
|
self.set_parameter("FENCE_ENABLE", 0)
|
|
|
|
self.set_parameter("PRX_TYPE", 10)
|
|
|
|
self.set_parameter("RC10_OPTION", 40) # proximity-enable
|
|
|
|
self.reboot_sitl()
|
|
|
|
self.progress("Enabling proximity")
|
|
|
|
self.set_rc(10, 2000)
|
|
|
|
self.check_avoidance_corners()
|
|
|
|
except Exception as e:
|
|
|
|
self.progress("Caught exception: %s" % str(e))
|
|
|
|
ex = e
|
|
|
|
self.context_pop()
|
|
|
|
self.mavproxy.send("fence clear\n")
|
|
|
|
self.disarm_vehicle(force=True)
|
|
|
|
self.reboot_sitl()
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2019-05-29 06:43:12 -03:00
|
|
|
def fly_fence_avoidance_test(self):
|
|
|
|
self.context_push()
|
|
|
|
ex = None
|
|
|
|
try:
|
|
|
|
avoid_filepath = os.path.join(self.mission_directory(),
|
|
|
|
"copter-avoidance-fence.txt")
|
|
|
|
self.mavproxy.send("fence load %s\n" % avoid_filepath)
|
|
|
|
self.mavproxy.expect("Loaded 5 geo-fence")
|
|
|
|
self.set_parameter("FENCE_ENABLE", 1)
|
|
|
|
self.check_avoidance_corners()
|
|
|
|
except Exception as e:
|
|
|
|
self.progress("Caught exception: %s" % str(e))
|
|
|
|
ex = e
|
|
|
|
self.context_pop()
|
|
|
|
self.mavproxy.send("fence clear\n")
|
|
|
|
self.disarm_vehicle(force=True)
|
2019-05-29 07:23:58 -03:00
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2019-06-28 22:37:16 -03:00
|
|
|
def fly_beacon_position(self):
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
|
|
|
self.wait_ready_to_arm(require_absolute=True)
|
|
|
|
|
|
|
|
old_pos = self.mav.location()
|
|
|
|
print("old_pos=%s" % str(old_pos))
|
|
|
|
|
|
|
|
self.context_push()
|
|
|
|
ex = None
|
|
|
|
try:
|
|
|
|
self.set_parameter("BCN_TYPE", 10)
|
|
|
|
self.set_parameter("BCN_LATITUDE", SITL_START_LOCATION.lat)
|
|
|
|
self.set_parameter("BCN_LONGITUDE", SITL_START_LOCATION.lng)
|
|
|
|
self.set_parameter("BCN_ALT", SITL_START_LOCATION.alt)
|
|
|
|
self.set_parameter("BCN_ORIENT_YAW", 45)
|
|
|
|
self.set_parameter("AVOID_ENABLE", 4)
|
|
|
|
self.set_parameter("GPS_TYPE", 0)
|
2019-06-28 23:18:21 -03:00
|
|
|
self.set_parameter("EK3_GPS_TYPE", 3) # NOGPS
|
|
|
|
self.set_parameter("EK3_ENABLE", 1)
|
|
|
|
self.set_parameter("EK2_ENABLE", 0)
|
2019-06-28 22:37:16 -03:00
|
|
|
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
|
|
|
# require_absolute=True infers a GPS is present
|
|
|
|
self.wait_ready_to_arm(require_absolute=False)
|
|
|
|
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
timeout = 20
|
|
|
|
while True:
|
|
|
|
if self.get_sim_time_cached() - tstart > timeout:
|
|
|
|
raise NotAchievedException("Did not get new position like old position")
|
|
|
|
self.progress("Fetching location")
|
|
|
|
new_pos = self.mav.location()
|
|
|
|
pos_delta = self.get_distance(old_pos, new_pos)
|
|
|
|
max_delta = 1
|
|
|
|
self.progress("delta=%u want <= %u" % (pos_delta, max_delta))
|
|
|
|
if pos_delta <= max_delta:
|
|
|
|
break
|
|
|
|
|
|
|
|
self.progress("Moving to ensure location is tracked")
|
|
|
|
self.takeoff(10, mode="LOITER")
|
|
|
|
self.set_rc(2, 1400)
|
|
|
|
self.delay_sim_time(5)
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
self.wait_groundspeed(0, 0.1)
|
|
|
|
pos_delta = self.get_distance(self.sim_location(), self.mav.location)
|
|
|
|
if pos_delta > max_delta:
|
|
|
|
raise NotAchievedException("Vehicle location not tracking simulated location (%u > %u)" %
|
|
|
|
(pos_delta, max_delta))
|
|
|
|
|
|
|
|
except Exception as e:
|
|
|
|
self.progress("Caught exception: %s" % str(e))
|
|
|
|
ex = e
|
|
|
|
self.context_pop()
|
|
|
|
self.disarm_vehicle(force=True)
|
|
|
|
self.reboot_sitl()
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
|
|
|
|
2019-05-29 07:23:58 -03:00
|
|
|
def fly_beacon_avoidance_test(self):
|
|
|
|
self.context_push()
|
|
|
|
ex = None
|
|
|
|
try:
|
|
|
|
self.set_parameter("BCN_TYPE", 10)
|
|
|
|
self.set_parameter("BCN_LATITUDE", int(SITL_START_LOCATION.lat))
|
|
|
|
self.set_parameter("BCN_LONGITUDE", int(SITL_START_LOCATION.lng))
|
|
|
|
self.set_parameter("BCN_ORIENT_YAW", 45)
|
|
|
|
self.set_parameter("AVOID_ENABLE", 4)
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
|
|
|
self.takeoff(10, mode="LOITER")
|
|
|
|
self.set_rc(2, 1400)
|
|
|
|
west_loc = mavutil.location(-35.362919, 149.165055, 0, 0)
|
|
|
|
self.wait_location(west_loc, accuracy=7)
|
|
|
|
self.reach_heading_manual(0)
|
|
|
|
north_loc = mavutil.location(-35.362881, 149.165103, 0, 0)
|
|
|
|
self.wait_location(north_loc, accuracy=7)
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
self.set_rc(1, 1600)
|
|
|
|
east_loc = mavutil.location(-35.362986, 149.165227, 0, 0)
|
|
|
|
self.wait_location(east_loc, accuracy=7)
|
|
|
|
self.set_rc(1, 1500)
|
|
|
|
self.set_rc(2, 1600)
|
|
|
|
south_loc = mavutil.location(-35.363025, 149.165182, 0, 0)
|
|
|
|
self.wait_location(south_loc, accuracy=7)
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
self.do_RTL()
|
|
|
|
|
|
|
|
except Exception as e:
|
|
|
|
self.progress("Caught exception: %s" % str(e))
|
|
|
|
ex = e
|
|
|
|
self.context_pop()
|
|
|
|
self.mavproxy.send("fence clear\n")
|
|
|
|
self.disarm_vehicle(force=True)
|
2019-05-29 06:43:12 -03:00
|
|
|
self.reboot_sitl()
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2019-06-05 20:33:28 -03:00
|
|
|
def get_mission_count(self):
|
|
|
|
return self.get_parameter("MIS_TOTAL")
|
|
|
|
|
|
|
|
def assert_mission_count(self, expected):
|
|
|
|
count = self.get_mission_count()
|
|
|
|
if count != expected:
|
|
|
|
raise NotAchievedException("Unexpected count got=%u want=%u" %
|
|
|
|
(count, expected))
|
|
|
|
|
|
|
|
def test_aux_switch_options(self):
|
|
|
|
self.set_parameter("RC7_OPTION", 58) # clear waypoints
|
|
|
|
self.load_mission("copter_loiter_to_alt.txt")
|
|
|
|
self.set_rc(7, 1000)
|
|
|
|
self.assert_mission_count(5)
|
|
|
|
self.progress("Clear mission")
|
|
|
|
self.set_rc(7, 2000)
|
2019-06-13 20:22:59 -03:00
|
|
|
self.delay_sim_time(1) # allow switch to debounce
|
2019-06-05 20:33:28 -03:00
|
|
|
self.assert_mission_count(0)
|
|
|
|
self.set_rc(7, 1000)
|
|
|
|
self.set_parameter("RC7_OPTION", 24) # reset mission
|
|
|
|
self.delay_sim_time(2)
|
|
|
|
self.load_mission("copter_loiter_to_alt.txt")
|
|
|
|
set_wp = 4
|
|
|
|
self.mavproxy.send("wp set %u\n" % set_wp)
|
|
|
|
self.delay_sim_time(1)
|
|
|
|
self.drain_mav()
|
|
|
|
self.wait_current_waypoint(set_wp, timeout=10)
|
|
|
|
self.progress("Reset mission")
|
|
|
|
self.set_rc(7, 2000)
|
|
|
|
self.delay_sim_time(1)
|
|
|
|
self.drain_mav()
|
|
|
|
self.wait_current_waypoint(0, timeout=10)
|
|
|
|
self.set_rc(7, 1000)
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
def tests(self):
|
|
|
|
'''return list of all tests'''
|
|
|
|
ret = super(AutoTestCopter, self).tests()
|
|
|
|
ret.extend([
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("NavDelayTakeoffAbsTime",
|
|
|
|
"Fly Nav Delay (takeoff)",
|
|
|
|
self.fly_nav_takeoff_delay_abstime),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("NavDelayAbsTime",
|
|
|
|
"Fly Nav Delay (AbsTime)",
|
|
|
|
self.fly_nav_delay_abstime),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("NavDelay",
|
|
|
|
"Fly Nav Delay",
|
|
|
|
self.fly_nav_delay),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("GuidedSubModeChange",
|
|
|
|
"Test submode change",
|
|
|
|
self.fly_guided_change_submode),
|
2018-05-10 04:15:38 -03:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("LoiterToAlt",
|
|
|
|
"Loiter-To-Alt",
|
|
|
|
self.fly_loiter_to_alt),
|
2018-04-27 15:21:53 -03:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("PayLoadPlaceMission",
|
|
|
|
"Payload Place Mission",
|
|
|
|
self.fly_payload_place_mission),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("PrecisionLoiterCompanion",
|
|
|
|
"Precision Loiter (Companion)",
|
|
|
|
self.fly_precision_companion),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("PrecisionLandingSITL",
|
|
|
|
"Precision Landing (SITL)",
|
|
|
|
self.fly_precision_sitl),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("SetModesViaModeSwitch",
|
|
|
|
"Set modes via modeswitch",
|
|
|
|
self.test_setting_modes_via_modeswitch),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("SetModesViaAuxSwitch",
|
|
|
|
"Set modes via auxswitch",
|
|
|
|
self.test_setting_modes_via_auxswitch),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2019-06-05 20:33:28 -03:00
|
|
|
("AuxSwitchOptions",
|
|
|
|
"Test random aux mode options",
|
|
|
|
self.test_aux_switch_options),
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("AutoTune", "Fly AUTOTUNE mode", self.fly_autotune),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-03-22 22:31:09 -03:00
|
|
|
("ThrowMode", "Fly Throw Mode", self.fly_throw_mode),
|
|
|
|
|
2019-03-23 01:30:34 -03:00
|
|
|
("BrakeMode", "Fly Brake Mode", self.fly_brake_mode),
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("RecordThenPlayMission",
|
|
|
|
"Use switches to toggle in mission, then fly it",
|
|
|
|
self.fly_square),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("ThrottleFailsafe",
|
|
|
|
"Test Throttle Failsafe",
|
|
|
|
self.fly_throttle_failsafe),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("BatteryFailsafe",
|
|
|
|
"Fly Battery Failsafe",
|
|
|
|
self.fly_battery_failsafe),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("StabilityPatch",
|
|
|
|
"Fly stability patch",
|
|
|
|
lambda: self.fly_stability_patch(30)),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2019-05-29 05:32:22 -03:00
|
|
|
("AC_Avoidance_Proximity",
|
|
|
|
"Test proximity avoidance slide behaviour",
|
|
|
|
self.fly_proximity_avoidance_test),
|
|
|
|
|
2019-05-29 06:43:12 -03:00
|
|
|
("AC_Avoidance_Fence",
|
|
|
|
"Test fence avoidance slide behaviour",
|
|
|
|
self.fly_fence_avoidance_test),
|
|
|
|
|
2019-05-29 07:23:58 -03:00
|
|
|
("AC_Avoidance_Beacon",
|
|
|
|
"Test beacon avoidance slide behaviour",
|
|
|
|
self.fly_beacon_avoidance_test),
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("HorizontalFence",
|
|
|
|
"Test horizontal fence",
|
2019-03-07 18:28:46 -04:00
|
|
|
self.fly_fence_test),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2019-06-06 21:57:13 -03:00
|
|
|
("HorizontalAvoidFence",
|
|
|
|
"Test horizontal Avoidance fence",
|
|
|
|
self.fly_fence_avoid_test),
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("MaxAltFence",
|
|
|
|
"Test Max Alt Fence",
|
|
|
|
self.fly_alt_max_fence_test),
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("GPSGlitchLoiter",
|
|
|
|
"GPS Glitch Loiter Test",
|
|
|
|
self.fly_gps_glitch_loiter_test),
|
2018-10-02 19:28:21 -03:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("GPSGlitchAuto",
|
|
|
|
"GPS Glitch Auto Test",
|
|
|
|
self.fly_gps_glitch_auto_test),
|
2018-04-27 15:21:53 -03:00
|
|
|
|
2019-03-05 21:29:32 -04:00
|
|
|
("ModeAltHold",
|
|
|
|
"Test AltHold Mode",
|
|
|
|
self.test_mode_ALT_HOLD),
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("ModeLoiter",
|
|
|
|
"Test Loiter Mode",
|
|
|
|
self.loiter),
|
2017-04-10 14:48:48 -03:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("SimpleMode",
|
|
|
|
"Fly in SIMPLE mode",
|
|
|
|
self.fly_simple),
|
2017-04-10 14:48:48 -03:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("SuperSimpleCircle",
|
|
|
|
"Fly a circle in SUPER SIMPLE mode",
|
|
|
|
self.fly_super_simple),
|
2017-04-10 14:48:48 -03:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("ModeCircle",
|
|
|
|
"Fly CIRCLE mode",
|
|
|
|
self.fly_circle),
|
2017-04-10 14:48:48 -03:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("OpticalFlowLimits",
|
|
|
|
"Fly Optical Flow limits",
|
|
|
|
self.fly_optical_flow_limits),
|
2018-05-10 04:15:38 -03:00
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("MotorFail",
|
|
|
|
"Fly motor failure test",
|
|
|
|
self.fly_motor_fail),
|
2011-10-30 23:50:34 -03:00
|
|
|
|
2019-02-02 00:19:18 -04:00
|
|
|
("Flip",
|
|
|
|
"Fly Flip Mode",
|
|
|
|
self.fly_flip),
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("CopterMission",
|
|
|
|
"Fly copter mission",
|
|
|
|
self.fly_auto_test),
|
2011-11-01 08:31:01 -03:00
|
|
|
|
2018-08-20 10:13:40 -03:00
|
|
|
# Gripper test
|
2018-12-17 02:42:21 -04:00
|
|
|
("Gripper",
|
|
|
|
"Test gripper",
|
|
|
|
self.test_gripper),
|
|
|
|
|
|
|
|
("TestGripperMission",
|
|
|
|
"Test Gripper mission items",
|
|
|
|
self.test_gripper_mission),
|
|
|
|
|
|
|
|
("VisionPosition",
|
|
|
|
"Fly Vision Position",
|
|
|
|
self.fly_vision_position),
|
|
|
|
|
2019-06-28 22:37:16 -03:00
|
|
|
("BeaconPosition",
|
|
|
|
"Fly Beacon Position",
|
|
|
|
self.fly_beacon_position),
|
|
|
|
|
2019-01-31 01:01:33 -04:00
|
|
|
("RTLSpeed",
|
|
|
|
"Fly RTL Speed",
|
|
|
|
self.fly_rtl_speed),
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("Mount",
|
|
|
|
"Test Camera/Antenna Mount",
|
|
|
|
self.test_mount),
|
|
|
|
|
2019-01-07 18:51:36 -04:00
|
|
|
("RangeFinder",
|
|
|
|
"Test RangeFinder Basic Functionality",
|
|
|
|
self.test_rangefinder),
|
|
|
|
|
2019-06-18 00:13:49 -03:00
|
|
|
("SurfaceTracking",
|
|
|
|
"Test Surface Tracking",
|
|
|
|
self.test_surface_tracking),
|
|
|
|
|
2019-01-08 21:40:49 -04:00
|
|
|
("Parachute",
|
|
|
|
"Test Parachute Functionality",
|
|
|
|
self.test_parachute),
|
|
|
|
|
2019-01-09 22:26:36 -04:00
|
|
|
("ParameterChecks",
|
|
|
|
"Test Arming Parameter Checks",
|
|
|
|
self.test_parameter_checks),
|
|
|
|
|
2019-03-06 02:37:26 -04:00
|
|
|
("ManualThrottleModeChange",
|
|
|
|
"Check manual throttle mode changes denied on high throttle",
|
|
|
|
self.fly_manual_throttle_mode_change),
|
|
|
|
|
2019-03-26 22:31:06 -03:00
|
|
|
("MANUAL_CONTROL",
|
|
|
|
"Test mavlink MANUAL_CONTROL",
|
|
|
|
self.test_manual_control),
|
|
|
|
|
2018-09-07 01:20:23 -03:00
|
|
|
# Zigzag mode test
|
|
|
|
("ZigZag", "Fly ZigZag Mode", self.fly_zigzag_mode),
|
|
|
|
|
2018-12-31 00:23:14 -04:00
|
|
|
("PosHoldTakeOff",
|
|
|
|
"Fly POSHOLD takeoff",
|
|
|
|
self.fly_poshold_takeoff),
|
|
|
|
|
2019-04-10 22:53:35 -03:00
|
|
|
("OnboardCompassCalibration",
|
|
|
|
"Test onboard compass calibration",
|
|
|
|
self.test_onboard_compass_calibration),
|
|
|
|
|
2018-12-17 02:42:21 -04:00
|
|
|
("LogDownLoad",
|
|
|
|
"Log download",
|
|
|
|
lambda: self.log_download(
|
2019-02-28 06:51:22 -04:00
|
|
|
self.buildlogs_path("ArduCopter-log.bin"),
|
2018-12-17 02:42:21 -04:00
|
|
|
upload_logs=len(self.fail_list) > 0))
|
|
|
|
])
|
|
|
|
return ret
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2019-01-14 01:06:41 -04:00
|
|
|
def disabled_tests(self):
|
|
|
|
return {
|
|
|
|
"Parachute": "See https://github.com/ArduPilot/ardupilot/issues/4702",
|
2019-06-07 01:52:13 -03:00
|
|
|
"HorizontalAvoidFence": "See https://github.com/ArduPilot/ardupilot/issues/11525",
|
2019-06-30 20:48:08 -03:00
|
|
|
"BeaconPosition": "See https://github.com/ArduPilot/ardupilot/issues/11689",
|
2019-01-14 01:06:41 -04:00
|
|
|
}
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-12-17 02:57:16 -04:00
|
|
|
class AutoTestHeli(AutoTestCopter):
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2019-03-09 00:20:36 -04:00
|
|
|
def log_name(self):
|
|
|
|
return "HeliCopter"
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2019-03-09 00:20:36 -04:00
|
|
|
def default_frame(self):
|
|
|
|
return "heli"
|
2015-05-27 05:13:55 -03:00
|
|
|
|
2019-02-15 20:46:33 -04:00
|
|
|
def sitl_start_location(self):
|
|
|
|
return SITL_START_LOCATION_AVC
|
|
|
|
|
2018-10-10 10:07:21 -03:00
|
|
|
def is_heli(self):
|
|
|
|
return True
|
|
|
|
|
2019-02-03 06:43:07 -04:00
|
|
|
def rc_defaults(self):
|
|
|
|
ret = super(AutoTestHeli, self).rc_defaults()
|
|
|
|
ret[8] = 1000
|
|
|
|
ret[3] = 1000 # collective
|
|
|
|
return ret
|
2015-05-27 05:13:55 -03:00
|
|
|
|
2019-03-10 08:17:21 -03:00
|
|
|
def loiter_requires_position(self):
|
|
|
|
self.progress("Skipping loiter-requires-position for heli; rotor runup issues")
|
|
|
|
|
2019-03-25 23:07:45 -03:00
|
|
|
# fly_avc_test - fly AVC mission
|
|
|
|
def fly_avc_test(self):
|
|
|
|
# Arm
|
|
|
|
self.mavproxy.send('switch 6\n') # stabilize mode
|
|
|
|
self.wait_mode('STABILIZE')
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.progress("Raising rotor speed")
|
|
|
|
self.set_rc(8, 2000)
|
|
|
|
|
|
|
|
# upload mission from file
|
|
|
|
self.progress("# Load copter_AVC2013_mission")
|
|
|
|
# load the waypoint count
|
|
|
|
num_wp = self.load_mission("copter_AVC2013_mission.txt")
|
|
|
|
if not num_wp:
|
|
|
|
raise NotAchievedException("load copter_AVC2013_mission failed")
|
|
|
|
|
|
|
|
self.progress("Fly AVC mission from 1 to %u" % num_wp)
|
|
|
|
self.mavproxy.send('wp set 1\n')
|
|
|
|
|
|
|
|
# wait for motor runup
|
|
|
|
self.wait_seconds(20)
|
|
|
|
|
|
|
|
# switch into AUTO mode and raise throttle
|
|
|
|
self.mavproxy.send('switch 4\n') # auto mode
|
|
|
|
self.wait_mode('AUTO')
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
|
|
|
|
# fly the mission
|
|
|
|
self.wait_waypoint(0, num_wp-1, timeout=500)
|
|
|
|
|
|
|
|
# set throttle to minimum
|
|
|
|
self.zero_throttle()
|
|
|
|
|
|
|
|
# wait for disarm
|
|
|
|
self.mav.motors_disarmed_wait()
|
|
|
|
self.progress("MOTORS DISARMED OK")
|
|
|
|
|
|
|
|
self.progress("Lowering rotor speed")
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
|
|
|
|
self.progress("AVC mission completed: passed!")
|
|
|
|
|
2018-11-27 23:21:29 -04:00
|
|
|
def fly_heli_poshold_takeoff(self):
|
|
|
|
"""ensure vehicle stays put until it is ready to fly"""
|
|
|
|
self.context_push()
|
|
|
|
|
|
|
|
ex = None
|
|
|
|
try:
|
|
|
|
self.set_parameter("PILOT_TKOFF_ALT", 700)
|
|
|
|
self.mavproxy.send('mode POSHOLD\n')
|
|
|
|
self.wait_mode('POSHOLD')
|
|
|
|
self.set_rc(3, 1000)
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.set_rc(8, 2000)
|
|
|
|
self.progress("wait for rotor runup to complete")
|
|
|
|
self.wait_servo_channel_value(8, 1900, timeout=10)
|
|
|
|
self.wait_seconds(20)
|
|
|
|
# check we are still on the ground...
|
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
if abs(m.relative_alt) > 100:
|
|
|
|
raise NotAchievedException("Took off prematurely")
|
|
|
|
self.progress("Pushing throttle past half-way")
|
|
|
|
self.set_rc(3, 1600)
|
|
|
|
self.wait_seconds(0.5)
|
|
|
|
self.progress("Bringing back to hover throttle")
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
|
|
|
|
# make sure we haven't already reached alt:
|
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
if abs(m.relative_alt) > 500:
|
|
|
|
raise NotAchievedException("Took off too fast")
|
|
|
|
|
|
|
|
self.progress("Monitoring takeoff-to-alt")
|
|
|
|
self.wait_altitude(6.9, 8, relative=True)
|
|
|
|
|
|
|
|
self.progress("Making sure we stop at our takeoff altitude")
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while self.get_sim_time() - tstart < 5:
|
|
|
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
|
|
|
delta = abs(7000 - m.relative_alt)
|
|
|
|
self.progress("alt=%f delta=%f" % (m.relative_alt/1000,
|
|
|
|
delta/1000))
|
|
|
|
if delta > 1000:
|
|
|
|
raise NotAchievedException("Failed to maintain takeoff alt")
|
|
|
|
self.progress("takeoff OK")
|
|
|
|
except Exception as e:
|
|
|
|
ex = e
|
|
|
|
|
|
|
|
self.mavproxy.send('mode LAND\n')
|
|
|
|
self.wait_mode('LAND')
|
|
|
|
self.mav.motors_disarmed_wait()
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
|
|
|
|
self.context_pop()
|
|
|
|
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2019-04-15 22:44:29 -03:00
|
|
|
def fly_spline_waypoint(self):
|
|
|
|
"""ensure basic spline functionality works"""
|
|
|
|
self.load_mission("copter_spline_mission.txt")
|
|
|
|
self.change_mode("LOITER")
|
|
|
|
self.wait_ready_to_arm()
|
2019-04-24 23:16:44 -03:00
|
|
|
self.arm_vehicle()
|
2019-04-15 22:44:29 -03:00
|
|
|
self.progress("Raising rotor speed")
|
|
|
|
self.set_rc(8, 2000)
|
|
|
|
self.wait_seconds(20)
|
|
|
|
self.change_mode("AUTO")
|
|
|
|
self.set_rc(3, 1500)
|
2019-04-26 02:10:11 -03:00
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while True:
|
|
|
|
if not self.armed():
|
|
|
|
break
|
|
|
|
remaining = self.get_sim_time() - tstart
|
|
|
|
if remaining <= 0:
|
|
|
|
raise AutoTestTimeoutException("Vehicle did not disarm after mission")
|
|
|
|
self.delay_sim_time(1)
|
2019-04-15 22:44:29 -03:00
|
|
|
self.progress("Lowering rotor speed")
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
|
2018-11-27 23:21:29 -04:00
|
|
|
def set_rc_default(self):
|
|
|
|
super(AutoTestCopter, self).set_rc_default()
|
|
|
|
self.progress("Lowering rotor speed")
|
|
|
|
self.set_rc(8, 1000)
|
|
|
|
|
2018-12-17 02:57:16 -04:00
|
|
|
def tests(self):
|
|
|
|
'''return list of all tests'''
|
|
|
|
ret = super(AutoTestCopter, self).tests()
|
|
|
|
ret.extend([
|
|
|
|
("AVCMission", "Fly AVC mission", self.fly_avc_test),
|
2013-05-29 02:33:32 -03:00
|
|
|
|
2018-11-27 23:21:29 -04:00
|
|
|
("PosHoldTakeOff",
|
|
|
|
"Fly POSHOLD takeoff",
|
|
|
|
self.fly_heli_poshold_takeoff),
|
|
|
|
|
2019-04-15 22:44:29 -03:00
|
|
|
("SplineWaypoint",
|
|
|
|
"Fly Spline Waypoints",
|
|
|
|
self.fly_spline_waypoint),
|
|
|
|
|
2018-12-17 02:57:16 -04:00
|
|
|
("LogDownLoad",
|
|
|
|
"Log download",
|
|
|
|
lambda: self.log_download(
|
2019-02-28 06:51:22 -04:00
|
|
|
self.buildlogs_path("ArduCopter-log.bin"),
|
2018-12-17 02:57:16 -04:00
|
|
|
upload_logs=len(self.fail_list) > 0))
|
|
|
|
])
|
|
|
|
return ret
|