/** * Stores the provided {@code value} to the provided byte {@code buffer} at the specified {@code * index} using the provided {@code wordSize} in bytes. Note that only integer and short sizes are * supported. * * @param buffer the byte buffer to which the value is stored * @param wordSize the number of bytes used to store the provided value * @param index the index to which the value is stored * @param value the value that is stored assuming it does not require more than the specified * number of bytes. */ private static void storeWordInBuffer(ByteBuffer buffer, int wordSize, int index, int value) { index *= wordSize; if (wordSize == SHORT_NUM_BYTES) { buffer.putShort(index, (short)value); } else { buffer.putInt(index, value); } }
/** * Send an object through the telemetry link. * @throws IOException * @param[in] obj Object handle to send * @param[in] type Transaction type \return Success (true), Failure (false) */ private bool transmitSingleObject(UAVObject obj, int type, bool allInstances) { int length; int allInstId = uavConsts.ALL_INSTANCES; ByteBuffer bbuf = new ByteBuffer(uavConsts.MAX_PACKET_LENGTH); // Determine data length if (type == uavConsts.TYPE_OBJ_REQ || type == uavConsts.TYPE_ACK) { length = 0; } else { length = obj.getNumBytes(); } // Setup type and object id fields bbuf.put((byte)(uavConsts.SYNC_VAL & 0xff)); bbuf.put((byte)(type & 0xff)); bbuf.putShort((UInt16)(length + 2 /* SYNC, Type */+ 2 /* Size */+ 4 /* ObjID */+ (obj .isSingleInstance() ? 0 : 2))); bbuf.putUint32((UInt32)obj.getObjID()); // Setup instance ID if one is required if (!obj.isSingleInstance()) { // Check if all instances are requested if (allInstances) bbuf.putShort((UInt16)(allInstId & 0xffff)); else bbuf.putShort((UInt16)(obj.getInstID() & 0xffff)); } // Check length if (length >= uavConsts.MAX_PAYLOAD_LENGTH) return false; // Copy data (if any) if (length > 0) try { if (obj.pack(bbuf) == 0) return false; } catch (Exception e) { // TODO Auto-generated catch block Debug.Write(e.Message); return false; } // Calculate checksum bbuf.put((byte)(CRC.updateCRC(0, bbuf.array(), bbuf.position()) & 0xff)); int packlen = bbuf.position(); bbuf.position(0); byte[] dst = new byte[packlen]; bbuf.get(dst, 0, packlen); if (type == uavConsts.TYPE_OBJ_ACK || type == uavConsts.TYPE_OBJ_REQ) { // Once we send a UAVTalk packet that requires an ack or object let's set up // the transaction here setupTransaction(obj, allInstances, type); } ch.write(dst); // Update stats ++txStats.Objects; txStats.Bytes += bbuf.position(); txStats.ObjectBytes += length; // Done return true; }