diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake
index d2e84558e..b516ed51b 100644
--- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake
+++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake
@@ -4,4 +4,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot")
 
 # "version if applicable"
 # set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+93f7b75a885aa32f44c5e9f53b74470c49d2b1af")
-set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+670797e3b8cbc185c7d457e24382495200486573")
+set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+5c6b7bd7d23f9bd8cfba3a0feb2e10ab6101e48a")
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 7512ab5f6..074464ff2 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -405,6 +405,10 @@ dai_set_test_labels(image_manip_v2_node_test ondevice rvc2_all rvc4 ci)
 dai_add_test(benchmark_test src/ondevice_tests/pipeline/node/benchmark_test.cpp)
 dai_set_test_labels(benchmark_test ondevice rvc2_all rvc4 ci)
 
+# Manual focus test
+dai_add_test(manual_focus_test src/ondevice_tests/manual_focus_test.cpp)
+dai_set_test_labels(manual_focus_test rvc2_all rvc4 ci)
+
 # IMU tests
 dai_add_test(imu_test src/ondevice_tests/pipeline/node/imu_test.cpp)
 dai_set_test_labels(imu_test ondevice rvc4 ci) # Many RVC2 devices do not have an IMU which supports the whole test suite
diff --git a/tests/src/ondevice_tests/manual_focus_test.cpp b/tests/src/ondevice_tests/manual_focus_test.cpp
new file mode 100644
index 000000000..d528f43c6
--- /dev/null
+++ b/tests/src/ondevice_tests/manual_focus_test.cpp
@@ -0,0 +1,29 @@
+#include <catch2/catch_all.hpp>
+
+#include "depthai/depthai.hpp"
+
+TEST_CASE("Test manual focus") {
+    dai::Pipeline pipeline;
+    auto fps = 30.0f;
+    auto camRgb = pipeline.create<dai::node::Camera>()->build();
+    auto* output = camRgb->requestOutput(std::make_pair(640, 480), std::nullopt, dai::ImgResizeMode::CROP, fps);
+    auto outputQueue = output->createOutputQueue();
+    auto controlQueue = camRgb->inputControl.createInputQueue();
+    pipeline.start();
+    for(auto requestedLensPos : {0.5f, 0.8f, 1.0f, 0.733f, 0.444f, 0.2101f, 0.0f}) {
+        auto ctrl = std::make_shared<dai::CameraControl>();
+        ctrl->setManualFocusRaw(requestedLensPos);
+        std::cout << "Setting lens position to " << requestedLensPos << "\n" << std::flush;
+        controlQueue->send(ctrl);
+        auto lastLensPosition = -1.0f;
+        for(int idx = 0; idx < static_cast<int>(fps); ++idx) {
+            auto imgFrame = outputQueue->get<dai::ImgFrame>();
+            lastLensPosition = imgFrame->getLensPositionRaw();
+        }
+        std::cout << "Lens position after 1s was: " << lastLensPosition << "\n" << std::flush;
+        auto lensPositionDiff = std::abs(lastLensPosition - requestedLensPos);
+        std::cout << "Diff was: " << lensPositionDiff << "\n" << std::flush;
+        REQUIRE(lensPositionDiff < 0.001);
+    }
+    pipeline.stop();
+}