Firmware progress 50%

firmware/src/air.c

@@ -35,9 +35,9 @@ void air_init()
     for (int i = 0; i < sizeof(TOF_LIST); i++) {
         sleep_us(10);
         i2c_select(TOF_I2C, 1 << TOF_LIST[i]);
-        gp2y0e_write(TOF_I2C, 0xa8, 0x00); // Accumulation 0:1, 1:5, 2:30, 3:10
+        gp2y0e_write(TOF_I2C, 0xa8, 0); // Accumulation 0:1, 1:5, 2:30, 3:10
         gp2y0e_write(TOF_I2C, 0x3f, 0x30); // Filter 0x00:7, 0x10:5, 0x20:9, 0x30:1
-        gp2y0e_write(TOF_I2C, 0x13, 0x03); // Pulse [3..7]:[40, 80, 160, 240, 320] us
+        gp2y0e_write(TOF_I2C, 0x13, 5); // Pulse [3..7]:[40, 80, 160, 240, 320] us
     }
 }
 
@@ -51,7 +51,9 @@ uint16_t air_value(uint8_t index)
     if (index >= sizeof(TOF_LIST)) {
         return 0;
     }
-    return distances[index];
+    uint16_t dist = distances[index] >> 6;
+
+    return dist < 63 ? dist : 0;
 }
 
 void air_update()

firmware/src/main.c

@@ -9,8 +9,14 @@
 
 #include "bsp/board.h"
 #include "pico/multicore.h"
+#include "pico/bootrom.h"
 #include "pico/stdio.h"
 
+#include "hardware/gpio.h"
+#include "hardware/sync.h"
+#include "hardware/structs/ioqspi.h"
+#include "hardware/structs/sio.h"
+
 #include "tusb.h"
 #include "usb_descriptors.h"
 
@@ -49,6 +55,22 @@ static void pause_core1(bool pause)
     }
 }
 
