From ba29193cfe8e9871d9de1a50bf63b8c54f311020 Mon Sep 17 00:00:00 2001
From: Filip Jeretina <59307111+zrezke@users.noreply.github.com>
Date: Tue, 11 Jun 2024 14:28:40 +0200
Subject: [PATCH] [RVC3] Added ability to set the lens position via a float, to
 enable a more precies movement. (#965)

* Added ability to set manual focus with a float. (gets you 10bit precision)

* Added lensPositionRaw to imgFrame

* Populate lens position on image frames

* Fix getLensPositionRaw in bindings

* Bump core

* Bump core

* Fix compilation

* clangformat

* Bump to develop
---
 depthai-core                                    |  2 +-
 src/pipeline/datatype/CameraControlBindings.cpp |  3 +++
 src/pipeline/datatype/ImgFrameBindings.cpp      |  1 +
 utilities/cam_test.py                           | 10 +++++-----
 4 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/depthai-core b/depthai-core
index 2eff188a7..280084f11 160000
--- a/depthai-core
+++ b/depthai-core
@@ -1 +1 @@
-Subproject commit 2eff188a702881bf21a81ad375c3d9e90d2c974c
+Subproject commit 280084f112db50c8391373476383b67664b7fac3
diff --git a/src/pipeline/datatype/CameraControlBindings.cpp b/src/pipeline/datatype/CameraControlBindings.cpp
index f6337bb6b..a5db2e453 100644
--- a/src/pipeline/datatype/CameraControlBindings.cpp
+++ b/src/pipeline/datatype/CameraControlBindings.cpp
@@ -158,6 +158,7 @@ std::vector<const char *> camCtrlAttr;
         .def_readwrite("cmdMask", &RawCameraControl::cmdMask)
         .def_readwrite("autoFocusMode", &RawCameraControl::autoFocusMode)
         .def_readwrite("lensPosition", &RawCameraControl::lensPosition)
+        .def_readwrite("lensPositionRaw", &RawCameraControl::lensPositionRaw)
         .def_readwrite("expManual", &RawCameraControl::expManual)
         .def_readwrite("afRegion", &RawCameraControl::afRegion)
         .def_readwrite("awbMode", &RawCameraControl::awbMode)
@@ -196,6 +197,7 @@ std::vector<const char *> camCtrlAttr;
         .def("setAutoFocusLensRange", &CameraControl::setAutoFocusLensRange, py::arg("infinityPosition"), py::arg("macroPosition"), DOC(dai, CameraControl, setAutoFocusLensRange))
         .def("setAutoFocusRegion", &CameraControl::setAutoFocusRegion, py::arg("startX"), py::arg("startY"), py::arg("width"), py::arg("height"), DOC(dai, CameraControl, setAutoFocusRegion))
         .def("setManualFocus", &CameraControl::setManualFocus, py::arg("lensPosition"), DOC(dai, CameraControl, setManualFocus))
+        .def("setManualFocusRaw", &CameraControl::setManualFocusRaw, py::arg("lensPosition"), DOC(dai, CameraControl, setManualFocusRaw))
         .def("setAutoExposureEnable", &CameraControl::setAutoExposureEnable, DOC(dai, CameraControl, setAutoExposureEnable))
         .def("setAutoExposureLock", &CameraControl::setAutoExposureLock, py::arg("lock"), DOC(dai, CameraControl, setAutoExposureLock))
         .def("setAutoExposureRegion", &CameraControl::setAutoExposureRegion, py::arg("startX"), py::arg("startY"), py::arg("width"), py::arg("height"), DOC(dai, CameraControl, setAutoExposureRegion))
@@ -225,6 +227,7 @@ std::vector<const char *> camCtrlAttr;
         .def("getExposureTime", &CameraControl::getExposureTime, DOC(dai, CameraControl, getExposureTime))
         .def("getSensitivity", &CameraControl::getSensitivity, DOC(dai, CameraControl, getSensitivity))
         .def("getLensPosition", &CameraControl::getLensPosition, DOC(dai, CameraControl, getLensPosition))
+        .def("getLensPositionRaw", &CameraControl::getLensPositionRaw, DOC(dai, CameraControl, getLensPositionRaw))
         .def("get",         &CameraControl::get, DOC(dai, CameraControl, get))
         ;
     // Add also enum attributes from RawCameraControl
diff --git a/src/pipeline/datatype/ImgFrameBindings.cpp b/src/pipeline/datatype/ImgFrameBindings.cpp
index 2db0a3267..6ed7c90a0 100644
--- a/src/pipeline/datatype/ImgFrameBindings.cpp
+++ b/src/pipeline/datatype/ImgFrameBindings.cpp
@@ -137,6 +137,7 @@ void bind_imgframe(pybind11::module& m, void* pCallstack){
         .def("getSensitivity", &ImgFrame::getSensitivity, DOC(dai, ImgFrame, getSensitivity))
         .def("getColorTemperature", &ImgFrame::getColorTemperature, DOC(dai, ImgFrame, getColorTemperature))
         .def("getLensPosition", &ImgFrame::getLensPosition, DOC(dai, ImgFrame, getLensPosition))
+        .def("getLensPositionRaw", &ImgFrame::getLensPositionRaw, DOC(dai, ImgFrame, getLensPositionRaw))
         .def("get", &ImgFrame::get, DOC(dai, ImgFrame, get))
 
         // OpenCV Support section
diff --git a/utilities/cam_test.py b/utilities/cam_test.py
index 490c35afd..7abd58822 100755
--- a/utilities/cam_test.py
+++ b/utilities/cam_test.py
@@ -322,16 +322,16 @@ def get(self):
     # Manual exposure/focus set step
     EXP_STEP = 500  # us
     ISO_STEP = 50
-    LENS_STEP = 3
+    LENS_STEP = 1 / 1024.0
     DOT_STEP = 100
     FLOOD_STEP = 100
     DOT_MAX = 1200
     FLOOD_MAX = 1500
 
     # Defaults and limits for manual focus/exposure controls
-    lensPos = 150
-    lensMin = 0
-    lensMax = 255
+    lensPos = 0.5
+    lensMin = 0.0
+    lensMax = 1.0
 
     expTime = 20000
     expMin = 1
@@ -469,7 +469,7 @@ def get(self):
             lensPos = clamp(lensPos, lensMin, lensMax)
             print("Setting manual focus, lens position: ", lensPos)
             ctrl = dai.CameraControl()
-            ctrl.setManualFocus(lensPos)
+            ctrl.setManualFocusRaw(lensPos)
             controlQueue.send(ctrl)
         elif key in [ord('i'), ord('o'), ord('k'), ord('l')]:
             if key == ord('i'): expTime -= EXP_STEP