refactoring

3 files changed, 120 insertions, 19 deletions

diff --git a/src/camera/icamera.h b/src/camera/icamera.h
index d6a2e9a..94408b6 100644
--- a/src/camera/icamera.h
+++ b/src/camera/icamera.h
@@ -1,24 +1,26 @@
 #pragma once
 
-#ifdef emit
-#define emit_backup emit
-#undef emit
-#endif
+#include <QObject>
 
-#ifdef slots
-#define slots_backup slots
-#undef slots
-#endif
+// #ifdef emit
+// #define emit_backup emit
+// #undef emit
+// #endif
 
-#include <libcamera/base/signal.h>
+// #ifdef slots
+// #define slots_backup slots
+// #undef slots
+// #endif
 
-#ifdef emit_backup
-#define emit emit_backup
-#endif
+// // #include <libcamera/base/signal.h>
 
-#ifdef slots_backup
-#define slots slots_backup
-#endif
+// #ifdef emit_backup
+// #define emit emit_backup
+// #endif
+
+// #ifdef slots_backup
+// #define slots slots_backup
+// #endif
 
 // cpp
 #include <chrono>
@@ -26,12 +28,24 @@
 // orpheus
 #include "image.h"
 
-class ICamera
+class IStand;
+
+class ICamera : public QObject
 {
+    Q_OBJECT
+
 public:
     virtual ~ICamera() = default;
 
+signals:
+    void moveMm(double Mm);
+
+public slots:
+    virtual void onMoveFinished() = 0;
+
 public:
+    virtual void startCalibration(std::shared_ptr<IStand> stand, double zRangeMm) = 0;
+
     [[nodiscard]] virtual bool set_autoExposure(const bool enable) = 0;
 
     /*!
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();
+                    }
+                }
+            }
+        }
     }
 }
 
diff --git a/src/camera/veyeimx287m.h b/src/camera/veyeimx287m.h
index b624fd2..bdc57d9 100644
--- a/src/camera/veyeimx287m.h
+++ b/src/camera/veyeimx287m.h
@@ -15,6 +15,8 @@
 #include "constants.h"
 #include "icamera.h"
 #include "image.h"
+// TODO: remove this include
+#include "printerclient.h"
 #include "utils/sem_queue.h"
 
 namespace veye {
@@ -24,6 +26,7 @@ class i2c;
 } // namespace veye
 
 class HttpServer;
+class IStand;
 
 /*
  * start calibration
@@ -58,12 +61,17 @@ public:
     VeyeIMX287m();
     ~VeyeIMX287m() override;
 
+public slots:
+    void onMoveFinished() override;
+
 public:
     static std::vector<std::shared_ptr<ICamera>> search();
 
 public:
     bool startStream() override;
 
+    void startCalibration(std::shared_ptr<IStand> stand, double zRangeMm) override;
+
     bool dequeueImageBuffer(size_t &image);
     std::shared_ptr<Image> getImage() override;
 
@@ -157,4 +165,13 @@ private:
 
     std::shared_ptr<veye::imx287m::i2c> m_i2c;
     std::shared_ptr<HttpServer> m_httpServer;
+
+    // calibration
+    // TODO: re-organize this logic
+    bool m_isCalibrating{false};
+    bool m_ignoreFrames{false};
+    std::vector<std::shared_ptr<Pixels>> m_calibrationPixels;
+    std::shared_ptr<IStand> m_stand;
+    double m_zRangeMm{std::numeric_limits<double>::quiet_NaN()};
+    bool m_isMoving{false};
 };