Identifies VL53L1x

firmware/src/airkey.c

@@ -23,6 +23,8 @@
 
 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 };
 
 static struct {
     uint8_t port_id;
@@ -62,9 +64,21 @@ void airkey_init()
         gpio_pull_up(sda);
 
         vl53l0x_init(i, tof_ports[i], 0);
+        vl53l1x_init(i, tof_ports[i], 0);
         vl53l0x_use(i);
+        vl53l1x_use(i);
+
+        if (vl53l0x_is_present()) {
+            tof_models[i] = TOF_VL53L0X;
             vl53l0x_init_tof();
             vl53l0x_start_continuous();
+        } else if (vl53l1x_is_present()) {
+            tof_models[i] = TOF_VL53L1X;
+            vl53l1x_init_tof();
+            vl53l1x_startContinuous(20);
+        } else {
+            tof_models[i] = 0;
+        }
     }
 }
 
@@ -74,8 +88,13 @@ static bool readings[AIRKEY_NUM];
 static void tof_read()
 {
     for (int i = 0; i < TOF_NUM; i++) {
+        if (tof_models[i] == TOF_VL53L0X) {
             vl53l0x_use(i);
-        tof_dist[i] = readRangeContinuousMillimeters(i);
+            tof_dist[i] = readRangeContinuousMillimeters();
+        } else if (tof_models[i] == TOF_VL53L1X) {
+            vl53l1x_use(i);
+            tof_dist[i] = vl53l1x_readContinuousMillimeters();
+        }
     }
 }
 
@@ -116,7 +135,31 @@ unsigned airkey_num()
     return AIRKEY_NUM;
 }
 
-bool airkey_get(int id)
+bool airkey_get(unsigned id)
 {
+    if (id >= AIRKEY_NUM) {
+        return false;
+    }
+
     return readings[id];
 }
+
+unsigned airkey_tof_num()
+{
+    return TOF_NUM;
+}
+
+const char *airkey_tof_model(unsigned tof_id)
+{
+    if (tof_id >= TOF_NUM) {
+        return "Unknown";
+    }
+
+    if (tof_models[tof_id] == TOF_VL53L0X) {
+        return "VL53L0X";
+    } else if (tof_models[tof_id] == TOF_VL53L1X) {
+        return "VL53L1X";
+    } else {
+        return "Unknown";
+    }
+}

firmware/src/airkey.h

@@ -13,6 +13,9 @@
 void airkey_init();
 void airkey_update();
 unsigned airkey_num();
-bool airkey_get(int id);
+bool airkey_get(unsigned id);
+
+unsigned airkey_tof_num();
+const char *airkey_tof_model();
 
 #endif

firmware/src/commands.c

@@ -18,6 +18,8 @@ extern uint8_t RING_DATA[];
 #include "nfc.h"
 #include "aime.h"
 
+#include "airkey.h"
+
 #include "usb_descriptors.h"
 
 #define SENSE_LIMIT_MAX 9
@@ -51,6 +53,15 @@ static void disp_hid()
            geki_cfg->hid.nkro ? "on" : "off" );
 }
 
