mavlink.py
Go to the documentation of this file.
00001 # -*- coding: utf-8 -*-
00002 # vim:set ts=4 sw=4 et:
00003 #
00004 # Copyright 2015 Vladimir Ermakov.
00005 #
00006 # This file is part of the mavros package and subject to the license terms
00007 # in the top-level LICENSE file of the mavros repository.
00008 # https://github.com/mavlink/mavros/tree/master/LICENSE.md
00009 
00010 import rospy
00011 import struct
00012 from pymavlink import mavutil
00013 from pymavlink.generator.mavcrc import x25crc
00014 from mavros_msgs.msg import Mavlink
00015 from std_msgs.msg import Header
00016 
00017 
00018 def convert_to_bytes(msg):
00019     """
00020     Re-builds the MAVLink byte stream from mavros_msgs/Mavlink messages.
00021     """
00022     payload_octets = len(msg.payload64)
00023     msg_len = 6 + msg.len  # header + payload length
00024     if payload_octets < msg.len / 8:
00025         raise ValueError("Specified payload length is bigger than actual payload64")
00026 
00027     msgdata = bytearray(
00028         struct.pack(
00029             '<BBBBBB%dQ' % payload_octets,
00030             254, msg.len, msg.seq, msg.sysid, msg.compid, msg.msgid,
00031             *msg.payload64))
00032 
00033     if payload_octets != msg.len / 8:
00034         # message is shorter than payload octets
00035         msgdata = msgdata[:msg_len]
00036 
00037     if hasattr(mag, 'checksum'):
00038         # since #286 Mavlink.msg save original checksum, so recalculation not needed.
00039         crc16 = msg.checksum
00040     else:
00041         # from MAVLink.decode()
00042         message_type = mavutil.mavlink.mavlink_map[msg.msgid]
00043         crc_extra = message_type.crc_extra
00044 
00045         # calculate crc16
00046         crcbuf = msgdata[1:]
00047         crcbuf.append(crc_extra)
00048         crc16 = x25crc(crcbuf).crc
00049 
00050     # finalize
00051     msgdata += struct.pack('<H', crc16)
00052     return msgdata
00053 
00054 
00055 def convert_to_payload64(payload_bytes):
00056     """
00057     Convert payload bytes to Mavlink.payload64
00058     """
00059     payload_bytes = bytearray(payload_bytes)
00060     payload_len = len(payload_bytes)
00061     payload_octets = payload_len / 8
00062     if payload_len % 8 > 0:
00063         payload_octets += 1
00064         payload_bytes += b'\0' * (8 - payload_len % 8)
00065 
00066     return struct.unpack('<%dQ' % payload_octets, payload_bytes)
00067 
00068 
00069 def convert_to_rosmsg(mavmsg, stamp=None):
00070     """
00071     Convert pymavlink message to Mavlink.msg
00072     """
00073     if stamp is not None:
00074         header = Header(stamp=stamp)
00075     else:
00076         header = Header(stamp=rospy.get_rostime())
00077 
00078     return Mavlink(
00079         header=header,
00080         is_valid=True,
00081         len=len(mavmsg.get_payload()),
00082         seq=mavmsg.get_seq(),
00083         sysid=mavmsg.get_srcSystem(),
00084         compid=mavmsg.get_srcComponent(),
00085         msgid=mavmsg.get_msgId(),
00086         checksum=mavmsg.get_crc(),
00087         payload64=convert_to_payload64(mavmsg.get_payload())
00088     )


mavros
Author(s): Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:17