Mix for multiple ToF

firmware/src/airkey.c

@@ -21,23 +21,34 @@
 #include "config.h"
 #include "board_defs.h"
 
-static i2c_inst_t *tof_ports[] = TOF_PORT_DEF;
-#define TOF_NUM (count_of(tof_ports))
-enum { TOF_VL53L0X = 1, TOF_VL53L1X = 2 };
-uint8_t tof_models[TOF_NUM] = { 0 };
-bool tof_init_ok[TOF_NUM] = { 0 };
+typedef enum { TOF_NONE = 0, TOF_VL53L0X = 1, TOF_VL53L1X = 2 } tof_model_t;
+typedef enum { SIDE_LEFT = 0, SIDE_RIGHT = 1 } airkey_side_t;
 
 static struct {
-    uint8_t port_id;
+    i2c_inst_t *port;
+    uint8_t gpio;
+    tof_model_t model;
+    bool init_ok;
+} tofs[4] = {
+    { TOF_LEFT_PORT, 0xff, },
+    { TOF_LEFT_PORT, TOF_LEFT_SECOND_GPIO, },
+    { TOF_RIGHT_PORT, 0xff, },
+    { TOF_RIGHT_PORT, TOF_RIGHT_SECOND_GPIO, },
+};
+
+#define TOF_NUM (count_of(tofs))
+
+static struct {
+    airkey_side_t side;
     uint16_t in_low;
     uint16_t in_high;
     uint16_t out_low;
     uint16_t out_high;
 } key_defs[] = {
-    { 0, 50, 200, 20, 230 },
-    { 1, 50, 200, 20, 230 },
-    { 0, 300, 400, 280, 430 },
-    { 1, 300, 400, 280, 430 }
+    { SIDE_LEFT, 50, 200, 20, 230 },
+    { SIDE_RIGHT, 50, 200, 20, 230 },
+    { SIDE_LEFT, 300, 400, 280, 430 },
+    { SIDE_RIGHT, 300, 400, 280, 430 }
 };
 
 #define AIRKEY_NUM (count_of(key_defs))
@@ -48,46 +59,46 @@ static uint64_t sw_freeze_time[AIRKEY_NUM];
 void airkey_init()
 {
     uint8_t tof_gpios[] = TOF_GPIO_DEF;
-    static_assert(count_of(tof_gpios) == TOF_NUM * 2);
+    for (int i = 0; i < count_of(tof_gpios); i++) {
+        gpio_init(tof_gpios[i]);
+        gpio_set_function(tof_gpios[i], GPIO_FUNC_I2C);
+        gpio_pull_up(tof_gpios[i]);
+    }
+
+    i2c_init(TOF_LEFT_PORT, TOF_I2C_FREQ);
+    if (TOF_LEFT_PORT != TOF_RIGHT_PORT) {
+        i2c_init(TOF_RIGHT_PORT, TOF_I2C_FREQ);
+    }
 
     for (int i = 0; i < TOF_NUM; i++) {
-        uint8_t scl = tof_gpios[i * 2];
-        uint8_t sda = tof_gpios[i * 2 + 1];
-
-        gpio_init(scl);
-        gpio_init(sda);
-
-        i2c_init(tof_ports[i], TOF_I2C_FREQ);
-        gpio_set_function(scl, GPIO_FUNC_I2C);
-        gpio_set_function(sda, GPIO_FUNC_I2C);
-
-        gpio_pull_up(scl);
-        gpio_pull_up(sda);
-
-        vl53l0x_init(i, tof_ports[i]);
-        vl53l1x_init(i, tof_ports[i]);
-        vl53l0x_use(i);
-        vl53l1x_use(i);
-
+        if (tofs[i].gpio < 32) { // valid GPIO
+            gpio_init(tofs[i].gpio);
+            gpio_set_function(tofs[i].gpio, GPIO_FUNC_SIO);
+            gpio_set_drive_strength(tofs[i].gpio, GPIO_DRIVE_STRENGTH_12MA);
+            gpio_set_dir(tofs[i].gpio, GPIO_OUT);
+            gpio_put(tofs[i].gpio, 1);
+        }
+        sleep_ms(1);
+        vl53l0x_init(i, tofs[i].port);
+        vl53l1x_init(i, tofs[i].port);
         if (vl53l0x_is_present()) {
-            tof_models[i] = TOF_VL53L0X;
-            tof_init_ok[i] = vl53l0x_init_tof();
+            tofs[i].model = TOF_VL53L0X;
+            tofs[i].init_ok = vl53l0x_init_tof();
             vl53l0x_start_continuous();
         } else if (vl53l1x_is_present()) {
-            tof_models[i] = TOF_VL53L1X;
-            tof_init_ok[i] = vl53l1x_init_tof();
-
+            tofs[i].model = TOF_VL53L1X;
+            tofs[i].init_ok = vl53l1x_init_tof();
             vl53l1x_setROISize(geki_cfg->tof.roi, geki_cfg->tof.roi);
             vl53l1x_setDistanceMode(Short);
             vl53l1x_setMeasurementTimingBudget(20000);
             vl53l1x_startContinuous(20);
         } else {
-            tof_models[i] = 0;
+            tofs[i].model = TOF_NONE;
         }
     }
 }
 
