zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Tools: adjust tests for vfr_hud getting absolute altitude

mission-4.1.18
Peter Barker 7 years ago committed by Andrew Tridgell
parent
1dbfb9943c
commit
0b8b23baf6
2 changed files with 44 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 53
      Tools/autotest/arducopter.py
  2. 19
      Tools/autotest/common.py

53
Tools/autotest/arducopter.py
Unescape View File

@ -152,9 +152,12 @@ class AutoTestCopter(AutoTest):
self.set_rc(3, 1000) self.set_rc(3, 1000)
self.arm_vehicle() self.arm_vehicle()
self.set_rc(3, takeoff_throttle) self.set_rc(3, takeoff_throttle)
m = self.mav.recv_match(type='VFR_HUD', blocking=True) m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
if m.alt < alt_min: alt = m.relative_alt / 1000.0 # mm -> m
self.wait_altitude(alt_min, (alt_min + 5)) if alt < alt_min:
self.wait_altitude(alt_min,
(alt_min + 5),
relative=True)
self.hover() self.hover()
self.progress("TAKEOFF COMPLETE") self.progress("TAKEOFF COMPLETE")
@ -164,7 +167,7 @@ class AutoTestCopter(AutoTest):
self.mavproxy.send('switch 2\n') # land mode self.mavproxy.send('switch 2\n') # land mode
self.wait_mode('LAND') self.wait_mode('LAND')
self.progress("Entered Landing Mode") self.progress("Entered Landing Mode")
self.wait_altitude(-5, 1) self.wait_altitude(-5, 1, relative=True)
self.progress("LANDING: ok!") self.progress("LANDING: ok!")
def hover(self, hover_throttle=1500): def hover(self, hover_throttle=1500):
@ -214,15 +217,16 @@ class AutoTestCopter(AutoTest):
def change_alt(self, alt_min, climb_throttle=1920, descend_throttle=1080): def change_alt(self, alt_min, climb_throttle=1920, descend_throttle=1080):
"""Change altitude.""" """Change altitude."""
m = self.mav.recv_match(type='VFR_HUD', blocking=True) m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
if m.alt < alt_min: alt = m.relative_alt / 1000.0 # mm -> m
self.progress("Rise to alt:%u from %u" % (alt_min, m.alt)) if alt < alt_min:
self.progress("Rise to alt:%u from %u" % (alt_min, alt))
self.set_rc(3, climb_throttle) self.set_rc(3, climb_throttle)
self.wait_altitude(alt_min, (alt_min + 5)) self.wait_altitude(alt_min, (alt_min + 5), relative=True)
else: else:
self.progress("Lower to alt:%u from %u" % (alt_min, m.alt)) self.progress("Lower to alt:%u from %u" % (alt_min, alt))
self.set_rc(3, descend_throttle) self.set_rc(3, descend_throttle)
self.wait_altitude((alt_min - 5), alt_min) self.wait_altitude((alt_min - 5), alt_min, relative=True)
self.hover() self.hover()
################################################# #################################################
@ -310,7 +314,7 @@ class AutoTestCopter(AutoTest):
self.progress("timeleft = %u" % time_left) self.progress("timeleft = %u" % time_left)
if time_left < 20: if time_left < 20:
time_left = 20 time_left = 20
self.wait_altitude(-10, 10, time_left) self.wait_altitude(-10, 10, time_left, relative=True)
self.save_wp() self.save_wp()
# enter RTL mode and wait for the vehicle to disarm # enter RTL mode and wait for the vehicle to disarm
@ -321,14 +325,15 @@ class AutoTestCopter(AutoTest):
self.set_rc(3, 1500) self.set_rc(3, 1500)
tstart = self.get_sim_time() tstart = self.get_sim_time()
while self.get_sim_time() < tstart + timeout: while self.get_sim_time() < tstart + timeout:
m = self.mav.recv_match(type='VFR_HUD', blocking=True) m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
alt = m.relative_alt / 1000.0 # mm -> m
pos = self.mav.location() pos = self.mav.location()
home_distance = self.get_distance(HOME, pos) home_distance = self.get_distance(HOME, pos)
home = "" home = ""
if m.alt <= 1 and home_distance < 10: if alt <= 1 and home_distance < 10:
home = "HOME" home = "HOME"
self.progress("Alt: %u HomeDist: %.0f %s" % self.progress("Alt: %u HomeDist: %.0f %s" %
(m.alt, home_distance, home)) (alt, home_distance, home))
# our post-condition is that we are disarmed: # our post-condition is that we are disarmed:
if not self.armed(): if not self.armed():
return return
@ -367,12 +372,13 @@ class AutoTestCopter(AutoTest):
tstart = self.get_sim_time() tstart = self.get_sim_time()
while self.get_sim_time() < tstart + timeout: while self.get_sim_time() < tstart + timeout:
m = self.mav.recv_match(type='VFR_HUD', blocking=True) m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
alt = m.alt / 1000.0 # mm -> m
pos = self.mav.location() pos = self.mav.location()
home_distance = self.get_distance(HOME, pos) home_distance = self.get_distance(HOME, pos)
self.progress("Alt: %u HomeDist: %.0f" % (m.alt, home_distance)) self.progress("Alt: %u HomeDist: %.0f" % (alt, home_distance))
# check if we've reached home # check if we've reached home
if m.alt <= 1 and home_distance < 10: if alt <= 1 and home_distance < 10:
# reduce throttle # reduce throttle
self.set_rc(3, 1100) self.set_rc(3, 1100)
# switch back to stabilize # switch back to stabilize
@ -496,18 +502,19 @@ class AutoTestCopter(AutoTest):
# start timer # start timer
tstart = self.get_sim_time() tstart = self.get_sim_time()
while self.get_sim_time() < tstart + timeout: while self.get_sim_time() < tstart + timeout:
m = self.mav.recv_match(type='VFR_HUD', blocking=True) m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
alt = m.relative_alt / 1000.0 # mm -> m
pos = self.mav.location() pos = self.mav.location()
home_distance = self.get_distance(HOME, pos) home_distance = self.get_distance(HOME, pos)
self.progress("Alt: %u HomeDistance: %.0f" % self.progress("Alt: %u HomeDistance: %.0f" %
(m.alt, home_distance)) (alt, home_distance))
# recenter pitch sticks once we're home so we don't fly off again # recenter pitch sticks once we're home so we don't fly off again
if pitching_forward and home_distance < 10: if pitching_forward and home_distance < 10:
pitching_forward = False pitching_forward = False
self.set_rc(2, 1500) self.set_rc(2, 1500)
# disable fence # disable fence
self.mavproxy.send('param set FENCE_ENABLE 0\n') self.mavproxy.send('param set FENCE_ENABLE 0\n')
if m.alt <= 1 and home_distance < 10: if alt <= 1 and home_distance < 10:
# reduce throttle # reduce throttle
self.set_rc(3, 1000) self.set_rc(3, 1000)
# switch mode to stabilize # switch mode to stabilize
@ -667,10 +674,12 @@ class AutoTestCopter(AutoTest):
# start displaying distance moved after all glitches applied # start displaying distance moved after all glitches applied
if glitch_current == -1: if glitch_current == -1:
m = self.mav.recv_match(type='VFR_HUD', blocking=True) m = self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True)
alt = m.alt/1000.0 # mm -> m
curr_pos = self.sim_location() curr_pos = self.sim_location()
moved_distance = self.get_distance(curr_pos, start_pos) moved_distance = self.get_distance(curr_pos, start_pos)
self.progress("Alt: %u Moved: %.0f" % (m.alt, moved_distance)) self.progress("Alt: %u Moved: %.0f" % (alt, moved_distance))
if moved_distance > max_distance: if moved_distance > max_distance:
self.progress("Moved over %u meters, Failed!" % self.progress("Moved over %u meters, Failed!" %
max_distance) max_distance)

