1 files changed, 73 insertions, 3 deletions

diff --git a/src/camera/veyeimx287m.cpp b/src/camera/veyeimx287m.cpp
index 1fd57f6..1d64395 100644
--- a/src/camera/veyeimx287m.cpp
+++ b/src/camera/veyeimx287m.cpp
@@ -14,8 +14,10 @@
 #include <QElapsedTimer>
 
 // orpheus
+#include "calibration.h"
 #include "camera/veye_i2c.h"
 #include "constants.h"
+#include "pixels.h"
 #include "protocols/httpserver.h"
 #include "veyeimx287m_types.h"
 
@@ -238,6 +240,12 @@ VeyeIMX287m::~VeyeIMX287m()
     std::cout << "camera closed" << std::endl;
 }
 
+void VeyeIMX287m::onMoveFinished()
+{
+    qDebug() << __func__;
+    m_isMoving = false;
+}
+
 std::vector<std::shared_ptr<ICamera> > VeyeIMX287m::search()
 {
     // FIXME: use saved params, get rid of hardcode
@@ -248,11 +256,11 @@ std::vector<std::shared_ptr<ICamera> > VeyeIMX287m::search()
     if (!cam->init())
         return {};
 
-    // if (!cam->set_autoExposure(false))
-    if (!cam->set_autoExposure(true))
+    if (!cam->set_autoExposure(false))
+        // if (!cam->set_autoExposure(true))
         return {};
 
-    if (!cam->set_exposureTime(std::chrono::microseconds(30)))
+    if (!cam->set_exposureTime(std::chrono::microseconds(18)))
         return {};
 
     if (!cam->set_autoGain(false))
@@ -292,6 +300,22 @@ bool VeyeIMX287m::startStream()
     return true;
 }
 
+void VeyeIMX287m::startCalibration(std::shared_ptr<IStand> stand, double zRangeMm)
+{
+    m_calibrationPixels.clear();
+    m_zRangeMm = zRangeMm;
+    m_stand = stand;
+
+    // auto standObj = dynamic_cast<Esp32Stand *>(stand.get());
+
+    // if (standObj) {
+    //     standObj->moveToThread(QThread::currentThread());
+    // }
+
+    m_stand->resetPosSteps();
+    m_isCalibrating = true;
+}
+
 bool VeyeIMX287m::init()
 {
     if (!openCam())
@@ -361,6 +385,7 @@ std::optional<bool> VeyeIMX287m::get_autoGain()
 bool VeyeIMX287m::set_exposureTime(const std::chrono::microseconds us)
 {
     using namespace veye::imx287m;
+    std::cout << __func__ << ": " << us << std::endl;
     return m_i2c->write(static_cast<uint16_t>(Register::ME_Time), us.count());
 }
 
@@ -652,6 +677,8 @@ void VeyeIMX287m::rotateFrameLoop(std::stop_token stopToken)
 
 void VeyeIMX287m::calcPixelsLoop(std::stop_token stopToken)
 {
+    uint8_t collectedInPos{0};
+
     while (!stopToken.stop_requested()) {
         // const auto idx = m_sync.rotSemQueue.dequeue();
         const auto image = m_sync.rotSemQueue.dequeue();
@@ -661,6 +688,49 @@ void VeyeIMX287m::calcPixelsLoop(std::stop_token stopToken)
             std::lock_guard l{m_lastImageMtx};
             m_lastProcessedImage = image;
         }
+
+        if (m_isCalibrating) {
+            // if (m_ignoreFrames) {
+            if (m_isMoving) {
+                continue;
+            } else {
+                pixels->counters.encoderPosition = m_stand->posSteps();
+                constexpr uint8_t neededCount{8};
+
+                if (collectedInPos < neededCount) {
+                    m_calibrationPixels.push_back(pixels);
+                    ++collectedInPos;
+                } else {
+                    // if (m_stand->posMm() < m_zRangeMm) {
+                    if (m_stand->posMm() < debugZRange) {
+                        // TODO: dump pixels
+                        // dumpCalibrationPixels(m_calibrationPixels);
+                        qDebug() << "moveMm:" << m_zRangeMm << ::discretesInRage
+                                 << "currPos:" << m_stand->posMm() << "zRange:" << m_zRangeMm
+                                 << m_zRangeMm / ::discretesInRage
+                                 << "pos steps:" << pixels->counters.encoderPosition;
+                        m_isMoving = true;
+
+                        collectedInPos = 0;
+                        emit moveMm(m_zRangeMm / ::discretesInRage);
+                        // m_stand->moveMm(m_zRangeMm / ::discretesInRage);
+                    } else {
+                        qDebug() << "move to home:" << -m_zRangeMm;
+                        qDebug() << "================================";
+                        m_isMoving = true;
+                        // emit moveMm(-m_zRangeMm);
+                        // emit moveMm(debugZRange);
+                        // m_stand->moveMm(-m_zRangeMm);
+                        m_isCalibrating = false;
+                        // m_stand.reset();
+                        const auto tmp = m_calibrationPixels;
+                        // dumpCalibrationPixels(m_calibrationPixels);
+                        dumpCalibrationPixels(tmp, DumpFormat::Binary);
+                        m_calibrationPixels.clear();
+                    }
+                }
+            }
+        }
     }
 }