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