19
Tools/autotest/common.py
Unescape View File

@ -467,7 +467,7 @@ class AutoTest(ABC):
while tstart + seconds_to_wait > tnow: while tstart + seconds_to_wait > tnow:
tnow = self.get_sim_time() tnow = self.get_sim_time()
def wait_altitude(self, alt_min, alt_max, timeout=30): def wait_altitude(self, alt_min, alt_max, timeout=30, relative=False):
"""Wait for a given altitude range.""" """Wait for a given altitude range."""
climb_rate = 0 climb_rate = 0
previous_alt = 0 previous_alt = 0
@ -476,12 +476,19 @@ class AutoTest(ABC):
self.progress("Waiting for altitude between %u and %u" % self.progress("Waiting for altitude between %u and %u" %
(alt_min, alt_max)) (alt_min, alt_max))
while self.get_sim_time() < tstart + timeout: while self.get_sim_time() < tstart + timeout:
m = self.mav.recv_match(type='VFR_HUD', blocking=True) m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
climb_rate = m.alt - previous_alt if m is None:
previous_alt = m.alt continue
if relative:
alt = m.relative_alt/1000.0 # mm -> m
else:
alt = m.alt/1000.0 # mm -> m
climb_rate = alt - previous_alt
previous_alt = alt
self.progress("Wait Altitude: Cur:%u, min_alt:%u, climb_rate: %u" self.progress("Wait Altitude: Cur:%u, min_alt:%u, climb_rate: %u"
% (m.alt, alt_min, climb_rate)) % (alt, alt_min, climb_rate))
if m.alt >= alt_min and m.alt <= alt_max: if alt >= alt_min and alt <= alt_max:
self.progress("Altitude OK") self.progress("Altitude OK")
return True return True
self.progress("Failed to attain altitude range") self.progress("Failed to attain altitude range")

Write Preview
Loading…
Cancel
Save