diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index deb763abd3..ff56854908 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -152,9 +152,12 @@ class AutoTestCopter(AutoTest): self.set_rc(3, 1000) self.arm_vehicle() self.set_rc(3, takeoff_throttle) - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - if m.alt < alt_min: - self.wait_altitude(alt_min, (alt_min + 5)) + m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) + alt = m.relative_alt / 1000.0 # mm -> m + if alt < alt_min: + self.wait_altitude(alt_min, + (alt_min + 5), + relative=True) self.hover() self.progress("TAKEOFF COMPLETE") @@ -164,7 +167,7 @@ class AutoTestCopter(AutoTest): self.mavproxy.send('switch 2\n') # land mode self.wait_mode('LAND') self.progress("Entered Landing Mode") - self.wait_altitude(-5, 1) + self.wait_altitude(-5, 1, relative=True) self.progress("LANDING: ok!") 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): """Change altitude.""" - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - if m.alt < alt_min: - self.progress("Rise to alt:%u from %u" % (alt_min, m.alt)) + m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) + alt = m.relative_alt / 1000.0 # mm -> m + if alt < alt_min: + self.progress("Rise to alt:%u from %u" % (alt_min, alt)) 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: - 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.wait_altitude((alt_min - 5), alt_min) + self.wait_altitude((alt_min - 5), alt_min, relative=True) self.hover() ################################################# @@ -310,7 +314,7 @@ class AutoTestCopter(AutoTest): self.progress("timeleft = %u" % time_left) if 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() # enter RTL mode and wait for the vehicle to disarm @@ -321,14 +325,15 @@ class AutoTestCopter(AutoTest): self.set_rc(3, 1500) tstart = self.get_sim_time() 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() home_distance = self.get_distance(HOME, pos) home = "" - if m.alt <= 1 and home_distance < 10: + if alt <= 1 and home_distance < 10: home = "HOME" self.progress("Alt: %u HomeDist: %.0f %s" % - (m.alt, home_distance, home)) + (alt, home_distance, home)) # our post-condition is that we are disarmed: if not self.armed(): return @@ -367,12 +372,13 @@ class AutoTestCopter(AutoTest): tstart = self.get_sim_time() 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() 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 - if m.alt <= 1 and home_distance < 10: + if alt <= 1 and home_distance < 10: # reduce throttle self.set_rc(3, 1100) # switch back to stabilize @@ -496,18 +502,19 @@ class AutoTestCopter(AutoTest): # start timer tstart = self.get_sim_time() 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() home_distance = self.get_distance(HOME, pos) 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 if pitching_forward and home_distance < 10: pitching_forward = False self.set_rc(2, 1500) # disable fence 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 self.set_rc(3, 1000) # switch mode to stabilize @@ -667,10 +674,12 @@ class AutoTestCopter(AutoTest): # start displaying distance moved after all glitches applied 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() 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: self.progress("Moved over %u meters, Failed!" % max_distance) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 0a591edc14..05cd72efdb 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -467,7 +467,7 @@ class AutoTest(ABC): while tstart + seconds_to_wait > tnow: 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.""" climb_rate = 0 previous_alt = 0 @@ -476,12 +476,19 @@ class AutoTest(ABC): self.progress("Waiting for altitude between %u and %u" % (alt_min, alt_max)) while self.get_sim_time() < tstart + timeout: - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - climb_rate = m.alt - previous_alt - previous_alt = m.alt + m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) + if m is None: + 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" - % (m.alt, alt_min, climb_rate)) - if m.alt >= alt_min and m.alt <= alt_max: + % (alt, alt_min, climb_rate)) + if alt >= alt_min and alt <= alt_max: self.progress("Altitude OK") return True self.progress("Failed to attain altitude range")