/** Encode message with raw data and other informations */ public byte[] encode() throws IOException { byte[] buffer = new byte[8 + 12]; LittleEndianDataOutputStream dos = new LittleEndianDataOutputStream(new ByteArrayOutputStream()); dos.writeByte((byte) 0xFE); dos.writeByte(length & 0x00FF); dos.writeByte(sequence & 0x00FF); dos.writeByte(sysId & 0x00FF); dos.writeByte(componentId & 0x00FF); dos.writeByte(messageType & 0x00FF); dos.writeShort(adc1 & 0x00FFFF); dos.writeShort(adc2 & 0x00FFFF); dos.writeShort(adc3 & 0x00FFFF); dos.writeShort(adc4 & 0x00FFFF); dos.writeShort(adc5 & 0x00FFFF); dos.writeShort(adc6 & 0x00FFFF); dos.flush(); byte[] tmp = dos.toByteArray(); for (int b = 0; b < tmp.length; b++) buffer[b] = tmp[b]; int crc = MAVLinkCRC.crc_calculate_encode(buffer, 12); crc = MAVLinkCRC.crc_accumulate((byte) IMAVLinkCRC.MAVLINK_MESSAGE_CRCS[messageType], crc); byte crcl = (byte) (crc & 0x00FF); byte crch = (byte) ((crc >> 8) & 0x00FF); buffer[18] = crcl; buffer[19] = crch; return buffer; }
/** Encode message with raw data and other informations */ public byte[] encode() throws IOException { byte[] buffer = new byte[12 + 2]; LittleEndianDataOutputStream dos = new LittleEndianDataOutputStream(new ByteArrayOutputStream()); dos.writeByte((byte) 0xFD); dos.writeByte(length & 0x00FF); dos.writeByte(incompat & 0x00FF); dos.writeByte(compat & 0x00FF); dos.writeByte(packet & 0x00FF); dos.writeByte(sysId & 0x00FF); dos.writeByte(componentId & 0x00FF); dos.writeByte(messageType & 0x00FF); dos.writeByte((messageType >> 8) & 0x00FF); dos.writeByte((messageType >> 16) & 0x00FF); dos.writeByte(target_system & 0x00FF); dos.writeByte(target_component & 0x00FF); dos.flush(); byte[] tmp = dos.toByteArray(); for (int b = 0; b < tmp.length; b++) buffer[b] = tmp[b]; int crc = MAVLinkCRC.crc_calculate_encode(buffer, 2); crc = MAVLinkCRC.crc_accumulate((byte) IMAVLinkCRC.MAVLINK_MESSAGE_CRCS[messageType], crc); byte crcl = (byte) (crc & 0x00FF); byte crch = (byte) ((crc >> 8) & 0x00FF); buffer[12] = crcl; buffer[13] = crch; dos.close(); return buffer; }
/** Encode message with raw data and other informations */ public byte[] encode() throws IOException { byte[] buffer = new byte[8 + 21]; LittleEndianDataOutputStream dos = new LittleEndianDataOutputStream(new ByteArrayOutputStream()); dos.writeByte((byte) 0xFE); dos.writeByte(length & 0x00FF); dos.writeByte(sequence & 0x00FF); dos.writeByte(sysId & 0x00FF); dos.writeByte(componentId & 0x00FF); dos.writeByte(messageType & 0x00FF); dos.writeFloat(roll); dos.writeFloat(pitch); dos.writeFloat(yaw); dos.writeFloat(thrust); dos.writeByte(target & 0x00FF); dos.writeByte(roll_manual & 0x00FF); dos.writeByte(pitch_manual & 0x00FF); dos.writeByte(yaw_manual & 0x00FF); dos.writeByte(thrust_manual & 0x00FF); dos.flush(); byte[] tmp = dos.toByteArray(); for (int b = 0; b < tmp.length; b++) buffer[b] = tmp[b]; int crc = MAVLinkCRC.crc_calculate_encode(buffer, 21); crc = MAVLinkCRC.crc_accumulate((byte) IMAVLinkCRC.MAVLINK_MESSAGE_CRCS[messageType], crc); byte crcl = (byte) (crc & 0x00FF); byte crch = (byte) ((crc >> 8) & 0x00FF); buffer[27] = crcl; buffer[28] = crch; dos.close(); return buffer; }
/** Encode message with raw data and other informations */ public byte[] encode() throws IOException { byte[] buffer = new byte[8 + 43]; LittleEndianDataOutputStream dos = new LittleEndianDataOutputStream(new ByteArrayOutputStream()); dos.writeByte((byte) 0xFE); dos.writeByte(length & 0x00FF); dos.writeByte(sequence & 0x00FF); dos.writeByte(sysId & 0x00FF); dos.writeByte(componentId & 0x00FF); dos.writeByte(messageType & 0x00FF); dos.writeFloat(x); dos.writeFloat(y); dos.writeFloat(z); dos.writeShort(timeout & 0x00FFFF); dos.writeByte(type & 0x00FF); dos.writeByte(color & 0x00FF); dos.writeByte(coordinate_system & 0x00FF); for (int i = 0; i < 26; i++) { dos.writeByte(name[i]); } dos.flush(); byte[] tmp = dos.toByteArray(); for (int b = 0; b < tmp.length; b++) buffer[b] = tmp[b]; int crc = MAVLinkCRC.crc_calculate_encode(buffer, 43); crc = MAVLinkCRC.crc_accumulate((byte) IMAVLinkCRC.MAVLINK_MESSAGE_CRCS[messageType], crc); byte crcl = (byte) (crc & 0x00FF); byte crch = (byte) ((crc >> 8) & 0x00FF); buffer[49] = crcl; buffer[50] = crch; dos.close(); return buffer; }
/** Encode message with raw data and other informations */ public byte[] encode() throws IOException { byte[] buffer = new byte[8 + 13]; LittleEndianDataOutputStream dos = new LittleEndianDataOutputStream(new ByteArrayOutputStream()); dos.writeByte((byte) 0xFE); dos.writeByte(length & 0x00FF); dos.writeByte(sequence & 0x00FF); dos.writeByte(sysId & 0x00FF); dos.writeByte(componentId & 0x00FF); dos.writeByte(messageType & 0x00FF); dos.writeInt((int) (latitude & 0x00FFFFFFFF)); dos.writeInt((int) (longitude & 0x00FFFFFFFF)); dos.writeInt((int) (altitude & 0x00FFFFFFFF)); dos.writeByte(target_system & 0x00FF); dos.flush(); byte[] tmp = dos.toByteArray(); for (int b = 0; b < tmp.length; b++) buffer[b] = tmp[b]; int crc = MAVLinkCRC.crc_calculate_encode(buffer, 13); crc = MAVLinkCRC.crc_accumulate((byte) IMAVLinkCRC.MAVLINK_MESSAGE_CRCS[messageType], crc); byte crcl = (byte) (crc & 0x00FF); byte crch = (byte) ((crc >> 8) & 0x00FF); buffer[19] = crcl; buffer[20] = crch; dos.close(); return buffer; }
/** Encode message with raw data and other informations */ public byte[] encode() throws IOException { byte[] buffer = new byte[12 + 28]; LittleEndianDataOutputStream dos = new LittleEndianDataOutputStream(new ByteArrayOutputStream()); dos.writeByte((byte) 0xFD); dos.writeByte(length & 0x00FF); dos.writeByte(incompat & 0x00FF); dos.writeByte(compat & 0x00FF); dos.writeByte(packet & 0x00FF); dos.writeByte(sysId & 0x00FF); dos.writeByte(componentId & 0x00FF); dos.writeByte(messageType & 0x00FF); dos.writeByte((messageType >> 8) & 0x00FF); dos.writeByte((messageType >> 16) & 0x00FF); dos.writeInt((int) (time_boot_ms & 0x00FFFFFFFF)); dos.writeFloat(x); dos.writeFloat(y); dos.writeFloat(z); dos.writeFloat(roll); dos.writeFloat(pitch); dos.writeFloat(yaw); dos.flush(); byte[] tmp = dos.toByteArray(); for (int b = 0; b < tmp.length; b++) buffer[b] = tmp[b]; int crc = MAVLinkCRC.crc_calculate_encode(buffer, 28); crc = MAVLinkCRC.crc_accumulate((byte) IMAVLinkCRC.MAVLINK_MESSAGE_CRCS[messageType], crc); byte crcl = (byte) (crc & 0x00FF); byte crch = (byte) ((crc >> 8) & 0x00FF); buffer[38] = crcl; buffer[39] = crch; dos.close(); return buffer; }
/** Encode message with raw data and other informations */ public byte[] encode() throws IOException { byte[] buffer = new byte[8 + 16]; LittleEndianDataOutputStream dos = new LittleEndianDataOutputStream(new ByteArrayOutputStream()); dos.writeByte((byte) 0xFE); dos.writeByte(length & 0x00FF); dos.writeByte(sequence & 0x00FF); dos.writeByte(sysId & 0x00FF); dos.writeByte(componentId & 0x00FF); dos.writeByte(messageType & 0x00FF); dos.writeLong(time_usec); dos.writeShort(press_abs & 0x00FFFF); dos.writeShort(press_diff1 & 0x00FFFF); dos.writeShort(press_diff2 & 0x00FFFF); dos.writeShort(temperature & 0x00FFFF); dos.flush(); byte[] tmp = dos.toByteArray(); for (int b = 0; b < tmp.length; b++) buffer[b] = tmp[b]; int crc = MAVLinkCRC.crc_calculate_encode(buffer, 16); crc = MAVLinkCRC.crc_accumulate((byte) IMAVLinkCRC.MAVLINK_MESSAGE_CRCS[messageType], crc); byte crcl = (byte) (crc & 0x00FF); byte crch = (byte) ((crc >> 8) & 0x00FF); buffer[22] = crcl; buffer[23] = crch; dos.close(); return buffer; }
/** Encode message with raw data and other informations */ public byte[] encode() throws IOException { byte[] buffer = new byte[8 + 26]; ByteBuffer dos = ByteBuffer.wrap(buffer).order(ByteOrder.LITTLE_ENDIAN); dos.put((byte) 0xFE); dos.put((byte) (length & 0x00FF)); dos.put((byte) (sequence & 0x00FF)); dos.put((byte) (sysId & 0x00FF)); dos.put((byte) (componentId & 0x00FF)); dos.put((byte) (messageType & 0x00FF)); dos.putFloat(nav_roll); dos.putFloat(nav_pitch); dos.putFloat(alt_error); dos.putFloat(aspd_error); dos.putFloat(xtrack_error); dos.putShort((short) (nav_bearing & 0x00FFFF)); dos.putShort((short) (target_bearing & 0x00FFFF)); dos.putShort((short) (wp_dist & 0x00FFFF)); int crc = MAVLinkCRC.crc_calculate_encode(buffer, 26); crc = MAVLinkCRC.crc_accumulate((byte) IMAVLinkCRC.MAVLINK_MESSAGE_CRCS[messageType], crc); byte crcl = (byte) (crc & 0x00FF); byte crch = (byte) ((crc >> 8) & 0x00FF); buffer[32] = crcl; buffer[33] = crch; return buffer; }
/** Encode message with raw data and other informations */ public byte[] encode() throws IOException { byte[] buffer = new byte[8 + 35]; LittleEndianDataOutputStream dos = new LittleEndianDataOutputStream(new ByteArrayOutputStream()); dos.writeByte((byte) 0xFE); dos.writeByte(length & 0x00FF); dos.writeByte(sequence & 0x00FF); dos.writeByte(sysId & 0x00FF); dos.writeByte(componentId & 0x00FF); dos.writeByte(messageType & 0x00FF); dos.writeFloat(param1); dos.writeFloat(param2); dos.writeFloat(param3); dos.writeFloat(param4); dos.writeInt((int) (x & 0x00FFFFFFFF)); dos.writeInt((int) (y & 0x00FFFFFFFF)); dos.writeFloat(z); dos.writeShort(command & 0x00FFFF); dos.writeByte(target_system & 0x00FF); dos.writeByte(target_component & 0x00FF); dos.writeByte(frame & 0x00FF); dos.writeByte(current & 0x00FF); dos.writeByte(autocontinue & 0x00FF); dos.flush(); byte[] tmp = dos.toByteArray(); for (int b = 0; b < tmp.length; b++) buffer[b] = tmp[b]; int crc = MAVLinkCRC.crc_calculate_encode(buffer, 35); crc = MAVLinkCRC.crc_accumulate((byte) IMAVLinkCRC.MAVLINK_MESSAGE_CRCS[messageType], crc); byte crcl = (byte) (crc & 0x00FF); byte crch = (byte) ((crc >> 8) & 0x00FF); buffer[41] = crcl; buffer[42] = crch; dos.close(); return buffer; }
/** Encode message with raw data and other informations */ public byte[] encode() throws IOException { byte[] buffer = new byte[8 + 20]; ByteBuffer dos = ByteBuffer.wrap(buffer).order(ByteOrder.LITTLE_ENDIAN); dos.put((byte) 0xFE); dos.put((byte) (length & 0x00FF)); dos.put((byte) (sequence & 0x00FF)); dos.put((byte) (sysId & 0x00FF)); dos.put((byte) (componentId & 0x00FF)); dos.put((byte) (messageType & 0x00FF)); dos.putInt((int) (time_boot_ms & 0x00FFFFFFFF)); dos.putFloat(roll_rate); dos.putFloat(pitch_rate); dos.putFloat(yaw_rate); dos.putFloat(thrust); int crc = MAVLinkCRC.crc_calculate_encode(buffer, 20); crc = MAVLinkCRC.crc_accumulate((byte) IMAVLinkCRC.MAVLINK_MESSAGE_CRCS[messageType], crc); byte crcl = (byte) (crc & 0x00FF); byte crch = (byte) ((crc >> 8) & 0x00FF); buffer[26] = crcl; buffer[27] = crch; return buffer; }
/** Encode message with raw data and other informations */ public byte[] encode() throws IOException { byte[] buffer = new byte[12 + 23]; LittleEndianDataOutputStream dos = new LittleEndianDataOutputStream(new ByteArrayOutputStream()); dos.writeByte((byte) 0xFD); dos.writeByte(length & 0x00FF); dos.writeByte(incompat & 0x00FF); dos.writeByte(compat & 0x00FF); dos.writeByte(packet & 0x00FF); dos.writeByte(sysId & 0x00FF); dos.writeByte(componentId & 0x00FF); dos.writeByte(messageType & 0x00FF); dos.writeByte((messageType >> 8) & 0x00FF); dos.writeByte((messageType >> 16) & 0x00FF); dos.writeInt((int) (time_boot_ms & 0x00FFFFFFFF)); dos.writeFloat(image_interval); dos.writeFloat(video_framerate); dos.writeShort(image_resolution_h & 0x00FFFF); dos.writeShort(image_resolution_v & 0x00FFFF); dos.writeShort(video_resolution_h & 0x00FFFF); dos.writeShort(video_resolution_v & 0x00FFFF); dos.writeByte(camera_id & 0x00FF); dos.writeByte(image_status & 0x00FF); dos.writeByte(video_status & 0x00FF); dos.flush(); byte[] tmp = dos.toByteArray(); for (int b = 0; b < tmp.length; b++) buffer[b] = tmp[b]; int crc = MAVLinkCRC.crc_calculate_encode(buffer, 23); crc = MAVLinkCRC.crc_accumulate((byte) IMAVLinkCRC.MAVLINK_MESSAGE_CRCS[messageType], crc); byte crcl = (byte) (crc & 0x00FF); byte crch = (byte) ((crc >> 8) & 0x00FF); buffer[33] = crcl; buffer[34] = crch; dos.close(); return buffer; }
/** * Generate MAVLink messages Java classes * * @param mavlink * @param targetPath */ protected void generateMessageClass(MAVLinkData mavlink, String targetPath) { StringBuffer sbRead, sbWrite, fieldWrite; String packageRootName = "org.mavlink.messages"; String xmlFilename = mavlink.getFile(); String packageName = packageRootName + "." + xmlFilename; String directory = targetPath + "/org/mavlink/messages/" + xmlFilename + "/"; OutputStream output = null; PrintWriter writer = null; String forToString = ""; for (MAVLinkMessage message : mavlink.getMessages().values()) { String className = "msg_" + message.getName().toLowerCase(); String filename = directory + className + ".java"; imports = imports + "import " + packageName + "." + className + ";\n"; try { File file = new File(directory); file.mkdirs(); output = new FileOutputStream(filename, false); writer = new PrintWriter(output); sbRead = new StringBuffer(); sbWrite = new StringBuffer(); fieldWrite = new StringBuffer(); if (forEmbeddedJava) { sbWrite.append(" dos.writeByte((byte)0xFE);\n"); sbWrite.append(" dos.writeByte(length & 0x00FF);\n"); sbWrite.append(" dos.writeByte(sequence & 0x00FF);\n"); sbWrite.append(" dos.writeByte(sysId & 0x00FF);\n"); sbWrite.append(" dos.writeByte(componentId & 0x00FF);\n"); sbWrite.append(" dos.writeByte(messageType & 0x00FF);\n"); } else { sbWrite.append(" dos.put((byte)0xFE);\n"); sbWrite.append(" dos.put((byte)(length & 0x00FF));\n"); sbWrite.append(" dos.put((byte)(sequence & 0x00FF));\n"); sbWrite.append(" dos.put((byte)(sysId & 0x00FF));\n"); sbWrite.append(" dos.put((byte)(componentId & 0x00FF));\n"); sbWrite.append(" dos.put((byte)(messageType & 0x00FF));\n"); } // Write Header writer.print("/**\n * Generated class : " + className + "\n * DO NOT MODIFY!\n **/\n"); writer.print("package " + packageName + ";\n"); writer.print("import " + packageRootName + ".MAVLinkMessage;\n"); writer.print("import org.mavlink.IMAVLinkCRC;\n"); writer.print("import org.mavlink.MAVLinkCRC;\n"); writer.print("import java.io.ByteArrayOutputStream;\n"); writer.print("import java.io.IOException;\n"); if (forEmbeddedJava) { if (isLittleEndian) { writer.print("import org.mavlink.io.LittleEndianDataInputStream;\n"); writer.print("import org.mavlink.io.LittleEndianDataOutputStream;\n"); } else { writer.print("import java.io.DataInputStream;\n"); writer.print("import java.io.DataOutputStream;\n"); } } else { writer.print("import java.nio.ByteBuffer;\n"); writer.print("import java.nio.ByteOrder;\n"); } String description = message.getDescription(); writer.print( "/**\n * Class " + className + "\n * " + (description == null ? "" : message.getDescription().trim()) + "\n **/\n"); writer.print("public class " + className + " extends MAVLinkMessage {\n"); String id = MAVLINK_MSG + "_ID_" + message.getName(); writer.print(" public static final int " + id + " = " + message.getId() + ";\n"); writer.print(" private static final long serialVersionUID = " + id + ";\n"); writer.print( " public " + className + "(int sysId, int componentId) {\n messageType = " + id + ";\n this.sysId = sysId;\n this.componentId = componentId;\n"); // Calculate extra_crc for Mavlinl 1.0 String extraCrcBuffer = message.getName() + " "; // Write Fields int fieldLen = 0; Collections.sort(message.getFields(), new FieldCompare()); for (int j = 0; j < message.getFields().size(); j++) { MAVLinkField field = message.getFields().get(j); fieldWrite.append(" /**\n * " + field.getDescription().trim() + "\n */\n"); MAVLinkDataType type = field.getType(); fieldWrite.append(" public " + type.getJavaType(field.getName()) + "\n"); sbRead.append(type.getReadType(field.getName(), forEmbeddedJava)); sbWrite.append(type.getWriteType(field.getName(), forEmbeddedJava)); String attr = field.getName(); if (type.isArray && type.type == MAVLinkDataType.CHAR) { String first = "" + attr.charAt(0); attr = first.toUpperCase() + field.getName().substring(1); fieldWrite.append(" public void set" + attr + "(String tmp) {\n"); fieldWrite.append(" int len = Math.min(tmp.length(), " + type.arrayLenth + ");\n"); fieldWrite.append( " for (int i=0; i<len; i++) {\n " + field.getName() + "[i] = tmp.charAt(i);\n }\n"); fieldWrite.append( " for (int i=len; i<" + type.arrayLenth + "; i++) {\n " + field.getName() + "[i] = 0;\n }\n }\n"); fieldWrite.append(" public String get" + attr + "() {\n"); fieldWrite.append(" String result=\"\";\n"); fieldWrite.append( " for (int i=0; i<" + type.arrayLenth + "; i++) {\n if (" + field.getName() + "[i] != 0) result=result+" + field.getName() + "[i]; else break;\n }\n return result;\n }\n"); } fieldLen += type.getLengthType(); forToString = forToString + (j != 0 ? "+" : "") + " \" " + field.getName() + "=\"+" + (field.getType().isArray && field.getType().type == MAVLinkDataType.CHAR ? "get" + attr + "()" : field.getName()); extraCrcBuffer = extraCrcBuffer + type.getCType() + " " + field.getName() + " "; if (type.isArray) { extraCrcBuffer = extraCrcBuffer + (char) type.arrayLenth; } } writer.print(" length = " + fieldLen + ";\n}\n\n"); writer.print(fieldWrite.toString()); int extra_crc = MAVLinkCRC.crc_calculate(MAVLinkCRC.stringToByte(extraCrcBuffer)); int magicNumber = (extra_crc & 0x00FF) ^ ((extra_crc >> 8 & 0x00FF)); MAVLINK_MESSAGE_CRCS[message.getId()] = magicNumber; writer.print("/**\n"); writer.print(" * Decode message with raw data\n"); writer.print(" */\n"); if (forEmbeddedJava) { if (isLittleEndian) { writer.print( "public void decode(LittleEndianDataInputStream dis) throws IOException {\n"); } else { writer.print("public void decode(DataInputStream dis) throws IOException {\n"); } } else { writer.print("public void decode(ByteBuffer dis) throws IOException {\n"); } writer.print(sbRead.toString()); writer.print("}\n"); writer.print("/**\n"); writer.print(" * Encode message with raw data and other informations\n"); writer.print(" */\n"); writer.print("public byte[] encode() throws IOException {\n"); writer.print(" byte[] buffer = new byte[8+" + fieldLen + "];\n"); if (forEmbeddedJava) { if (isLittleEndian) { writer.print( " LittleEndianDataOutputStream dos = new LittleEndianDataOutputStream(new ByteArrayOutputStream());\n"); } else { writer.print(" ByteArrayOutputStream baos = new ByteArrayOutputStream();\n"); writer.print(" DataOutputStream dos = new DataOutputStream(baos);\n"); } } else { if (isLittleEndian) { writer.print( " ByteBuffer dos = ByteBuffer.wrap(buffer).order(ByteOrder.LITTLE_ENDIAN);\n"); } else { writer.print( " ByteBuffer dos = ByteBuffer.wrap(buffer).order(ByteOrder.BIG_ENDIAN);\n"); } } writer.print(sbWrite.toString()); if (forEmbeddedJava) { if (isLittleEndian) { writer.print(" dos.flush();\n byte[] tmp = dos.toByteArray();\n"); } else { writer.print(" dos.flush();\n byte[] tmp = baos.toByteArray();\n"); } writer.print(" for (int b=0; b<tmp.length; b++) buffer[b]=tmp[b];\n"); } else { // nothing } writer.print(" int crc = MAVLinkCRC.crc_calculate_encode(buffer, " + fieldLen + ");\n"); writer.print( " crc = MAVLinkCRC.crc_accumulate((byte) IMAVLinkCRC.MAVLINK_MESSAGE_CRCS[messageType], crc);\n"); writer.print(" byte crcl = (byte) (crc & 0x00FF);\n"); writer.print(" byte crch = (byte) ((crc >> 8) & 0x00FF);\n"); writer.print(" buffer[" + (fieldLen + 6) + "] = crcl;\n"); writer.print(" buffer[" + (fieldLen + 7) + "] = crch;\n"); writer.print(" return buffer;\n}\n"); if (debug) { writer.print("public String toString() {\n"); writer.print("return \"" + id + " : \" + " + forToString + ";"); writer.print("}\n"); } writer.print("}\n"); forToString = ""; } catch (Exception e) { System.err.println("ERROR : " + e); e.printStackTrace(); } finally { try { writer.close(); output.close(); } catch (Exception ex) { System.err.println("ERROR : " + ex); ex.printStackTrace(); } } } }