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 event_packet(self, packet): ignore = packet.read(4) event_id = packet.read(1)[0] tmp = packet.read(8) tmp_buf = ByteBuffer(tmp, 0, 8) owner_length = tmp_buf.get_SLInt32() event_length = tmp_buf.get_SLInt32() owner = str(packet.read(owner_length)) event_json = str(packet.read(event_length)) print "Got event #" + str( event_id) + " from " + owner + ". Data: " + event_json event_data = json2obj(event_json) if event_id in self.event_callbacks: for callback in self.event_callbacks[event_id]: callback(event_data, owner)
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
def json_sensor_information_packet(self, packet): tmp = packet.read(4) tmp_buf = ByteBuffer(tmp, 0, 4) robot_length = tmp_buf.get_SLInt32() robot = str(packet.read(robot_length)) tmp = packet.read(4) tmp_buf = ByteBuffer(tmp, 0, 4) json_length = tmp_buf.get_SLInt32() json = str(packet.read(json_length)) info = json2obj(json) if robot in self.sensor_callbacks: for callback in self.sensor_callbacks[robot]: callback(robot, info) print packet
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 body(dev, *args): global alpha global previousAcc global velocity global was_moving acc = freenect.get_accel(dev) accX = int(acc[0]) accY = int(acc[1]) accZ = int(acc[2]) accX = accX - previousAcc[0] accY = accY - previousAcc[1] accZ = accZ - previousAcc[2] velocity[0] += accX velocity[1] += accY velocity[2] += accZ previousAcc[0] = int(acc[0]) previousAcc[1] = int(acc[1]) previousAcc[2] = int(acc[2]) if abs(velocity[0]) > 0 or abs(velocity[2]) > 0: print("Moving!!\tX: " + str(velocity[0]) + "m/s\tZ: " + str(velocity[2]) + "m/s") was_moving = True elif was_moving: print("Stopped moving") was_moving = False buf = ByteBuffer(bytearray([0] * 24), 0, 24) buf.put_SLInt32(accX) buf.put_SLInt32(accY) buf.put_SLInt32(accZ) buf.put_SLInt32(velocity[0]) buf.put_SLInt32(velocity[1]) buf.put_SLInt32(velocity[2]) arr = bytearray([0] * 25) arr[0] = 0x03 buf.set_position(0) buf.get(arr, 1, 24) sock.send(arr) if not keep_running: raise freenect.Kill
def send_robot_command(self, motor1, motor2, motor3, motor4, robot_name): total_size = 24 + len(robot_name) array = bytearray([0] * total_size) buf = ByteBuffer(array, 0, total_size) buf.put_SLInt32(total_size + 1) buf.put_SLInt32(motor1) buf.put_SLInt32(motor2) buf.put_SLInt32(motor3) buf.put_SLInt32(motor4) buf.put_SLInt32(len(robot_name)) temp = bytearray() temp.extend(robot_name) buf.put_bytes(temp, 0, len(temp)) to_send = bytearray([0] * (total_size + 1)) to_send[0] = 0x08 buf.set_position(0) buf.get(to_send, 1, total_size) self.socket.sendto(to_send, self.server_address)
def trigger_event(self, event_id, event_data, filter): event = json.dumps(event_data) should_filter = filter != "" filter_size = 0 if should_filter: filter_size = 4 + len(filter) total_size = 10 + len(event) + filter_size array = bytearray([0] * total_size) buf = ByteBuffer(array, 0, total_size) # 4 bytes buf.put_SLInt32(total_size + 1) # 4 + 2 = 6 temp = bytearray([0] * 2) temp[0] = event_id if should_filter: temp[1] = 1 else: temp[1] = 0 buf.put_bytes(temp, 0, 2) # 4 + 2 + 4 = 10 buf.put_SLInt32(len(event)) temp2 = bytearray() temp2.extend(event) buf.put_bytes(temp2, 0, len(temp2)) if should_filter: buf.put_SLInt32(len(filter)) temp3 = bytearray() temp3.extend(filter) buf.put_bytes(temp3, 0, len(temp3)) to_send = bytearray([0] * (total_size + 1)) to_send[0] = 0x06 buf.set_position(0) buf.get(to_send, 1, total_size) self.socket.sendto(to_send, self.server_address)
def request_sensor_information(self, robot, filter, callback): if not callable(callback): print "Invalid callback!" return should_filter = filter != "" filter_size = 0 if should_filter: filter_size = 4 + len(filter) total_size = 9 + len(robot) + filter_size array = bytearray([0] * total_size) buf = ByteBuffer(array, 0, total_size) # 4 bytes buf.put_SLInt32(total_size + 1) # 4 + 4 = 8 buf.put_SLInt32(len(robot)) # 4 + 4 + 1 = 9 tmp = bytearray([0]) if should_filter: tmp[0] = 1 buf.put_bytes(tmp, 0, 1) temp2 = bytearray() temp2.extend(robot) buf.put_bytes(temp2, 0, len(temp2)) if should_filter: buf.put_SLInt32(len(filter)) temp3 = bytearray() temp3.extend(filter) buf.put_bytes(temp3, 0, len(temp3)) to_send = bytearray([0] * (total_size + 1)) to_send[0] = 0x07 buf.set_position(0) buf.get(to_send, 1, total_size) self.socket.sendto(to_send, self.server_address) if robot not in self.sensor_callbacks: self.sensor_callbacks[robot] = list() self.sensor_callbacks[robot].append(callback)