mavlink.py
Go to the documentation of this file.
1 # -*- coding: utf-8 -*-
2 # vim:set ts=4 sw=4 et:
3 #
4 # Copyright 2015 Vladimir Ermakov.
5 #
6 # This file is part of the mavros package and subject to the license terms
7 # in the top-level LICENSE file of the mavros repository.
8 # https://github.com/mavlink/mavros/tree/master/LICENSE.md
9 
10 import rospy
11 import struct
12 from pymavlink import mavutil
13 from pymavlink.generator.mavcrc import x25crc
14 from mavros_msgs.msg import Mavlink
15 from std_msgs.msg import Header
16 
17 
19  """
20  Re-builds the MAVLink byte stream from mavros_msgs/Mavlink messages.
21 
22  Support both v1.0 and v2.0.
23  """
24  payload_octets = len(msg.payload64)
25  if payload_octets < msg.len / 8:
26  raise ValueError("Specified payload length is bigger than actual payload64")
27 
28  if msg.magic == Mavlink.MAVLINK_V10:
29  msg_len = 6 + msg.len # header + payload length
30  msgdata = bytearray(
31  struct.pack(
32  '<BBBBBB%dQ' % payload_octets,
33  msg.magic, msg.len, msg.seq, msg.sysid, msg.compid, msg.msgid,
34  *msg.payload64))
35  else: # MAVLINK_V20
36  msg_len = 10 + msg.len # header + payload length
37  msgdata = bytearray(
38  struct.pack(
39  '<BBBBBBBBBB%dQ' % payload_octets,
40  msg.magic, msg.len, msg.incompat_flags, msg.compat_flags, msg.seq,
41  msg.sysid, msg.compid,
42  msg.msgid & 0xff, (msg.msgid >> 8) & 0xff, (msg.msgid >> 16) & 0xff,
43  *msg.payload64))
44 
45  if payload_octets != msg.len / 8:
46  # message is shorter than payload octets
47  msgdata = msgdata[:msg_len]
48 
49  # finalize
50  msgdata += struct.pack('<H', msg.checksum)
51 
52  if msg.magic == Mavlink.MAVLINK_V20:
53  msgdata += bytearray(msg.signature)
54 
55  return msgdata
56 
57 
58 def convert_to_payload64(payload_bytes):
59  """
60  Convert payload bytes to Mavlink.payload64
61  """
62  payload_bytes = bytearray(payload_bytes)
63  payload_len = len(payload_bytes)
64  payload_octets = payload_len / 8
65  if payload_len % 8 > 0:
66  payload_octets += 1
67  payload_bytes += b'\0' * (8 - payload_len % 8)
68 
69  return struct.unpack('<%dQ' % payload_octets, payload_bytes)
70 
71 
72 def convert_to_rosmsg(mavmsg, stamp=None):
73  """
74  Convert pymavlink message to Mavlink.msg
75 
76  Currently supports MAVLink v1.0 only.
77  """
78  if stamp is not None:
79  header = Header(stamp=stamp)
80  else:
81  header = Header(stamp=rospy.get_rostime())
82 
83  if mavutil.mavlink20():
84  # XXX Need some api to retreive signature block.
85  if mavmsg.get_signed():
86  rospy.logerr("Signed message can't be converted to rosmsg.")
87 
88  hdr = mavmsg.get_header()
89  return Mavlink(
90  header=header,
91  framing_status=Mavlink.FRAMING_OK,
92  magic=Mavlink.MAVLINK_V20,
93  len=hdr.mlen,
94  incompat_flags=hdr.incompat_flags,
95  compat_flags=hdr.compat_flags,
96  sysid=hdr.srcSystem,
97  compid=hdr.srcComponent,
98  msgid=hdr.msgId,
99  checksum=mavmsg.get_crc(),
100  payload64=convert_to_payload64(mavmsg.get_payload()),
101  signature=None, # FIXME #569
102  )
103 
104  else:
105  return Mavlink(
106  header=header,
107  framing_status=Mavlink.FRAMING_OK,
108  magic=Mavlink.MAVLINK_V10,
109  len=len(mavmsg.get_payload()),
110  seq=mavmsg.get_seq(),
111  sysid=mavmsg.get_srcSystem(),
112  compid=mavmsg.get_srcComponent(),
113  msgid=mavmsg.get_msgId(),
114  checksum=mavmsg.get_crc(),
115  payload64=convert_to_payload64(mavmsg.get_payload())
116  )
ssize_t len


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26