def load_waypoints_from_array(self, arr): # print(arr) self.wploader.target_system = self.target_system self.wploader.target_component = self.target_component try: self.wploader.clear() fn = None if mavutil.mavlink10(): fn = mavutil.mavlink.MAVLink_mission_item_message else: fn = mavutil.mavlink.MAVLink_waypoint_message for a in arr: w = fn(self.target_system, self.target_component, int(a['seq']), # seq int(a['frame']), # frame int(a['command']), # command int(a['current']), # current int(a['autocontinue']), # autocontinue float(a['param1']), # param1, float(a['param2']), # param2, float(a['param3']), # param3 float(a['param4']), # param4 float(a['x']), # x (latitude) float(a['y']), # y (longitude) float(a['z']) # z (altitude) ) self.wploader.add(w, '') except Exception as msg: print("Unable to load %s" % msg) return print("Loaded %u waypoints" % self.wploader.count()) self.send_all_waypoints()
def position(self, t, max_deltat=0, roll=None, maxroll=45): '''return a MavPosition estimate given a time''' self.advance_log(t) scaled_pressure = self._find_msg('SCALED_PRESSURE', t) # extrapolate our latitude/longitude gpst = t + self.gps_lag if mavutil.mavlink10(): gps_raw = self._find_msg('GPS_RAW_INT', gpst) gps_timestamp = self.gps_time(gps_raw) #print gps_raw._timestamp, gps_timestamp, gpst, t, gpst - gps_timestamp (lat, lon) = cuav_util.gps_newpos( gps_raw.lat / 1.0e7, gps_raw.lon / 1.0e7, gps_raw.cog * 0.01, (gps_raw.vel * 0.01) * (gpst - gps_timestamp)) else: gps_raw = self._find_msg('GPS_RAW', gpst) gps_timestamp = self.gps_time(gps_raw) (lat, lon) = cuav_util.gps_newpos(gps_raw.lat, gps_raw.lon, gps_raw.hdg, gps_raw.v * (gpst - gps_timestamp)) # get altitude altitude = self._altitude(scaled_pressure) # and attitude mavroll = math.degrees( self.interpolate_angle('ATTITUDE', 'roll', t, max_deltat)) if roll is None: roll = mavroll elif abs(mavroll) < maxroll: roll = 0 elif mavroll >= maxroll: roll = mavroll - maxroll else: roll = mavroll + maxroll pitch = math.degrees( self.interpolate_angle('ATTITUDE', 'pitch', t, max_deltat)) yaw = math.degrees( self.interpolate_angle('ATTITUDE', 'yaw', t, max_deltat)) return MavPosition(lat, lon, altitude, roll, pitch, yaw, t)
def position(self, t, max_deltat=0, roll=None, maxroll=45): """return a MavPosition estimate given a time""" self.advance_log(t) scaled_pressure = self._find_msg("SCALED_PRESSURE", t) # extrapolate our latitude/longitude gpst = t + self.gps_lag if mavutil.mavlink10(): gps_raw = self._find_msg("GPS_RAW_INT", gpst) gps_timestamp = self.gps_time(gps_raw) # print gps_raw._timestamp, gps_timestamp, gpst, t, gpst - gps_timestamp (lat, lon) = cuav_util.gps_newpos( gps_raw.lat / 1.0e7, gps_raw.lon / 1.0e7, gps_raw.cog * 0.01, (gps_raw.vel * 0.01) * (gpst - gps_timestamp), ) else: gps_raw = self._find_msg("GPS_RAW", gpst) gps_timestamp = self.gps_time(gps_raw) (lat, lon) = cuav_util.gps_newpos(gps_raw.lat, gps_raw.lon, gps_raw.hdg, gps_raw.v * (gpst - gps_timestamp)) # get altitude altitude = self._altitude(scaled_pressure) # and attitude mavroll = math.degrees(self.interpolate_angle("ATTITUDE", "roll", t, max_deltat)) if roll is None: roll = mavroll elif abs(mavroll) < maxroll: roll = 0 elif mavroll >= maxroll: roll = mavroll - maxroll else: roll = mavroll + maxroll pitch = math.degrees(self.interpolate_angle("ATTITUDE", "pitch", t, max_deltat)) yaw = math.degrees(self.interpolate_angle("ATTITUDE", "yaw", t, max_deltat)) return MavPosition(lat, lon, altitude, roll, pitch, yaw, t)
def exportToMAVProxy(self, MAVpointLoader=None, loiterInSearchArea=1): '''Exports the airfield home, entry points, search pattern and exit points to MAVProxy''' #make a fake waypoint loader for testing purposes, if we're not #running within MAVProxy if MAVpointLoader is None: print "No loader - creating one" MAVpointLoader = mavwp.MAVWPLoader() entryjump = [] exitjump = [] #clear out all the old waypoints MAVpointLoader.clear() TargetSys = MAVpointLoader.target_system TargetComp = MAVpointLoader.target_component #Check the MAVLink version and handle appropriately if mavutil.mavlink10(): fn = mavutil.mavlink.MAVLink_mission_item_message else: fn = mavutil.mavlink.MAVLink_waypoint_message # a dummy loiter waypoint dummyw = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, 0, 0, 0) #WP0 - add "airfield home" as waypoint 0. This gets replaced when GPS gets lock w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1, 1, 0, 0, 0, 0, self.airfieldHome[0], self.airfieldHome[1], opts.basealt) MAVpointLoader.add(w, comment='Airfield home') # form is fn(target_system=0, target_component=0, seq, frame=0/3, command=16, current=1/0, autocontinue=1, param1=0, param2=0, param3=0, param4=0, x, y, z) #WP1 - add in a jmp to entry lanes entryjump.append(MAVpointLoader.count()) w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Jump to entry lane') #WP2 - takeoff, then jump to entry lanes w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 1, 12, 0, 0, 0, self.takeoffPt[0], self.takeoffPt[1], self.takeoffPt[2]) MAVpointLoader.add(w, comment="Takeoff") entryjump.append(MAVpointLoader.count()) w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Jump to entry lane') # MAVpointLoader.add(dummyw, 'takeoff2') # MAVpointLoader.add(dummyw, 'takeoff3') # MAVpointLoader.add(dummyw, 'takeoff4') # landing approach landing_approach_wpnum = MAVpointLoader.count() w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.landingApproach[0], self.landingApproach[1], 80) MAVpointLoader.add(w, comment='Landing approach') # drop our speed w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 1, 0, 25, 20, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Change to 25 m/s') w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 1, 1, 25, 20, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Change throttle to 20%%') # landing approach w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.landingApproach2[0], self.landingApproach2[1], 30) MAVpointLoader.add(w, comment='Landing approach 2') # drop our speed again w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 1, 0, 20, 12, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Change to 20 m/s') w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 1, 1, 20, 12, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Change throttle to 12%%') # landing w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 1, 0, 0, 0, 0, self.landingPt[0], self.landingPt[1], 0) MAVpointLoader.add(w, comment='Landing') # comms Failure. Loiter at EL-1 for 2 minutes then fly to airfield home and loiter point = self.entryPoints[0] w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, point[0], point[1], self.airportHeight) MAVpointLoader.add(w, comment='Comms Failure') w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, 0, 1, 120, 0, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='loiter 2 minutes') w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.airfieldHome[0], self.airfieldHome[1], self.airportHeight) MAVpointLoader.add(w, comment='Airfield home') w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 1, 0, 0, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='loiter') # GPS failure. Loiter in place for 30s then direct to airfield home w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, 0, 1, 30, 0, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='GPS fail - loiter 30 secs') w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.airfieldHome[0], self.airfieldHome[1], self.airportHeight) MAVpointLoader.add(w, comment='Airfield home') w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 1, 0, 0, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='loiter') # joe drop approach w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.joeApproach[0], self.joeApproach[1], int(self.joeApproach[2])) MAVpointLoader.add(w, comment='Joe approach') w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.joeDrop[0], self.joeDrop[1], int(self.joeDrop[2])) MAVpointLoader.add(w, comment='Joe Drop location') w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_DO_SET_SERVO, 0, 1, 7, 1430, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Drop bottle') # after drop, jump to exit lane exitjump.append(MAVpointLoader.count()) w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Jump to exit lane') #print "Done AF home" #WP12 - WPn - and add in the rest of the waypoints - Entry lane, search area, exit lane entry_wpnum = MAVpointLoader.count() for i in range(1): point = self.entryPoints[i] w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, point[0], point[1], self.airportHeight) MAVpointLoader.add(w, comment='Entry %u' % (i+1)) endentry_wpnum = MAVpointLoader.count() w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Jump to search mission') # exit points exit_wpnum = MAVpointLoader.count() for i in range(len(self.exitPoints)): point = self.exitPoints[i] w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, point[0], point[1], self.airportHeight) MAVpointLoader.add(w, comment='Exit point %u' % (i+1)) w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_DO_JUMP, 0, 1, landing_approach_wpnum, -1, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Jump to landing approach') # search pattern MAVpointLoader.wp(endentry_wpnum).param1 = MAVpointLoader.count() for i in range(len(self.SearchPattern)): point = self.SearchPattern[i] w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, point[0], point[1], point[2]) MAVpointLoader.add(w, comment='Search %u' % (i+1)) #if desired, loiter in the search area for a bit if loiterInSearchArea == 1: meanPoint = tuple(numpy.average(self.SearchPattern, axis=0)) w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, 0, 1, 600, 0, 0, 0, meanPoint[0], meanPoint[1], meanPoint[2]) MAVpointLoader.add(w, comment='Loiter in search area for 10 minutes') exitjump.append(MAVpointLoader.count()) w = fn(TargetSys, TargetComp, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Jump to exit lane') # fixup jump waypoint numbers for wnum in entryjump: MAVpointLoader.wp(wnum).param1 = entry_wpnum for wnum in exitjump: MAVpointLoader.wp(wnum).param1 = exit_wpnum #export the waypoints to a MAVLink compatible format/file waytxt = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'data', opts.outname) MAVpointLoader.save(waytxt) print "Waypoints exported to %s" % waytxt # create fence.txt fenceloader = mavwp.MAVFenceLoader() fp = mavutil.mavlink.MAVLink_fence_point_message(0, 0, 0, 0, self.airfieldHome[0], self.airfieldHome[1]) fenceloader.add(fp) for p in gen.missionBounds: fp = mavutil.mavlink.MAVLink_fence_point_message(0, 0, 0, 0, float(p[0]), float(p[1])) fenceloader.add(fp) # close the polygon p = gen.missionBounds[0] fp = mavutil.mavlink.MAVLink_fence_point_message(0, 0, 0, 0, float(p[0]), float(p[1])) fenceloader.add(fp) fencetxt = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'data', "fence.txt") fenceloader.save(fencetxt) print "Fence exported to %s" % fencetxt
def exportToMAVProxy(self, MAVpointLoader=None, loiterInSearchArea=1): '''Exports the airfield home, entry points, search pattern and exit points to MAVProxy''' #make a fake waypoint loader for testing purposes, if we're not #running within MAVProxy if MAVpointLoader is None: print "No loader - creating one" MAVpointLoader = mavwp.MAVWPLoader() entryjump = [] exitjump = [] #clear out all the old waypoints MAVpointLoader.clear() TargetSys = MAVpointLoader.target_system TargetComp = MAVpointLoader.target_component #Check the MAVLink version and handle appropriately if mavutil.mavlink10(): fn = mavutil.mavlink.MAVLink_mission_item_message else: fn = mavutil.mavlink.MAVLink_waypoint_message # a dummy loiter waypoint dummyw = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, 0, 0, 0) #WP0 - add "airfield home" as waypoint 0. This gets replaced when GPS gets lock w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT, 1, 1, 0, 0, 0, 0, self.airfieldHome[0], self.airfieldHome[1], opts.basealt) MAVpointLoader.add(w, comment='Airfield home') # form is fn(target_system=0, target_component=0, seq, frame=0/3, command=16, current=1/0, autocontinue=1, param1=0, param2=0, param3=0, param4=0, x, y, z) #WP1 - add in a jmp to entry lanes entryjump.append(MAVpointLoader.count()) w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Jump to entry lane') MAVpointLoader.add(dummyw, 'jump dummy') #WP2 - takeoff, then jump to entry lanes w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_TAKEOFF, 0, 1, 12, 0, 0, 0, self.takeoffPt[0], self.takeoffPt[1], self.takeoffPt[2]) MAVpointLoader.add(w, comment="Takeoff") entryjump.append(MAVpointLoader.count()) w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Jump to entry lane') MAVpointLoader.add(dummyw, 'jump dummy') # MAVpointLoader.add(dummyw, 'takeoff2') # MAVpointLoader.add(dummyw, 'takeoff3') # MAVpointLoader.add(dummyw, 'takeoff4') # landing approach landing_approach_wpnum = MAVpointLoader.count() w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.landingApproach[0], self.landingApproach[1], 80) MAVpointLoader.add(w, comment='Landing approach') # drop our speed w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_CHANGE_SPEED, 0, 1, 0, 25, 20, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Change to 25 m/s') w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_CHANGE_SPEED, 0, 1, 1, 25, 20, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Change throttle to 20%%') # landing approach w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.landingApproach2[0], self.landingApproach2[1], 30) MAVpointLoader.add(w, comment='Landing approach 2') # drop our speed again w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_CHANGE_SPEED, 0, 1, 0, 20, 12, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Change to 20 m/s') w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_CHANGE_SPEED, 0, 1, 1, 20, 12, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Change throttle to 12%%') # landing w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_LAND, 0, 1, 0, 0, 0, 0, self.landingPt[0], self.landingPt[1], 0) MAVpointLoader.add(w, comment='Landing') MAVpointLoader.add(dummyw, 'landing dummy') # comms Failure. Loiter at EL-1 for 2 minutes then fly to airfield home and loiter point = self.entryPoints[0] w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, point[0], point[1], int(point[2] - self.airportHeight)) MAVpointLoader.add(w, comment='Comms Failure') w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_LOITER_TIME, 0, 1, 120, 0, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='loiter 2 minutes') w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.airfieldHome[0], self.airfieldHome[1], 90) MAVpointLoader.add(w, comment='Airfield home') w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_LOITER_UNLIM, 0, 1, 0, 0, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='loiter') # GPS failure. Loiter in place for 30s then direct to airfield home w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_LOITER_TIME, 0, 1, 30, 0, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='GPS fail - loiter 30 secs') w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.airfieldHome[0], self.airfieldHome[1], 90) MAVpointLoader.add(w, comment='Airfield home') w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_LOITER_UNLIM, 0, 1, 0, 0, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='loiter') # joe drop approach w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.joeApproach[0], self.joeApproach[1], int(self.joeApproach[2])) MAVpointLoader.add(w, comment='Joe approach') w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.joeDrop[0], self.joeDrop[1], int(self.joeDrop[2])) MAVpointLoader.add(w, comment='Joe Drop location') w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_SET_SERVO, 0, 1, 7, 1430, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Drop bottle') # after drop, jump to exit lane exitjump.append(MAVpointLoader.count()) w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Jump to exit lane') MAVpointLoader.add(dummyw, 'jump dummy') #print "Done AF home" #WP12 - WPn - and add in the rest of the waypoints - Entry lane, search area, exit lane entry_wpnum = MAVpointLoader.count() for i in range(1): point = self.entryPoints[i] w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, point[0], point[1], int(point[2] - self.airportHeight)) MAVpointLoader.add(w, comment='Entry %u' % (i + 1)) endentry_wpnum = MAVpointLoader.count() w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Jump to search mission') MAVpointLoader.add(dummyw, 'jump dummy') # exit points exit_wpnum = MAVpointLoader.count() for i in range(len(self.exitPoints)): point = self.exitPoints[i] w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, point[0], point[1], int(point[2] - self.airportHeight)) MAVpointLoader.add(w, comment='Exit point %u' % (i + 1)) w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_JUMP, 0, 1, landing_approach_wpnum, -1, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Jump to landing approach') MAVpointLoader.add(dummyw, 'jump dummy') # search pattern MAVpointLoader.wp(endentry_wpnum).param1 = MAVpointLoader.count() for i in range(len(self.SearchPattern)): point = self.SearchPattern[i] w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, point[0], point[1], int(point[2] - self.airportHeight)) MAVpointLoader.add(w, comment='Search %u' % (i + 1)) #if desired, loiter in the search area for a bit if loiterInSearchArea == 1: meanPoint = tuple(numpy.average(self.SearchPattern, axis=0)) w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_LOITER_TIME, 0, 1, 600, 0, 0, 0, meanPoint[0], meanPoint[1], int(meanPoint[2] - self.airportHeight)) MAVpointLoader.add(w, comment='Loiter in search area for 10 minutes') exitjump.append(MAVpointLoader.count()) w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0) MAVpointLoader.add(w, comment='Jump to exit lane') MAVpointLoader.add(dummyw, 'jump dummy') # fixup jump waypoint numbers for wnum in entryjump: MAVpointLoader.wp(wnum).param1 = entry_wpnum for wnum in exitjump: MAVpointLoader.wp(wnum).param1 = exit_wpnum #export the waypoints to a MAVLink compatible format/file waytxt = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'data', opts.outname) MAVpointLoader.save(waytxt) print "Waypoints exported to %s" % waytxt # create fence.txt fenceloader = mavwp.MAVFenceLoader() fp = mavutil.mavlink.MAVLink_fence_point_message( 0, 0, 0, 0, self.airfieldHome[0], self.airfieldHome[1]) fenceloader.add(fp) for p in gen.missionBounds: fp = mavutil.mavlink.MAVLink_fence_point_message( 0, 0, 0, 0, float(p[0]), float(p[1])) fenceloader.add(fp) # close the polygon p = gen.missionBounds[0] fp = mavutil.mavlink.MAVLink_fence_point_message( 0, 0, 0, 0, float(p[0]), float(p[1])) fenceloader.add(fp) fencetxt = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'data', "fence.txt") fenceloader.save(fencetxt) print "Fence exported to %s" % fencetxt
# Now load waypoint.txt file. wp.load("/home/atharva/ardupilot/ArduPlane/waypoint2.txt") # Load 2nd wp file into a list. file = open('/home/atharva/ardupilot/ArduPlane/waypoint3.txt', "r") for line in file: if line.startswith('#'): comment = line[1:].lstrip() continue line = line.strip() if not line: continue a = line.split() # if len(a) != 12: # raise MAVWPError("invalid waypoint line with %u values" % len(a)) if mavutil.mavlink10(): fn = mavutil.mavlink.MAVLink_mission_item_message else: fn = mavutil.mavlink.MAVLink_waypoint_message w = fn( wp.target_system, wp.target_component, int(a[0]), # seq int(a[2]), # frame int(a[3]), # command int(a[1]), # current int(a[11]), # autocontinue float(a[4]), # param1, float(a[5]), # param2, float(a[6]), # param3 float(a[7]), # param4