AP_Mount: Siyi supports set_lens

libraries/AP_Mount/AP_Mount_Siyi.cpp

@@ -797,6 +797,50 @@ SetFocusResult AP_Mount_Siyi::set_focus(FocusType focus_type, float focus_value)
     return SetFocusResult::INVALID_PARAMETERS;
 }
 
+// set camera lens as a value from 0 to 8
+bool AP_Mount_Siyi::set_lens(uint8_t lens)
+{
+    // only supported on ZT30.  sanity check lens values
+    if ((_hardware_model != HardwareModel::ZT30) || (lens > 8)) {
+        return false;
+    }
+
+    // maps lens to siyi camera image type so that lens of 0, 1, 2 are more useful
+    CameraImageType cam_image_type = CameraImageType::MAIN_ZOOM_SUB_THERMAL;
+    switch (lens) {
+        case 0:
+            cam_image_type = CameraImageType::MAIN_ZOOM_SUB_THERMAL; // 3
+            break;
+        case 1:
+            cam_image_type = CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL; // 5
+            break;
+        case 2:
+            cam_image_type = CameraImageType::MAIN_THERMAL_SUB_ZOOM; // 7
+            break;
+        case 3:
+            cam_image_type = CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE; // 0
+            break;
+        case 4:
+            cam_image_type = CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM; // 1
+            break;
+        case 5:
+            cam_image_type = CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL; // 2
+            break;
+        case 6:
+            cam_image_type = CameraImageType::MAIN_ZOOM_SUB_WIDEANGLE; // 4
+            break;
+        case 7:
+            cam_image_type = CameraImageType::MAIN_WIDEANGLE_SUB_ZOOM; // 6
+            break;
+        case 8:
+            cam_image_type = CameraImageType::MAIN_THERMAL_SUB_WIDEANGLE; // 8
+            break;
+    }
+
+    // send desired image type to camera
+    return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type);
+}
+
 // send camera information message to GCS
 void AP_Mount_Siyi::send_camera_information(mavlink_channel_t chan) const
 {

libraries/AP_Mount/AP_Mount_Siyi.h

@@ -72,6 +72,9 @@ public:
     // focus in = -1, focus hold = 0, focus out = 1
     SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
 
+    // set camera lens as a value from 0 to 8.  ZT30 only
+    bool set_lens(uint8_t lens) override;
+
     // send camera information message to GCS
     void send_camera_information(mavlink_channel_t chan) const override;
 
@@ -98,7 +101,8 @@ private:
         FUNCTION_FEEDBACK_INFO = 0x0B,
         PHOTO = 0x0C,
         ACQUIRE_GIMBAL_ATTITUDE = 0x0D,
-        ABSOLUTE_ZOOM = 0x0F
+        ABSOLUTE_ZOOM = 0x0F,
+        SET_CAMERA_IMAGE_TYPE = 0x11,
     };
 
     // Function Feedback Info packet info_type values
@@ -145,13 +149,26 @@ private:
         ZT30
     } _hardware_model;
 
-    // gimbal mounting method/direction
+    // lens value
     enum class GimbalMountingDirection : uint8_t {
         UNDEFINED = 0,
         NORMAL = 1,
         UPSIDE_DOWN = 2,
     } _gimbal_mounting_dir;
 
+    // camera image types (aka lens)
+    enum class CameraImageType : uint8_t {
+        MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE = 0,
+        MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM = 1,
+        MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL = 2,
+        MAIN_ZOOM_SUB_THERMAL = 3,
+        MAIN_ZOOM_SUB_WIDEANGLE = 4,
+        MAIN_WIDEANGLE_SUB_THERMAL = 5,
+        MAIN_WIDEANGLE_SUB_ZOOM = 6,
+        MAIN_THERMAL_SUB_ZOOM = 7,
+        MAIN_THERMAL_SUB_WIDEANGLE = 8
+    };
+
     // reading incoming packets from gimbal and confirm they are of the correct format
     // results are held in the _parsed_msg structure
     void read_incoming_packets();