ROI setting in CLI

firmware/src/airkey.c

@@ -77,7 +77,7 @@ void airkey_init()
             tof_models[i] = TOF_VL53L1X;
             tof_init_ok[i] = vl53l1x_init_tof();
 
-            vl53l1x_setROISize(4, 4);
+            vl53l1x_setROISize(geki_cfg->tof.roi, geki_cfg->tof.roi);
             vl53l1x_setDistanceMode(Short);
             vl53l1x_setMeasurementTimingBudget(20000);
             vl53l1x_startContinuous(20);
@@ -92,7 +92,7 @@ static bool readings[AIRKEY_NUM];
 
 static void print_tof(const char *name, uint16_t mm)
 {
-    printf("\t%s: %3d", name, mm > 1000 ? 0 : mm);
+    //printf("\t%s: %3d", name, mm > 1000 ? 0 : mm);
 }
 
 static void tof_read()
@@ -108,7 +108,7 @@ static void tof_read()
             print_tof("L1x", tof_dist[i]);
         }
     }
-    printf("\n");
+    //printf("\n");
 }
 
 #define BETWEEN(x, a, b) (((x) >= (a)) && ((x) <= (b)))
@@ -176,3 +176,13 @@ const char *airkey_tof_model(unsigned tof_id)
         return "Unknown";
     }
 }
+
+void airkey_tof_update_roi()
+{
+    for (int i = 0; i < TOF_NUM; i++) {
+        if (tof_models[i] == TOF_VL53L1X) {
+            vl53l1x_use(i);
+            vl53l1x_setROISize(geki_cfg->tof.roi, geki_cfg->tof.roi);
+        }
+    }
+}

firmware/src/airkey.h

@@ -18,4 +18,6 @@ bool airkey_get(unsigned id);
 unsigned airkey_tof_num();
 const char *airkey_tof_model();
 
+void airkey_tof_update_roi();
+
 #endif

firmware/src/commands.c

@@ -60,6 +60,8 @@ 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");
 }
 
 static void disp_aime()
@@ -244,6 +246,28 @@ static void handle_lever(int argc, char *argv[])
     disp_lever();
 }
 
+static void handle_tof(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;
+    }
+
+    int roi = cli_extract_non_neg_int(argv[1], 0);
+    if ((roi < 4) || (roi > 16)) {
+        printf(usage);
+        return;
+    }
+
+    geki_cfg->tof.roi = roi;
+    airkey_tof_update_roi();
+
+    config_changed();
+    disp_tof();
+}
+
 static void handle_save()
 {
     save_request(true);
@@ -349,6 +373,7 @@ void commands_init()
     cli_register("level", handle_level, "Set LED brightness level.");
     cli_register("hid", handle_hid, "Set HID mode.");
     cli_register("lever", handle_lever, "Lever related settings.");
+    cli_register("tof", handle_tof, "Tof tweaks.");
     cli_register("volume", handle_volume, "Sound feedback volume settings.");
     cli_register("save", handle_save, "Save config to flash.");
     cli_register("factory", handle_factory_reset, "Reset everything to default.");

firmware/src/config.c

@@ -19,6 +19,10 @@ static geki_cfg_t default_cfg = {
         .level = 128,
         .reserved = { 0 },
     },
+    .tof = {
+        .roi = 12,
+        .reserved = { 0 },
+    },
     .sound = {
         .volume = 127,
         .reserved = { 0 },

firmware/src/config.h

@@ -30,6 +30,10 @@ typedef struct __attribute__((packed)) {
         uint8_t volume;
         uint8_t reserved[3];
     } sound;
+    struct {
+        uint8_t roi;
+        uint8_t reserved[7];
+    } tof;
     struct {
         uint8_t joy : 4;
         uint8_t nkro : 4;

firmware/src/vl53l1x.c

@@ -1686,7 +1686,7 @@ uint32_t vl53l1x_getMeasurementTimingBudget()
 // reading that document carefully.
 void vl53l1x_setROISize(uint8_t width, uint8_t height)
 {
-    if ( width > 16) {
+    if (width > 16) {
         width = 16;
     }
     if (height > 16) {