+static volatile uint8_t lcob_seen = I2C_COB_NONE;
+static volatile uint8_t rcob_seen = I2C_COB_NONE;
+
+uint8_t cob_detect_left(void)
+{
+ uint8_t flags;
+ uint8_t ret;
+ IRQ_LOCK(flags);
+ ret = lcob_seen;
+ lcob_seen = I2C_COB_NONE;
+ IRQ_UNLOCK(flags);
+ return ret;
+}
+
+uint8_t cob_detect_right(void)
+{
+ uint8_t flags;
+ uint8_t ret;
+ IRQ_LOCK(flags);
+ ret = rcob_seen;
+ rcob_seen = I2C_COB_NONE;
+ IRQ_UNLOCK(flags);
+ return ret;
+}
+
+#define COB_MIN_DETECT 4
+#define COB_MAX_DETECT 50
+static void do_cob_detection(void)
+{
+ uint8_t flags;
+ uint16_t tmp = sensor_get_all();
+ uint8_t l_us = !!(tmp & _BV(S_L_US));
+ uint8_t r_us = !!(tmp & _BV(S_R_US));
+ uint8_t l_ir = !!(tmp & _BV(S_L_IR));
+ uint8_t r_ir = !!(tmp & _BV(S_R_IR));
+ static uint8_t l_us_prev, r_us_prev, l_ir_prev, r_ir_prev;
+ static uint8_t l_cpt_on, l_cpt_off;
+ static uint8_t r_cpt_on, r_cpt_off;
+
+ /* rising edge on US */
+ if (l_us_prev == 0 && l_us == 1) {
+ l_cpt_off = 0;
+ l_cpt_on = 0;
+ }
+
+ /* us is on */
+ if (l_us) {
+ if (l_ir && l_cpt_on < COB_MAX_DETECT)
+ l_cpt_on ++;
+ else if (l_cpt_off < COB_MAX_DETECT)
+ l_cpt_off ++;
+ }
+
+ /* falling edge on US */
+ if (l_us_prev == 1 && l_us == 0) {
+ /* detection should not be too short or too long */
+ if ((l_cpt_off + l_cpt_on) < COB_MAX_DETECT &&
+ (l_cpt_off + l_cpt_on) > COB_MIN_DETECT) {
+ IRQ_LOCK(flags);
+ if (l_cpt_on > l_cpt_off)
+ lcob_seen = I2C_COB_WHITE;
+ else
+ lcob_seen = I2C_COB_BLACK;
+ IRQ_UNLOCK(flags);
+ if (l_cpt_on > l_cpt_off)
+ DEBUG(E_USER_SENSOR, "left white %d %d",
+ l_cpt_on, l_cpt_off);
+ else
+ DEBUG(E_USER_SENSOR, "left black %d %d",
+ l_cpt_on, l_cpt_off);
+ }
+ }
+