def _handle_device_change(self, msg):
if msg.event == MsgHubAttachedIO.EVENT_DETACHED:
log.debug("Detaching peripheral: %s", self.peripherals[msg.port])
self.peripherals.pop(msg.port)
return
assert msg.event in (msg.EVENT_ATTACHED, msg.EVENT_ATTACHED_VIRTUAL)
port = msg.port
dev_type = ushort(msg.payload, 0)
if dev_type in PERIPHERAL_TYPES:
self.peripherals[port] = PERIPHERAL_TYPES[dev_type](self, port)
else:
#log.warning("Have not dedicated class for peripheral type %x on port %x", dev_type, port)
self.peripherals[port] = Peripheral(self, port)
log.info("Attached peripheral: %s", self.peripherals[msg.port])
if msg.event == msg.EVENT_ATTACHED:
hw_revision = reversed([usbyte(msg.payload, x) for x in range(2, 6)])
sw_revision = reversed([usbyte(msg.payload, x) for x in range(6, 10)])
# what to do with this info? it's useless, I guess
del hw_revision, sw_revision
elif msg.event == msg.EVENT_ATTACHED_VIRTUAL:
self.peripherals[port].virtual_ports = (usbyte(msg.payload, 2), usbyte(msg.payload, 3))
def _notify(self, handle, data):
orig = data
if handle != MOVE_HUB_HARDWARE_HANDLE:
log.warning("Unsupported notification handle: 0x%s", handle)
return
log.debug("Notification on %s: %s", handle, str2hex(orig))
msg_type = usbyte(data, 2)
if msg_type == MSG_PORT_INFO:
self._handle_port_info(data)
elif msg_type == MSG_PORT_STATUS:
self._handle_port_status(data)
elif msg_type == MSG_SENSOR_DATA:
self._handle_sensor_data(data)
elif msg_type == MSG_SENSOR_SUBSCRIBE_ACK:
port = usbyte(data, 3)
log.debug("Sensor subscribe ack on port %s", PORTS[port])
self.devices[port].finished()
elif msg_type == MSG_PORT_CMD_ERROR:
log.warning("Command error: %s", str2hex(data[3:]))
port = usbyte(data, 3)
self.devices[port].finished()
elif msg_type == MSG_DEVICE_SHUTDOWN:
log.warning("Device reported shutdown: %s", str2hex(data))
raise KeyboardInterrupt("Device shutdown")
elif msg_type == MSG_DEVICE_INFO:
self._handle_device_info(data)
else:
log.warning("Unhandled msg type 0x%x: %s", msg_type, str2hex(orig))
def _handle_device_info(self, data):
kind = usbyte(data, 3)
if kind == 2:
self.button.handle_port_data(data)
if usbyte(data, 4) == 0x06:
self.info[kind] = data[5:]
else:
log.warning("Unhandled device info: %s", str2hex(data))
def _handle_port_info(self, data):
port = usbyte(data, 3)
status = usbyte(data, 4)
if status == self.DEV_STATUS_DETACHED:
log.info("Detached %s", self.devices[port])
self.devices[port] = None
elif status == self.DEV_STATUS_DEVICE or status == self.DEV_STATUS_GROUP:
dev_type = usbyte(data, 5)
self._attach_device(dev_type, port)
else:
raise ValueError("Unhandled device status: %s", status)
self._update_field(port)
if self.devices[port] is None:
del self.devices[port]
def _handle_sensor_data(self, data):
port = usbyte(data, 3)
if port not in self.devices:
log.warning("Notification on port with no device: %s", PORTS[port])
return
device = self.devices[port]
device.queue_port_data(data)
def _decode_port_data(self, msg):
data = msg.payload
if self._port_mode.mode == self.MODE_2AXIS_ANGLE:
roll = unpack('<b', data[0:1])[0]
pitch = unpack('<b', data[1:2])[0]
return (roll, pitch)
elif self._port_mode.mode == self.MODE_3AXIS_SIMPLE:
state = usbyte(data, 0)
return (state, )
elif self._port_mode.mode == self.MODE_2AXIS_SIMPLE:
state = usbyte(data, 0)
return (state, )
elif self._port_mode.mode == self.MODE_IMPACT_COUNT:
bump_count = usint(data, 0)
return (bump_count, )
elif self._port_mode.mode == self.MODE_3AXIS_ACCEL:
roll = unpack('<b', data[0:1])[0]
pitch = unpack('<b', data[1:2])[0]
yaw = unpack('<b', data[2:3])[0] # did I get the order right?
return (roll, pitch, yaw)
elif self._port_mode.mode == self.MODE_ORIENT_CF:
state = usbyte(data, 0)
return (state, )
elif self._port_mode.mode == self.MODE_IMPACT_CF:
state = usbyte(data, 0)
return (state, )
elif self._port_mode.mode == self.MODE_CALIBRATION:
return (usbyte(data, 0), usbyte(data, 1), usbyte(data, 2))
else:
log.debug("Got tilt sensor data while in unexpected mode: %r",
self._port_mode)
return ()
def _get_upstream_msg(self, data):
msg_type = usbyte(data, 2)
msg = None
for msg_kind in UPSTREAM_MSGS:
if msg_type == msg_kind.TYPE:
msg = msg_kind.decode(data)
log.debug("Decoded message: %r", msg)
break
assert msg
return msg
def _handle_port_status(self, data):
port = usbyte(data, 3)
status = usbyte(data, 4)
if status == STATUS_STARTED:
self.devices[port].started()
elif status == STATUS_FINISHED:
self.devices[port].finished()
elif status == STATUS_CONFLICT:
log.warning("Command conflict on port %s", PORTS[port])
self.devices[port].finished()
elif status == STATUS_INPROGRESS:
log.warning("Another command is in progress on port %s",
PORTS[port])
self.devices[port].finished()
elif status == STATUS_INTERRUPTED:
log.warning("Command interrupted on port %s", PORTS[port])
self.devices[port].finished()
else:
log.warning("Unhandled status value: 0x%x on port %s", status,
PORTS[port])
def handle_port_data(self, data):
if self._port_subscription_mode == self.SENSOR_ANGLE:
rotation = unpack("<l", data[4:8])[0]
self._notify_subscribers(rotation)
elif self._port_subscription_mode == self.SENSOR_SOMETHING1:
# TODO: understand what it means
rotation = usbyte(data, 4)
self._notify_subscribers(rotation)
elif self._port_subscription_mode == self.SENSOR_SPEED:
rotation = unpack("<b", data[4])[0]
self._notify_subscribers(rotation)
else:
log.debug("Got motor sensor data while in unexpected mode: %s",
self._port_subscription_mode)
def _report_status(self):
# maybe add firmware version
name = self.send(MsgHubProperties(MsgHubProperties.ADVERTISE_NAME, MsgHubProperties.UPD_REQUEST))
mac = self.send(MsgHubProperties(MsgHubProperties.PRIMARY_MAC, MsgHubProperties.UPD_REQUEST))
log.info("%s on %s", name.payload, str2hex(mac.payload))
voltage = self.send(MsgHubProperties(MsgHubProperties.VOLTAGE_PERC, MsgHubProperties.UPD_REQUEST))
assert isinstance(voltage, MsgHubProperties)
log.info("Voltage: %s%%", usbyte(voltage.parameters, 0))
voltage = self.send(MsgHubAlert(MsgHubAlert.LOW_VOLTAGE, MsgHubAlert.UPD_REQUEST))
assert isinstance(voltage, MsgHubAlert)
if not voltage.is_ok():
log.warning("Low voltage, check power source (maybe replace battery)")
def _decode_port_data(self, msg):
data = msg.payload
if self._port_mode.mode == self.SENSOR_ANGLE:
angle = unpack("<l", data[0:4])[0]
return (angle,)
elif self._port_mode.mode == self.SENSOR_SOMETHING1:
smth = usbyte(data, 0)
return (smth,)
elif self._port_mode.mode == self.SENSOR_SPEED:
speed = unpack("<b", data[0])[0]
return (speed,)
else:
log.debug("Got motor sensor data while in unexpected mode: %r", self._port_mode)
return ()
def test_hub_properties(self):
conn = ConnectionMock().connect()
hub = Hub(conn)
conn.notification_delayed('060001060600', 0.1)
msg = MsgHubProperties(MsgHubProperties.VOLTAGE_PERC,
MsgHubProperties.UPD_REQUEST)
resp = hub.send(msg)
assert isinstance(resp, MsgHubProperties)
self.assertEqual(1, len(resp.parameters))
self.assertEqual(0, usbyte(resp.parameters, 0))
conn.notification_delayed('12000101064c45474f204d6f766520487562', 0.1)
msg = MsgHubProperties(MsgHubProperties.ADVERTISE_NAME,
MsgHubProperties.UPD_REQUEST)
resp = hub.send(msg)
assert isinstance(resp, MsgHubProperties)
self.assertEqual(b"LEGO Move Hub", resp.parameters)
conn.wait_notifications_handled()
def _decode_port_data(self, msg):
data = msg.payload
if self._port_mode.mode == self.COLOR_INDEX:
color = usbyte(data, 0)
return (color, )
elif self._port_mode.mode == self.COLOR_DISTANCE_FLOAT:
color = usbyte(data, 0)
val = usbyte(data, 1)
partial = usbyte(data, 3)
if partial:
val += 1.0 / partial
return (color, float(val))
elif self._port_mode.mode == self.DISTANCE_INCHES:
val = usbyte(data, 0)
return (val, )
elif self._port_mode.mode == self.DISTANCE_REFLECTED:
val = usbyte(data, 0) / 100.0
return (val, )
elif self._port_mode.mode == self.AMBIENT_LIGHT:
val = usbyte(data, 0) / 100.0
return (val, )
elif self._port_mode.mode == self.COUNT_2INCH:
count = usint(data, 0)
return (count, )
elif self._port_mode.mode == self.COLOR_RGB:
val1 = int(255 * ushort(data, 0) / 1023.0)
val2 = int(255 * ushort(data, 2) / 1023.0)
val3 = int(255 * ushort(data, 4) / 1023.0)
return (val1, val2, val3)
elif self._port_mode.mode == self.DEBUG:
val1 = 10 * ushort(data, 0) / 1023.0
val2 = 10 * ushort(data, 2) / 1023.0
return (val1, val2)
elif self._port_mode.mode == self.CALIBRATE:
return [ushort(data, x * 2) for x in range(8)]
else:
log.debug("Unhandled data in mode %s: %s", self._port_mode.mode,
str2hex(data))
return ()
def handle_port_data(self, data):
if self._port_subscription_mode == self.MODE_3AXIS_SIMPLE:
state = usbyte(data, 4)
self._notify_subscribers(state)
elif self._port_subscription_mode == self.MODE_2AXIS_SIMPLE:
state = usbyte(data, 4)
self._notify_subscribers(state)
elif self._port_subscription_mode == self.MODE_BUMP_COUNT:
bump_count = ushort(data, 4)
self._notify_subscribers(bump_count)
elif self._port_subscription_mode == self.MODE_2AXIS_FULL:
roll = self._byte2deg(usbyte(data, 4))
pitch = self._byte2deg(usbyte(data, 5))
self._notify_subscribers(roll, pitch)
elif self._port_subscription_mode == self.MODE_3AXIS_FULL:
roll = self._byte2deg(usbyte(data, 4))
pitch = self._byte2deg(usbyte(data, 5))
yaw = self._byte2deg(usbyte(data, 6)) # did I get the order right?
self._notify_subscribers(roll, pitch, yaw)
else:
log.debug("Got tilt sensor data while in unexpected mode: %s",
self._port_subscription_mode)
def handle_port_data(self, data):
if self._port_subscription_mode == self.COLOR_DISTANCE_FLOAT:
color = usbyte(data, 4)
distance = usbyte(data, 5)
partial = usbyte(data, 7)
if partial:
distance += 1.0 / partial
self._notify_subscribers(color, float(distance))
elif self._port_subscription_mode == self.COLOR_ONLY:
color = usbyte(data, 4)
self._notify_subscribers(color)
elif self._port_subscription_mode == self.DISTANCE_INCHES:
distance = usbyte(data, 4)
self._notify_subscribers(distance)
elif self._port_subscription_mode == self.DISTANCE_HOW_CLOSE:
distance = usbyte(data, 4)
self._notify_subscribers(distance)
elif self._port_subscription_mode == self.DISTANCE_SUBINCH_HOW_CLOSE:
distance = usbyte(data, 4)
self._notify_subscribers(distance)
elif self._port_subscription_mode == self.OFF1 or self._port_subscription_mode == self.OFF2:
log.info("Turned off led on %s", self)
elif self._port_subscription_mode == self.COUNT_2INCH:
count = unpack("<L", data[4:8])[0] # is it all 4 bytes or just 2?
self._notify_subscribers(count)
elif self._port_subscription_mode == self.STREAM_3_VALUES:
# TODO: understand better meaning of these 3 values
val1 = ushort(data, 4)
val2 = ushort(data, 6)
val3 = ushort(data, 8)
self._notify_subscribers(val1, val2, val3)
elif self._port_subscription_mode == self.LUMINOSITY:
luminosity = ushort(data, 4) / 1023.0
self._notify_subscribers(luminosity)
else: # TODO: support whatever we forgot
log.debug("Unhandled data in mode %s: %s",
self._port_subscription_mode, str2hex(data))
def _props_msg(self, msg):
"""
:type msg: MsgHubProperties
"""
if msg.property == MsgHubProperties.BUTTON and msg.operation == MsgHubProperties.UPSTREAM_UPDATE:
self._notify_subscribers(usbyte(msg.parameters, 0))
def handle_port_data(self, data):
param = usbyte(data, 5)
if self.in_progress():
self.finished()
self._notify_subscribers(bool(param))
def _decode_port_data(self, msg):
if len(msg.payload) == 3:
return usbyte(msg.payload, 0), usbyte(msg.payload,
1), usbyte(msg.payload, 2),
else:
return usbyte(msg.payload, 0),
