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 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 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 _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 _decode_port_data(self, msg):
val = ushort(msg.payload, 0)
milliampers = 2444 * val / 4095.0
return (milliampers, )
def _decode_port_data(self, msg):
data = msg.payload
val = ushort(data, 0)
volts = 9600.0 * val / 3893.0 / 1000.0
return (volts, )
def handle_port_data(self, data):
val = ushort(data, 4)
self.last_value = val / 4096.0
self._notify_subscribers(self.last_value)
def handle_port_data(self, data):
val = ushort(data, 4)
self.last_value = val / 4096.0
if self.last_value < 0.2:
log.warning("Battery low! %s%%", int(100 * self.last_value))
self._notify_subscribers(self.last_value)
def _decode_port_data(self, msg):
data = msg.payload
x = ushort(msg.payload, 0)
y = ushort(msg.payload, 2)
z = ushort(msg.payload, 4)
return (x, y, z)