+static void fps_count()
+{
+    static uint64_t last = 0;
+    static int fps_counter = 0;
+
+    fps_counter++;
+
+    uint64_t now = time_us_64();
+    if (now - last < 1000000) {
+        return;
+    }
+    last = now;
+    printf("FPS: %d\n", fps_counter);
+    fps_counter = 0;
+}
+
 static void core1_loop()
 {
     while (1) {
@@ -59,31 +81,45 @@ static void core1_loop()
 
 static void core0_loop()
 {
+    for (int i = 0; i < 15; i++) {
+        int x = 15 - i;
+        uint8_t r = (x & 0x01) ? 10 : 0;
+        uint8_t g = (x & 0x02) ? 10 : 0;
+        uint8_t b = (x & 0x04) ? 10 : 0;
+        rgb_gap_color(i, rgb32(r, g, b, false));
+    }
+
     while(1) {
         tud_task();
         hid_report.buttons = 0xcccc;
         report_usb_hid();
-
         slider_update();
         air_update();
 
-        for (int i = 0; i < 32; i++) {
-            bool v = slider_touched(i);
-            printf("%d", v);
+        static uint16_t old_touch = 0;
+        uint16_t touch = slider_hw_touch(0);
+        if (touch != old_touch) {
+            printf("Touch: %04x\n", touch);
+            old_touch = touch;
+        }
+        for (int i = 0; i < 16; i++) {
+            bool k1 = slider_touched(i * 2);
+            bool k2 = slider_touched(i * 2 + 1);
+            uint8_t r = k1 ? 255 : 0;
+            uint8_t g = k2 ? 255 : 0;
+            if (k1 || k2) {
+                printf("U:%3d D:%3d\n", slider_delta(i * 2), slider_delta(i * 2 + 1));
+            }
+            rgb_key_color(i, rgb32(r, g, g, false));
         }
 
         for (int i = 0; i < air_num(); i++) {
-            uint8_t v = air_value(i) >> 4;
-            printf(" %3d", air_value(i));
-            if (v == 255) {
-                rgb_set_color(31 + i, 0);
-            } else {
-                rgb_set_color(31 + i, rgb32(v, v, v, true));
-            }
+            uint8_t v = air_value(i) << 2;
+            rgb_set_color(31 + i, rgb32(v, v, v, false));
         }
-        printf("\n");
 
-        sleep_ms(1);
+        slider_update_baseline();
+        fps_count();
     }
 }
 

firmware/src/rgb.c

@@ -85,6 +85,23 @@ void rgb_set_color(unsigned index, uint32_t color)
     rgb_buf[index] = color;
 }
 
+void rgb_key_color(unsigned index, uint32_t color)
+{
+    if (index > 16) {
+        return;
+    }
+    rgb_buf[index * 2] = color;
+}
+
+void rgb_gap_color(unsigned index, uint32_t color)
+{
+    if (index > 15) {
+        return;
+    }
+    rgb_buf[index * 2 + 1] = color;
+}
+
+
 void rgb_init()
 {
     uint pio0_offset = pio_add_program(pio0, &ws2812_program);

firmware/src/rgb.h

@@ -19,5 +19,7 @@ uint32_t rgb32(uint32_t r, uint32_t g, uint32_t b, bool gamma_fix);
 
 void rgb_set_colors(const uint32_t *colors, unsigned index, size_t num);
 void rgb_set_color(unsigned index, uint32_t color);
+void rgb_key_color(unsigned index, uint32_t color);
+void rgb_gap_color(unsigned index, uint32_t color);
 
 #endif

firmware/src/slider.c

@@ -18,17 +18,49 @@
 
 #include "mpr121.h"
 
-#define TOUCH_THRESHOLD 12
-#define RELEASE_THRESHOLD 10
+#define TOUCH_THRESHOLD 16
+#define RELEASE_THRESHOLD 8
 
-#define MPR121_ADDR 0x5C
+#define MPR121_ADDR 0x5A
 
-static uint16_t baseline[32];
-static uint16_t readout[32];
-static uint16_t touched[3];
+static uint16_t baseline[36];
+static int16_t error[36];
+static uint16_t readout[36];
+static bool touched[36];
+static uint16_t debounce[36];
+static uint16_t touch[3];
 
 static struct mpr121_sensor mpr121[3];
 
+#define ABS(x) ((x) < 0 ? -(x) : (x))
+
+static void mpr121_read_many(uint8_t addr, uint8_t reg, uint8_t *buf, size_t n)
+{
+    i2c_write_blocking_until(MPR121_I2C, addr, &reg, 1, true, time_us_64() + 2000);
+    i2c_read_blocking_until(MPR121_I2C, addr, buf, n, false, time_us_64() + 2000);
+}
+
+static void mpr121_read_many16(uint8_t addr, uint8_t reg, uint16_t *buf, size_t n)
+{
+    uint8_t vals[n * 2];
+    mpr121_read_many(addr, reg, vals, n * 2);
+    for (int i = 0; i < n; i++) {
+        buf[i] = (vals[i * 2 + 1] << 8) | vals[i * 2];
+    }
+}
+
+static void init_baseline()
+{
+    sleep_ms(100);
+    for (int m = 0; m < 3; m++) {
+        uint8_t vals[12];
+        mpr121_read_many(MPR121_ADDR + m, MPR121_BASELINE_VALUE_REG, vals, 12);
+        for (int i = 0; i < 12; i++) {
+            baseline[m * 12 + i] = vals[i] * 4;
+        }
+    }
+}
+
 void slider_init()
 {
     i2c_init(MPR121_I2C, 400 * 1000);
@@ -39,22 +71,52 @@ void slider_init()
 
     for (int m = 0; m < 3; m++) {
         mpr121_init(MPR121_I2C, MPR121_ADDR + m, mpr121 + m);
-        mpr121_set_thresholds(TOUCH_THRESHOLD, RELEASE_THRESHOLD, mpr121 + m);
-        mpr121_enable_electrodes(12, mpr121 + m);
     }
+
+    init_baseline();
 }
 
 void slider_update()
 {
-    for (int m = 0; m < 3; m++) {
-        for (int i = 0; i < 12; i++) {
-            int id = m * 12 + i;
-            if (id >= 32) {
-                break;
-            }
-            mpr121_filtered_data(i, readout + id, mpr121 + m);
+    uint8_t reg = MPR121_ELECTRODE_FILTERED_DATA_REG;
+    mpr121_read_many16(MPR121_ADDR, reg, readout, 12);
+    mpr121_read_many16(MPR121_ADDR + 1, reg, readout + 12, 12);
+    mpr121_read_many16(MPR121_ADDR + 2, reg, readout + 24, 12);
+    mpr121_touched(touch, mpr121);
+    mpr121_touched(touch + 1, mpr121 + 1);
+    mpr121_touched(touch + 2, mpr121 + 2);
+}
+
+void slider_update_baseline()
+{
+    static int iteration = 0;
+    iteration++;
+
+    for (int i = 0; i < 32; i++) {
+        int16_t delta = readout[i] - baseline[i];
+        if (ABS(delta) > RELEASE_THRESHOLD) {
+            continue;
         }
-        mpr121_touched(touched + m, mpr121 + m);
+        error[i] += delta;
+    }
+
+    if (iteration > 100) {
+        iteration = 0;
+        printf("B: ");
+        for (int i = 0; i < 32; i++) {
+            if (error[i] > 100) {
+                baseline[i] ++;
+                printf("+");
+
+            } else if (error[i] < -100) {
+                baseline[i] --;
+                printf("-");
+            } else {
+                printf(" ");
+            }
+            error[i] = 0;
+        }
+        printf("\n");
     }
 }
 
@@ -66,6 +128,14 @@ int slider_value(unsigned key)
     return readout[key];
 }
 
+int slider_baseline(unsigned key)
+{
+    if (key >= 32) {
+        return 0;
+    }
+    return baseline[key];
+}
+
 int slider_delta(unsigned key)
 {
     if (key >= 32) {
@@ -76,5 +146,32 @@ int slider_delta(unsigned key)
 
 bool slider_touched(unsigned key)
 {
-    return touched[key / 12] & (1 << (key % 12));
+    if (key >= 32) {
+        return 0;
+    }
+    int delta =  baseline[key] - readout[key];
+
+    if (touched[key]) {
+        if (delta > TOUCH_THRESHOLD) {
+            debounce[key] = 0;
+        }
+        if (debounce[key] > 4) {
+            if (delta < RELEASE_THRESHOLD) {
+                touched[key] = false;
+            }
+        } else {
+            debounce[key]++;
+        }
+    } else if (!touched[key]) {
+        if (delta > TOUCH_THRESHOLD) {
+            touched[key] = true;
+        }
+    }
+
+    return touched[key];
+}
+
+uint16_t slider_hw_touch(unsigned m)
+{
+    return touch[m];
 }

firmware/src/slider.h

@@ -11,8 +11,11 @@
 
 void slider_init();
 int slider_value(unsigned key);
+int slider_baseline(unsigned key);
 int slider_delta(unsigned key);
 bool slider_touched(unsigned key);
+uint16_t slider_hw_touch(unsigned m);
 void slider_update();
+void slider_update_baseline();
 
 #endif