feat: 完善Camera对焦代码

server/src/Peripherals/CameraClient.cs

@@ -148,6 +148,9 @@ class Camera
         // var resetResult2 = await Reset();
         // if (!resetResult2.IsSuccessful) return resetResult2;
 
+        var autofocusResult = await InitAutoFocus();
+        if (!autofocusResult.IsSuccessful) return autofocusResult;
+
         return true;
     }
 
@@ -1290,6 +1293,32 @@ class Camera
         var result = await ConfigureRegisters(focusRegisters);
         if (!result.IsSuccessful) return result;
 
+        // 读取寄存器判断初始化是否完毕
+        for (int iteration = 1000; iteration > 0; iteration--)
+        {
+            var readResult = await ReadRegister(0x3029);
+            if (!readResult.IsSuccessful)
+            {
+                logger.Error($"读取自动对焦初始化状态失败: {readResult.Error}");
+                return new(readResult.Error);
+            }
+            
+            logger.Trace($"自动对焦初始化状态检查, state=0x{readResult.Value:X2}");
+            
+            if (readResult.Value == 0x70)
+            {
+                break; // 初始化完成
+            }
+            
+            if (iteration == 1)
+            {
+                logger.Error($"自动对焦初始化状态检查超时!! state=0x{readResult.Value:X2}");
+                return new(new Exception($"自动对焦初始化状态检查超时, state=0x{readResult.Value:X2}"));
+            }
+            
+            await Task.Delay(1);
+        }
+
         logger.Info("OV5640自动对焦功能初始化完成");
         return true;
     }
@@ -1313,10 +1342,7 @@ class Camera
         logger.Info("已发送单点对焦命令 (0x3022 = 0x03)");
 
         // 步骤2: 读取寄存器 0x3029 的状态，如果返回值为 0x10，代表对焦已完成
-        int iteration = 5000;
+        for (int iteration = 5000; iteration > 0; iteration--)
-        byte focusStatus = 0x00;
-
-        do
         {
             var readResult = await ReadRegister(0x3029);
             if (!readResult.IsSuccessful)
@@ -1325,23 +1351,20 @@ class Camera
                 return new(readResult.Error);
             }
 
-            focusStatus = readResult.Value;
+            if (readResult.Value == 0x10)
-
-            if (focusStatus == 0x10)
             {
                 logger.Info("对焦已完成 (0x3029 = 0x10)");
                 break;
             }
 
-            if (iteration-- == 0)
+            if (iteration == 1)
             {
-                logger.Error($"自动对焦超时，状态: 0x{focusStatus:X2}");
+                logger.Error($"自动对焦超时，状态: 0x{readResult.Value:X2}");
-                return new(new Exception($"自动对焦超时，状态: 0x{focusStatus:X2}"));
+                return new(new Exception($"自动对焦超时，状态: 0x{readResult.Value:X2}"));
             }
 
             await Task.Delay(10);
         }
-        while (focusStatus != 0x10);
 
         // 步骤3: 写寄存器 0x3022 为 0x06，暂停对焦过程，使镜头将保持在此对焦位置
         var pauseResult = await ConfigureRegisters([[0x3022, 0x06]]);
@@ -1351,7 +1374,7 @@ class Camera
             return pauseResult;
         }
 
-        logger.Info($"自动对焦完成并暂停，镜头保持在对焦位置，剩余迭代次数: {iteration}");
+        logger.Info("自动对焦完成并暂停，镜头保持在对焦位置");
         return true;
     }