+static void disp_tof()
+{
+    printf("[TOF]\n");
+    for (int i = 0; i < airkey_tof_num(); i++) {
+        printf("  TOF %d: %s", i, airkey_tof_model(i));
+    }
+    printf("\n");
+}
+
 static void disp_aime()
 {
     printf("[AIME]\n");
@@ -61,7 +72,7 @@ static void disp_aime()
 
 void handle_display(int argc, char *argv[])
 {
-    const char *usage = "Usage: display [light|sound|hid|lever|aime]\n";
+    const char *usage = "Usage: display [light|sound|hid|lever|tof|aime]\n";
     if (argc > 1) {
         printf(usage);
         return;
@@ -72,11 +83,12 @@ void handle_display(int argc, char *argv[])
         disp_lever();
         disp_sound();
         disp_hid();
+        disp_tof();
         disp_aime();
         return;
     }
 
-    const char *choices[] = {"light", "lever", "sound", "hid", "aime"};
+    const char *choices[] = {"light", "lever", "sound", "hid", "tof", "aime"};
     switch (cli_match_prefix(choices, count_of(choices), argv[0])) {
         case 0:
             disp_light();
@@ -91,6 +103,9 @@ void handle_display(int argc, char *argv[])
             disp_hid();
             break;
         case 4:
+            disp_tof();
+            break;
+        case 5:
             disp_aime();
             break;
         default:

firmware/src/vl53l0x.c

@@ -216,18 +216,18 @@ const uint16_t reg_tuning[] = { 80,
         0xff00, 0x8001, 0x01f8, 0xff01, 0x8e01, 0x0001, 0xff00, 0x8000,
     };
 
-void vl53l0x_init(unsigned index, i2c_inst_t *i2c_port, uint8_t i2c_addr)
+void vl53l0x_init(unsigned instance, i2c_inst_t *i2c_port, uint8_t i2c_addr)
 {
-    if (index < INSTANCE_NUM) {
+    if (instance < INSTANCE_NUM) {
-        instances[index].port = i2c_port;
+        instances[instance].port = i2c_port;
-        instances[index].addr = i2c_addr ? i2c_addr : VL53L0X_DEF_ADDR;
+        instances[instance].addr = i2c_addr ? i2c_addr : VL53L0X_DEF_ADDR;
     }
 }
 
-void vl53l0x_use(unsigned index)
+void vl53l0x_use(unsigned instance)
 {
-    if (index < INSTANCE_NUM) {
+    if (instance < INSTANCE_NUM) {
-        current_instance = index;
+        current_instance = instance;
     }
 }
 

firmware/src/vl53l0x.h

@@ -13,8 +13,8 @@
 
 #include "hardware/i2c.h"
 
-void vl53l0x_init(unsigned index, i2c_inst_t *i2c_port, uint8_t i2c_addr);
+void vl53l0x_init(unsigned instance, i2c_inst_t *i2c_port, uint8_t i2c_addr);
-void vl53l0x_use(unsigned index);
+void vl53l0x_use(unsigned instance);
 bool vl53l0x_is_present();
 bool vl53l0x_init_tof();
 

firmware/src/vl53l1x.c

@@ -1339,21 +1339,6 @@ static uint32_t calcMacroPeriod(uint8_t vcsel_period);
 // Convert count rate from fixed point 9.7 format to float
 static float countRateFixedToFloat(uint16_t count_rate_fixed);
 
-void vl53l1x_init(unsigned index, i2c_inst_t *i2c_port, uint8_t i2c_addr)
-{
-    if (index < INSTANCE_NUM) {
-        instances[index].port = i2c_port;
-        instances[index].addr = i2c_addr ? i2c_addr : VL53L1X_DEF_ADDR;
-    }
-}
-
-void vl53l1x_use(unsigned index)
-{
-    if (index < INSTANCE_NUM) {
-        current_instance = index;
-    }
-}
-
 static void write_reg(uint16_t reg, uint8_t value)
 {
     uint8_t data[] = { reg >> 8, reg, value };
@@ -1406,6 +1391,26 @@ static void read_many(uint16_t reg, uint8_t *dst, uint8_t len)
                             false, time_us_64() + IO_TIMEOUT_US * len);
 }
 
+void vl53l1x_init(unsigned instance, i2c_inst_t *i2c_port, uint8_t i2c_addr)
+{
+    if (instance < INSTANCE_NUM) {
+        instances[instance].port = i2c_port;
+        instances[instance].addr = i2c_addr ? i2c_addr : VL53L1X_DEF_ADDR;
+    }
+}
+
+void vl53l1x_use(unsigned instance)
+{
+    if (instance < INSTANCE_NUM) {
+        current_instance = instance;
+    }
+}
+
+bool vl53l1x_is_present()
+{
+    return read_reg16(IDENTIFICATION__MODEL_ID) == 0xeacc;
+}
+
 // Initialize sensor using settings taken mostly from VL53L1_DataInit() and
 // VL53L1_StaticInit().
 bool vl53l1x_init_tof()

firmware/src/vl53l1x.h

@@ -12,8 +12,8 @@
 #include <stdbool.h>
 #include "hardware/i2c.h"
 
-void vl53l1x_init(unsigned index, i2c_inst_t *i2c_port, uint8_t i2c_addr);
+void vl53l1x_init(unsigned instance, i2c_inst_t *i2c_port, uint8_t i2c_addr);
-void vl53l1x_use(unsigned index);
+void vl53l1x_use(unsigned instance);
 bool vl53l1x_is_present();
 bool vl53l1x_init_tof();