-static uint16_t tof_dist[count_of(tof_ports)];
+static uint16_t tof_dist[TOF_NUM];
 static bool readings[AIRKEY_NUM];
 
 static void print_tof(const char *name, uint16_t mm)
@@ -98,11 +109,11 @@ static void print_tof(const char *name, uint16_t mm)
 static void tof_read()
 {
     for (int i = 0; i < TOF_NUM; i++) {
-        if (tof_models[i] == TOF_VL53L0X) {
+        if (tofs[i].model == TOF_VL53L0X) {
             vl53l0x_use(i);
             tof_dist[i] = readRangeContinuousMillimeters();
             print_tof("L0x", tof_dist[i]);
-        } else if (tof_models[i] == TOF_VL53L1X) {
+        } else if (tofs[i].model == TOF_VL53L1X) {
             vl53l1x_use(i);
             tof_dist[i] = vl53l1x_readContinuousMillimeters();
             print_tof("L1x", tof_dist[i]);
@@ -111,12 +122,78 @@ static void tof_read()
     //printf("\n");
 }
 
+static uint16_t mix_tof(airkey_side_t side, uint16_t primary, uint16_t secondary)
+{
+    tof_mix_algo_t algo = geki_cfg->tof.mix[side].algo;
+
+    if (algo == MIX_PRIMARY) {
+        return primary;
+    }
+
+    if (algo == MIX_SECONDARY) {
+        return secondary;
+    }
+
+    if (geki_cfg->tof.mix[side].strict && (primary == 0 || secondary == 0)) {
+        return 0;
+    }
+
+    int window = geki_cfg->tof.mix[side].window;
+    if ((algo != MIX_AVG) || (window == 0)) {
+        if (primary == 0) {
+            return secondary;
+        }
+        if (secondary == 0) {
+            return primary;
+        }
+    }
+
+    if (algo == MIX_MAX) {
+        return primary > secondary ? primary : secondary;
+    }
+    
+    if (algo == MIX_MIN) {
+        return primary < secondary ? primary : secondary;
+    }
+    
+    if (algo == MIX_AVG) {
+        int delta = primary - secondary;
+        int max = primary;
+        if (delta < 0) {
+            delta = -delta;
+            max = secondary;
+        }
+    
+        if ((window == 0) || (window >= delta * 100 / max)) {
+            return (primary + secondary) / 2;
+        }
+    }
+
+    return 0;
+}
+
 #define BETWEEN(x, a, b) (((x) >= (a)) && ((x) <= (b)))
 static bool airkey_read(unsigned index)
 {
-    uint16_t dist = tof_dist[key_defs[index].port_id];
+    airkey_side_t side = key_defs[index].side;
+    
+    int a_id = side * 2;
+    int b_id = side * 2 + 1;
 
-    if (readings[index]) {
+    uint16_t dist;
+
+    if ((tofs[a_id].init_ok && tofs[b_id].init_ok) && 
+        (tofs[a_id].model == tofs[b_id].model)) {
+        dist = mix_tof(side, tof_dist[a_id], tof_dist[b_id]);
+    } else if (tofs[a_id].init_ok) {
+        dist = tof_dist[a_id];
+    } else if (tofs[b_id].init_ok) {
+        dist = tof_dist[b_id];
+    } else {
+        return false;
+    }
+
+    if (readings[index]) { // currently triggered
         return BETWEEN(dist, key_defs[index].out_low, key_defs[index].out_high);
     } else {
         return BETWEEN(dist, key_defs[index].in_low, key_defs[index].in_high);
@@ -165,22 +242,22 @@ unsigned airkey_tof_num()
 const char *airkey_tof_model(unsigned tof_id)
 {
     if (tof_id >= TOF_NUM) {
-        return "Unknown";
+        return "None";
     }
 
-    if (tof_models[tof_id] == TOF_VL53L0X) {
+    if (tofs[tof_id].model == TOF_VL53L0X) {
         return "VL53L0X";
-    } else if (tof_models[tof_id] == TOF_VL53L1X) {
+    } else if (tofs[tof_id].model == TOF_VL53L1X) {
         return "VL53L1X";
     } else {
-        return "Unknown";
+        return "None";
     }
 }
 
 void airkey_tof_update_roi()
 {
     for (int i = 0; i < TOF_NUM; i++) {
-        if (tof_models[i] == TOF_VL53L1X) {
+        if (tofs[i].model == TOF_VL53L1X) {
             vl53l1x_use(i);
             vl53l1x_setROISize(geki_cfg->tof.roi, geki_cfg->tof.roi);
         }

firmware/src/airkey.h

@@ -18,6 +18,15 @@ bool airkey_get(unsigned id);
 unsigned airkey_tof_num();
 const char *airkey_tof_model();
 
+void airkey_tof_init();
 void airkey_tof_update_roi();
 
+typedef enum {
+    MIX_PRIMARY = 0,
+    MIX_SECONDARY,
+    MIX_MAX,
+    MIX_MIN,
+    MIX_AVG,
+} tof_mix_algo_t;
+
 #endif

firmware/src/board_defs.h

@@ -11,7 +11,12 @@
 
 #define BUTTON_DEF { 12, 11, 10, 5, 4, 3, 13, 2 }
 #define SOUND_DEF { 8, 6 }
-#define TOF_PORT_DEF { i2c1, i2c0 }
+
+#define TOF_LEFT_PORT i2c1
+#define TOF_RIGHT_PORT i2c0
+#define TOF_LEFT_SECOND_GPIO 9
+#define TOF_RIGHT_SECOND_GPIO 7
+
 #define TOF_GPIO_DEF { 18, 19, 0, 1 }
 #define TOF_I2C_FREQ 400*1000
 

firmware/src/commands.c

@@ -60,8 +60,23 @@ static void disp_tof()
         printf("  TOF %d: %s", i, airkey_tof_model(i));
     }
     printf("\n");
-    printf("  ROI: %d (only for VL53L1X)", geki_cfg->tof.roi);
-    printf("\n");
+    for (int i = 0; i < 2; i++) {
+        if (geki_cfg->tof.mix[i].algo > 4) {
+            geki_cfg->tof.mix[i].algo = default_cfg.tof.mix[i].algo;
+            config_changed();
+        }
+        const char *algos[] = { "Primary", "Secondary", "Max", "Min", "Avg" };
+        printf("  %s: %s", i == 0 ? "Left Mix" : "Right Mix",
+                           algos[geki_cfg->tof.mix[i].algo]);
+        if (geki_cfg->tof.mix[i].algo == MIX_AVG) {
+            const char *windows[] = { "No", "5%", "10%", "15%", "20%", "25%", "30%", "35%" };
+            printf(", %s Window", windows[geki_cfg->tof.mix[i].window]);
+        } else {
+            printf("%s", geki_cfg->tof.mix[i].strict ? ", Strict" : "");
+        }
+        printf("\n");
+    }
+    printf("  ROI: %d (only for VL53L1X)\n", geki_cfg->tof.roi);
 }
 
 static void disp_aime()
@@ -246,19 +261,15 @@ static void handle_lever(int argc, char *argv[])
     disp_lever();
 }
 
-static void handle_tof(int argc, char *argv[])
+static bool handle_tof_roi(int argc, char *argv[])
 {
-    const char *usage = "Usage: tof roi <4..16>\n";
-
-    if ((argc != 2) || (strncasecmp(argv[0], "roi", strlen(argv[0])) != 0)) {
-        printf(usage);
-        return;
+    if (argc != 1) {
+        return false;
     }
 
-    int roi = cli_extract_non_neg_int(argv[1], 0);
+    int roi = cli_extract_non_neg_int(argv[0], 0);
     if ((roi < 4) || (roi > 16)) {
-        printf(usage);
-        return;
+        return false;
     }
 
     geki_cfg->tof.roi = roi;
@@ -266,6 +277,76 @@ static void handle_tof(int argc, char *argv[])
 
     config_changed();
     disp_tof();
+    return true;
+}
+
+static bool handle_tof_mix(int side, int argc, char *argv[])
+{
+    if ((argc < 1) || (argc > 2)) {
+        printf("%d, %d\n", __LINE__, argc);
+        return false;
+    }
+
+    const char *algos[] = { "primary", "secondary", "max", "min", "avg" };
+    int algo = cli_match_prefix(algos, 5, argv[0]);
+    if (algo < 0) {
+        printf("%d\n", __LINE__);
+        return false;
+    }
+
+    if (algo == MIX_AVG) {
+        int window = 0;
+        if (argc == 2) {
+            window = cli_extract_non_neg_int(argv[1], 0);
+            if ((window < 1) || (window > 7)) {
+        printf("%d\n", __LINE__);
+                return false;
+            }
+        }
+        geki_cfg->tof.mix[side].window = window;
+        geki_cfg->tof.mix[side].strict = (window > 0);
+    } else {
+        geki_cfg->tof.mix[side].window = 0;
+        geki_cfg->tof.mix[side].strict = false;
+        if ((argc == 2) &&
+            (strncasecmp(argv[1], "strict", strlen(argv[1])) == 0)) {
+            geki_cfg->tof.mix[side].strict = true;
+        }
+    }
+    geki_cfg->tof.mix[side].algo = algo;
+    config_changed();
+    disp_tof();
+    return true;
+}
+
+static void handle_tof(int argc, char *argv[])
+{
+    const char *usage = "Usage: tof roi <4..16>\n"
+                        "       tof <left|right> <primary|secondary>\n"
+                        "       tof <left|right> <max|min> [strict]\n"
+                        "       tof <left|right> <avg> [window]\n"
+                        " window: 1..7 (5% ~ 35%)\n";
+
+    if (argc < 1) {
+        printf(usage);
+        return;
+    }
+
+    const char *commands[] = { "left", "right", "roi" };
+    int match = cli_match_prefix(commands, 2, argv[0]);
+
+    if (match == 2) {
+        if (handle_tof_roi(argc - 1, argv + 1)) {
+            return;
+        }
+    } else if (match >= 0) {
+        if (handle_tof_mix(match, argc - 1, argv + 1)) {
+            return;
+        }
+    }
+
+    printf("%d\n", __LINE__);
+    printf(usage);
 }
 
 static void handle_save()

firmware/src/config.c

@@ -9,9 +9,11 @@
 #include "config.h"
 #include "save.h"
 
+#include "airkey.h"
+
 geki_cfg_t *geki_cfg;
 
-static geki_cfg_t default_cfg = {
+geki_cfg_t default_cfg = {
     .lever = {
          2000, 2500, 0,
     },
@@ -21,6 +23,10 @@ static geki_cfg_t default_cfg = {
     },
     .tof = {
         .roi = 12,
+        .mix = {
+            { .strict = 0, .algo = MIX_MAX, .window = 0 },
+            { .strict = 0, .algo = MIX_MAX, .window = 0 },
+        },
         .reserved = { 0 },
     },
     .sound = {

firmware/src/config.h

@@ -32,7 +32,12 @@ typedef struct __attribute__((packed)) {
     } sound;
     struct {
         uint8_t roi;
-        uint8_t reserved[7];
+        struct {
+            uint8_t strict:1;
+            uint8_t algo:4;
+            uint8_t window:3;
+        } mix[2];
+        uint8_t reserved[5];
     } tof;
     struct {
         uint8_t joy : 4;
@@ -50,6 +55,7 @@ typedef struct {
 } geki_runtime_t;
 
 extern geki_cfg_t *geki_cfg;
+extern geki_cfg_t default_cfg;
 extern geki_runtime_t geki_runtime;
 
 void config_init();