Skip to content

Commit

Permalink
mavutil: Return mode as 'Mode(msg.custom_mode)' for high-latency streams
Browse files Browse the repository at this point in the history
Before, when reading high-latency 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 _all_ `HIGH_LATENCY2`
messages is returnes as `Mode(msg.custom_mode)`.
  • Loading branch information
Jannik Birk committed Feb 16, 2023
1 parent 2ca2c13 commit 5b81644
Showing 1 changed file with 6 additions and 1 deletion.
7 changes: 6 additions & 1 deletion mavutil.py
Original file line number Diff line number Diff line change
Expand Up @@ -2272,14 +2272,19 @@ def mode_mapping_bynumber(mav_type):

def mode_string_v10(msg):
'''mode string for 1.0 protocol, from heartbeat'''
if msg.get_type() == "HIGH_LATENCY2":
return f"Mode({msg.custom_mode})"

if msg.autopilot == mavlink.MAV_AUTOPILOT_PX4:
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:

if not msg.base_mode & mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED:
return "Mode(0x%08x)" % msg.base_mode

mode_map = mode_mapping_bynumber(msg.type)
if mode_map and msg.custom_mode in mode_map:
return mode_map[msg.custom_mode]

return "Mode(%u)" % msg.custom_mode

def mode_string_apm(mode_number):
Expand Down

0 comments on commit 5b81644

Please sign in to comment.