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)