ardupilot/Tools/autotest/blimp.py

253 lines
7.6 KiB
Python

'''
Fly Blimp in SITL
AP_FLAKE8_CLEAN
'''
from __future__ import print_function
import os
import shutil
from pymavlink import mavutil
from common import AutoTest
# get location of scripts
testdir = os.path.dirname(os.path.realpath(__file__))
SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 584, 0)
# Flight mode switch positions are set-up in blimp.parm to be
# switch 1 = Land
# switch 2 = Manual
# switch 3 = Velocity
# switch 4 = Loiter
# switch 5 = Manual
# switch 6 = Manual
class AutoTestBlimp(AutoTest):
@staticmethod
def get_not_armable_mode_list():
return []
@staticmethod
def get_not_disarmed_settable_modes_list():
return []
@staticmethod
def get_no_position_not_settable_modes_list():
return []
@staticmethod
def get_position_armable_modes_list():
return []
@staticmethod
def get_normal_armable_modes_list():
return []
def log_name(self):
return "Blimp"
def default_mode(self):
return "MANUAL"
def test_filepath(self):
return os.path.realpath(__file__)
def default_speedup(self):
return 100
def set_current_test_name(self, name):
self.current_test_name_directory = "Blimp_Tests/" + name + "/"
def sitl_start_location(self):
return SITL_START_LOCATION
def sitl_streamrate(self):
return 5
def vehicleinfo_key(self):
return 'Blimp'
def default_frame(self):
return "Blimp"
def apply_defaultfile_parameters(self):
# Blimp passes in a defaults_filepath in place of applying
# parameters afterwards.
pass
def defaults_filepath(self):
return self.model_defaults_filepath(self.frame)
def wait_disarmed_default_wait_time(self):
return 120
def close(self):
super(AutoTestBlimp, self).close()
# [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
# This flag tells me that I need to copy the data out
if self.copy_tlog:
shutil.copy(self.logfile, self.buildlog)
def is_blimp(self):
return True
def get_stick_arming_channel(self):
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)
def FlyManual(self):
'''test manual mode'''
self.change_mode('MANUAL')
self.wait_ready_to_arm()
self.arm_vehicle()
acc = 0.5
# make sure we don't drift:
bl = self.mav.location()
tl = self.offset_location_ne(location=bl, metres_north=2, metres_east=0)
ttl = self.offset_location_ne(location=bl, metres_north=4, metres_east=0)
tr = self.offset_location_ne(location=bl, metres_north=4, metres_east=2)
ttr = self.offset_location_ne(location=bl, metres_north=4, metres_east=4)
if self.mavproxy is not None:
self.mavproxy.send(f"map icon {bl.lat} {bl.lng} flag\n")
self.mavproxy.send(f"map icon {tl.lat} {tl.lng} flag\n")
self.mavproxy.send(f"map icon {ttl.lat} {ttl.lng} flag\n")
self.mavproxy.send(f"map icon {tr.lat} {tr.lng} flag\n")
self.mavproxy.send(f"map icon {ttr.lat} {ttr.lng} flag\n")
self.set_rc(2, 2000)
self.wait_distance_to_location(tl, 0, acc, timeout=10)
self.set_rc(2, 1500)
self.wait_distance_to_location(ttl, 0, acc, timeout=15)
self.set_rc(1, 2000)
self.wait_distance_to_location(tr, 0, acc, timeout=10)
self.set_rc(1, 1500)
self.wait_distance_to_location(ttr, 0, acc, timeout=15)
self.change_mode('RTL')
self.wait_distance_to_location(bl, 0, 0.5, timeout=30, minimum_duration=5) # make sure it can hold position
self.change_mode('MANUAL')
self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot
self.set_rc(3, 2000)
self.wait_altitude(5, 5.5, relative=True, timeout=15)
self.set_rc(3, 1500)
self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot
self.set_rc(4, 1000)
self.wait_heading(340, accuracy=5, timeout=5) # short timeout to check yawrate
self.set_rc(4, 1500)
self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot
self.set_rc(3, 1000)
self.wait_altitude(0, 0.5, relative=True, timeout=20)
self.set_rc(3, 1500)
self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot
self.set_rc(4, 2000)
self.wait_heading(135, accuracy=5, timeout=10) # short timeout to check yawrate
self.set_rc(4, 1500)
self.wait_distance_to_location(bl, 0, acc, timeout=5) # make sure we haven't moved from the spot
self.disarm_vehicle()
def FlyLoiter(self):
'''test loiter mode'''
self.change_mode('LOITER')
self.wait_ready_to_arm()
self.arm_vehicle()
siz = 5
tim = 60
# make sure we don't drift:
bl = self.mav.location()
tl = self.offset_location_ne(location=bl, metres_north=siz, metres_east=0)
tr = self.offset_location_ne(location=bl, metres_north=siz, metres_east=siz)
br = self.offset_location_ne(location=bl, metres_north=0, metres_east=siz)
print("Locations are:")
print("bottom left ", bl.lat, bl.lng)
print("top left ", tl.lat, tl.lng)
print("top right ", tr.lat, tr.lng)
print("bottom right ", br.lat, br.lng)
if self.mavproxy is not None:
self.mavproxy.send(f"map icon {bl.lat} {bl.lng} flag\n")
self.mavproxy.send(f"map icon {tl.lat} {tl.lng} flag\n")
self.mavproxy.send(f"map icon {tr.lat} {tr.lng} flag\n")
self.mavproxy.send(f"map icon {br.lat} {br.lng} flag\n")
self.set_parameter("SIMPLE_MODE", 1)
self.set_rc(2, 2000)
self.wait_distance_to_location(tl, 0, 0.2, timeout=tim)
self.set_rc(2, 1500)
self.set_rc(1, 2000)
self.wait_distance_to_location(tr, 0, 0.5, timeout=tim)
self.set_rc(1, 1500)
self.set_rc(2, 1000)
self.wait_distance_to_location(br, 0, 0.5, timeout=tim)
self.set_rc(2, 1500)
self.set_rc(1, 1000)
self.wait_distance_to_location(bl, 0, 0.5, timeout=tim)
self.set_rc(1, 1500)
fin = self.mav.location()
self.set_rc(4, 1700)
self.wait_heading(135, accuracy=2, timeout=tim)
self.set_rc(4, 1500)
self.wait_distance_to_location(fin, 0, 0.15, timeout=5) # make sure we haven't moved from the spot
self.set_rc(3, 2000)
self.wait_altitude(5, 5.5, relative=True, timeout=60)
self.set_rc(3, 1000)
self.wait_altitude(0, 0.5, relative=True, timeout=60)
self.set_rc(3, 1500)
self.wait_distance_to_location(fin, 0, 0.15, timeout=5) # make sure we haven't moved from the spot
self.set_rc(4, 1300)
self.wait_heading(0, accuracy=2, timeout=tim)
self.set_rc(4, 1500)
self.wait_distance_to_location(fin, 0, 0.15, timeout=5) # make sure we haven't moved from the spot
self.disarm_vehicle()
def tests(self):
'''return list of all tests'''
# ret = super(AutoTestBlimp, self).tests()
ret = []
ret.extend([
self.FlyManual,
self.FlyLoiter,
])
return ret
def disabled_tests(self):
return {
}