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};