diff options
Diffstat (limited to 'main.cpp')
| -rw-r--r-- | main.cpp | 236 |
1 files changed, 162 insertions, 74 deletions
@@ -6,7 +6,10 @@ #include <QJsonArray> #include <QJsonDocument> #include <QJsonObject> +#include <QSerialPort> +#include <QTextStream> #include <QtConcurrent/QtConcurrentRun> +#include <QTimer> #undef QT_NO_KEYWORDS #include <chrono> @@ -20,6 +23,8 @@ #include "genetic_algos.h" #include "imagealgos.h" #include "LibCamera.h" +#include "pigpio.h" +#include "rotaryencoder.h" #define try_apply_config() \ if(!applyConfig(config)) \ @@ -34,6 +39,8 @@ if(!applyConfig(config)) \ const QString exposureTimeKey = "exposureTime"; const QString laserLevelKey = "laserLevel"; +extern volatile int64_t positionSteps; + struct requested_params_t { int32_t exposureTime = { 200 }; int32_t laserLevel = { 7000 }; @@ -54,7 +61,53 @@ static void printControls(); int main(int argc, char *argv[]) { QCoreApplication app(argc, argv); - qDebug() << "Hello qt"; + + 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; + + qDebug() << "msecs before camera:" << t.elapsed(); // FIXME: don't use one var for everything int ret; std::unique_ptr<CameraManager> cm = std::make_unique<CameraManager>(); @@ -217,16 +270,16 @@ int main(int argc, char *argv[]) { std::make_pair(memory, plane.length); } - // size_t desiredFPS = 200; + // 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 }; request->controls().set(libcamera::controls::AnalogueGain, 1.0); - request->controls().set(libcamera::controls::ExposureTime, 4321 + expOffset++ * 100); + request->controls().set(libcamera::controls::ExposureTime, 100); // request->controls().set( - // libcamera::controls::FrameDurationLimits, - // libcamera::Span<const std::int64_t, 2>(value_pair)); + // libcamera::controls::FrameDurationLimits, + // libcamera::Span<const std::int64_t, 2>(value_pair)); requests.push_back(std::move(request)); } @@ -270,10 +323,10 @@ int main(int argc, char *argv[]) { // qDebug() << "image"; return QByteArray((const char*)pgm_image, pgm_image_size); }); - qHttpServer.route("/v1/sensor/exposureTimeUs", [&]() { - std::lock_guard<std::mutex> lg(pgm_image_mtx); - return "123"; - }); + // qHttpServer.route("/v1/sensor/exposureTimeUs", [&]() { + // // std::lock_guard<std::mutex> lg(pgm_image_mtx); + // return "123"; + // }); qHttpServer.route("/v1/pixels", [&]() { std::lock_guard<std::mutex> lg(pgm_image_mtx); @@ -285,10 +338,26 @@ int main(int argc, char *argv[]) { QJsonObject json; json["pixels"] = pixels; + json["encoderPosition"] = qint64 { encoder.position() }; + json["measurementCounter"] = qint64 { img.measurementCounter }; + json["timestampUs"] = qint64 { img.timestampNs / 1000. } ; return QHttpServerResponse(QJsonDocument(json).toJson()); }); + + qHttpServer.route("/v1/commands/resetEncoder", [&](const QHttpServerRequest &request) -> QHttpServerResponse { + if (request.method() != QHttpServerRequest::Method::Post) { + return QHttpServerResponse::StatusCode::NotFound; + } + + qDebug() << "reset encoder"; + + positionSteps = 0; + + return QHttpServerResponse::StatusCode::Ok; + }); + qHttpServer.route("/v1/sensor/params", [&](const QHttpServerRequest &request) -> QHttpServerResponse { switch (request.method()) { @@ -366,6 +435,8 @@ int main(int argc, char *argv[]) { s << value; s >> requested_params.laserLevel; + + qDebug() << "done with laser level"; } return QHttpServerResponse(request.body()); @@ -505,6 +576,13 @@ int main(int argc, char *argv[]) { */ void onRequestCompleted(Request *completed_request) { + static std::chrono::steady_clock::time_point fpsTimstamp = std::chrono::steady_clock::now(); + + QElapsedTimer t; + t.start(); + static uint32_t performanceCounter { 0 }; + static uint32_t elapsedSum { 0 }; + bool verbose = false; if (completed_request->status() == Request::RequestCancelled) @@ -528,12 +606,12 @@ void onRequestCompleted(Request *completed_request) const FrameMetadata &metadata = buffer->metadata(); - if (verbose) - { - std::cout << " seq: " << std::setw(6) << std::setfill('0') - << metadata.sequence - << " bytesused: "; - } + // if (verbose) + // { + // std::cout << " seq: " << std::setw(6) << std::setfill('0') + // << metadata.sequence + // << " bytesused: "; + // } for (size_t i = 0; i < buffer->planes().size(); ++i) { @@ -559,6 +637,8 @@ void onRequestCompleted(Request *completed_request) 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); @@ -567,58 +647,60 @@ void onRequestCompleted(Request *completed_request) rotate(img); process_columns(img); - static bool done = false; + // 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; + // 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; - // img.data[int_p][i] = 256 * 256 * fract; - // img.data[int_p + 1][i] = 256 * 256 * (1.0 - fract); + // // img.data[int_p][i] = 256 * 256 * fract; + // // img.data[int_p + 1][i] = 256 * 256 * (1.0 - fract); - // if (!done) { - // std::cout << fract << " "; - // } + // // if (!done) { + // // std::cout << fract << " "; + // // } - img.data[size_t(img.pixels[i])][i] = 0; - img.data[size_t(img.pixels[i]) - 6][i] = 0xffff; - img.data[size_t(img.pixels[i]) + 6][i] = 0xffff; - } - done = true; + // 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"); + // 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; - } + // 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); } } } const libcamera::ControlList &metadata = completed_request->metadata(); const ControlInfoMap & control_map = camera->controls(); - const ControlIdMap & ctrlIdMap = control_map.idmap(); + // const ControlIdMap & ctrlIdMap = control_map.idmap(); auto frameDurationCtrl = control_map.find(&controls::FrameDurationLimits); - auto expTimeCtrl = control_map.find(&controls::ExposureTime); + // auto expTimeCtrl = control_map.find(&controls::ExposureTime); double fps = frameDurationCtrl == control_map.end() ? std::numeric_limits<double>::quiet_NaN() : (1e6 / frameDurationCtrl->second.min().get<int64_t>()); @@ -641,10 +723,10 @@ void onRequestCompleted(Request *completed_request) << std::endl; } - for (const auto & [id, value] : metadata) - { + // for (const auto & [id, value] : metadata) + // { - } + // } // metadata.set(controls::ExposureTime, 300); @@ -687,39 +769,45 @@ void onRequestCompleted(Request *completed_request) // completed_request->reuse(Request::ReuseBuffers); // camera->queueRequest(completed_request); + qDebug() << "set exposureTime to" << requested_params.exposureTime; + 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); - // completed_request->controls().set(libcamera::controls::ExposureTime, rand() % 1000 + 100); - static bool done0 = false; - static bool done1 = false; + camera->queueRequest(completed_request); + + ++performanceCounter; + elapsedSum += t.elapsed(); + + // if (performanceCounter == 20) + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - if (!done0 || !done1) + if ((now - fpsTimstamp) > 1000ms) { + auto msPerFrame { float(elapsedSum / performanceCounter) }; - if (completed_request->sequence() % 2 == 0) - { - // qDebug() << "set 0" << completed_request->sequence(); - completed_request->controls().set( - libcamera::controls::ExposureTime, - requested_params.exposureTime); - done0 == true; - } - else - { - // qDebug() << "set 1" << completed_request->sequence(); - completed_request->controls().set( - libcamera::controls::ExposureTime, - requested_params.exposureTime); - done1 = true; - } + double configFps = frameDurationCtrl == control_map.end() ? + std::numeric_limits<double>::quiet_NaN() : + (1e6 / frameDurationCtrl->second.min().get<int64_t>()); + + auto fps { 1000.f / msPerFrame }; + + qDebug() << "fps ideal/real is" << configFps << "/" << fps + << "; ms per frame is" << msPerFrame << "counted fps" << performanceCounter; + + elapsedSum = 0; + performanceCounter = 0; + fpsTimstamp = now; } - camera->queueRequest(completed_request); + // qDebug() << "-------------------------------------------"; + } static bool applyConfig(const std::unique_ptr<CameraConfiguration> & config) |