import util, pexpect, time, math # a list of pexpect objects to read while waiting for # messages. This keeps the output to stdout flowing expect_list = [] def message_hook(mav, msg): '''called as each mavlink msg is received''' global expect_list # if msg.get_type() in [ 'NAV_CONTROLLER_OUTPUT', 'GPS_RAW' ]: # print(msg) for p in expect_list: try: p.read_nonblocking(100, timeout=0) except pexpect.TIMEOUT: pass def expect_callback(e): '''called when waiting for a expect pattern''' global expect_list for p in expect_list: if p == e: continue try: while p.read_nonblocking(100, timeout=0): pass except pexpect.TIMEOUT: pass class location(object): '''represent a GPS coordinate''' def __init__(self, lat, lng, alt=0): self.lat = lat self.lng = lng self.alt = alt def __str__(self): return "lat=%.6f,lon=%.6f,alt=%.1f" % (self.lat, self.lng, self.alt) def get_distance(loc1, loc2): '''get ground distance between two locations''' dlat = loc2.lat - loc1.lat dlong = loc2.lng - loc1.lng return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5 def get_bearing(loc1, loc2): '''get bearing from loc1 to loc2''' off_x = loc2.lng - loc1.lng off_y = loc2.lat - loc1.lat bearing = 90.00 + math.atan2(-off_y, off_x) * 57.2957795 if bearing < 0: bearing += 360.00 return bearing; def current_location(mav): '''return current location''' # ensure we have a position mav.recv_match(type='VFR_HUD', blocking=True) mav.recv_match(type='GPS_RAW', blocking=True) return location(mav.messages['GPS_RAW'].lat, mav.messages['GPS_RAW'].lon, mav.messages['VFR_HUD'].alt) def wait_altitude(mav, alt_min, alt_max, timeout=30): '''wait for a given altitude range''' tstart = time.time() print("Waiting for altitude between %u and %u" % (alt_min, alt_max)) while time.time() < tstart + timeout: m = mav.recv_match(type='VFR_HUD', blocking=True) print("Altitude %u" % m.alt) if m.alt >= alt_min and m.alt <= alt_max: return True print("Failed to attain altitude range") return False def wait_roll(mav, roll, accuracy, timeout=30): '''wait for a given roll in degrees''' tstart = time.time() print("Waiting for roll of %u" % roll) while time.time() < tstart + timeout: m = mav.recv_match(type='ATTITUDE', blocking=True) r = math.degrees(m.roll) print("Roll %u" % r) if math.fabs(r - roll) <= accuracy: return True print("Failed to attain roll %u" % roll) return False def wait_pitch(mav, pitch, accuracy, timeout=30): '''wait for a given pitch in degrees''' tstart = time.time() print("Waiting for pitch of %u" % pitch) while time.time() < tstart + timeout: m = mav.recv_match(type='ATTITUDE', blocking=True) r = math.degrees(m.pitch) print("Pitch %u" % r) if math.fabs(r - pitch) <= accuracy: return True print("Failed to attain pitch %u" % pitch) return False def wait_heading(mav, heading, accuracy=5, timeout=30): '''wait for a given heading''' tstart = time.time() while time.time() < tstart + timeout: m = mav.recv_match(type='VFR_HUD', blocking=True) print("Heading %u" % m.heading) if math.fabs(m.heading - heading) <= accuracy: return True print("Failed to attain heading %u" % heading) return False def wait_distance(mav, distance, accuracy=5, timeout=30): '''wait for flight of a given distance''' tstart = time.time() start = current_location(mav) while time.time() < tstart + timeout: m = mav.recv_match(type='GPS_RAW', blocking=True) pos = current_location(mav) delta = get_distance(start, pos) print("Distance %.2f meters" % delta) if math.fabs(delta - distance) <= accuracy: return True print("Failed to attain distance %u" % distance) return False def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height_accuracy=-1): '''wait for arrival at a location''' tstart = time.time() if target_altitude is None: target_altitude = loc.alt while time.time() < tstart + timeout: m = mav.recv_match(type='GPS_RAW', blocking=True) pos = current_location(mav) delta = get_distance(loc, pos) print("Distance %.2f meters" % delta) if delta <= accuracy: if height_accuracy != -1 and math.fabs(pos.alt - target_altitude) > height_accuracy: continue print("Reached location (%.2f meters)" % delta) return True print("Failed to attain location") return False def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, timeout=400): '''wait for waypoint ranges''' tstart = time.time() # this message arrives after we set the current WP m = mav.recv_match(type='WAYPOINT_CURRENT', blocking=True) start_wp = m.seq current_wp = start_wp print("\ntest: wait for waypoint ranges start=%u end=%u\n\n" % (wpnum_start, wpnum_end)) # if start_wp != wpnum_start: # print("test: Expected start waypoint %u but got %u" % (wpnum_start, start_wp)) # return False while time.time() < tstart + timeout: m = mav.recv_match(type='WAYPOINT_CURRENT', blocking=True) seq = m.seq m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True) wp_dist = m.wp_dist print("test: WP %u (wp_dist=%u)" % (seq, wp_dist)) if seq == current_wp+1 or (seq > current_wp+1 and allow_skip): print("test: Starting new waypoint %u" % seq) tstart = time.time() current_wp = seq # the wp_dist check is a hack until we can sort out the right seqnum # for end of mission if current_wp == wpnum_end or (current_wp == wpnum_end-1 and wp_dist < 2): print("Reached final waypoint %u" % seq) return True if seq > current_wp+1: print("Skipped waypoint! Got wp %u expected %u" % (seq, current_wp+1)) return False print("Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end)) return False def save_wp(mavproxy, mav): mavproxy.send('rc 7 2000\n') mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==2000', blocking=True) mavproxy.send('rc 7 1000\n') mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True) #mavproxy.send('wp list\n') #mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True) def wait_mode(mav, mode): '''wait for a flight mode to be engaged''' mav.recv_match(condition='MAV.flightmode=="%s"' % mode, blocking=True)