From 23deeb3a00678b003c53ac8a30617e2a2099f6b7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 24 Aug 2023 10:01:58 +0900 Subject: [PATCH] AP_Mount: Siyi supports set_lens --- libraries/AP_Mount/AP_Mount_Siyi.cpp | 44 ++++++++++++++++++++++++++++ libraries/AP_Mount/AP_Mount_Siyi.h | 21 +++++++++++-- 2 files changed, 63 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index dee0951fc1..875cda5fd8 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/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 { diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 9aa5ebbdac..5e0279586d 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/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();