mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
Added altitude
This commit is contained in:
parent
c385db412e
commit
56de5d58c5
@ -195,7 +195,9 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
|
|||||||
seq = m.seq
|
seq = m.seq
|
||||||
m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
|
m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
|
||||||
wp_dist = m.wp_dist
|
wp_dist = m.wp_dist
|
||||||
print("test: WP %u (wp_dist=%u), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, current_wp, wpnum_end))
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||||
|
|
||||||
|
print("test: WP %u (wp_dist=%u Alt=%d), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, m.alt, current_wp, wpnum_end))
|
||||||
if seq == current_wp+1 or (seq > current_wp+1 and allow_skip):
|
if seq == current_wp+1 or (seq > current_wp+1 and allow_skip):
|
||||||
print("test: Starting new waypoint %u" % seq)
|
print("test: Starting new waypoint %u" % seq)
|
||||||
tstart = time.time()
|
tstart = time.time()
|
||||||
|
Loading…
Reference in New Issue
Block a user