From 7b1c41d3cae8166fae1a67e655c8d13178b9bd3f Mon Sep 17 00:00:00 2001
From: whowe <whowe@gmail.com>
Date: Thu, 21 Sep 2023 23:01:13 +0800
Subject: [PATCH] Explicitly identify both TOF

---
 firmware/src/air.c    | 16 +++++++++++-----
 firmware/src/gp2y0e.h |  7 +++++++
 2 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/firmware/src/air.c b/firmware/src/air.c
index 98d94ec..031f8c2 100644
--- a/firmware/src/air.c
+++ b/firmware/src/air.c
@@ -37,11 +37,17 @@ void air_init()
 
     for (int i = 0; i < sizeof(TOF_LIST); i++) {
         i2c_select(I2C_PORT, 1 << TOF_LIST[i]);
-        tof_model[i] = vl53l0x_is_present() ? 0 : 1;
-        if (tof_model[i] == 0) {
+        if (vl53l0x_is_present()) {
+            tof_model[i] = 1;
+        } else if (gp2y0e_is_present(I2C_PORT)) {
+            tof_model[i] = 2;
+        } else {
+            tof_model[i] = 0;
+        }
+        if (tof_model[i] == 1) {
             vl53l0x_init_tof(true);
             vl53l0x_start_continuous(0);
-        } else if (tof_model[i] == 1) {
+        } else if (tof_model[i] == 2) {
             gp2y0e_write(I2C_PORT, 0xa8, 0); // Accumulation 0:1, 1:5, 2:30, 3:10
             gp2y0e_write(I2C_PORT, 0x3f, 0x30); // Filter 0x00:7, 0x10:5, 0x20:9, 0x30:1
             gp2y0e_write(I2C_PORT, 0x13, 5); // Pulse [3..7]:[40, 80, 160, 240, 320] us
@@ -104,9 +110,9 @@ void air_update()
 {
     for (int i = 0; i < sizeof(TOF_LIST); i++) {
         i2c_select(I2C_PORT, 1 << TOF_LIST[i]);
-        if (tof_model[i] == 0) {
+        if (tof_model[i] == 1) {
             distances[i] = readRangeContinuousMillimeters() * 10;
-        } else if (tof_model[i] == 1) {
+        } else if (tof_model[i] == 2) {
             distances[i] = gp2y0e_dist16(I2C_PORT);
         }
     }
diff --git a/firmware/src/gp2y0e.h b/firmware/src/gp2y0e.h
index 9aa9612..52f7733 100644
--- a/firmware/src/gp2y0e.h
+++ b/firmware/src/gp2y0e.h
@@ -11,6 +11,13 @@
 
 #define GP2Y0E_DEF_ADDR 0x40
 
+static inline bool gp2y0e_is_present(i2c_inst_t *i2c_port)
+{
+    uint8_t cmd[] = {0x5e};
+    return i2c_write_blocking_until(i2c_port, GP2Y0E_DEF_ADDR, cmd, 1, true,
+                                    time_us_64() + 1000) == 1;
+}
+
 static inline uint16_t gp2y0e_write(i2c_inst_t *i2c_port, uint8_t addr, uint8_t val)
 {
     uint8_t cmd[] = {addr, val};