def _sendCmd(self, pacType, cmdID, data):
bb = None
payload = None
out = None
seq = 0
len = 0
if cmdID == self.TELLO_CMD_CONN:
out = ByteBuffer.allocate(11)
out.clear()
out.put_bytes('conn_req:'.encode())
out.put_ULInt16(self.TELLO_PORT_VIDEO)
self.seqID = self.seqID + 1
elif cmdID == self.TELLO_CMD_STICK:
now = datetime.datetime.now()
bb = ByteBuffer.allocate(11)
bb.clear()
# put 64bit stick data
pp = ByteBuffer.allocate(8)
pp.put_ULInt64(self.stickData)
pp.flip()
# get 48bit data only
bb.put(pp.get_array(), 0, 6)
bb.put_ULInt8(now.hour)
bb.put_ULInt8(now.minute)
bb.put_ULInt8(now.second)
bb.put_ULInt16(now.microsecond & 0xffff)
seq = 0
elif cmdID == self.TELLO_CMD_DATE_TIME:
seq = self.seqID
now = datetime.datetime.now()
bb = ByteBuffer.allocate(15)
bb.clear()
bb.put_ULInt8(0x00)
bb.put_ULInt16(now.year)
bb.put_ULInt16(now.month)
bb.put_ULInt16(now.day)
bb.put_ULInt16(now.hour)
bb.put_ULInt16(now.minute)
bb.put_ULInt16(now.second)
bb.put_ULInt16(now.microsecond & 0xffff)
self.seqID = self.seqID + 1
elif cmdID == self.TELLO_CMD_REQ_VIDEO_SPS_PPS:
seq = 0
else:
seq = self.seqID
self.seqID = self.seqID + 1
if bb != None:
payload = bb.get_array()
else:
payload = data
if out == None:
out = self._buildPacket(pacType, cmdID, seq, payload)
self.sockCmd.sendto(out.get_array(), self.addrCmd)
return None
def _parsePacket(self, buf):
dataSize = 0
cmdID = 0
if len(buf) >= 11:
bb = ByteBuffer.wrap(buf)
mark = bb.get_ULInt8()
if mark == 0xCC:
size = bb.get_ULInt16() >> 3
crc8 = bb.get_ULInt8()
calcCRC8 = self._calcCRC8(buf, 3)
if crc8 != calcCRC8:
print('wrong CRC8 {0:02x / 1:02x}'.format(crc8, calcCRC8))
pacType = bb.get_ULInt8()
cmdID = bb.get_ULInt16()
seqID = bb.get_ULInt16()
dataSize = size - 11
data = None
# if dataSize > 0:
# data = bytearray(dataSize)
# bb.get(data)
bb.set_position(size - 2)
crc16 = bb.get_ULInt16()
calcCRC16 = self._calcCRC16(buf, size - 2)
if crc16 != calcCRC16:
print('wrong CRC8 {0:04x / 1:04x}'.format(
crc16, calcCRC16))
# print 'pt:{0:02x}, cmd:{1:4d}={2:04x}, seq:{3:04x}, data_sz:{4:d} - '.format(pacType, cmdID, cmdID, seqID, dataSize)
else:
if mark == 0x63:
ack = ByteBuffer.allocate(11)
ack.put_bytes('conn_ack:'.encode())
ack.put_ULInt16(self.TELLO_PORT_VIDEO)
ack.flip()
if ack.get_array() == buf:
cmdID = self.TELLO_CMD_CONN_ACK
else:
print('wrong video port !!')
else:
print('wrong mark !! {0:02x}'.format(mark))
elif buf != None:
print('wrong packet length={0:d}, 1st byte={1:02x}'.format(
len(buf), buf[0]))
return cmdID
def _buildPacket(self, pacType, cmdID, seqID, data):
size = len(data) if data != None else 0
size = 11 + size
bb = ByteBuffer.allocate(size)
bb.clear()
bb.put_ULInt8(0xCC)
bb.put_ULInt16(size << 3)
crc8 = self._calcCRC8(bb.get_array(), 3)
bb.put_ULInt8(crc8)
bb.put_ULInt8(pacType)
bb.put_ULInt16(cmdID)
bb.put_ULInt16(seqID)
if data:
bb.put(data)
crc16 = self._calcCRC16(bb.get_array(), size - 2)
bb.put_ULInt16(crc16)
bb.flip()
return bb