00001 /* AUTO-GENERATED FILE. DO NOT MODIFY. 00002 * 00003 * This class was automatically generated by the 00004 * java mavlink generator tool. It should not be modified by hand. 00005 */ 00006 00007 package com.MAVLink.Messages; 00008 00009 import java.nio.ByteBuffer; 00010 00011 public class MAVLinkPayload { 00012 00013 private static final byte UNSIGNED_BYTE_MIN_VALUE = 0; 00014 private static final short UNSIGNED_BYTE_MAX_VALUE = Byte.MAX_VALUE - Byte.MIN_VALUE; 00015 00016 private static final short UNSIGNED_SHORT_MIN_VALUE = 0; 00017 private static final int UNSIGNED_SHORT_MAX_VALUE = Short.MAX_VALUE - Short.MIN_VALUE; 00018 00019 private static final int UNSIGNED_INT_MIN_VALUE = 0; 00020 private static final long UNSIGNED_INT_MAX_VALUE = (long) Integer.MAX_VALUE - Integer.MIN_VALUE; 00021 00022 private static final long UNSIGNED_LONG_MIN_VALUE = 0; 00023 00024 public static final int MAX_PAYLOAD_SIZE = 255; 00025 00026 public final ByteBuffer payload; 00027 public int index; 00028 00029 public MAVLinkPayload(int payloadSize) { 00030 if(payloadSize > MAX_PAYLOAD_SIZE) { 00031 payload = ByteBuffer.allocate(MAX_PAYLOAD_SIZE); 00032 } else { 00033 payload = ByteBuffer.allocate(payloadSize); 00034 } 00035 } 00036 00037 public ByteBuffer getData() { 00038 return payload; 00039 } 00040 00041 public int size() { 00042 return payload.position(); 00043 } 00044 00045 public void add(byte c) { 00046 payload.put(c); 00047 } 00048 00049 public void resetIndex() { 00050 index = 0; 00051 } 00052 00053 public byte getByte() { 00054 byte result = 0; 00055 result |= (payload.get(index + 0) & 0xFF); 00056 index += 1; 00057 return result; 00058 } 00059 00060 public short getUnsignedByte(){ 00061 short result = 0; 00062 result |= payload.get(index + 0) & 0xFF; 00063 index+= 1; 00064 return result; 00065 } 00066 00067 public short getShort() { 00068 short result = 0; 00069 result |= (payload.get(index + 1) & 0xFF) << 8; 00070 result |= (payload.get(index + 0) & 0xFF); 00071 index += 2; 00072 return result; 00073 } 00074 00075 public int getUnsignedShort(){ 00076 int result = 0; 00077 result |= (payload.get(index + 1) & 0xFF) << 8; 00078 result |= (payload.get(index + 0) & 0xFF); 00079 index += 2; 00080 return result; 00081 } 00082 00083 public int getInt() { 00084 int result = 0; 00085 result |= (payload.get(index + 3) & 0xFF) << 24; 00086 result |= (payload.get(index + 2) & 0xFF) << 16; 00087 result |= (payload.get(index + 1) & 0xFF) << 8; 00088 result |= (payload.get(index + 0) & 0xFF); 00089 index += 4; 00090 return result; 00091 } 00092 00093 public long getUnsignedInt(){ 00094 long result = 0; 00095 result |= (payload.get(index + 3) & 0xFFL) << 24; 00096 result |= (payload.get(index + 2) & 0xFFL) << 16; 00097 result |= (payload.get(index + 1) & 0xFFL) << 8; 00098 result |= (payload.get(index + 0) & 0xFFL); 00099 index += 4; 00100 return result; 00101 } 00102 00103 public long getLong() { 00104 long result = 0; 00105 result |= (payload.get(index + 7) & 0xFFL) << 56; 00106 result |= (payload.get(index + 6) & 0xFFL) << 48; 00107 result |= (payload.get(index + 5) & 0xFFL) << 40; 00108 result |= (payload.get(index + 4) & 0xFFL) << 32; 00109 result |= (payload.get(index + 3) & 0xFFL) << 24; 00110 result |= (payload.get(index + 2) & 0xFFL) << 16; 00111 result |= (payload.get(index + 1) & 0xFFL) << 8; 00112 result |= (payload.get(index + 0) & 0xFFL); 00113 index += 8; 00114 return result; 00115 } 00116 00117 public long getUnsignedLong(){ 00118 return getLong(); 00119 } 00120 00121 public long getLongReverse() { 00122 long result = 0; 00123 result |= (payload.get(index + 0) & 0xFFL) << 56; 00124 result |= (payload.get(index + 1) & 0xFFL) << 48; 00125 result |= (payload.get(index + 2) & 0xFFL) << 40; 00126 result |= (payload.get(index + 3) & 0xFFL) << 32; 00127 result |= (payload.get(index + 4) & 0xFFL) << 24; 00128 result |= (payload.get(index + 5) & 0xFFL) << 16; 00129 result |= (payload.get(index + 6) & 0xFFL) << 8; 00130 result |= (payload.get(index + 7) & 0xFFL); 00131 index += 8; 00132 return result; 00133 } 00134 00135 public float getFloat() { 00136 return Float.intBitsToFloat(getInt()); 00137 } 00138 00139 public void putByte(byte data) { 00140 add(data); 00141 } 00142 00143 public void putUnsignedByte(short data){ 00144 if(data < UNSIGNED_BYTE_MIN_VALUE || data > UNSIGNED_BYTE_MAX_VALUE){ 00145 throw new IllegalArgumentException("Value is outside of the range of an unsigned byte: " + data); 00146 } 00147 00148 putByte((byte) data); 00149 } 00150 00151 public void putShort(short data) { 00152 add((byte) (data >> 0)); 00153 add((byte) (data >> 8)); 00154 } 00155 00156 public void putUnsignedShort(int data){ 00157 if(data < UNSIGNED_SHORT_MIN_VALUE || data > UNSIGNED_SHORT_MAX_VALUE){ 00158 throw new IllegalArgumentException("Value is outside of the range of an unsigned short: " + data); 00159 } 00160 00161 putShort((short) data); 00162 } 00163 00164 public void putInt(int data) { 00165 add((byte) (data >> 0)); 00166 add((byte) (data >> 8)); 00167 add((byte) (data >> 16)); 00168 add((byte) (data >> 24)); 00169 } 00170 00171 public void putUnsignedInt(long data){ 00172 if(data < UNSIGNED_INT_MIN_VALUE || data > UNSIGNED_INT_MAX_VALUE){ 00173 throw new IllegalArgumentException("Value is outside of the range of an unsigned int: " + data); 00174 } 00175 00176 putInt((int) data); 00177 } 00178 00179 public void putLong(long data) { 00180 add((byte) (data >> 0)); 00181 add((byte) (data >> 8)); 00182 add((byte) (data >> 16)); 00183 add((byte) (data >> 24)); 00184 add((byte) (data >> 32)); 00185 add((byte) (data >> 40)); 00186 add((byte) (data >> 48)); 00187 add((byte) (data >> 56)); 00188 } 00189 00190 public void putUnsignedLong(long data){ 00191 if(data < UNSIGNED_LONG_MIN_VALUE){ 00192 throw new IllegalArgumentException("Value is outside of the range of an unsigned long: " + data); 00193 } 00194 00195 putLong(data); 00196 } 00197 00198 public void putFloat(float data) { 00199 putInt(Float.floatToIntBits(data)); 00200 } 00201 00202 }