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.change_mode("AUTO")
self.mavproxy.expect('Executing RTL')
self.drain_mav()
m = self.mav.recv_match(type='NAV_CONTROLLER_OUTPUT',
blocking=True,
timeout=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 %f metres from destination (is=%f)" %
(wp_dist_min, m.wp_dist))
self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" % (
m.wp_dist,
wp_dist_min,
))
tstart = self.get_sim_time()
while True:
if self.get_sim_time_cached() - tstart > 600:
raise NotAchievedException("Did not get home")
self.progress("Distance home: %f (mode=%s)" %
(self.distance_to_home(), self.mav.flightmode))
if self.mode_is('HOLD') or self.mode_is(
'LOITER'): # loiter for balancebot
break
# the EKF doesn't pull us down to 0 speed:
self.wait_groundspeed(0, 0.5, timeout=600)
# current Rover blows straight past the home position and ends
# up ~6m past the home point.
home_distance = self.distance_to_home()
home_distance_min = 5.5
home_distance_max = self.drive_rtl_mission_max_distance_from_home()
if home_distance > home_distance_max:
raise NotAchievedException(
"Did not stop near home (%f metres distant (%f > want > %f))" %
(home_distance, home_distance_min, home_distance_max))
self.disarm_vehicle()
self.progress("RTL Mission OK (%fm)" % home_distance)
def do_get_banner(self):
self.mavproxy.send("long DO_SEND_BANNER 1\n")
start = time.time()
while True:
m = self.mav.recv_match(type='STATUSTEXT',
blocking=True,
timeout=1)
if m is not None and "ArduRover" in m.text:
self.progress("banner received: %s" % m.text)
return
if time.time() - start > 10:
break
raise MsgRcvTimeoutException("banner not received")
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 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")