hdzero camera switch support manual mode

Author: ligenxxxx

src/i2c_device.c

@@ -197,12 +197,12 @@ void select_camera(uint8_t camera_id) {
                 hdzcs_set(0x00, g_camera_id);
             }
 
-            if (camera_last != camera_list[g_camera_id]) {
-                if (camera_list[g_camera_id] < CAMERA_TYPE_NUM) { // camera has inited
+            if (camera_last != camera_list[g_camera_id - 1]) {
+                if (camera_list[g_camera_id - 1] < CAMERA_TYPE_NUM) { // camera has inited
                     camera_reinit();
                 } else {
                     camera_init();
-                    camera_list[g_camera_id] = camera_type;
+                    camera_list[g_camera_id - 1] = camera_type;
                 }
                 camera_last = camera_type;
             }
@@ -244,5 +244,28 @@ void manual_select_camera(void) {
             uint8_t camera_id = ((command & 0x80) >> 7) + 1;
             select_camera(camera_id);
         }
+    } else if (g_camera_switch == SWITCH_TYPE_HDZCS) {
+        uint8_t command = hdzcs_get(0x01);
+        switch (command) {
+        case 0:
+            g_manual_camera_sel = 3; // manual analog camera
+            break;
+        case 1:
+            g_manual_camera_sel = 2; // manual mipi camera 2
+            break;
+        case 2:
+            g_manual_camera_sel = 1; // manual mipi camera 1
+            break;
+        case 3:
+            g_manual_camera_sel = 0;
+            break; // assign by FC
+        default:
+            g_manual_camera_sel = 0;
+            break;
+        }
+
+        if (g_manual_camera_sel) {
+            select_camera(g_manual_camera_sel);
+        }
     }
 }

src/i2c_device.h

@@ -36,6 +36,7 @@ void Init_TC3587(uint8_t fmt);
 
 uint8_t pi4io_get(uint8_t reg);
 void pi4io_set(uint8_t reg, uint8_t val);
+uint8_t hdzcs_get(uint8_t reg);
 void camera_switch_init(void);
 void select_camera(uint8_t camera_id);
 void manual_select_camera(void);