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")