3 Parse a MAVLink protocol XML file and generate a Java implementation 5 Copyright Andrew Tridgell 2011 6 Released under GNU GPL version 3 or later 8 from __future__
import print_function
10 from builtins
import range
11 from builtins
import object
14 from .
import mavparse, mavtemplate
19 '''generate main header per XML file''' 20 directory = os.path.join(basename,
'''enums''')
21 mavparse.mkdir_p(directory)
23 f = open(os.path.join(directory, en.name+
".java"), mode=
'w')
25 /* AUTO-GENERATED FILE. DO NOT MODIFY. 27 * This class was automatically generated by the 28 * java mavlink generator tool. It should not be modified by hand. 31 package com.MAVLink.enums; 36 public class ${name} { 37 ${{entry: public static final int ${name} = ${value}; /* ${description} |${{param:${description}| }} */ 47 xml.message_crcs_array =
'' 48 for msgid
in range(256):
49 crc = xml.message_crcs.get(msgid, 0)
50 xml.message_crcs_array +=
'%u, ' % crc
51 xml.message_crcs_array = xml.message_crcs_array[:-2]
53 f = open(os.path.join(directory,
"CRC.java"), mode=
'w')
55 /* AUTO-GENERATED FILE. DO NOT MODIFY. 57 * This class was automatically generated by the 58 * java mavlink generator tool. It should not be modified by hand. 61 package com.MAVLink.${basename}; 64 * X.25 CRC calculation for MAVlink messages. The checksum must be initialized, 65 * updated with witch field of the message, and then finished with the message 70 private static final int[] MAVLINK_MESSAGE_CRCS = {${message_crcs_array}}; 71 private static final int CRC_INIT_VALUE = 0xffff; 75 * Accumulate the X.25 CRC by adding one char at a time. 77 * The checksum function adds the hash of one char at a time to the 16 bit 78 * checksum (uint16_t). 83 public void update_checksum(int data) { 84 data = data & 0xff; //cast because we want an unsigned type 85 int tmp = data ^ (crcValue & 0xff); 86 tmp ^= (tmp << 4) & 0xff; 87 crcValue = ((crcValue >> 8) & 0xff) ^ (tmp << 8) ^ (tmp << 3) ^ ((tmp >> 4) & 0xf); 91 * Finish the CRC calculation of a message, by running the CRC with the 92 * Magic Byte. This Magic byte has been defined in MAVlink v1.0. 95 * The message id number 97 public void finish_checksum(int msgid) { 98 update_checksum(MAVLINK_MESSAGE_CRCS[msgid]); 102 * Initialize the buffer for the X.25 CRC 105 public void start_checksum() { 106 crcValue = CRC_INIT_VALUE; 109 public int getMSB() { 110 return ((crcValue >> 8) & 0xff); 113 public int getLSB() { 114 return (crcValue & 0xff); 128 '''generate per-message header for a XML file''' 129 f = open(os.path.join(directory,
'msg_%s.java' % m.name_lower), mode=
'w')
131 (path_head, path_tail) = os.path.split(directory)
133 (path_head, path_tail) = os.path.split(path_head)
135 /* AUTO-GENERATED FILE. DO NOT MODIFY. 137 * This class was automatically generated by the 138 * java mavlink generator tool. It should not be modified by hand. 141 // MESSAGE ${name} PACKING 142 package com.MAVLink.%s; 143 import com.MAVLink.MAVLinkPacket; 144 import com.MAVLink.Messages.MAVLinkMessage; 145 import com.MAVLink.Messages.MAVLinkPayload; 150 public class msg_${name_lower} extends MAVLinkMessage{ 152 public static final int MAVLINK_MSG_ID_${name} = ${id}; 153 public static final int MAVLINK_MSG_LENGTH = ${wire_length}; 154 private static final long serialVersionUID = MAVLINK_MSG_ID_${name}; 161 public ${type} ${name}${array_suffix}; 165 * Generates the payload for a mavlink message for a message of this type 168 public MAVLinkPacket pack(){ 169 MAVLinkPacket packet = new MAVLinkPacket(MAVLINK_MSG_LENGTH); 172 packet.msgid = MAVLINK_MSG_ID_${name}; 180 * Decode a ${name_lower} message into this class fields 182 * @param payload The message to decode 184 public void unpack(MAVLinkPayload payload) { 185 payload.resetIndex(); 192 * Constructor for a new message, just initializes the msgid 194 public msg_${name_lower}(){ 195 msgid = MAVLINK_MSG_ID_${name}; 199 * Constructor for a new message, initializes the message with the payload 200 * from a mavlink packet 203 public msg_${name_lower}(MAVLinkPacket mavLinkPacket){ 204 this.sysid = mavLinkPacket.sysid; 205 this.compid = mavLinkPacket.compid; 206 this.msgid = MAVLINK_MSG_ID_${name}; 207 unpack(mavLinkPacket.payload); 210 ${{ordered_fields: ${getText} }} 212 * Returns a string with the MSG name and data 214 public String toString(){ 215 return "MAVLINK_MSG_ID_${name} - sysid:"+sysid+" compid:"+compid+${{ordered_fields:" ${name}:"+${name}+}}""; 223 f = open(os.path.join(directory,
"MAVLinkPacket.java"), mode=
'w')
228 importString =
"import com.MAVLink.{}.*;".format(xml.basename)
229 imports.append(importString)
231 xml_list[0].importString = os.linesep.join(imports)
234 /* AUTO-GENERATED FILE. DO NOT MODIFY. 236 * This class was automatically generated by the 237 * java mavlink generator tool. It should not be modified by hand. 242 import java.io.Serializable; 243 import com.MAVLink.Messages.MAVLinkPayload; 244 import com.MAVLink.Messages.MAVLinkMessage; 245 import com.MAVLink.${basename}.CRC; 250 * Common interface for all MAVLink Messages 252 * This is the anatomy of one packet. It is inspired by the CAN and SAE AS-4 standards. 254 * Byte Index Content Value Explanation 255 * 0 Packet start sign v1.0: 0xFE Indicates the start of a new packet. (v0.9: 0x55) 256 * 1 Payload length 0 - 255 Indicates length of the following payload. 257 * 2 Packet sequence 0 - 255 Each component counts up his send sequence. Allows to detect packet loss 258 * 3 System ID 1 - 255 ID of the SENDING system. Allows to differentiate different MAVs on the same network. 259 * 4 Component ID 0 - 255 ID of the SENDING component. Allows to differentiate different components of the same system, e.g. the IMU and the autopilot. 260 * 5 Message ID 0 - 255 ID of the message - the id defines what the payload means and how it should be correctly decoded. 261 * 6 to (n+6) Payload 0 - 255 Data of the message, depends on the message id. 262 * (n+7)to(n+8) Checksum (low byte, high byte) ITU X.25/SAE AS-4 hash, excluding packet start sign, so bytes 1..(n+6) Note: The checksum also includes MAVLINK_CRC_EXTRA (Number computed from message fields. Protects the packet from decoding a different version of the same packet but with different variables). 264 * The checksum is the same as used in ITU X.25 and SAE AS-4 standards (CRC-16-CCITT), documented in SAE AS5669A. Please see the MAVLink source code for a documented C-implementation of it. LINK TO CHECKSUM 265 * The minimum packet length is 8 bytes for acknowledgement packets without payload 266 * The maximum packet length is 263 bytes for full payload 269 public class MAVLinkPacket implements Serializable { 270 private static final long serialVersionUID = 2095947771227815314L; 272 public static final int MAVLINK_STX = 254; 275 * Message length. NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 277 public final int len; 285 * ID of the SENDING system. Allows to differentiate different MAVs on the 291 * ID of the SENDING component. Allows to differentiate different components 292 * of the same system, e.g. the IMU and the autopilot. 297 * ID of the message - the id defines what the payload means and how it 298 * should be correctly decoded. 303 * Data of the message, depends on the message id. 305 public MAVLinkPayload payload; 308 * ITU X.25/SAE AS-4 hash, excluding packet start sign, so bytes 1..(n+6) 309 * Note: The checksum also includes MAVLINK_CRC_EXTRA (Number computed from 310 * message fields. Protects the packet from decoding a different version of 311 * the same packet but with different variables). 315 public MAVLinkPacket(int payloadLength){ 317 payload = new MAVLinkPayload(payloadLength); 321 * Check if the size of the Payload is equal to the "len" byte 323 public boolean payloadIsFilled() { 324 return payload.size() >= len; 328 * Update CRC for this packet. 330 public void generateCRC(){ 335 crc.start_checksum(); 338 crc.update_checksum(len); 339 crc.update_checksum(seq); 340 crc.update_checksum(sysid); 341 crc.update_checksum(compid); 342 crc.update_checksum(msgid); 344 payload.resetIndex(); 346 final int payloadSize = payload.size(); 347 for (int i = 0; i < payloadSize; i++) { 348 crc.update_checksum(payload.getByte()); 350 crc.finish_checksum(msgid); 354 * Encode this packet for transmission. 356 * @return Array with bytes to be transmitted 358 public byte[] encodePacket() { 359 byte[] buffer = new byte[6 + len + 2]; 362 buffer[i++] = (byte) MAVLINK_STX; 363 buffer[i++] = (byte) len; 364 buffer[i++] = (byte) seq; 365 buffer[i++] = (byte) sysid; 366 buffer[i++] = (byte) compid; 367 buffer[i++] = (byte) msgid; 369 final int payloadSize = payload.size(); 370 for (int j = 0; j < payloadSize; j++) { 371 buffer[i++] = payload.payload.get(j); 375 buffer[i++] = (byte) (crc.getLSB()); 376 buffer[i++] = (byte) (crc.getMSB()); 381 * Unpack the data in this packet and return a MAVLink message 383 * @return MAVLink message decoded from this packet 385 public MAVLinkMessage unpack() { 391 case msg_${name_lower}.MAVLINK_MSG_ID_${name}: 392 return new msg_${name_lower}(this); 408 '''copy the fixed protocol headers to the target directory''' 410 hlist = [
'Parser.java',
'Messages/MAVLinkMessage.java',
'Messages/MAVLinkPayload.java',
'Messages/MAVLinkStats.java' ]
411 basepath = os.path.dirname(os.path.realpath(__file__))
412 srcpath = os.path.join(basepath,
'java/lib')
413 print(
"Copying fixed headers")
415 src = os.path.realpath(os.path.join(srcpath, h))
416 dest = os.path.realpath(os.path.join(directory, h))
419 destdir = os.path.realpath(os.path.join(directory,
'Messages'))
423 print(
"Not re-creating Messages directory")
424 shutil.copy(src, dest)
432 '''work out the struct format for a type''' 434 'float' : (
'float',
'Float'),
435 'double' : (
'double',
'Double'),
436 'char' : (
'byte',
'Byte'),
437 'int8_t' : (
'byte',
'Byte'),
438 'uint8_t' : (
'short',
'UnsignedByte'),
439 'uint8_t_mavlink_version' : (
'short',
'UnsignedByte'),
440 'int16_t' : (
'short',
'Short'),
441 'uint16_t' : (
'int',
'UnsignedShort'),
442 'int32_t' : (
'int',
'Int'),
443 'uint32_t' : (
'long',
'UnsignedInt'),
444 'int64_t' : (
'long',
'Long'),
445 'uint64_t' : (
'long',
'UnsignedLong'),
449 return map[field.type][1]
451 return map[field.type][0]
454 '''generate headers for one XML file''' 456 directory = os.path.join(basename, xml.basename)
458 print(
"Generating Java implementation in directory %s" % directory)
459 mavparse.mkdir_p(directory)
461 if xml.little_endian:
462 xml.mavlink_endian =
"MAVLINK_LITTLE_ENDIAN" 464 xml.mavlink_endian =
"MAVLINK_BIG_ENDIAN" 467 xml.crc_extra_define =
"1" 469 xml.crc_extra_define =
"0" 472 xml.aligned_fields_define =
"1" 474 xml.aligned_fields_define =
"0" 477 xml.include_list = []
478 for i
in xml.include:
483 xml.message_lengths_array =
'' 484 for mlen
in xml.message_lengths:
485 xml.message_lengths_array +=
'%u, ' % mlen
486 xml.message_lengths_array = xml.message_lengths_array[:-2]
491 xml.message_info_array =
'' 492 for name
in xml.message_names:
494 xml.message_info_array +=
'MAVLINK_MESSAGE_INFO_%s, ' % name
499 xml.message_info_array +=
'{"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, ' 500 xml.message_info_array = xml.message_info_array[:-2]
503 for m
in xml.message:
506 m.crc_extra_arg =
", %s" % m.crc_extra
510 if f.print_format
is None:
511 f.c_print_format =
'NULL' 513 f.c_print_format =
'"%s"' % f.print_format
515 if f.array_length != 0:
516 f.array_suffix =
'[] = new %s[%u]' % (
mavfmt(f),f.array_length)
518 f.array_tag =
'_array' 519 f.array_arg =
', %u' % f.array_length
520 f.array_return_arg =
'%s, %u, ' % (f.name, f.array_length)
521 f.array_const =
'const ' 523 f.decode_right =
'm.%s' % (f.name)
526 for (int i = 0; i < this.%s.length; i++) { 527 this.%s[i] = payload.get%s(); 529 ''' % (f.name, f.name,
mavfmt(f,
True) )
531 for (int i = 0; i < %s.length; i++) { 532 packet.payload.put%s(%s[i]); 534 ''' % (f.name,
mavfmt(f,
True),f.name)
535 f.return_type =
'uint16_t' 536 f.get_arg =
', %s *%s' % (f.type, f.name)
538 f.c_test_value =
'"%s"' % f.test_value
541 * Sets the buffer of this message with a string, adds the necessary padding 543 public void set%s(String str) { 544 int len = Math.min(str.length(), %d); 545 for (int i=0; i<len; i++) { 546 %s[i] = (byte) str.charAt(i); 549 for (int i=len; i<%d; i++) { // padding for the rest of the buffer 555 * Gets the message, formated as a string 557 public String get%s() { 558 StringBuffer buf = new StringBuffer(); 559 for (int i = 0; i < %d; i++) { 561 buf.append((char) %s[i]); 565 return buf.toString(); 568 ''' % (f.name.title(),f.array_length,f.name,f.array_length,f.name,f.name.title(),f.array_length,f.name,f.name)
571 for v
in f.test_value:
572 test_strings.append(
str(v))
573 f.c_test_value =
'{ %s }' %
', '.join(test_strings)
579 f.array_return_arg =
'' 581 f.decode_left =
'%s' % (f.name)
583 f.unpackField =
'this.%s = payload.get%s();' % (f.name,
mavfmt(f,
True))
584 f.packField =
'packet.payload.put%s(%s);' % (
mavfmt(f,
True),f.name)
588 f.return_type = f.type
590 f.c_test_value =
"'%s'" % f.test_value
591 elif f.type ==
'uint64_t':
592 f.c_test_value =
"%sULL" % f.test_value
593 elif f.type ==
'int64_t':
594 f.c_test_value =
"%sLL" % f.test_value
596 f.c_test_value = f.test_value
599 for m
in xml.message:
603 for f
in m.ordered_fields:
604 if f.array_length != 0:
605 m.array_fields.append(f)
607 m.scalar_fields.append(f)
610 m.arg_fields.append(f)
613 f.putname = f.const_value
616 for m
in xml.message:
617 for f
in m.ordered_fields:
622 for m
in xml.message:
627 '''generate complete MAVLink Java implemenation'''
def copy_fixed_headers(directory, xml)
def generate_MAVLinkMessage(directory, xml_list)
def generate_one(basename, xml)
def generate(basename, xml_list)
def generate_enums(basename, xml)
def mavfmt(field, typeInfo=False)
def generate_message_h(directory, m)
def generate_CRC(directory, xml)