def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, fftLength=32, peakhz=None):
# find a motor peak
self.takeoff(10, mode="QHOVER")
hover_time = 15
tstart = self.get_sim_time()
self.progress("Hovering for %u seconds" % hover_time)
while self.get_sim_time_cached() < tstart + hover_time:
self.mav.recv_match(type='ATTITUDE', blocking=True)
vfr_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
tend = self.get_sim_time()
self.do_RTL()
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
freq = psd["F"][numpy.argmax(psd["X"][minhz:maxhz]) + minhz] * (1000. / 1024.)
peakdb = numpy.amax(psd["X"][minhz:maxhz])
if peakdb < dblevel or (peakhz is not None and abs(freq - peakhz) / peakhz > 0.05):
raise NotAchievedException("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
else:
self.progress("Detected motor peak at %fHz, throttle %f%%, %fdB" % (freq, vfr_hud.throttle, peakdb))
# we have a peak make sure that the FFT detected something close
# logging is at 10Hz
mlog = self.dfreader_for_current_onboard_log()
# accuracy is determined by sample rate and fft length, given our use of quinn we could probably use half of this
freqDelta = 1000. / fftLength
pkAvg = freq
while True:
m = mlog.recv_match(
type='FTN1',
blocking=True,
condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6))
if m is None:
break
pkAvg = pkAvg + (0.1 * (m.PkAvg - pkAvg))
# peak within resolution of FFT length
if abs(pkAvg - freq) > freqDelta:
raise NotAchievedException("FFT did not detect a motor peak at %f, found %f, wanted %f" % (dblevel, pkAvg, freq))
return freq
def test_wheelencoders(self):
'''make sure wheel encoders are generally working'''
ex = None
try:
self.set_parameter("ATC_BAL_SPD_FF", 0)
self.set_parameter("WENC_TYPE", 10)
self.set_parameter("AHRS_EKF_TYPE", 10)
self.reboot_sitl()
self.set_parameter("WENC2_TYPE", 10)
self.set_parameter("WENC_POS_Y", 0.075)
self.set_parameter("WENC2_POS_Y", -0.075)
self.reboot_sitl()
self.change_mode("HOLD")
self.wait_ready_to_arm()
self.change_mode("ACRO")
self.arm_vehicle()
self.set_rc(3, 1600)
m = self.mav.recv_match(type='WHEEL_DISTANCE',
blocking=True,
timeout=5)
if m is None:
raise NotAchievedException("Did not get WHEEL_DISTANCE")
tstart = self.get_sim_time()
while True:
if self.get_sim_time_cached() - tstart > 10:
break
dist_home = self.distance_to_home(use_cached_home=True)
m = self.mav.messages.get("WHEEL_DISTANCE")
delta = abs(m.distance[0] - dist_home)
self.progress("dist-home=%f wheel-distance=%f delta=%f" %
(dist_home, m.distance[0], delta))
if delta > 5:
raise NotAchievedException("wheel distance incorrect")
self.disarm_vehicle()
except Exception as e:
self.progress("Caught exception: %s" %
self.get_exception_stacktrace(e))
self.disarm_vehicle()
ex = e
self.reboot_sitl()
if ex is not None:
raise ex
def enum_state_name(self, enum_name, state, pretrim=None):
e = mavutil.mavlink.enums[enum_name]
e_value = e[state]
name = e_value.name
if pretrim is not None:
if not pretrim.startswith(pretrim):
raise NotAchievedException("Expected %s to pretrim" %
(pretrim))
name = name.replace(pretrim, "")
return name
def test_sysid_enforce(self):
'''Run the same arming code with correct then incorrect SYSID'''
self.context_push()
ex = None
try:
# if set_parameter is ever changed to not use MAVProxy
# this test is going to break horribly. Sorry.
self.set_parameter("SYSID_MYGCS",
255) # assume MAVProxy does this!
self.set_parameter("SYSID_ENFORCE",
1) # assume MAVProxy does this!
self.change_mode('MANUAL')
self.progress("make sure I can arm ATM")
self.wait_ready_to_arm()
self.arm_vehicle(timeout=5)
self.disarm_vehicle()
# temporarily set a different system ID than MAVProxy:
self.progress("Attempting to arm vehicle myself")
old_srcSystem = self.mav.mav.srcSystem
try:
self.mav.mav.srcSystem = 243
self.arm_vehicle(timeout=5)
self.disarm_vehicle()
success = False
except AutoTestTimeoutException as e:
success = True
pass
self.mav.srcSystem = old_srcSystem
if not success:
raise NotAchievedException(
"Managed to arm with SYSID_ENFORCE set")
self.progress("Attempting to arm vehicle from vehicle component")
old_srcSystem = self.mav.mav.srcSystem
comp_arm_exception = None
try:
self.mav.mav.srcSystem = 1
self.arm_vehicle(timeout=5)
self.disarm_vehicle()
except Exception as e:
comp_arm_exception = e
pass
self.mav.srcSystem = old_srcSystem
if comp_arm_exception is not None:
raise comp_arm_exception
except Exception as e:
self.progress("Exception caught")
ex = e
self.context_pop()
if ex is not None:
raise ex
def test_gcs_fence(self):
self.progress("Testing FENCE_POINT protocol")
self.set_parameter("FENCE_TOTAL", 1)
target_system = 1
target_component = 1
lat = 1.2345
lng = 5.4321
self.mav.mav.fence_point_send(target_system, target_component, 0, 1,
lat, lng)
self.progress("Requesting fence return point")
self.mav.mav.fence_fetch_point_send(target_system, target_component, 0)
m = self.mav.recv_match(type="FENCE_POINT", blocking=True, timeout=1)
print("m: %s" % str(m))
if m is None:
raise NotAchievedException("Did not get fence return point back")
if abs(m.lat - lat) > 0.000001:
raise NotAchievedException(
"Did not get correct lat in fencepoint: got=%f want=%f" %
(m.lat, lat))
if abs(m.lng - lng) > 0.000001:
raise NotAchievedException(
"Did not get correct lng in fencepoint: got=%f want=%f" %
(m.lng, lng))
self.progress("Now testing a different value")
lat = 2.345
lng = 4.321
self.mav.mav.fence_point_send(target_system, target_component, 0, 1,
lat, lng)
self.progress("Requesting fence return point")
self.mav.mav.fence_fetch_point_send(target_system, target_component, 0)
m = self.mav.recv_match(type="FENCE_POINT", blocking=True, timeout=1)
print("m: %s" % str(m))
if abs(m.lat - lat) > 0.000001:
raise NotAchievedException(
"Did not get correct lat in fencepoint: got=%f want=%f" %
(m.lat, lat))
if abs(m.lng - lng) > 0.000001:
raise NotAchievedException(
"Did not get correct lng in fencepoint: got=%f want=%f" %
(m.lng, lng))
def wait_distance_home_gt(self, distance, timeout=60):
home_distance = None
tstart = self.get_sim_time()
while self.get_sim_time() - tstart < timeout:
# m = self.mav.recv_match(type='VFR_HUD', blocking=True)
pos = self.mav.location()
home_distance = self.get_distance(HOME, pos)
if home_distance > distance:
return
raise NotAchievedException("Failed to get %fm from home (now=%f)" %
(distance, home_distance))
def test_alt_hold(self):
"""Test ALT_HOLD mode
"""
self.wait_ready_to_arm()
self.arm_vehicle()
self.mavproxy.send('mode ALT_HOLD\n')
self.wait_mode('ALT_HOLD')
msg = self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True,
timeout=5)
if msg is None:
raise NotAchievedException("Did not get GLOBAL_POSITION_INT")
pwm = 1000
if msg.relative_alt / 1000.0 < -5.5:
# need to g`o up, not down!
pwm = 2000
self.set_rc(Joystick.Throttle, pwm)
self.wait_altitude(altitude_min=-6, altitude_max=-5)
self.set_rc(Joystick.Throttle, 1500)
# let the vehicle settle (momentum / stopping point shenanigans....)
self.delay_sim_time(1)
self.watch_altitude_maintained()
self.set_rc(Joystick.Throttle, 1000)
self.wait_altitude(altitude_min=-20, altitude_max=-19)
self.set_rc(Joystick.Throttle, 1500)
# let the vehicle settle (momentum / stopping point shenanigans....)
self.delay_sim_time(1)
self.watch_altitude_maintained()
self.set_rc(Joystick.Throttle, 1900)
self.wait_altitude(altitude_min=-14, altitude_max=-13)
self.set_rc(Joystick.Throttle, 1500)
# let the vehicle settle (momentum / stopping point shenanigans....)
self.delay_sim_time(1)
self.watch_altitude_maintained()
self.set_rc(Joystick.Throttle, 1900)
self.wait_altitude(altitude_min=-5, altitude_max=-4)
self.set_rc(Joystick.Throttle, 1500)
# let the vehicle settle (momentum / stopping point shenanigans....)
self.delay_sim_time(1)
self.watch_altitude_maintained()
self.disarm_vehicle()
def achieve_attitude(self, desyaw, despitch, tolerance=1, target_system=2, target_component=1):
'''use set_attitude_target to achieve desyaw / despitch'''
tstart = self.get_sim_time()
last_attitude_target_sent = 0
last_debug = 0
self.progress("Using set_attitude_target to achieve attitude")
while True:
now = self.get_sim_time()
if now - tstart > 60:
raise NotAchievedException("Did not achieve attitude")
if now - last_attitude_target_sent > 0.5:
last_attitude_target_sent = now
type_mask = (
1 << 0 | # ignore roll rate
1 << 6 # ignore throttle
)
self.mav.mav.set_attitude_target_send(
0, # time_boot_ms
target_system, # target sysid
target_component, # target compid
type_mask, # bitmask of things to ignore
mavextra.euler_to_quat([0,
math.radians(despitch),
math.radians(desyaw)]), # att
0, # yaw rate (rad/s)
0, # pitch rate
0, # yaw rate
0) # thrust, 0 to 1, translated to a climb/descent rate
m = self.mav.recv_match(type='ATTITUDE', blocking=True, timeout=2)
if m is None:
raise NotAchievedException("Did not get ATTITUDE")
if now - last_debug > 1:
last_debug = now
self.progress("yaw=%f desyaw=%f pitch=%f despitch=%f" %
(math.degrees(m.yaw), desyaw,
math.degrees(m.pitch), despitch))
yaw_ok = abs(math.degrees(m.yaw) - desyaw) < tolerance
pitch_ok = abs(math.degrees(m.pitch) - despitch) < tolerance
if yaw_ok and pitch_ok:
self.progress("Achieved attitude")
break
def wait_distance_home_gt(self, distance, timeout=60):
home_distance = None
tstart = self.get_sim_time()
while self.get_sim_time_cached() - tstart < timeout:
# m = self.mav.recv_match(type='VFR_HUD', blocking=True)
distance_home = self.distance_to_home(use_cached_home=True)
self.progress("distance_home=%f want=%f" % (distance_home, distance))
if distance_home > distance:
return
self.drain_mav()
raise NotAchievedException("Failed to get %fm from home (now=%f)" %
(distance, home_distance))
def loiter(self, holdtime=10, maxaltchange=5, maxdistchange=5):
"""Hold loiter position."""
self.mavproxy.send('switch 5\n') # loiter mode
self.wait_mode('LOITER')
# first aim south east
self.progress("turn south east")
self.set_rc(4, 1580)
self.wait_heading(170)
self.set_rc(4, 1500)
# fly south east 50m
self.set_rc(2, 1100)
self.wait_distance(50)
self.set_rc(2, 1500)
# wait for copter to slow moving
self.wait_groundspeed(0, 2)
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
start_altitude = m.alt
start = self.mav.location()
tstart = self.get_sim_time()
self.progress("Holding loiter at %u meters for %u seconds" %
(start_altitude, holdtime))
while self.get_sim_time() < tstart + holdtime:
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)
self.progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
if alt_delta > maxaltchange:
self.progress("Loiter alt shifted %u meters (> limit of %u)" %
(alt_delta, maxaltchange))
raise NotAchievedException()
if delta > maxdistchange:
self.progress("Loiter shifted %u meters (> limit of %u)" %
(delta, maxdistchange))
raise NotAchievedException()
self.progress("Loiter OK for %u seconds" % holdtime)
def test_button(self):
self.set_parameter("SIM_PIN_MASK", 0)
self.set_parameter("BTN_ENABLE", 1)
btn = 2
pin = 3
self.set_parameter("BTN_PIN%u" % btn, pin)
self.drain_mav()
m = self.mav.recv_match(type='BUTTON_CHANGE', blocking=True, timeout=1)
self.progress("m: %s" % str(m))
if m is None:
raise NotAchievedException("Did not get BUTTON_CHANGE event")
mask = 1<<btn
if m.state & mask:
raise NotAchievedException("Bit incorrectly set in mask (got=%u dontwant=%u)" % (m.state, mask))
# SITL instantly reverts the pin to its old value
m2 = self.mav.recv_match(type='BUTTON_CHANGE', blocking=True, timeout=1)
self.progress("m2: %s" % str(m2))
if m2 is None:
raise NotAchievedException("Did not get repeat message")
# wait for messages to stop coming:
self.drain_mav_seconds(15)
self.set_parameter("SIM_PIN_MASK", 0)
m3 = self.mav.recv_match(type='BUTTON_CHANGE', blocking=True, timeout=1)
self.progress("m3: %s" % str(m3))
if m3 is None:
raise NotAchievedException("Did not get new message")
if m.last_change_ms == m3.last_change_ms:
raise NotAchievedException("last_change_ms same as first message")
if m3.state != 0:
raise NotAchievedException("Didn't get expected mask back in message (mask=0 state=%u" % (m3.state))
def test_mot_thst_hover_ignore(self):
"""Test if we are ignoring MOT_THST_HOVER parameter
"""
# Test default parameter value
mot_thst_hover_value = self.get_parameter("MOT_THST_HOVER")
if mot_thst_hover_value != 0.5:
raise NotAchievedException("Unexpected default MOT_THST_HOVER parameter value {}".format(mot_thst_hover_value))
# Test if parameter is being ignored
for value in [0.25, 0.75]:
self.set_parameter("MOT_THST_HOVER", value)
self.test_alt_hold()
def check_airspeeds(mav, m):
m_type = m.get_type()
if (m_type == 'NAMED_VALUE_FLOAT' and m.name == 'AS2'):
airspeed[1] = m.value
elif m_type == 'VFR_HUD':
airspeed[0] = m.airspeed
else:
return
if airspeed[0] is None or airspeed[1] is None:
return
delta = abs(airspeed[0] - airspeed[1])
if delta > 3:
raise NotAchievedException(
"Airspeed mismatch (as1=%f as2=%f)" %
(airspeed[0], airspeed[1]))
def test_rc_option_camera_trigger(self):
'''test toggling channel 12 takes picture'''
x = self.mav.messages.get("CAMERA_FEEDBACK", None)
if x is not None:
raise PreconditionFailedException("Receiving CAMERA_FEEDBACK?!")
self.set_rc(12, 2000)
tstart = self.get_sim_time()
while self.get_sim_time() - tstart < 10:
x = self.mav.messages.get("CAMERA_FEEDBACK", None)
if x is not None:
break
self.mav.wait_heartbeat()
self.set_rc(12, 1000)
if x is None:
raise NotAchievedException("No CAMERA_FEEDBACK message received")
def assert_extended_sys_state(self, vtol_state, landed_state):
m = self.mav.recv_match(type='EXTENDED_SYS_STATE',
blocking=True,
timeout=1)
if m is None:
raise NotAchievedException(
"Did not get extended_sys_state message")
if m.vtol_state != vtol_state:
raise ValueError("Bad MAV_VTOL_STATE. Want=%s got=%s" %
(self.vtol_state_name(vtol_state),
self.vtol_state_name(m.vtol_state)))
if m.landed_state != landed_state:
raise ValueError("Bad MAV_LANDED_STATE. Want=%s got=%s" %
(self.landed_state_name(landed_state),
self.landed_state_name(m.landed_state)))
def test_camera_mission_items(self):
self.context_push()
ex = None
try:
self.mavproxy.send(
'wp load %s\n' %
os.path.join(testdir, "rover-camera-mission.txt"))
self.wait_ready_to_arm()
self.mavproxy.send('mode auto\n')
self.wait_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
prev_cf = None
while True:
cf = self.mav.recv_match(type='CAMERA_FEEDBACK', blocking=True)
if prev_cf is None:
prev_cf = cf
continue
dist_travelled = self.get_distance_int(prev_cf, cf)
prev_cf = cf
mc = self.mav.messages.get("MISSION_CURRENT", None)
if mc is None:
continue
elif mc.seq == 2:
expected_distance = 2
elif mc.seq == 4:
expected_distance = 5
elif mc.seq == 5:
break
else:
continue
self.progress("Expected distance %f got %f" %
(expected_distance, dist_travelled))
error = abs(expected_distance - dist_travelled)
# Rover moves at ~5m/s; we appear to do something at
# 5Hz, so we do see over a meter of error!
max_error = 1.5
if error > max_error:
raise NotAchievedException(
"Camera distance error: %f (%f)" % (error, max_error))
self.disarm_vehicle()
except Exception as e:
self.progress("Exception caught")
ex = e
self.context_pop()
if ex is not None:
raise ex
def wait_level_flight(self, accuracy=5, timeout=30):
"""Wait for level flight."""
tstart = self.get_sim_time()
self.progress("Waiting for level flight")
self.set_rc(1, 1500)
self.set_rc(2, 1500)
self.set_rc(4, 1500)
while self.get_sim_time_cached() < tstart + timeout:
m = self.mav.recv_match(type='ATTITUDE', blocking=True)
roll = math.degrees(m.roll)
pitch = math.degrees(m.pitch)
self.progress("Roll=%.1f Pitch=%.1f" % (roll, pitch))
if math.fabs(roll) <= accuracy and math.fabs(pitch) <= accuracy:
self.progress("Attained level flight")
return
raise NotAchievedException("Failed to attain level flight")
def wait_servo_channel_value(self, channel, value, timeout=2):
channel_field = "servo%u_raw" % channel
tstart = self.get_sim_time()
while True:
remaining = timeout - (self.get_sim_time_cached() - tstart)
if remaining <= 0:
raise NotAchievedException()
m = self.mav.recv_match(type='SERVO_OUTPUT_RAW',
blocking=True,
timeout=remaining)
m_value = getattr(m, channel_field, None)
self.progress("SERVO_OUTPUT_RAW.%s=%u want=%u" %
(channel_field, m_value, value))
if m_value is None:
raise ValueError() #?
if m_value == value:
return
def test_rc_option_camera_trigger(self):
'''test toggling channel 12 takes picture'''
self.set_parameter("RC12_OPTION", 9) # CameraTrigger
self.reboot_sitl() # needed for RC12_OPTION to take effect
x = self.mav.messages.get("CAMERA_FEEDBACK", None)
if x is not None:
raise PreconditionFailedException("Receiving CAMERA_FEEDBACK?!")
self.set_rc(12, 2000)
tstart = self.get_sim_time()
while self.get_sim_time_cached() - tstart < 10:
x = self.mav.messages.get("CAMERA_FEEDBACK", None)
if x is not None:
break
self.wait_heartbeat()
self.set_rc(12, 1000)
if x is None:
raise NotAchievedException("No CAMERA_FEEDBACK message received")
def airspeed_autocal(self):
self.progress("Ensure no AIRSPEED_AUTOCAL on ground")
self.set_parameter("ARSPD_AUTOCAL", 1)
m = self.mav.recv_match(type='AIRSPEED_AUTOCAL',
blocking=True,
timeout=5)
if m is not None:
raise NotAchievedException("Got autocal on ground")
mission_filepath = os.path.join(testdir, "flaps.txt")
self.load_mission(mission_filepath)
self.wait_ready_to_arm()
self.arm_vehicle()
self.change_mode("AUTO")
self.progress("Ensure AIRSPEED_AUTOCAL in air")
m = self.mav.recv_match(type='AIRSPEED_AUTOCAL',
blocking=True,
timeout=5)
self.mav.motors_disarmed_wait()
def dive_set_position_target(self):
self.change_mode('GUIDED')
self.wait_ready_to_arm()
self.arm_vehicle()
startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True)
lat = 5
lon = 5
alt = -10
# 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
)
tstart = self.get_sim_time()
while True:
if self.get_sim_time_cached() - tstart > 200:
raise NotAchievedException("Did not move far enough")
pos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True)
delta = self.get_distance_int(startpos, pos)
self.progress("delta=%f (want >10)" % delta)
if delta > 10:
break
self.change_mode('MANUAL')
self.disarm_vehicle()
def drive_rtl_mission(self):
self.wait_ready_to_arm()
self.arm_vehicle()
mission_filepath = os.path.join("ArduRover-Missions", "rtl.txt")
self.load_mission(mission_filepath)
self.mavproxy.send('switch 4\n') # auto mode
self.set_rc(3, 1500)
self.wait_mode('AUTO')
self.mavproxy.expect('Executing RTL')
m = self.mav.recv_match(type='NAV_CONTROLLER_OUTPUT',
blocking=True,
timeout=0.1)
if m is None:
raise MsgRcvTimeoutException(
"Did not receive NAV_CONTROLLER_OUTPUT message")
wp_dist_min = 5
if m.wp_dist < wp_dist_min:
raise PreconditionFailedException(
"Did not start at least %u metres from destination" %
(wp_dist_min))
self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" % (
m.wp_dist,
wp_dist_min,
))
self.wait_mode('HOLD', timeout=600) # balancebot can take a long time!
pos = self.mav.location()
home_distance = self.get_distance(HOME, pos)
home_distance_max = 5
if home_distance > home_distance_max:
raise NotAchievedException(
"Did not get home (%u metres distant > %u)" %
(home_distance, home_distance_max))
self.mavproxy.send('switch 6\n')
self.wait_mode('MANUAL')
self.disarm_vehicle()
self.progress("RTL Mission OK")
def tailsitter(self):
'''tailsitter test'''
self.set_parameter('Q_FRAME_CLASS', 10)
self.set_parameter('Q_ENABLE', 1)
self.reboot_sitl()
self.wait_ready_to_arm()
value_before = self.get_servo_channel_value(3)
self.progress("Before: %u" % value_before)
self.change_mode('QHOVER')
tstart = self.get_sim_time()
while True:
now = self.get_sim_time_cached()
if now - tstart > 60:
break
value_after = self.get_servo_channel_value(3)
self.progress("After: t=%f output=%u" % ((now - tstart), value_after))
if value_before != value_after:
raise NotAchievedException("Changed throttle output on mode change to QHOVER")
self.disarm_vehicle()
def watch_altitude_maintained(self, delta=1, timeout=5.0):
"""Watch and wait for the actual altitude to be maintained
Keyword Arguments:
delta {float} -- Maximum altitude range to be allowed from actual point (default: {0.5})
timeout {float} -- Timeout time in simulation seconds (default: {5.0})
Raises:
NotAchievedException: Exception when altitude fails to hold inside the time and
altitude range
"""
tstart = self.get_sim_time_cached()
previous_altitude = self.mav.recv_match(type='VFR_HUD', blocking=True).alt
self.progress('Altitude to be watched: %f' % (previous_altitude))
while True:
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
if self.get_sim_time_cached() - tstart > timeout:
self.progress('Altitude hold done: %f' % (previous_altitude))
return
if abs(m.alt - previous_altitude) > delta:
raise NotAchievedException("Altitude not maintained: want %.2f (+/- %.2f) got=%.2f" % (previous_altitude, delta, m.alt))
def reboot_sitl(self):
"""Reboot SITL instance and wait it to reconnect."""
# out battery is reset to full on reboot. So reduce it to 10%
# and wait for it to go above 50.
self.run_cmd(mavutil.mavlink.MAV_CMD_BATTERY_RESET,
255, # battery mask
10, # percentage
0,
0,
0,
0,
0,
0)
self.run_cmd_reboot()
tstart = time.time()
while True:
if time.time() - tstart > 30:
raise NotAchievedException("Did not detect reboot")
# ask for the message:
batt = None
try:
self.send_cmd(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE,
mavutil.mavlink.MAVLINK_MSG_ID_BATTERY_STATUS,
0,
0,
0,
0,
0,
0)
batt = self.mav.recv_match(type='BATTERY_STATUS',
blocking=True,
timeout=1)
except ConnectionResetError:
pass
self.progress("Battery: %s" % str(batt))
if batt is None:
continue
if batt.battery_remaining > 50:
break
self.initialise_after_reboot_sitl()
def fly_avc_test(self):
# Arm
self.change_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", strict=False)
if not num_wp:
raise NotAchievedException("load copter_AVC2013_mission failed")
self.progress("Fly AVC mission from 1 to %u" % num_wp)
self.set_current_waypoint(1)
# wait for motor runup
self.delay_sim_time(20)
# switch into AUTO mode and raise throttle
self.change_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.wait_disarmed()
self.progress("MOTORS DISARMED OK")
self.progress("Lowering rotor speed")
self.set_rc(8, 1000)
self.progress("AVC mission completed: passed!")
def drive_rtl_mission(self):
mission_filepath = os.path.join(testdir, "ArduRover-Missions",
"rtl.txt")
self.mavproxy.send('wp load %s\n' % mission_filepath)
self.mavproxy.expect('Flight plan received')
self.mavproxy.send('switch 4\n') # auto mode
self.set_rc(3, 1500)
self.wait_mode('AUTO')
self.mavproxy.expect('Executing RTL')
m = self.mav.recv_match(type='NAV_CONTROLLER_OUTPUT',
blocking=True,
timeout=0.1)
if m is None:
self.progress("Did not receive NAV_CONTROLLER_OUTPUT message")
raise MsgRcvTimeoutException()
wp_dist_min = 5
if m.wp_dist < wp_dist_min:
self.progress("Did not start at least 5 metres from destination")
raise PreconditionFailedException()
self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" % (
m.wp_dist,
wp_dist_min,
))
self.wait_mode('HOLD')
pos = self.mav.location()
home_distance = self.get_distance(HOME, pos)
home_distance_max = 5
if home_distance > home_distance_max:
self.progress("Did not get home (%u metres distant > %u)" %
(home_distance, home_distance_max))
raise NotAchievedException()
self.mavproxy.send('switch 6\n')
self.wait_mode('MANUAL')
self.progress("RTL Mission OK")
def wait_extended_sys_state(self, vtol_state, landed_state):
tstart = self.get_sim_time()
while True:
if self.get_sim_time() - tstart > 10:
raise NotAchievedException("Did not achieve vol/landed states")
self.progress("Waiting for MAV_VTOL_STATE=%s MAV_LANDED_STATE=%s" %
(self.vtol_state_name(vtol_state),
self.landed_state_name(landed_state)))
m = self.assert_receive_message('EXTENDED_SYS_STATE', verbose=True)
if m.landed_state != landed_state:
self.progress("Wrong MAV_LANDED_STATE (want=%s got=%s)" %
(self.landed_state_name(landed_state),
self.landed_state_name(m.landed_state)))
continue
if m.vtol_state != vtol_state:
self.progress("Wrong MAV_VTOL_STATE (want=%s got=%s)" %
(self.vtol_state_name(vtol_state),
self.vtol_state_name(m.vtol_state)))
continue
self.progress("vtol and landed states match")
return
def fly_heli_stabilize_takeoff(self):
""""""
self.context_push()
ex = None
try:
self.change_mode('STABILIZE')
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, 1660, timeout=10)
self.delay_sim_time(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.progress("Monitoring takeoff")
self.wait_altitude(6.9, 8, relative=True)
self.progress("takeoff OK")
except Exception as e:
self.print_exception_caught(e)
ex = e
self.land_and_disarm()
self.set_rc(8, 1000)
self.context_pop()
if ex is not None:
raise ex
def drive_brake(self):
old_using_brake = self.get_parameter('ATC_BRAKE')
old_cruise_speed = self.get_parameter('CRUISE_SPEED')
self.set_parameter('CRUISE_SPEED', 15)
self.set_parameter('ATC_BRAKE', 0)
distance_without_brakes = self.drive_brake_get_stopping_distance(15)
# brakes on:
self.set_parameter('ATC_BRAKE', 1)
distance_with_brakes = self.drive_brake_get_stopping_distance(15)
# revert state:
self.set_parameter('ATC_BRAKE', old_using_brake)
self.set_parameter('CRUISE_SPEED', old_cruise_speed)
delta = distance_without_brakes - distance_with_brakes
if delta < distance_without_brakes * 0.05: # 5% isn't asking for much
raise NotAchievedException("""
Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
""" % (distance_with_brakes, distance_without_brakes, delta))
self.progress("Brakes work (with=%0.2fm without=%0.2fm delta=%0.2fm)" %
(distance_with_brakes, distance_without_brakes, delta))