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 _threadCmdRX(self, stop_event, arg):
# print '_threadCmdRX started !!!'
statusCtr = 0
data = bytearray(1024)
payload = None
while not stop_event.is_set():
try:
size, addr = self.sockCmd.recvfrom_into(data)
except socket.timeout as e:
time.sleep(.5)
continue
except socket.error as e:
print(e)
continue
else:
cmdID = self._parsePacket(data[:size])
payload = ByteBuffer.wrap(data[9:size - 1])
if cmdID == self.TELLO_CMD_CONN_ACK:
print('connection successful !')
# self._printArray(data[:size])
elif cmdID == self.TELLO_CMD_DATE_TIME:
self._sendCmd(0x50, cmdID, None)
elif cmdID == self.TELLO_CMD_STATUS:
if statusCtr == 3:
self._sendCmd(0x60, self.TELLO_CMD_REQ_VIDEO_SPS_PPS,
None)
self._sendCmd(0x48, self.TELLO_CMD_VERSION_STRING,
None)
self._sendCmd(0x48, self.TELLO_CMD_SET_VIDEO_BIT_RATE,
None)
self._sendCmd(0x48, self.TELLO_CMD_ALT_LIMIT, None)
self._sendCmd(0x48, self.TELLO_CMD_LOW_BATT_THRESHOLD,
None)
self._sendCmd(0x48, self.TELLO_CMD_ATT_ANGLE, None)
self._sendCmd(0x48, self.TELLO_CMD_REGION, None)
self._sendCmd(0x48, self.TELLO_CMD_SET_EV,
bytearray([0x00]))
statusCtr = statusCtr + 1
elif cmdID == self.TELLO_CMD_VERSION_STRING:
if size >= 42:
print('Version:' + data[10:30].decode())
elif cmdID == self.TELLO_CMD_SMART_VIDEO_START:
if payload.get_remaining() > 0:
print('smart video start')
elif cmdID == self.TELLO_CMD_ALT_LIMIT:
if payload.get_remaining() > 0:
payload.get_ULInt8() # 0x00
height = payload.get_ULInt16()
print('alt limit : {0:2d} meter'.format(height))
if height != self.NEW_ALT_LIMIT:
print('set new alt limit : {0:2d} meter'.format(
self.NEW_ALT_LIMIT))
self._sendCmd(
0x68, self.TELLO_CMD_SET_ALT_LIMIT,
bytearray([
self.NEW_ALT_LIMIT & 0xff,
(self.NEW_ALT_LIMIT >> 8) & 0xff
]))
elif cmdID == self.TELLO_CMD_SMART_VIDEO_STATUS:
if payload.get_remaining() > 0:
resp = payload.get_ULInt8()
dummy = resp & 0x07
start = (resp >> 3) & 0x03
mode = (resp >> 5) & 0x07
print('smart video status - mode:{0:d}, start:{1:d}'.
format(mode, start))
self._sendCmd(0x50, self.TELLO_CMD_SMART_VIDEO_STATUS,
bytearray([0x00]))
def covert_string_to_byte(content):
return ByteBuffer.wrap(bytes(content, encoding="utf8"))
statusCtr = 0
data = bytearray(1024)
payload = None
while not stop_event.is_set():
try:
size, addr = self.sockCmd.recvfrom_into(data)
except socket.timeout, e:
time.sleep(.5)
continue
except socket.error, e:
print e
continue
else:
cmdID = self._parsePacket(data[:size])
payload = ByteBuffer.wrap(data[9:size - 1])
if cmdID == self.TELLO_CMD_CONN_ACK:
print 'connection successful !'
# self._printArray(data[:size])
elif cmdID == self.TELLO_CMD_DATE_TIME:
self._sendCmd(0x50, cmdID, None)
elif cmdID == self.TELLO_CMD_STATUS:
if statusCtr == 3:
self._sendCmd(0x60, self.TELLO_CMD_REQ_VIDEO_SPS_PPS,
None)
self._sendCmd(0x48, self.TELLO_CMD_VERSION_STRING,
None)
self._sendCmd(0x48, self.TELLO_CMD_SET_VIDEO_BIT_RATE,
