/** * 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; }
/** * Send an object through the telemetry link. * @param[in] obj Object to send * @param[in] type Transaction type * @param[in] allInstances True is all instances of the object are to be sent * @return Success (true), Failure (false) * @throws IOException */ private bool transmitObject(UAVObject obj, int type, bool allInstances) { // If all instances are requested on a single instance object it is an // error if (allInstances && obj.isSingleInstance()) { allInstances = false; } // Process message type if (type == uavConsts.TYPE_OBJ || type == uavConsts.TYPE_OBJ_ACK) { if (allInstances) { // Get number of instances int numInst = objMgr.getNumInstances(obj.getObjID()); // Send all instances for (int instId = 0; instId < numInst; ++instId) { // TODO: This code is buggy probably. We should send each request // and wait for an ack in the case of an TYPE_OBJ_ACK Debug.Assert(type != uavConsts.TYPE_OBJ_ACK); // catch any buggy calls UAVObject inst = objMgr.getObject(obj.getObjID(), instId); transmitSingleObject(inst, type, false); } return true; } else { return transmitSingleObject(obj, type, false); } } else if (type == uavConsts.TYPE_OBJ_REQ) { return transmitSingleObject(obj, uavConsts.TYPE_OBJ_REQ, allInstances); } else if (type == uavConsts.TYPE_ACK) { if (!allInstances) { return transmitSingleObject(obj, uavConsts.TYPE_ACK, false); } else { return false; } } else { return false; } }