Dual sensor mode optimization and doc

README.md

@@ -158,7 +158,8 @@ You need to rotate 135 degrees on Z axis to fit the bed.
 
 * Steps
   1. Install the lever to the bottom part, using M3\*8mm screws.
-  2. Wire (solder) the ToF sensors to the main PCB, each needs 4 wires.
+  2. Wire (solder) the ToF sensors to the main PCB, each needs 4 wires.  
+     If you're using an older PCB that only supports one sensor on each side, you can manually wire the secondary sensor. The secondary sensor uses the same SDA, SCL and GND pins as the primary sensor, but the VCC pin should be directly connected to the pins of the Pico, GP7 is for the right secondary sensor, GP9 is for the left secondary sensor.
   3. Use VHB tape to fix the ToF sensors on the ToF seats on the bottom part.
   4. Wire (solder) the speakers to the main PCB, each needs 2 wires.
   5. Use VHB tape to fix the speakers on the floor of the bottom part.
@@ -190,6 +191,12 @@ You need to rotate 135 degrees on Z axis to fit the bed.
   <img src="doc/tof_view_1.png" width="40%"> <img src="doc/tof_view_2.png" width="45%">
 * To emulate IO4 TEST/SERVICE/COIN, you can put your hand in SHIFT zone. When you see the WAD lights flashing, the AUX buttons become TEST and SERVICE, and swinging the lever for "INSERT COINS".
 * AIME is on a secondary COM port. You can set mode or toggle virtual AIC function.
+* ToF mixing algorithms in dual sensor each side configuration.
+  * `primary` and `secondary`: always use primary or secondary sensor, fallback to the other one if the chosen one has no reading, `strict` option means no fallback.
+  * `max` and `min`: use the larger or smaller reading of the two sensors, `strict` option means if one sensor has no reading, result will be no reading.
+  * `average`: use the average of the two sensors, `window` option means readings from both sensors must be within a certain range, otherwise result will be no reading. `window` set to 0 means no range check.
+* You can set trigger window for left WAD, right WAD and shift by `tof trigger <left|right|shift> ...` command. The window is in millimeters. In is the window to trigger, out is the window to release.
+* You can use `tof diagnose [on|off]` command to toggle diagnose mode. In diagnose mode, the ToF sensor will continuously print the raw distance data from all sensors. It's useful for diagnostics.
 
 ## CAD Source File
 I'm using OnShape free subscription. It's powerful but it can't archive original designs to local, so I can only share the link here. STL/DXF/DWG files are exported from this online document.  

README_CN.md

@@ -158,7 +158,8 @@ https://discord.gg/M8f2PPQFEA
 
 * 步骤
   1. 用 M3\*8mm 螺丝把摇杆总成固定到底部。
-  2. 把 ToF 传感器焊接到主 PCB 上，每个需要 4 根线。
+  2. 把 ToF 传感器焊接到主 PCB 上，每个需要 4 根线。  
+     如果你用的是旧版本 PCB，它很可能只支持每边一个传感器，这时候你需要手动接线。`次 ToF` 使用和 `主 ToF` 使用相同的 SDA, SCL 和 GND 引脚，但 VCC 引脚需要直接从 Pico 上去接，右边的 `次 ToF` 的 VCC 接 GP7，左边的 `次 ToF` 的 VCC 接 GP9。
   3. 用 VHB 胶带把 ToF 传感器固定到底部的两侧座位上。
   4. 把扬声器焊接到主 PCB 上，每个需要 2 根线。
   5. 用 VHB 胶带把扬声器固定到底部的地板上。
@@ -190,6 +191,12 @@ https://discord.gg/M8f2PPQFEA
   <img src="doc/tof_view_1.png" width="40%"> <img src="doc/tof_view_2.png" width="45%">
 * 为了模拟 IO4 的 TEST/SERVICE/COIN 这些按钮和功能，你可以把手放到 SHIFT 区域，当你看到控制器两侧指示灯闪烁的时候，两个辅助按钮就变为了 TEST 和 SERVICE，摇动摇杆则是“投币”。
 * AIME 在第二个串口端口，你可以设置工作模式或者开关虚拟 AIC 功能。
+* 每边两个 ToF 情况下的混合算法。
+  * `primary` 和 `secondary`: 总是使用主 ToF 或者次 ToF，如果选择的那个没有读数，就会使用另一个，`strict` 选项表示即使没有读数也不允许使用另一个。
+  * `max` 和 `min`: 使用两个传感器中的较大或较小读数，`strict` 选项表示如果一个传感器没有读数，结果将是没有读数。
+  * `average`: 使用两个传感器的平均值，`window` 选项表示两个传感器的读数必须在一定范围内，否则结果将是没有读数。当 `window` 为 0 表示不比对范围。
+* 你可以使用 `tof trigger <left|right|shift> ...` 命令来设置左右 ToF 和 SHIFT 的触发窗口。窗口单位是毫米。In 是触发窗口，Out 是释放窗口。
+* 你可以使用 `tof diagnose [on|off]` 命令来切换诊断模式。在诊断模式下，ToF 传感器将持续打印所有传感器的原始距离数据。方便调试。
 
 ## CAD Source File
 我使用的是 OnShape 的免费订阅。它很强大，但是它不能将原始设计存档到本地，所以我只能在这里分享链接。STL/DXF/DWG 文件是从这个在线文档导出的。    

firmware/src/airkey.c

@@ -91,6 +91,11 @@ static uint16_t tof_mix[2];
 static void tof_read()
 {
     for (int i = 0; i < TOF_NUM; i++) {
+        if (!tofs[i].init_ok) {
+            tof_dist[i] = 0;
+            continue;
+        }
+    
         if (tofs[i].model == TOF_VL53L0X) {
             vl53l0x_use(i);
             tof_dist[i] = readRangeContinuousMillimeters();
@@ -107,16 +112,17 @@ static void tof_read()
 static uint16_t mix_dist(airkey_side_t side, uint16_t primary, uint16_t secondary)
 {
     tof_mix_algo_t algo = geki_cfg->tof.mix[side].algo;
+    bool strict = geki_cfg->tof.mix[side].strict;
 
     if (algo == MIX_PRIMARY) {
-        return primary;
+        return (strict || (primary != 0)) ? primary : secondary;
     }
 
     if (algo == MIX_SECONDARY) {
-        return secondary;
+        return (strict || (secondary != 0)) ? secondary : primary;
     }
 
-    if (geki_cfg->tof.mix[side].strict && (primary == 0 || secondary == 0)) {
+    if (strict && (primary == 0 || secondary == 0)) {
         return 0;
     }
 

firmware/src/commands.c

@@ -410,8 +410,7 @@ static bool handle_tof_diag(int argc, char *argv[])
 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> <primary|secondary|max|min> [strict]\n"
                         "       tof <left|right> <avg> [window]\n"
                         "       tof trigger <left|right|shift> <in_low> <in_high> [<out_low> [out_high]]\n"
                         "       tof diagnose [on|off]\n"