From 5bcbf56a47f7d9189a9eac9cc90602d890b47717 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 21 Aug 2024 20:11:08 +0900 Subject: [PATCH] AP_Mount: Siyi ZT6 set lens fix ZT6 has RGB and Thermal image sensors only --- libraries/AP_Mount/AP_Mount_Siyi.cpp | 61 ++++++++++++++++++++-------- 1 file changed, 43 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 0a5c9fe166d4da..fcaa4581a15c2b 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -910,30 +910,55 @@ SetFocusResult AP_Mount_Siyi::set_focus(FocusType focus_type, float focus_value) // 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; + CameraImageType selected_lens; + + switch (_hardware_model) { + + case HardwareModel::ZT30: { + // maps lens to siyi camera image type so that lens of 0, 1, 2 are more useful + static const CameraImageType cam_image_type_table[] { + CameraImageType::MAIN_ZOOM_SUB_THERMAL, // 3 + CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL, // 5 + CameraImageType::MAIN_THERMAL_SUB_ZOOM, // 7 + CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE, // 0 + CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM, // 1 + CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL, // 2 + CameraImageType::MAIN_ZOOM_SUB_WIDEANGLE, // 4 + CameraImageType::MAIN_WIDEANGLE_SUB_ZOOM, // 6 + CameraImageType::MAIN_THERMAL_SUB_WIDEANGLE, // 8 + }; + + // sanity check lens values + if (lens >= ARRAY_SIZE(cam_image_type_table)) { + return false; + } + selected_lens = cam_image_type_table[lens]; + break; } - // maps lens to siyi camera image type so that lens of 0, 1, 2 are more useful - static const CameraImageType cam_image_type_table[] { - CameraImageType::MAIN_ZOOM_SUB_THERMAL, // 3 - CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL, // 5 - CameraImageType::MAIN_THERMAL_SUB_ZOOM, // 7 - CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE, // 0 - CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM, // 1 - CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL, // 2 - CameraImageType::MAIN_ZOOM_SUB_WIDEANGLE, // 4 - CameraImageType::MAIN_WIDEANGLE_SUB_ZOOM, // 6 - CameraImageType::MAIN_THERMAL_SUB_WIDEANGLE, // 8 - }; - - if (lens >= ARRAY_SIZE(cam_image_type_table)) { + case HardwareModel::ZT6: { + // maps lens to siyi camera image type so that lens of 0, 1, 2 are more useful + static const CameraImageType cam_image_type_table[] { + CameraImageType::MAIN_ZOOM_SUB_THERMAL, // 3 + CameraImageType::MAIN_THERMAL_SUB_ZOOM, // 7 + CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE, // 0 + }; + + // sanity check lens values + if (lens >= ARRAY_SIZE(cam_image_type_table)) { + return false; + } + selected_lens = cam_image_type_table[lens]; + break; + } + + default: + // set lens not supported on this camera return false; } // send desired image type to camera - return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type_table[lens]); + return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)selected_lens); } // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type