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,