Skip to content

Commit 8cda33f

Browse files
committed
mavwp: use is_location attribute of WP commands
1 parent 10defcf commit 8cda33f

File tree

1 file changed

+10
-10
lines changed

1 file changed

+10
-10
lines changed

mavwp.py

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -325,17 +325,17 @@ def save(self, filename):
325325
f.close()
326326

327327
def is_location_command(self, cmd):
328-
'''see if cmd is a MAV_CMD with a latitude/longitude.
329-
We check if it has Latitude and Longitude params in the right indexes'''
328+
'''see if cmd is a MAV_CMD with a latitude/longitude'''
330329
mav_cmd = mavutil.mavlink.enums['MAV_CMD']
331330
if not cmd in mav_cmd:
332331
return False
333-
if not mav_cmd[cmd].param.get(5,'').startswith('Latitude'):
334-
return False
335-
if not mav_cmd[cmd].param.get(6,'').startswith('Longitude'):
336-
return False
337-
return True
332+
return getattr(mav_cmd[cmd],'has_location',True)
338333

334+
def is_location_wp(self, w):
335+
'''see if w.command is a MAV_CMD with a latitude/longitude'''
336+
if w.x == 0 and w.y == 0:
337+
return False
338+
return self.is_location_command(w.command)
339339

340340
def view_indexes(self, done=None):
341341
'''return a list waypoint indexes in view order'''
@@ -357,14 +357,14 @@ def view_indexes(self, done=None):
357357
while idx < self.count():
358358
w = self.wp(idx)
359359
if idx in done:
360-
if w.x != 0 or w.y != 0:
360+
if self.is_location_wp(w):
361361
ret.append(idx)
362362
break
363363
done.add(idx)
364364
if w.command == mavutil.mavlink.MAV_CMD_DO_JUMP:
365365
idx = int(w.param1)
366366
w = self.wp(idx)
367-
if w.x != 0 or w.y != 0:
367+
if self.is_location_wp(w):
368368
ret.append(idx)
369369
continue
370370
# display loops for exclusion and inclusion zones
@@ -384,7 +384,7 @@ def view_indexes(self, done=None):
384384
ret.append(idx)
385385
ret.append(inclusion_start)
386386
return ret
387-
if (w.x != 0 or w.y != 0) and self.is_location_command(w.command):
387+
if self.is_location_wp(w):
388388
ret.append(idx)
389389
if w.command in [ mavutil.mavlink.MAV_CMD_NAV_LAND,
390390
mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND ]:

0 commit comments

Comments
 (0)