From 073a9e3390207d058fc8913b7064650a4e949757 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 22 Mar 2021 11:42:09 +1100 Subject: [PATCH] autotest: fix frsky passthrough test Mainly through polling much faster, but also allowing for statustexts in different orders from frsky/statustext --- Tools/autotest/arducopter.py | 8 ++-- Tools/autotest/common.py | 93 +++++++++++++++++++++++++----------- 2 files changed, 70 insertions(+), 31 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 687cb3b9aa..d9fe5e0520 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -2116,8 +2116,8 @@ class AutoTestCopter(AutoTest): self.reboot_sitl() # Test UAVCAN GPS ordering working - gps1_det_text = self.wait_statustext("GPS 1: specified as UAVCAN.*", regex=True, check_context=True) - gps2_det_text = self.wait_statustext("GPS 2: specified as UAVCAN.*", regex=True, check_context=True) + gps1_det_text = self.wait_text("GPS 1: specified as UAVCAN.*", regex=True, check_context=True) + gps2_det_text = self.wait_text("GPS 2: specified as UAVCAN.*", regex=True, check_context=True) gps1_nodeid = int(gps1_det_text.split('-')[1]) gps2_nodeid = int(gps2_det_text.split('-')[1]) if gps1_nodeid is None or gps2_nodeid is None: @@ -2145,11 +2145,11 @@ class AutoTestCopter(AutoTest): gps1_det_text = None gps2_det_text = None try: - gps1_det_text = self.wait_statustext("GPS 1: specified as UAVCAN.*", regex=True, check_context=True) + gps1_det_text = self.wait_text("GPS 1: specified as UAVCAN.*", regex=True, check_context=True) except AutoTestTimeoutException: pass try: - gps2_det_text = self.wait_statustext("GPS 2: specified as UAVCAN.*", regex=True, check_context=True) + gps2_det_text = self.wait_text("GPS 2: specified as UAVCAN.*", regex=True, check_context=True) except AutoTestTimeoutException: pass diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 43fed46194..0c0671e1e1 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -968,8 +968,8 @@ class FRSkySPort(FRSky): return ret def check_poll(self): - self.progress("check poll") now = self.get_time() + # self.progress("check poll (%u)" % now) # sometimes ArduPilot will not respond to a poll - for # example, if you poll an unhealthy RPM sensor then we will @@ -4114,6 +4114,13 @@ class AutoTest(ABC): return context.collections[msg_type] = [] + def context_collection(self, msg_type): + '''return messages in collection''' + context = self.context_get() + if msg_type not in context.collections: + raise NotAchievedException("Not collecting (%s)" % str(msg_type)) + return context.collections[msg_type] + def context_clear_collection(self, msg_type): '''clear collection of message type msg_type''' context = self.context_get() @@ -4671,7 +4678,7 @@ class AutoTest(ABC): ################################################# def delay_sim_time(self, seconds_to_wait): """Wait some second in SITL time.""" - self.drain_mav_unparsed() + self.drain_mav() tstart = self.get_sim_time() tnow = tstart self.progress("Delaying %f seconds" % (seconds_to_wait,)) @@ -5386,7 +5393,7 @@ Also, ignores heartbeats not from our target system''' def wait_ekf_flags(self, required_value, error_bits, timeout=30): self.progress("Waiting for EKF value %u" % required_value) - self.drain_mav_unparsed() + self.drain_mav() last_print_time = 0 tstart = self.get_sim_time() while timeout is None or self.get_sim_time_cached() < tstart + timeout: @@ -5440,22 +5447,28 @@ Also, ignores heartbeats not from our target system''' raise AutoTestTimeoutException("Failed to get EKF.flags=%u disabled" % not_required_value) def wait_text(self, *args, **kwargs): - return self.wait_statustext(*args, **kwargs) + '''wait for text to appear from vehicle, return that text''' + statustext = self.wait_statustext(*args, **kwargs) + if statustext is None: + return None + return statustext.text def statustext_in_collections(self, text, regex=False): + '''searches for text in STATUSTEXT collection, returns message if found''' c = self.context_get() if "STATUSTEXT" not in c.collections: raise NotAchievedException("Asked to check context but it isn't collecting!") - for statustext in [x.text for x in c.collections["STATUSTEXT"]]: + for x in c.collections["STATUSTEXT"]: + self.progress(" statustext=%s vs text=%s" % (x.text, text)) if regex: - if re.match(text, statustext): - return statustext - elif text.lower() in statustext.lower(): - return statustext + if re.match(text, x.text): + return x + elif text.lower() in x.text.lower(): + return x return None def wait_statustext(self, text, timeout=20, the_function=None, check_context=False, regex=False, wallclock_timeout=False): - """Wait for a specific STATUSTEXT.""" + """Wait for a specific STATUSTEXT, return that statustext message""" # Statustexts are often triggered by something we've just # done, so we have to be careful not to read any traffic that @@ -5484,11 +5497,11 @@ Also, ignores heartbeats not from our target system''' self.re_match = re.match(text, m.text) if self.re_match: statustext_found = True - statustext_full = m.text + statustext_full = m if text.lower() in m.text.lower(): self.progress("Received expected text: %s" % m.text.lower()) statustext_found = True - statustext_full = m.text + statustext_full = m self.install_message_hook(mh) if wallclock_timeout: @@ -5937,7 +5950,7 @@ Also, ignores heartbeats not from our target system''' '''mavlink2 required''' target_system = 1 target_component = 1 - self.drain_mav_unparsed() + self.drain_mav() self.progress("Sending mission_request_list") tstart = self.get_sim_time() self.mav.mav.mission_request_list_send(target_system, @@ -7525,7 +7538,7 @@ Also, ignores heartbeats not from our target system''' def poll_message(self, message_id, timeout=10): if type(message_id) == str: message_id = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % message_id) - self.drain_mav_unparsed() + self.drain_mav() tstart = self.get_sim_time() # required for timeout in run_cmd_get_ack to work self.send_poll_message(message_id) self.run_cmd_get_ack( @@ -9342,7 +9355,7 @@ switch value''' self.progress("received param (0x%02x) (id=%u value=%u)" % (value, param_id, param_value)) frame_type = param_value - hb = self.wait_heartbeat() + hb = self.mav.messages['HEARTBEAT'] hb_type = hb.type self.progress("validate_params: HEARTBEAT type==%f frsky==%f param_id=%u" % (hb_type, frame_type, param_id)) if param_id != 1: @@ -9390,7 +9403,8 @@ switch value''' while len(wants): self.progress("Still wanting (%s)" % ",".join([("0x%02x" % x) for x in wants.keys()])) wants_copy = copy.copy(wants) - t2 = self.get_sim_time() + self.drain_mav() + t2 = self.get_sim_time_cached() if t2 - tstart > 300: self.progress("Failed to get frsky passthrough data") self.progress("Counts of sensor_id polls sent:") @@ -9425,17 +9439,24 @@ switch value''' # test we get statustext strings. This relies on ArduPilot # emitting statustext strings when we fetch parameters. (or, # now, an updating-barometer statustext) - tstart = self.get_sim_time_cached() + tstart = self.get_sim_time() old_data = None text = "" - target_text = self.mav.recv_match( - type='STATUSTEXT', - blocking=True, - timeout=10 - ) - self.progress("Got STATUSTEXT: %s, waiting for same text from frsky" % target_text.text) + self.context_collect('STATUSTEXT') + self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + 0, # p1 + 0, # p2 + 1, # p3, baro + 0, # p4 + 0, # p5 + 0, # p6 + 0) # p7 + + received_frsky_texts = [] + last_len_received_statustexts = 0 while True: - now = self.get_sim_time() + self.drain_mav() + now = self.get_sim_time_cached() if now - tstart > 60: # it can take a *long* time to get these messages down! raise NotAchievedException("Did not get statustext in time") frsky.update() @@ -9459,14 +9480,32 @@ switch value''' if (x & 0x7f) == 0x00: last = True if last: - m = re.match(target_text.text, text) + m = None + text = text.rstrip("\0") + self.progress("Received frsky text (%s)" % (text,)) + self.progress("context texts: %s" % str([x.text for x in self.context_collection('STATUSTEXT')])) + m = self.statustext_in_collections(text) if m is not None: - want_sev = target_text.severity + want_sev = m.severity if severity != want_sev: raise NotAchievedException("Incorrect severity; want=%u got=%u" % (want_sev, severity)) - self.progress("Got statustext (%s)" % m.group(0)) + self.progress("Got statustext (%s)" % m.text) break + received_frsky_texts.append((severity, text)) text = "" + received_statustexts = self.context_collection('STATUSTEXT') + if len(received_statustexts) != last_len_received_statustexts: + last_len_received_statustexts = len(received_statustexts) + self.progress("received statustexts: %s" % str([x.text for x in received_statustexts])) + self.progress("received frsky texts: %s" % str(received_frsky_texts)) + for (want_sev, text) in received_frsky_texts: + for m in received_statustexts: + if m.text == text: + if want_sev != m.severity: + raise NotAchievedException("Incorrect severity; want=%u got=%u" % (want_sev, severity)) + self.progress("Got statustext (%s)" % text) + break + self.context_stop_collecting('STATUSTEXT') self.wait_ready_to_arm()