def _read_waypoint_v110(self, line):
'''read a version 110 waypoint'''
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(
self.target_system,
self.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
float(a[8]), # x (latitude)
float(a[9]), # y (longitude)
float(a[10]) # z (altitude)
)
return w
def _read_waypoints_v110(self, file):
'''read a version 110 waypoint'''
for line in file:
if line.startswith('#'):
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(self.target_system, self.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
float(a[8]), # x (latitude)
float(a[9]), # y (longitude)
float(a[10]) # z (altitude)
)
self.add(w)
def _read_waypoints_v100(self, file):
'''read a version 100 waypoint'''
cmdmap = {
2: mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
3: mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,
4: mavutil.mavlink.MAV_CMD_NAV_LAND,
24: mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
26: mavutil.mavlink.MAV_CMD_NAV_LAND,
25: mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
27: mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM
}
comment = ''
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) != 13:
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(
self.target_system,
self.target_component,
int(a[0]), # seq
int(a[1]), # frame
int(a[2]), # action
int(a[7]), # current
int(a[12]), # autocontinue
float(a[5]), # param1,
float(a[6]), # param2,
float(a[3]), # param3
float(a[4]), # param4
float(a[9]), # x, latitude
float(a[8]), # y, longitude
float(a[10]) # z
)
if not w.command in cmdmap:
raise MAVWPError("Unknown v100 waypoint action %u" % w.command)
w.command = cmdmap[w.command]
self.add(w, comment)
comment = ''
def _read_waypoints_v100(self, file):
"""read a version 100 waypoint"""
cmdmap = {
2: mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
3: mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,
4: mavutil.mavlink.MAV_CMD_NAV_LAND,
24: mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
26: mavutil.mavlink.MAV_CMD_NAV_LAND,
25: mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
27: mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM,
}
comment = ""
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) != 13:
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(
self.target_system,
self.target_component,
int(a[0]), # seq
int(a[1]), # frame
int(a[2]), # action
int(a[7]), # current
int(a[12]), # autocontinue
float(a[5]), # param1,
float(a[6]), # param2,
float(a[3]), # param3
float(a[4]), # param4
float(a[9]), # x, latitude
float(a[8]), # y, longitude
float(a[10]), # z
)
if not w.command in cmdmap:
raise MAVWPError("Unknown v100 waypoint action %u" % w.command)
w.command = cmdmap[w.command]
self.add(w, comment)
comment = ""
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 _read_waypoints_v110(self, file):
'''read a version 110 waypoint'''
comment = ''
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(
self.target_system,
self.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
float(a[8]), # x (latitude)
float(a[9]), # y (longitude)
float(a[10]) # z (altitude)
)
if w.command == 0 and w.seq == 0 and self.count() == 0:
# special handling for Mission Planner created home wp
w.command = mavutil.mavlink.MAV_CMD_NAV_WAYPOINT
self.add(w, comment)
comment = ''
def _read_waypoints_v110(self, file):
"""read a version 110 waypoint"""
comment = ""
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(
self.target_system,
self.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
float(a[8]), # x (latitude)
float(a[9]), # y (longitude)
float(a[10]), # z (altitude)
)
if w.command == 0 and w.seq == 0 and self.count() == 0:
# special handling for Mission Planner created home wp
w.command = mavutil.mavlink.MAV_CMD_NAV_WAYPOINT
self.add(w, comment)
comment = ""
def _read_waypoint_v100(self, line):
'''read a version 100 waypoint'''
cmdmap = {
2 : mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
3 : mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,
4 : mavutil.mavlink.MAV_CMD_NAV_LAND,
24: mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
26: mavutil.mavlink.MAV_CMD_NAV_LAND,
25: mavutil.mavlink.MAV_CMD_NAV_WAYPOINT ,
27: mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM
}
a = line.split()
if len(a) != 13:
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(self.target_system, self.target_component,
int(a[0]), # seq
int(a[1]), # frame
int(a[2]), # action
int(a[7]), # current
int(a[12]), # autocontinue
float(a[5]), # param1,
float(a[6]), # param2,
float(a[3]), # param3
float(a[4]), # param4
float(a[9]), # x, latitude
float(a[8]), # y, longitude
float(a[10]) # z
)
if not w.command in cmdmap:
raise MAVWPError("Unknown v100 waypoint action %u" % w.command)
w.command = cmdmap[w.command]
return w
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