Skip to content

Commit

Permalink
mavutil: return mode as 'Mode(msg.custom_mode)' for px4 HIGH_LATENCY2
Browse files Browse the repository at this point in the history
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)`.
  • Loading branch information
Jannik Birk committed Mar 2, 2023
1 parent 3a6eed4 commit c6318c9
Showing 1 changed file with 4 additions and 0 deletions.
4 changes: 4 additions & 0 deletions mavutil.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down

0 comments on commit c6318c9

Please sign in to comment.