From c6318c977d12644f199194ada5f60d17feb6b9de Mon Sep 17 00:00:00 2001 From: Jannik Birk Date: Thu, 2 Mar 2023 10:45:08 +0100 Subject: [PATCH] mavutil: return mode as 'Mode(msg.custom_mode)' for px4 HIGH_LATENCY2 Before, when reading HIGH_LATENCY2 messages that came from a PX4 controller mavutil threw an exception because after the check for PX4, `msg.base_mode` was read immediately. It was not regarded that the `HIGH_LATENCY2` message does not contain a `base_mode` field. This introduces a fix where the mode string for HIGH_LATENCY2 messages from a PX4 controller is returned as `Mode(msg.custom_mode)`. --- mavutil.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/mavutil.py b/mavutil.py index 925fda933..ce73ca7bb 100644 --- a/mavutil.py +++ b/mavutil.py @@ -2273,7 +2273,11 @@ def mode_mapping_bynumber(mav_type): def mode_string_v10(msg): '''mode string for 1.0 protocol, from heartbeat''' if msg.autopilot == mavlink.MAV_AUTOPILOT_PX4: + if msg.get_type() == "HIGH_LATENCY2": + return f"Mode({msg.custom_mode})" + return interpret_px4_mode(msg.base_mode, msg.custom_mode) + if msg.get_type() != 'HIGH_LATENCY2' and not msg.base_mode & mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED: return "Mode(0x%08x)" % msg.base_mode