implement printer controls; implement calibration data collection

1 files changed, 310 insertions, 177 deletions

diff --git a/main.cpp b/main.cpp
index 1a121a3..33f94ee 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,6 +1,16 @@
+
+#include "httpservice.h"
+#include "genetic_algos.h"
+#include "imagealgos.h"
+#include "LibCamera.h"
+#include "pigpio.h"
+#include "printerclient.h"
+#include "rotaryencoder.h"
+
 #define QT_NO_KEYWORDS
 #include <QCoreApplication>
 #include <QDebug>
+#include <QDir>
 #include <QFile>
 #include <QHttpServer>
 #include <QJsonArray>
@@ -19,13 +29,6 @@
 #include <string.h>
 #include <thread>
 
-#include "httpservice.h"
-#include "genetic_algos.h"
-#include "imagealgos.h"
-#include "LibCamera.h"
-#include "pigpio.h"
-#include "rotaryencoder.h"
-
 #define try_apply_config() \
 if(!applyConfig(config)) \
     { \
@@ -39,13 +42,37 @@ if(!applyConfig(config)) \
 const QString exposureTimeKey = "exposureTime";
 const QString laserLevelKey = "laserLevel";
 
-extern volatile int64_t positionSteps;
+enum ScanningModeFlags : uint8_t {
+    None = 0,
+    Calibration
+};
+
+// ScanningModeFlags operator|(ScanningModeFlags lhs, ScanningModeFlags rhs)
+// {
+//     using T = std::underlying_type<ScanningModeFlags>::type;
+//     return static_cast<ScanningModeFlags>(static_cast<T>(lhs) | static_cast<T>(rhs));
+// }
+
+ScanningModeFlags scanningModeFlags { ScanningModeFlags::None };
+
+QElapsedTimer calibrationTimer;
+
+extern volatile int32_t positionSteps;
 
 struct requested_params_t {
-    int32_t exposureTime = { 200 };
-    int32_t laserLevel = { 7000 };
+    int32_t exposureTime = { 1000 };
+    int32_t laserLevel = { 3000 };
+    uint32_t stepsPerMm { 200 };
 } requested_params;
+
+namespace {
 Image img;
+Pixels pixels;
+std::vector<Pixels> calibrationPixels;
+QMutex calibrationPixelsMutex;
+}
+
+const QString dumpsRoot { QStringLiteral("/home/user/dumps") };
 
 using namespace std::chrono_literals;
 
@@ -58,6 +85,8 @@ ControlList lastControls;
 static bool applyConfig(const std::unique_ptr<CameraConfiguration> & config);
 static void onRequestCompleted(Request *completed_request);
 static void printControls();
+static void dumpCalibrationPixels();
+static std::vector<Pixels> openDump(const QString dumpPath = "");
 
 int main(int argc, char *argv[]) {
     QCoreApplication app(argc, argv);
@@ -65,47 +94,10 @@ int main(int argc, char *argv[]) {
     QElapsedTimer t;
     t.start();
 
-    // qDebug() << "msecs before serial:" << t.elapsed();
-
-    // QSerialPort *serialPort { new QSerialPort { "/dev/ttyUSB0", qApp } };
-
-    // if (!serialPort->setBaudRate(QSerialPort::Baud115200)) {
-    //     qDebug() << "serial port: cannot set baud rate:"
-    //              << serialPort->errorString();
-
-    //     return EXIT_FAILURE;
-    // }
-
-    // if (!serialPort->open(QFile::ReadWrite)) {
-    //     qDebug() << "cannot open serial port:" << serialPort->errorString();
-
-    //     return EXIT_FAILURE;
-    // }
-
-    // qDebug() << "serial port baud rate:" << serialPort->baudRate();
-
-    // qDebug() << "serial port data bits:" << serialPort->dataBits();
-    // qDebug() << "serial port parity:" << serialPort->parity();
-    // qDebug() << "serial port stop bits:" << serialPort->stopBits();
-
-    // QObject::connect(serialPort, &QSerialPort::readyRead,
-    //                  serialPort, [=](){
-    //                      qDebug() << "serialPort: " << serialPort->readAll();
-    //                  });
-
-    // QObject::connect(serialPort, &QSerialPort::errorOccurred,
-    //                  serialPort, [=](){
-    //                      qWarning() << "serial port error:" << serialPort->errorString();
-    //                  });
-
-    // qDebug() << "msecs before write and flush:" << t.elapsed();
-
-    // serialPort->write(QByteArray { "G91\n" });
-    // serialPort->flush();
-
     qDebug() << "msecs before encoder:" << t.elapsed();
 
-    rotaryencoder encoder;
+    RotaryEncoder encoder;
+    PrinterClient printerClient;
 
     qDebug() << "msecs before camera:" << t.elapsed();
     // FIXME: don't use one var for everything
@@ -115,6 +107,8 @@ int main(int argc, char *argv[]) {
 
     const auto cameras = cm->cameras();
 
+    openDump();
+
     if (cameras.empty())
     {   
         std::cout << "No cameras were identified on the system." << std::endl;
@@ -270,16 +264,16 @@ int main(int argc, char *argv[]) {
                 std::make_pair(memory, plane.length);
         }
 
-        // size_t desiredFPS = 144;
+        size_t desiredFPS = 144;
 
-        // std::int64_t lowerUS = 1 * 1000 * 1000 / desiredFPS;
-        // std::int64_t higherUS = lowerUS;
-        // std::int64_t value_pair[2] = { higherUS / 2, higherUS };
+        std::int64_t lowerUS = 1 * 1000 * 1000 / desiredFPS;
+        std::int64_t higherUS = lowerUS;
+        std::int64_t value_pair[2] = { higherUS / 2, higherUS };
         request->controls().set(libcamera::controls::AnalogueGain, 1.0);
         request->controls().set(libcamera::controls::ExposureTime, 100);
-        // request->controls().set(
-        // libcamera::controls::FrameDurationLimits,
-        // libcamera::Span<const std::int64_t, 2>(value_pair));
+        request->controls().set(
+            libcamera::controls::FrameDurationLimits,
+            libcamera::Span<const std::int64_t, 2>(value_pair));
 
         requests.push_back(std::move(request));
     }
@@ -333,14 +327,15 @@ int main(int argc, char *argv[]) {
         QJsonArray pixels;
 
         for (size_t i = 0; i < img_width; ++i) {
-            pixels << img_height - img.pixels[i];
+            // pixels << img_height - img.pixels[i];
+            pixels << img_height - ::pixels.pixels[i];
         }
 
         QJsonObject json;
         json["pixels"] = pixels;
         json["encoderPosition"] = qint64 { encoder.position() };
-        json["measurementCounter"] = qint64 { img.measurementCounter };
-        json["timestampUs"] = qint64 { img.timestampNs / 1000. } ;
+        json["measurementCounter"] = qint64 { img.counters.measurementCounter };
+        json["timestampUs"] = qint64(img.counters.timestampUs);
 
         return QHttpServerResponse(QJsonDocument(json).toJson());
     });
@@ -358,6 +353,48 @@ int main(int argc, char *argv[]) {
         return QHttpServerResponse::StatusCode::Ok;
     });
 
+    qHttpServer.route("/v1/commands/startCalibration", [&](const QHttpServerRequest &request) -> QHttpServerResponse {
+        if (request.method() != QHttpServerRequest::Method::Post) {
+            return QHttpServerResponse::StatusCode::NotFound;
+        }
+
+        qDebug() << "start calibration";
+
+        // TODO: use flags
+        scanningModeFlags = ScanningModeFlags::Calibration;
+        calibrationTimer.start();
+
+        return QHttpServerResponse::StatusCode::Ok;
+    });
+
+    qHttpServer.route("/v1/commands/gCode", [&](const QHttpServerRequest &request) -> QHttpServerResponse {
+        if (request.method() != QHttpServerRequest::Method::Post) {
+            return QHttpServerResponse::StatusCode::NotFound;
+        }
+
+        const auto command = request.body();
+
+        qDebug() << "send gCode:" << command;
+
+        printerClient.sendCommand(command);
+
+        return QHttpServerResponse::StatusCode::Ok;
+    });
+
+    qHttpServer.route("/v1/commands/startCalibration", [&](const QHttpServerRequest &request) -> QHttpServerResponse {
+        if (request.method() != QHttpServerRequest::Method::Post) {
+            return QHttpServerResponse::StatusCode::NotFound;
+        }
+
+        const auto command = request.body();
+
+        qDebug() << "send gCode:" << command;
+
+        printerClient.sendCommand(command);
+
+        return QHttpServerResponse::StatusCode::Ok;
+    });
+
     qHttpServer.route("/v1/sensor/params", [&](const QHttpServerRequest &request) -> QHttpServerResponse {
 
         switch (request.method()) {
@@ -621,77 +658,65 @@ void onRequestCompleted(Request *completed_request)
             size_t size = std::min(metaplane.bytesused, plane.length);
             void * data = mappedBuffers_[plane.fd.get()].first;
 
-            // std::cout << metaplane.bytesused << "/" << plane.length;
-
-            // std::cout << " stride " << stride;
-            // std::cout << " planes count: " << buffer->planes().size() << " ";
-            // std::cout << std::endl;
-
-            // if (metadata.sequence == 20)
-            {
-                // FIXME: remove hardcode
-                img.width = imageSize.width;
-                img.height = imageSize.height;
-                // img.data = data;
-                memcpy(img.data, data, size);
-                img.dataSize = size;
-                img.stride = stride;
-                img.pixelFormat = pixelFormat;
-                img.measurementCounter = metadata.sequence;
-                img.timestampNs = metadata.timestamp;
-
-                // uint16_t unpacked[img.width * img.height] = { 0 };
-                // unpack_16bit((uint8_t*)img.data, img, (uint16_t*)&unpacked);
-                // img.data = unpacked;
-                // img.dataSize = img.width * img.height * sizeof(uint16_t);
-                rotate(img);
-                process_columns(img);
 
-                // static bool done = false;
-                // mark pixels and max region
-                // for (size_t i = 0; i < img_width; ++i)
-                // {
-                //     // std::cout << "\t" << img.pixels[i] << std::endl;
-                //     // uint
-                //     // const auto & p = img.pixels[i];
-                //     // const auto int_p = int(p);
-                //     // const auto fract = p - int_p;
+            // FIXME: remove hardcode
+            img.width = imageSize.width;
+            img.height = imageSize.height;
+            // img.data = data;
+            memcpy(img.data, data, size);
+            img.dataSize = size;
+            img.stride = stride;
+            img.pixelFormat = pixelFormat;
+            img.counters.measurementCounter = metadata.sequence;
+            img.counters.timestampUs = metadata.timestamp / 1000;
+            img.counters.encoderPosition = RotaryEncoder::instance()->position();
+            // qDebug() << "pos:" << img.counters.encoderPosition;
 
+            // uint16_t unpacked[img.width * img.height] = { 0 };
+            // unpack_16bit((uint8_t*)img.data, img, (uint16_t*)&unpacked);
+            // img.data = unpacked;
+            // img.dataSize = img.width * img.height * sizeof(uint16_t);
+            rotate(img);
+            Pixels pixels = process_columns(img);
+            ::pixels = pixels;
 
-                //     // img.data[int_p][i] = 256 * 256 * fract;
-                //     // img.data[int_p + 1][i] = 256 * 256 * (1.0 - fract);
+            // qDebug() << "calibration mode" << scanningModeFlags;
 
+            if (scanningModeFlags == ScanningModeFlags::Calibration) {
+                constexpr int32_t hardcodedZRangeMm { 175 };
+                const int32_t maxEncoderPosition = hardcodedZRangeMm * requested_params.stepsPerMm;
+                // qDebug() << "calibration max range" << maxEncoderPosition;
+                // qDebug() << "calibration encoder pos" << pixels.counters.encoderPosition;
+                if (pixels.counters.encoderPosition >= 0 &&
+                    pixels.counters.encoderPosition <= maxEncoderPosition)
+                {
+                    qDebug() << "calibration save at pos:" << pixels.counters.encoderPosition;
+                    QMutexLocker l(&calibrationPixelsMutex);
+                    ::calibrationPixels.push_back(std::move(pixels));
+                }
+                else if (pixels.counters.encoderPosition > maxEncoderPosition)
+                {
+                    // save to files
+                    QMutexLocker l(&calibrationPixelsMutex);
+                    qDebug() << "calibration pixels count:"
+                             << ::calibrationPixels.size();
+                    qDebug() << "calibration elapsed (s):"
+                             << calibrationTimer.elapsed() / 1000;
+                    // ::calibrationPixels.clear();
+                    // TODO: use flags
+                    // qDebug() << "stop calibration mode";
+                    scanningModeFlags = ScanningModeFlags::None;
 
-                //     // if (!done) {
-                //     //     std::cout << fract << " ";
-                //     // }
-
-                //     img.data[size_t(img.pixels[i])][i] = 0;
-                //     // crash is possible
-                //     img.data[size_t(img.pixels[i]) - patternSize / 2][i] = 0xffff;
-                //     img.data[size_t(img.pixels[i]) + patternSize / 2][i] = 0xffff;
-                // }
-                // done = true;
-
-                // // FILE * f = fopen("/tmp/R16.pgm", "w");
-                // FILE * f = fopen("/tmp/img.pgm", "w");
-                // // // FILE * f = fopen("/tmp/MONO_PISP_COMP1.pgm", "w");
-
-                // if (f == NULL)
-                // {
-                //     std::cerr << "cannot open output file: "
-                //               << strerror(errno)
-                //               << std::endl;
-                // }
-                // else
-                // {
-                //     // pgm_save(&img, f);
-                //     // pgm_save(&img, f);
-                //     fclose(f);
-                //     // std::cout << "file written" << std::endl;
-                // }
-                pgm_save(&img, nullptr);
+                    QFuture<void> dumpCalirationPixelsFuture =
+                        QtConcurrent::run(&dumpCalibrationPixels);
+                }
+                else
+                {
+                    // qDebug() << "calibration skip at pos:" << pixels.counters.encoderPosition;
+                }
             }
+
+            pgm_save(&img, nullptr);
         }
     }
 
@@ -723,63 +748,17 @@ void onRequestCompleted(Request *completed_request)
                   << std::endl;
     }
 
-    // for (const auto & [id, value] : metadata)
-    // {
-
-    // }
-
-    // metadata.set(controls::ExposureTime, 300);
-
-    // exp->set(*exp + 1);
-    // expTimeCtrl->second().set(*exp + 1);
-    // auto expTimeCtrlId= expTimeCtrl->id();
-
-
-    // properties.set(controls::ExposureTime, 1000);
-
-    // std::optional<uint32_t> expTimeOptional = properties.get(controls::ExposureTime);
-
-    // if (expTimeOptional.has_value())
-    // {
-    //     // uint32_t value = expTimeOptional.value();
-
-    //     auto frameDurationLimits = controls.find(&controls::FrameDurationLimits)->second;
-    //     auto min = frameDurationLimits.min().get<int64_t>();
-    //     auto max = frameDurationLimits.max().get<int64_t>();
-    //     // auto val = properties.find(controls::FrameDurationLimits)->value();//.second().min().get<int64_t>()
-    //     // auto second = val.second();
-    //     auto framerate = 1.0e6 / min;
-    //     auto rAG = request->controls().get<float>(libcamera::controls::AnalogueGain);
-    //     auto rET = request->controls().get<int32_t>(libcamera::controls::ExposureTime);
-    //     int32_t randET = rand() % 9000 + 1000;
-    //     request->controls().set(libcamera::controls::ExposureTime, 100);
-    //     std::cout << "exposure time (us): "
-    //               << properties.get(controls::ExposureTime).value()
-    //               << " frame duration limits (ns): " << min << "/" << max
-    //               << " framerate: " << framerate
-    //               << " " << rAG.value_or(321)
-    //               << " " << rET.value_or(321)
-    //               << std::endl;
-    //     // std::cout << "noise reduction mode: "
-    //     //           << properties.get(controls::AwbMode).value()
-    //     //           << std::endl;
-    //     // NoiseReductionMode
-    // }
-
-    // completed_request->reuse(Request::ReuseBuffers);
-    // camera->queueRequest(completed_request);
-
-    qDebug() << "set exposureTime to" << requested_params.exposureTime;
+    completed_request->reuse(Request::ReuseBuffers);
+    completed_request->controls().set(libcamera::controls::AeEnable, false);
+    completed_request->controls().set(libcamera::controls::draft
+                                      ::NoiseReductionMode,
+                                      libcamera::controls::draft
+                                      ::NoiseReductionModeEnum
+                                      ::NoiseReductionModeHighQuality);
     completed_request->controls().set(libcamera::controls::ExposureTime,
                                       requested_params.exposureTime);
 
 
-    completed_request->reuse(Request::ReuseBuffers);
-
-    completed_request->controls().set(libcamera::controls::AeEnable, false);
-    completed_request->controls().set(libcamera::controls::draft::NoiseReductionMode,
-                                      libcamera::controls::draft::NoiseReductionModeEnum::NoiseReductionModeHighQuality);
-
     camera->queueRequest(completed_request);
 
     ++performanceCounter;
@@ -869,3 +848,157 @@ static void printControls()
         std::cout << " ]\n";
     }
 }
+
+static void dumpCalibrationPixels()
+{
+    std::vector<Pixels> rawProfiles;
+
+    {
+        QMutexLocker l(&calibrationPixelsMutex);
+        std::swap(rawProfiles, ::calibrationPixels);
+    }
+
+    const QString dumpSubdir {
+        QDateTime::currentDateTime().toString("yyyy.MM.dd_hh.mm.ss")
+    };
+    const QDir dumpPath { dumpsRoot + "/" + dumpSubdir };
+
+    if (!dumpPath.mkdir(dumpPath.path()))
+    {
+        qWarning() << "cannot create dump dir: " << dumpPath.path();
+
+        return;
+    }
+
+    for (const auto& rawProfile : rawProfiles)
+    {
+        const auto filename = QLatin1String("raw_profile_meas_%1_enc_%2")
+        .arg(QString::number(rawProfile.counters.measurementCounter))
+            .arg(rawProfile.counters.encoderPosition);
+        const auto filepath = dumpPath.path() + "/" + filename;
+
+        QFile f { filepath };
+
+        if (!f.open(QFile::WriteOnly))
+        {
+            qWarning() << "cannot open dump dump file" << f.fileName();
+            qWarning() << "error is:" << f.errorString();
+
+            return;
+        }
+
+        QJsonObject jsonCounters {
+            { "timestampUs", qint64(rawProfile.counters.timestampUs) },
+            { "measurementCounter", qint64(rawProfile.counters.measurementCounter) },
+            { "encoderPosition", qint64(rawProfile.counters.encoderPosition) },
+        };
+
+        QJsonObject json;
+
+        json["counters"] = jsonCounters;
+
+        QJsonArray jsonPixels;
+
+        for (const auto& pixel : rawProfile.pixels)
+        {
+            jsonPixels << pixel;
+        }
+
+        json["pixels"] = jsonPixels;
+
+        if (!f.write(QJsonDocument(json).toJson()))
+        {
+            qWarning() << "cannot write file" << f.fileName();
+            qWarning() << "error is" << f.errorString();
+
+            return;
+        }
+
+        qDebug() << "file written: " << f.fileName();
+    }
+
+    qDebug() << "dump finished";
+}
+
+static std::vector<Pixels> openDump(const QString dumpPath)
+{
+    std::vector<Pixels> result;
+
+    QString dirToRead { dumpPath };
+
+    if (dirToRead.isEmpty())
+    {
+        qDebug() << "dumpPath not specified. looking into" << dumpsRoot;
+
+        QDir dumpsRootDir { dumpsRoot };
+
+        const auto filter = QDir::Dirs | QDir::NoDotAndDotDot | QDir::Readable;
+        // there is no battery in my rpi5 for now
+        const auto sort = QDir::Name;
+        const auto entries = dumpsRootDir.entryList(filter, sort);
+
+        if (entries.isEmpty())
+        {
+            qWarning() << "dumps root" << dumpsRoot << "contains no dumps. "
+                       << "specify existing dump path";
+
+            return {};
+        }
+
+        dirToRead = entries.last();
+    }
+
+    QDir dumpDir { dumpsRoot + "/" + dirToRead };
+
+    const auto filter = QDir::Files;
+    const auto sort = QDir::Name;
+
+    const auto filenames = dumpDir.entryList(filter, sort);
+
+    if (filenames.isEmpty())
+    {
+        qDebug() << "no filenames found in" << dumpDir.path();
+    }
+
+    for (const auto& filename : filenames)
+    {
+        qDebug() << "raw profile:" << filename;
+
+        QFile f { dumpDir.path() + "/" + filename };
+
+        if (!f.open(QFile::ReadOnly))
+        {
+            qWarning() << "cannot open file for reading:" << f.fileName();
+            qWarning() << "error string:" << f.errorString();
+
+            return {};
+        }
+
+        // TODO: rewrite to remove manual serialization/deserialization
+        const auto json = QJsonDocument::fromJson(f.readAll()).object();
+
+        const auto jsonCounters = json["counters"].toObject();
+        qDebug() << jsonCounters;
+
+        Pixels rawProfile;
+
+        rawProfile.counters.timestampUs = jsonCounters["timestampUs"].toInteger();
+        rawProfile.counters.measurementCounter = jsonCounters["measurementCounter"].toInteger();
+        rawProfile.counters.encoderPosition = jsonCounters["encoderPosition"].toInteger();
+
+        const auto jsonPixels = json["pixels"].toArray();
+        qDebug() << jsonPixels.count() << rawProfile.pixels.size();
+
+        for (size_t i = 0; i < jsonPixels.count() && i < rawProfile.pixels.size(); ++i)
+        {
+            rawProfile.pixels[i] = jsonPixels[i].toDouble();
+        }
+    }
+
+    // {
+    //     QMutexLocker l(&calibrationPixelsMutex);
+    //     std::swap(result, ::calibrationPixels);
+    // }
+
+    return result;
+}