diff --git a/demo.py b/demo.py
index c02ecad..7a7b68c 100644
--- a/demo.py
+++ b/demo.py
@@ -111,16 +111,17 @@ def vernie_head(movehub):
 
 
 def demo_color_sensor(movehub):
-    log.info("Color sensor test: give it 3 things to detect color")
+    log.info("Color sensor test: wave your hand in front of it")
     demo_color_sensor.cnt = 0
+    limit = 2000
 
-    def callback(color, distance):
+    def callback(color, distance=None, param=None):
         demo_color_sensor.cnt += 1
-        clr = COLORS[color] if color in COLORS else None
-        log.info("#%s: Color %s, distance %s", demo_color_sensor.cnt, clr, distance)
+        clr = COLORS[color] if color in COLORS else color
+        log.info("#%s/%s: Color %s, distance %s, param %s", demo_color_sensor.cnt, limit, clr, distance, param)
 
-    movehub.color_distance_sensor.subscribe(callback)
-    while demo_color_sensor.cnt < 300:
+    movehub.color_distance_sensor.subscribe(callback, 0x08)
+    while demo_color_sensor.cnt < limit:
         time.sleep(1)
 
     movehub.color_distance_sensor.unsubscribe(callback)
diff --git a/pylgbst/constants.py b/pylgbst/constants.py
index 20a7d18..51067f8 100644
--- a/pylgbst/constants.py
+++ b/pylgbst/constants.py
@@ -120,3 +120,16 @@ TILT_STATES = {
     TILT_SOME1: "LEFT1",
     TILT_SOME2: "SOME2",
 }
+
+# COLOR & DISTANCE SENSOR
+CLR_DIST_MODE_COLOR_ONLY = 0x00
+CLR_DIST_MODE_DISTANCE_INCHES = 0x01
+CLR_DIST_MODE_COUNT_2INCH = 0x02
+CLR_DIST_MODE_DISTANCE_HOW_CLOSE = 0x03
+CLR_DIST_MODE_DISTANCE_SUBINCH_HOW_CLOSE = 0x04
+CLR_DIST_MODE_OFF1 = 0x05
+CLR_DIST_MODE_STREAM_3_VALUES = 0x06
+CLR_DIST_MODE_OFF2 = 0x07
+CLR_DIST_MODE_COLOR_DISTANCE_INCHES_SUBINCHES = 0x08
+CLR_DIST_MODE_LUMINOSITY = 0x09
+CLR_DIST_MODE_SOME_14BYTES = 0x0a
diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py
index b62ad52..ccb332b 100644
--- a/pylgbst/peripherals.py
+++ b/pylgbst/peripherals.py
@@ -147,6 +147,7 @@ class TiltSensor(Peripheral):
 
         if not self._subscribers:
             self._write_to_hub(MSG_SENSOR_SUBSCRIBE, int2byte(self.mode) + b'\x00\x00\x00' + int2byte(0))
+            self.mode = None
 
     def handle_notification(self, data):
         if self.mode == TILT_SENSOR_MODE_BASIC:
@@ -179,8 +180,15 @@ class TiltSensor(Peripheral):
 
 
 class ColorDistanceSensor(Peripheral):
-    def subscribe(self, callback):
-        params = b'\x08\x01\x00\x00\x00'
+    def __init__(self, parent, port):
+        super(ColorDistanceSensor, self).__init__(parent, port)
+        self.mode = None
+
+    def subscribe(self, callback, mode=CLR_DIST_MODE_COLOR_DISTANCE_INCHES_SUBINCHES, granularity=1):
+        self.mode = mode
+        params = int2byte(mode)
+        params += int2byte(granularity)
+        params += b'\x00\x00\x00'
         params += int2byte(1)  # enable
         self._write_to_hub(MSG_SENSOR_SUBSCRIBE, params)
         self._subscribers.add(callback)
@@ -190,15 +198,45 @@ class ColorDistanceSensor(Peripheral):
             self._subscribers.remove(callback)
 
         if not self._subscribers:
-            self._write_to_hub(MSG_SENSOR_SUBSCRIBE, b'\x08\x01\x00\x00\x00' + int2byte(0))
+            self._write_to_hub(MSG_SENSOR_SUBSCRIBE, int2byte(self.mode) + b'\x01\x00\x00\x00' + int2byte(0))
+            self.mode = None
 
     def handle_notification(self, data):
-        color = get_byte(data, 4)
-        distance = get_byte(data, 5)
-        partial = get_byte(data, 7)
-        if partial:
-            distance += 1.0 / partial
-        self._notify_subscribers(color if color != 0xFF else None, float(distance))
+        if self.mode == CLR_DIST_MODE_COLOR_DISTANCE_INCHES_SUBINCHES:
+            color = get_byte(data, 4)
+            distance = get_byte(data, 5)
+            partial = get_byte(data, 7)
+            if partial:
+                distance += 1.0 / partial
+            self._notify_subscribers(color if color != 0xFF else None, float(distance))
+        elif self.mode == CLR_DIST_MODE_COLOR_ONLY:
+            color = get_byte(data, 4)
+            self._notify_subscribers(color if color != 0xFF else None)
+        elif self.mode == CLR_DIST_MODE_DISTANCE_INCHES:
+            distance = get_byte(data, 4)
+            self._notify_subscribers(float(distance))
+        elif self.mode == CLR_DIST_MODE_DISTANCE_HOW_CLOSE:
+            distance = get_byte(data, 4)
+            self._notify_subscribers(float(distance))
+        elif self.mode == CLR_DIST_MODE_DISTANCE_SUBINCH_HOW_CLOSE:
+            distance = get_byte(data, 4)
+            self._notify_subscribers(float(distance))
+        elif self.mode == CLR_DIST_MODE_OFF1 or self.mode == CLR_DIST_MODE_OFF2:
+            log.info("Turned off led on %s", self)
+        elif self.mode == CLR_DIST_MODE_COUNT_2INCH:
+            count = struct.unpack("<L", data[4:8])[0]  # is it all 4 bytes or just 2?
+            self._notify_subscribers(count)
+        elif self.mode == CLR_DIST_MODE_STREAM_3_VALUES:
+            # TODO: understand better meaning of these 3 values
+            val1 = struct.unpack("<H", data[4:6])[0]
+            val2 = struct.unpack("<H", data[6:8])[0]
+            val3 = struct.unpack("<H", data[8:10])[0]
+            self._notify_subscribers(val1, val2, val3)
+        elif self.mode == CLR_DIST_MODE_LUMINOSITY:
+            luminosity = struct.unpack("<H", data[4:6])[0]
+            self._notify_subscribers(luminosity)
+        else:
+            log.warning("Unhandled data in mode %s: %s", self.mode, str2hex(data))
 
 
 # 0a00 41 01 01 enable
@@ -209,12 +247,6 @@ class Button(Peripheral):
         super(Button, self).__init__(parent, 0)
 
 
-LISTEN_COLOR_SENSOR_ON_C = b'   \x0a\x00 \x41\x01 \x08\x01\x00\x00\x00\x01'
-LISTEN_COLOR_SENSOR_ON_D = b'   \x0a\x00 \x41\x02 \x08\x01\x00\x00\x00\x01'
-
-LISTEN_DIST_SENSOR_ON_C = b'    \x0a\x00 \x41\x01 \x08\x01\x00\x00\x00\x01'
-LISTEN_DIST_SENSOR_ON_D = b'    \x0a\x00 \x41\x02 \x08\x01\x00\x00\x00\x01'
-
 LISTEN_ENCODER_ON_A = b'        \x0a\x00 \x41\x37 \x02\x01\x00\x00\x00\x01'
 LISTEN_ENCODER_ON_B = b'        \x0a\x00 \x41\x38 \x02\x01\x00\x00\x00\x01'
 LISTEN_ENCODER_ON_C = b'        \x0a\x00 \x41\x01 \x02\x01\x00\x00\x00\x01'