From 38acf876313c9bf28e41acd8bc29d6115c1e9285 Mon Sep 17 00:00:00 2001 From: Nikita Kostovsky Date: Tue, 14 Jan 2025 18:04:32 +0100 Subject: refactoring --- CMakeLists.txt | 1 + main.cpp | 639 ++++++-------------------------------------------- src/calibration.cpp | 176 +++++--------- src/camera/ov9281.cpp | 25 +- src/image.cpp | 5 + src/laser.cpp | 70 ++++++ src/laser.h | 33 +++ src/pixels.cpp | 63 ++--- 8 files changed, 298 insertions(+), 714 deletions(-) create mode 100644 src/laser.cpp create mode 100644 src/laser.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 9c1b165..2ffc2f1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,6 +100,7 @@ qt_add_executable(apporpheus profile.h profile.cpp src/camera/ov9281.h src/camera/ov9281.cpp src/image.h src/image.cpp + src/laser.h src/laser.cpp ) target_link_libraries(app${PROJECT_NAME} PRIVATE diff --git a/main.cpp b/main.cpp index 5f2ba82..341693e 100644 --- a/main.cpp +++ b/main.cpp @@ -15,6 +15,7 @@ #include "genetic_algos.h" #include "httpservice.h" #include "imagealgos.h" +#include "laser.h" #include "pigpio.h" #include "printerclient.h" #include "profile.h" @@ -50,13 +51,12 @@ extern volatile int32_t positionSteps; requested_params_t requested_params; -namespace -{ +namespace { Image img; Pixels pixels; std::vector calibrationPixels; QMutex calibrationPixelsMutex; -} // namespace +} // namespace using namespace std::chrono_literals; @@ -66,11 +66,10 @@ using namespace std::chrono_literals; // std::vector> requests; libcamera::ControlList lastControls; -namespace -{ +namespace { CalibrationTablePtr calibrationTableZ; CalibrationTablePtr calibrationTableX; -} // namespace +} // namespace // static bool applyConfig( // const std::unique_ptr& config @@ -82,40 +81,51 @@ CalibrationTablePtr calibrationTableX; auto printPixels = [](const auto& pixels) { for (size_t i = (img_width - 10) / 2; i < img_width - ((img_width - 10) / 2); - ++i) - { + ++i) { std::cout << pixels[i] << " "; } std::cout << std::endl; }; -void onNewPixels(std::shared_ptr pixels) +void onNewPixels( + std::shared_ptr pixels) { - if (!*pixels) - { + if (!*pixels) { qDebug() << "got empty pixels"; } ::pixels = *pixels; - if (!::pixels) - { + if (!::pixels) { qDebug() << "empty pixels after copy"; } -}; +} bool initLaser(); -int main(int argc, char* argv[]) +int main( + int argc, char* argv[]) { QCoreApplication app(argc, argv); // if (false) qDebug() << "size of raw profile" << sizeof(Pixels); - if (true) - { - if (true) - { + if (true) { + // open binary calibration table + if (false) { + if (!openCalibrationTable("/home/user/dumps/binz.calibration_table", + ::calibrationTableZ)) { + exit(EXIT_FAILURE); + } + interpolate(::calibrationTableZ); + if (!openCalibrationTable("/home/user/dumps/binx.calibration_table", + ::calibrationTableX)) { + exit(EXIT_FAILURE); + } + interpolate(::calibrationTableX); + } + + if (false) { // z // if (!openCalibrationTable( // "/home/user/dumps/binz.calibration_table", @@ -148,23 +158,23 @@ int main(int argc, char* argv[]) ::calibrationTableX = calibrateX(std::move(filteredRawProfiles)); - for (size_t i = 9471; i < 9472; i++) { - std::cout << "row #" << i << ": "; + // for (size_t i = 9471; i < 9472; i++) { + // std::cout << "row #" << i << ": "; - for (size_t j = 0; j < 1280; ++j) { - const auto& p = ::calibrationTableX->at(j).at(i); - std::cout << p << ' '; - } + // for (size_t j = 0; j < 1280; ++j) { + // const auto& p = ::calibrationTableX->at(j).at(i); + // std::cout << p << ' '; + // } - std::cout << std::endl; - } + // std::cout << std::endl; + // } // x - qDebug() << "open x table"; - if (!openCalibrationTable("/home/user/dumps/binx.calibration_table", - ::calibrationTableX)) { - exit(EXIT_FAILURE); - } + // qDebug() << "open x table"; + // if (!openCalibrationTable("/home/user/dumps/binx.calibration_table", + // ::calibrationTableX)) { + // exit(EXIT_FAILURE); + // } // if (!calibrationTableToImage(::calibrationTableX) // .save("/home/user/dumps/imageX.png")) { @@ -172,16 +182,16 @@ int main(int argc, char* argv[]) // exit(EXIT_FAILURE); // } - for (size_t i = 9471; i < 9472; i++) { - std::cout << "row #" << i << ": "; + // for (size_t i = 9471; i < 9472; i++) { + // std::cout << "row #" << i << ": "; - for (size_t j = 0; j < 1280; ++j) { - const auto& p = ::calibrationTableX->at(j).at(i); - std::cout << p << ' '; - } + // for (size_t j = 0; j < 1280; ++j) { + // const auto& p = ::calibrationTableX->at(j).at(i); + // std::cout << p << ' '; + // } - std::cout << std::endl; - } + // std::cout << std::endl; + // } // exit(EXIT_SUCCESS); interpolate(::calibrationTableX); @@ -190,8 +200,7 @@ int main(int argc, char* argv[]) // .save("/home/user/dumps/imageX_interpolated.png"); } - if (false) - { + if (true) { auto rawProfiles = openDump("/home/user/dumps/binz"); // auto rawProfiles = openDump("/home/user/dumps/z"); qDebug() << "raw z-profiles count is" << rawProfiles.size(); @@ -229,8 +238,7 @@ int main(int argc, char* argv[]) qDebug() << "--------------------------------------------------------"; - if (false) - { + if (true) { auto rawProfiles = openDump("/home/user/dumps/binx"); qDebug() << "raw x-profiles count is" << rawProfiles.size(); // qDebug() << "height" << calibrationColumnHeight; @@ -267,8 +275,7 @@ int main(int argc, char* argv[]) // exit(EXIT_SUCCESS); - if (!initLaser()) - { + if (!initLaser()) { return EXIT_FAILURE; } @@ -291,8 +298,7 @@ int main(int argc, char* argv[]) // const auto cameras = cm->cameras(); const auto cameras = OV9281::search(cm); - if (cameras.empty()) - { + if (cameras.empty()) { std::cerr << "No cameras were identified on the system." << std::endl; cm->stop(); @@ -305,231 +311,24 @@ int main(int argc, char* argv[]) camera->newPixels.connect(&onNewPixels); - if (!camera->startStream()) - { + if (!camera->startStream()) { cm->stop(); return EXIT_FAILURE; } - /* - std::string cameraId = cameras[0]->id(); - - std::cout << "using " << cameraId << std::endl; - - // - // Note that `camera` may not compare equal to `cameras[0]`. - // In fact, it might simply be a `nullptr`, as the particular - // device might have disappeared (and reappeared) in the meantime. - // - // std::shared_ptr camera = cm->get(cameraId); - camera = cm->get(cameraId); - - if (camera->acquire() != EXIT_SUCCESS) - { - std::cout << "Cannot acquire camera." << std::endl; - cm->stop(); - - return EXIT_FAILURE; - } - - // FIXME: nullptr - // std::unique_ptr config = - // camera->generateConfiguration( { StreamRole::Viewfinder } ); - config = camera->generateConfiguration({libcamera::StreamRole::Raw}); - - if (config->empty()) - { - std::cerr << "No configurations generated." << std::endl; - cm->stop(); - - return EXIT_FAILURE; - } - - config->orientation = libcamera::Orientation::Rotate90; - - // if (config->validate() != EXIT_SUCCESS) - - // if (camera->configure(config.get()) != EXIT_SUCCESS) - // { - // std::cerr << "cannot configure camera" << std::endl << std::flush; - // cm->stop(); - - // return EXIT_FAILURE; - // } - - // FIXME: nullptr - libcamera::StreamConfiguration& streamConfig = config->at(0); - std::cout << "Default viewfinder configuration is: " - << streamConfig.toString() << std::endl; - std::cout << "Pixel format is: " << streamConfig.pixelFormat.toString() - << std::endl; - std::cout << "Buffer count is: " << streamConfig.bufferCount << std::endl; - // FIXME: empty variant - std::cout << "Color space is: " - << streamConfig.colorSpace.value().toString() << std::endl; - std::cout << "Orientation is: " << config->orientation << std::endl; - // formats::R8, - // formats::R10, - // formats::R12, - // formats::R16, - // formats::R10_CSI2P, // camera->configure failure - // formats::R12_CSI2P, // camera->configure failure - // streamConfig.pixelFormat = PixelFormat::fromString("R8"); - // streamConfig.pixelFormat = PixelFormat::fromString("Y8_1X8"); - - // streamConfig.pixelFormat = formats::R8; - streamConfig.pixelFormat = libcamera::formats::R16; - streamConfig.bufferCount = 2; - // what is default R10_CSI2P? MONO_PISP_COMP1? - // MONO_PISP_COMP1 - check rpicam-apps sources for decoding algos - // streamConfig.pixelFormat = formats::R10_CSI2P; - // streamConfig.bufferCount = 16; - try_apply_config() - - // #define doit(rotation) \ - // std::cout << "set rotation to: " << libcamera::Orientation:: rotation \ - // << std::endl; \ - // config->orientation = libcamera::Orientation:: rotation; \ - // try_apply_config() - - // doit(Rotate0Mirror); - // doit(Rotate180); - // doit(Rotate180Mirror); - // doit(Rotate90Mirror); - // doit(Rotate270); - // doit(Rotate270Mirror); - // doit(Rotate90); - - std::cout - << "new config " << streamConfig.toString() << std::endl; - - // FIXME: may crassh even on success (e.g. by setting pixelFormat to "8") - if (camera->configure(config.get()) != EXIT_SUCCESS) - { - std::cout << "cannot apply config, quit." << std::endl; - camera->release(); - cm->stop(); - - return EXIT_FAILURE; - } - - // TODO: try custom FrameBufferAllocator and compare performance - - auto allocator = std::make_shared(camera); - - auto stream = streamConfig.stream(); - - ret = allocator->allocate(stream); - - // TODO: check if zero - if (ret < 0) - { - std::cerr << "Can't allocate buffers" << std::endl; - // return -ENOMEM; - return ret; - } - - size_t allocated = size_t(ret); - std::cout << "Allocated " << allocated << " buffers for stream" - << std::endl; - - const std::vector>& buffers = - allocator->buffers(stream); - - // for (size_t i = 0; i < buffers.size(); ++i) - static int expOffset = 0; - for (const auto& buffer : buffers) - { - std::unique_ptr request = camera->createRequest(); - - if (!request) - { - std::cerr << "Can't create request" << std::endl; - return -ENOMEM; - } - - // TODO: try multiple buffers per request and compare performance - int ret = request->addBuffer(stream, buffer.get()); - - if (ret < 0) - { - std::cerr << "Can't set buffer for request" << std::endl; - - return ret; - } - - for (const auto& plane : buffer->planes()) - { - void* memory = mmap( - NULL, - plane.length, - PROT_READ, - MAP_SHARED, - plane.fd.get(), - 0 - ); - mappedBuffers_[plane.fd.get()] = - std::make_pair(memory, plane.length); - } - - 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, 100); - request->controls().set( - libcamera::controls::FrameDurationLimits, - libcamera::Span(value_pair) - ); - - requests.push_back(std::move(request)); - } - - camera->requestCompleted.connect(onRequestCompleted); - - std::unique_ptr camcontrols{ - new libcamera::ControlList() - }; - // camcontrols->set(controls::FrameDurationLimits, libcamera::Span({8702, 10718903})); - // camcontrols->set(controls::ExposureTime, 100); - // camcontrols->set(controls::AnalogueGain, 0.1); - - std::this_thread::sleep_for(500ms); - - if (camera->start(camcontrols.get())) - { - qDebug() << "failed to start camera"; - return EXIT_FAILURE; - } - - // camera->start(); - - for (auto& request : requests) - { - camera->queueRequest(request.get()); - } -*/ - - // std::this_thread::sleep_for(2s); - // TODO: move to thread - // Http::listenAndServe(Pistache::Address("*:8080")); - QHttpServer qHttpServer; qHttpServer.route("/v1/sensor/image", [&]() { std::cout << "http: image" << std::endl; std::lock_guard lg(pgm_image_mtx); // qDebug() << "image"; - return QByteArray((const char*)pgm_image, pgm_image_size); + return QByteArray((const char*) pgm_image, pgm_image_size); }); qHttpServer.route("/v1/sensor/image2", [&]() { std::cout << "http: image2" << std::endl; std::lock_guard lg(pgm_image_mtx); // qDebug() << "image"; - return QByteArray((const char*)pgm_image, pgm_image_size); + return QByteArray((const char*) pgm_image, pgm_image_size); }); // qHttpServer.route("/v1/sensor/exposureTimeUs", [&]() { // // std::lock_guard lg(pgm_image_mtx); @@ -541,8 +340,7 @@ int main(int argc, char* argv[]) QJsonArray pixels; - for (size_t i = 0; i < img_width; ++i) - { + for (size_t i = 0; i < img_width; ++i) { // pixels << img_height - img.pixels[i]; pixels << ::pixels.pixels[i]; } @@ -559,12 +357,9 @@ int main(int argc, char* argv[]) QJsonArray jsonLines; - for (const auto& l : lines) - { - jsonLines << QJsonArray{ - QJsonArray{l.p1().x(), l.p1().y()}, - QJsonArray{l.p2().x(), l.p2().y()} - }; + for (const auto& l : lines) { + jsonLines << QJsonArray{QJsonArray{l.p1().x(), l.p1().y()}, + QJsonArray{l.p2().x(), l.p2().y()}}; } json["lines"] = jsonLines; @@ -589,8 +384,7 @@ int main(int argc, char* argv[]) .route("/v1/commands/resetEncoder", [&](const QHttpServerRequest& request) -> QHttpServerResponse { std::cout << "http: resetEncoder" << std::endl; - if (request.method() != QHttpServerRequest::Method::Post) - { + if (request.method() != QHttpServerRequest::Method::Post) { return QHttpServerResponse::StatusCode::NotFound; } @@ -605,8 +399,7 @@ int main(int argc, char* argv[]) .route("/v1/commands/startCalibration", [&](const QHttpServerRequest& request) -> QHttpServerResponse { std::cout << "http: startCalibration" << std::endl; - if (request.method() != QHttpServerRequest::Method::Post) - { + if (request.method() != QHttpServerRequest::Method::Post) { return QHttpServerResponse::StatusCode::NotFound; } @@ -623,8 +416,7 @@ int main(int argc, char* argv[]) .route("/v1/commands/gCode", [&](const QHttpServerRequest& request) -> QHttpServerResponse { std::cout << "http: gCode" << std::endl; - if (request.method() != QHttpServerRequest::Method::Post) - { + if (request.method() != QHttpServerRequest::Method::Post) { return QHttpServerResponse::StatusCode::NotFound; } @@ -659,8 +451,7 @@ int main(int argc, char* argv[]) "/v1/sensor/params", [&](const QHttpServerRequest& request) -> QHttpServerResponse { std::cout << "http: params" << std::endl; - switch (request.method()) - { + switch (request.method()) { case QHttpServerRequest::Method::Get: { std::lock_guard lg(pgm_image_mtx); QJsonObject json; @@ -697,12 +488,10 @@ int main(int argc, char* argv[]) auto json = QJsonDocument::fromJson(request.body()).object(); - if (json.contains(exposureTimeKey)) - { + if (json.contains(exposureTimeKey)) { const int32_t value{json[exposureTimeKey].toInt()}; - if (value == 0) - { + if (value == 0) { return QHttpServerResponse::StatusCode::NotFound; } @@ -710,12 +499,10 @@ int main(int argc, char* argv[]) requested_params.exposureTime = value; } - if (json.contains(laserLevelKey)) - { + if (json.contains(laserLevelKey)) { const int32_t value{json[laserLevelKey].toInt()}; - if (value == 0) - { + if (value == 0) { return QHttpServerResponse::StatusCode::NotFound; } @@ -723,17 +510,14 @@ int main(int argc, char* argv[]) requested_params.laserLevel = value; const QString laserLevelFile{ - "/sys/class/pwm/pwmchip2/pwm1/duty_cycle" - }; + "/sys/class/pwm/pwmchip2/pwm1/duty_cycle"}; QFile f{laserLevelFile}; - if (!f.open(QFile::ReadWrite)) - { + if (!f.open(QFile::ReadWrite)) { qDebug() << "cannot open laser level file:" << f.errorString(); qDebug() << "file path is" << f.fileName(); - return QHttpServerResponse::StatusCode:: - InternalServerError; + return QHttpServerResponse::StatusCode::InternalServerError; } QTextStream s{&f}; @@ -748,8 +532,8 @@ int main(int argc, char* argv[]) return QHttpServerResponse(request.body()); } default: { - return QHttpServerResponse(QByteArray("unsupported http method") - ); + return QHttpServerResponse( + QByteArray("unsupported http method")); } } }); @@ -801,269 +585,6 @@ int main(int argc, char* argv[]) return result; } -/* - * Signals operate in the libcamera CameraManager thread context, so it is - * important not to block the thread for a long time, as this blocks internal - * processing of the camera pipelines, and can affect realtime performance. - */ -/* -void onRequestCompleted(libcamera::Request* completed_request) -{ - using namespace libcamera; - - 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) - { - std::cerr << "request canceled" << std::endl; - - return; - } - - const std::map& buffers = - completed_request->buffers(); - - // std::cout << "request completed, buffers count is " << buffers.size(); - - // // TODO: rewrite this shit - for (auto [stream, buffer] : buffers) - { - const auto& streamConfig = stream->configuration(); - const auto& imageSize = streamConfig.size; - const auto& pixelFormat = streamConfig.pixelFormat; - const auto& stride = streamConfig.stride; - - const FrameMetadata& metadata = buffer->metadata(); - - for (size_t i = 0; i < buffer->planes().size(); ++i) - { - const FrameBuffer::Plane& plane = buffer->planes()[i]; - const FrameMetadata::Plane& metaplane = - buffer->metadata().planes()[i]; - - size_t size = std::min(metaplane.bytesused, plane.length); - void* data = mappedBuffers_[plane.fd.get()].first; - - // 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); - img.rotate(); - Pixels pixels = process_columns(img); - ::pixels = pixels; - - // qDebug() << "calibration mode" << scanningModeFlags; - - if (scanningModeFlags == ScanningModeFlags::Calibration) - { - const int32_t maxEncoderPosition = - int32_t(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; - - { - QMutexLocker l(&calibrationPixelsMutex); - // QFuture dumpCalirationPixelsFuture - // = QtConcurrent::run(&dumpCalibrationPixels, - // calibrationPixels); - auto future = QtConcurrent::run([&]() { - dumpCalibrationPixels( - std::move(::calibrationPixels)); - }); - } - } - else - { - // qDebug() << "calibration skip at pos:" << - // pixels.counters.encoderPosition; - } - } - - pgm_save(&img, nullptr); - } - } - - const libcamera::ControlList& metadata = completed_request->metadata(); - const ControlInfoMap& control_map = camera->controls(); - // const ControlIdMap & ctrlIdMap = control_map.idmap(); - - auto frameDurationCtrl = control_map.find(&controls::FrameDurationLimits); - // auto expTimeCtrl = control_map.find(&controls::ExposureTime); - double fps = frameDurationCtrl == control_map.end() - ? std::numeric_limits::quiet_NaN() - : (1e6 / frameDurationCtrl->second.min().get()); - - auto exp = metadata.get(controls::ExposureTime); - auto ag = metadata.get(controls::AnalogueGain); - auto ae = metadata.get(controls::AeEnable); - // auto br= metadata.get(controls::Brightness); - lastControls = completed_request->controls(); - - if (verbose) - { - std::cout << "fps: " << fps << " exp: " << *exp << " ag: " - << *ag - // << " br: " << *br - << " ae: " << *ae << " aa: " - << *completed_request->controls().get( - libcamera::controls::ExposureTime - ) - << std::endl; - } - - 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 - ); - - camera->queueRequest(completed_request); - - ++performanceCounter; - elapsedSum += t.elapsed(); - - // if (performanceCounter == 20) - std::chrono::steady_clock::time_point now = - std::chrono::steady_clock::now(); - - if ((now - fpsTimstamp) > 1000ms) - { - auto msPerFrame{float(elapsedSum / performanceCounter)}; - - double configFps = - frameDurationCtrl == control_map.end() - ? std::numeric_limits::quiet_NaN() - : (1e6 / frameDurationCtrl->second.min().get()); - - 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; - } - - // qDebug() << "-------------------------------------------"; -} -*/ - -// static bool applyConfig( -// const std::unique_ptr& config -// ) -// { -// using namespace libcamera; - -// auto status = config->validate(); - -// // WARNING: unsafe -// libcamera::StreamConfiguration& streamConfig = config->at(0); - -// switch (status) -// { -// case CameraConfiguration::Status::Valid: -// std::cout << "config is valid" << std::endl; -// break; -// case CameraConfiguration::Status::Adjusted: -// std::cout << "\tpixelFormat: " << streamConfig.pixelFormat.toString() -// << std::endl; -// std::cout << "\tbufferCount: " << streamConfig.bufferCount << std::endl; -// std::cout << "\torientation: " << config->orientation << std::endl; -// break; -// case CameraConfiguration::Status::Invalid: -// std::cout << "config is invalid, quit." << std::endl; - -// return false; -// } - -// return true; -// } - -// static void printControls() -// { -// using namespace libcamera; -// const libcamera::ControlInfoMap& control_map = camera->controls(); - -// // for (const auto & [id, info]: control_map) -// for (const std::pair& pair : control_map) -// { -// const ControlId* const& id = pair.first; -// const ControlInfo& info = pair.second; - -// std::cout << "\tc " << id->name() << " (" << id->id() -// << "): " << info.toString() -// << (info.def().isNone() -// ? "" -// : " (dflt:" + info.def().toString() + ")"); - -// if (!info.values().size()) -// { -// std::cout << std::endl; -// continue; -// } - -// std::cout << " - ["; - -// for (const auto& v : info.values()) -// { -// std::cout << " " << v.toString(); -// } - -// std::cout << " ]\n"; -// } -// } - bool initLaser() { const QLatin1String pwmChip{"pwmchip2"}; @@ -1075,8 +596,7 @@ bool initLaser() QFile f{pwmExportFile}; - if (!f.open(QFile::WriteOnly)) - { + if (!f.open(QFile::WriteOnly)) { qWarning() << "cannot open" << f.fileName() << "for writing"; qWarning() << "error:" << f.errorString(); @@ -1093,8 +613,7 @@ bool initLaser() f.close(); f.setFileName(periodFilename); - if (!f.open(QFile::WriteOnly)) - { + if (!f.open(QFile::WriteOnly)) { qWarning() << "cannot open" << f.fileName() << "for writing"; qWarning() << "error:" << f.errorString(); @@ -1109,8 +628,7 @@ bool initLaser() f.close(); f.setFileName(dutyCycleFilename); - if (!f.open(QFile::WriteOnly)) - { + if (!f.open(QFile::WriteOnly)) { qWarning() << "cannot open" << f.fileName() << "for writing"; qWarning() << "error:" << f.errorString(); @@ -1125,8 +643,7 @@ bool initLaser() f.close(); f.setFileName(enableFilename); - if (!f.open(QFile::WriteOnly)) - { + if (!f.open(QFile::WriteOnly)) { qWarning() << "cannot open" << f.fileName() << "for writing"; qWarning() << "error:" << f.errorString(); diff --git a/src/calibration.cpp b/src/calibration.cpp index ff37e73..7fcdbcc 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -13,7 +13,8 @@ #include "imagealgos.h" -bool openCalibrationTable(const QString& filename, CalibrationTablePtr& table) +bool openCalibrationTable( + const QString& filename, CalibrationTablePtr& table) { QFile f(filename); @@ -46,7 +47,8 @@ bool openCalibrationTable(const QString& filename, CalibrationTablePtr& table) return true; } -bool dump(const CalibrationTablePtr& table, const QString& filename) +bool dump( + const CalibrationTablePtr& table, const QString& filename) { qDebug() << Q_FUNC_INFO << "size is" << sizeof(CalibrationTable); @@ -71,7 +73,8 @@ bool dump(const CalibrationTablePtr& table, const QString& filename) return true; } -void interpolate(CalibrationTablePtr& table) +void interpolate( + CalibrationTablePtr& table) { std::for_each(std::execution::par, table->begin(), @@ -105,7 +108,8 @@ void interpolate(CalibrationTablePtr& table) // } } -void interpolate(CalibrationColumn& column) +void interpolate( + CalibrationColumn& column) { size_t start{0}; auto& c = column; @@ -147,7 +151,8 @@ void interpolate(CalibrationColumn& column) } } -QImage calibrationTableToImage(const CalibrationTablePtr& calibrationTable) +QImage calibrationTableToImage( + const CalibrationTablePtr& calibrationTable) { QImage result(QSize(calibrationTable->size(), calibrationTable->at(0).size()), @@ -185,28 +190,27 @@ QImage calibrationTableToImage(const CalibrationTablePtr& calibrationTable) } qDebug() << "not null count" << notNull << "of" - << sizeof(CalibrationTable) - / sizeof(calibrationTable->at(0).at(0)); + << sizeof(CalibrationTable) / + sizeof(calibrationTable->at(0).at(0)); return result; } -QList filter(const QList& rawProfiles) +QList filter( + const QList& rawProfiles) { QList result; QList::const_iterator it = rawProfiles.constBegin(); - while (it != rawProfiles.constEnd()) - { + while (it != rawProfiles.constEnd()) { Pixels sum = *it; size_t count{1}; ++it; while (it != rawProfiles.constEnd() && - it->counters.encoderPosition == sum.counters.encoderPosition) - { + it->counters.encoderPosition == sum.counters.encoderPosition) { sum += *it; ++count; ++it; @@ -219,127 +223,79 @@ QList filter(const QList& rawProfiles) return result; } -CalibrationTablePtr calibrateZ(const QList& rawProfiles, - const uint32_t& stepsPerMm) +CalibrationTablePtr calibrateZ( + const QList& rawProfiles, const uint32_t& stepsPerMm) { CalibrationTablePtr result{new CalibrationTable{{0}}}; for (const auto& rawProfile : rawProfiles) { - // std::cout << "calibration: pos is " - // << rawProfile.counters.encoderPosition << std::endl; - const float positionMm{float(rawProfile.counters.encoderPosition) - / float(stepsPerMm)}; - - if (positionMm >= 200) { - qWarning() << "got invalid calibration distance:" << positionMm; - qWarning() << "encoder position:" - << rawProfile.counters.encoderPosition; - exit(EXIT_FAILURE); - } + const float positionMm{float(rawProfile.counters.encoderPosition) / + float(stepsPerMm)}; const auto& pixels = rawProfile.pixels; - // printPixels(pixels); - // static size_t counter { 0 }; - // qDebug() << "calibrated" << counter++; - const float pos = rawProfile.counters.encoderPosition; - const float divider = stepsPerMm; - // std::cout << pos << " " << divider << " " << pos / divider << - // std::endl; std::cout << std::setw(5) << - // rawProfile.counters.encoderPosition - // << std::setw(5) << requested_params.stepsPerMm - // << std::setw(8) << positionMm - // << std::setw(8) << - // float(rawProfile.counters.encoderPosition) / - // float(requested_params.stepsPerMm) - // << ": "; for (size_t columnIdx = 0; columnIdx < pixels.size(); ++columnIdx) { const auto& pixelValue = pixels.at(columnIdx); - - // float[0, img_height] -> uint16_t[0, calibrationColumnHeight] const uint16_t discretePixelValue{ uint16_t(pixelValue * discretesInRage / img_height)}; - Q_ASSERT_X(discretePixelValue > 0 - && discretePixelValue < calibrationColumnHeight, - Q_FUNC_INFO, - ("got ivalid discrete value " - + QString::number(discretePixelValue) - + ". pixelValues is " + QString::number(pixelValue) - + ". calc result is " - + QString::number(pixelValue * discretesInRage - / img_height)) - .toStdString() - .c_str()); - - // auto& calibrationColumn = result[columnIdx]; - auto& calibrationColumn = (*result)[columnIdx]; + // TODO: move this validation to some better place + if (Q_UNLIKELY(discretePixelValue >= calibrationColumnHeight)) { + std::cerr << __func__ + << ":/tinvalid discrete value. col: " << columnIdx + << ", val: " << pixelValue << std::endl; - calibrationColumn[discretePixelValue] = positionMm; + return {}; + } - // if (columnIdx >= ((img_width - 10) / 2) && - // columnIdx < img_width - ((img_width - 10) / 2)) - // { - // std::cout << discretePixelValue << ";"; - // } + auto& calibrationColumn = (*result)[columnIdx]; + calibrationColumn[discretePixelValue] = positionMm; } - - // std::cout << std::endl << std::flush; } return result; - // TODO: try something interesting - // return {}; } -CalibrationTablePtr calibrateX(const QList& rawProfiles) +CalibrationTablePtr calibrateX( + const QList& rawProfiles) { + // TODO: move to settings + constexpr double triangleBaseMm{8.}; CalibrationTablePtr result{new CalibrationTable{{0}}}; - static size_t tmp_counter = 0; - for (const auto& rawProfile : rawProfiles) { const auto& pixels = rawProfile.pixels; auto lines = pixelsToLines(rawProfile); - tmp_counter++; - - Q_ASSERT_X(lines.count() > 2, Q_FUNC_INFO, "no lines"); - - if (tmp_counter == 9471) { - for (const auto& l : lines) { - qDebug() << "line:" << l; - } + if (lines.count() < 2) { + continue; } + // x coords of line endings - [l1.p1.x; lN.p2.x] QList xAnchors(lines.size() + 1); - std::transform(std::execution::par_unseq, lines.constBegin(), lines.constEnd(), xAnchors.begin(), [](const auto& l) { return l.x1(); }); - xAnchors.last() = lines.last().x2(); - constexpr double triangleHeightMm{5.}; - constexpr double triangleBaseMm{8.}; - - auto nearestAnchorToX0 = std::min_element(std::execution::par_unseq, - xAnchors.constBegin(), - xAnchors.constEnd(), - [](const auto& a, - const auto& b) { - return std::abs(a) - < std::abs(b); - }); + auto centralAnchorIt = std::min_element(std::execution::par_unseq, + xAnchors.constBegin(), + xAnchors.constEnd(), + [](const auto& a, + const auto& b) { + return std::abs(a) < + std::abs(b); + }); - int nearestAnchorToX0Idx = nearestAnchorToX0 - xAnchors.constBegin(); + int centralAnchorIdx = centralAnchorIt - xAnchors.constBegin(); + // convert line image coords to mm coords QList xAnchorsMm(xAnchors.count()); for (int i = 0; i < xAnchors.size(); ++i) { - xAnchorsMm[i] = (i - nearestAnchorToX0Idx) * triangleBaseMm / 2.; + xAnchorsMm[i] = (i - centralAnchorIdx) * triangleBaseMm / 2.; } auto xAnchorIt = xAnchors.constBegin() + 1; @@ -349,17 +305,23 @@ CalibrationTablePtr calibrateX(const QList& rawProfiles) // skip points with to the left from left line and to the right from // right line const auto columnX = int(columnIdx) - int(img_width / 2); + if (columnX < xAnchors.first() || columnX > xAnchors.last()) { continue; } - if (columnX > *xAnchorIt) { + // if [...(anchor-1)...(anchor)...(column)...] + // then use next anchor to have [...(anchor-1)...(column)...anchor...] + if (*xAnchorIt < columnX) { ++xAnchorIt; ++xAnchorMmIt; } const auto xLeft = *(xAnchorIt - 1); const auto xRight = *(xAnchorIt); + // there could be points which don't belong to any lines, because + // some real lines can be too short and will be ignored by + // `pixelsToLines`. e.g. the most left/right lines. skip such points if (columnX < xLeft || columnX > xRight) { if (rawProfile.counters.encoderPosition >= 0) { qWarning() @@ -372,37 +334,29 @@ CalibrationTablePtr calibrateX(const QList& rawProfiles) const uint16_t discretePixelValue{ uint16_t(pixelValue * discretesInRage / img_height)}; - Q_ASSERT_X(discretePixelValue > 0 - && discretePixelValue < calibrationColumnHeight, - Q_FUNC_INFO, - ("got ivalid discrete value " - + QString::number(discretePixelValue) - + ". pixelValues is " + QString::number(pixelValue) - + ". calc result is " - + QString::number(pixelValue * discretesInRage - / img_height)) - .toStdString() - .c_str()); + // TODO: move this validation to some better place + if (Q_UNLIKELY(discretePixelValue >= calibrationColumnHeight)) { + std::cerr << __func__ + << ":/tinvalid discrete value. col: " << columnIdx + << ", val: " << pixelValue << std::endl; + return {}; + } + // use value interpolated between line endings (anchors) const auto xLineLen = xRight - xLeft; const auto xLeftMm = *(xAnchorMmIt - 1); const auto xRelative = float(columnX - xLeft) / xLineLen; const auto xMmValue = xLeftMm + xRelative * (triangleBaseMm / 2.); - auto& calibrationColumn = (*result)[columnIdx]; - - if (tmp_counter == 9471) { - qDebug() << "calibration value" << columnIdx - << discretePixelValue << xMmValue; - } - calibrationColumn[discretePixelValue] = xMmValue; + (*result)[columnIdx][discretePixelValue] = xMmValue; } } return result; } -void dumpCalibrationPixels(std::vector&& calibrationPixels) +void dumpCalibrationPixels( + std::vector&& calibrationPixels) { std::vector rawProfiles; diff --git a/src/camera/ov9281.cpp b/src/camera/ov9281.cpp index bdb9f89..4d393a0 100644 --- a/src/camera/ov9281.cpp +++ b/src/camera/ov9281.cpp @@ -56,14 +56,6 @@ bool OV9281::init() m_config->orientation = libcamera::Orientation::Rotate90; - if (m_config->empty()) - { - std::cerr << __func__ << ": " << m_camera->id() << ": config is empty" - << std::endl; - - return false; - } - libcamera::StreamConfiguration &streamConfig = m_config->at(0); streamConfig.pixelFormat = OV9281::pixelFormat; @@ -71,9 +63,10 @@ bool OV9281::init() if (!validateConfig()) { - std::cerr << __func__ << ": " << m_camera->id() - << ": cannot apply default config" << std::endl; + return false; + } + if (!applyConfig()) { return false; } @@ -135,6 +128,11 @@ bool OV9281::applyConfig() return true; } +/* + * Signals operate in the libcamera CameraManager thread context, so it is + * important not to block the thread for a long time, as this blocks internal + * processing of the camera pipelines, and can affect realtime performance. + */ void OV9281::onRequestCompleted(libcamera::Request *completed_request) { using namespace libcamera; @@ -184,6 +182,9 @@ void OV9281::onRequestCompleted(libcamera::Request *completed_request) auto pixels = img->pixels(); #ifdef emit #undef emit + if (!pixels) { + std::cerr << "emit empty pixels" << std::endl; + } newPixels.emit(pixels); #define emit #endif @@ -225,7 +226,9 @@ std::vector> OV9281::search( for (const auto &camera : manager->cameras()) { - auto ov9281 = std::shared_ptr(new OV9281(camera)); + auto id = camera->id(); + auto c = manager->get(id); + auto ov9281 = std::shared_ptr(new OV9281(c)); if (!ov9281->init()) { diff --git a/src/image.cpp b/src/image.cpp index a9280a4..7d1c824 100644 --- a/src/image.cpp +++ b/src/image.cpp @@ -104,6 +104,11 @@ std::shared_ptr Image::pixels() const result->pixels[i] = process_column(rotated_cw[i]); } + // for (size_t i = 640 - 5; i < 640 + 5; ++i) { + // std::cout << result->pixels[i] << ' '; + // } + // std::cout << std::endl; + stop_timer(process_columns); return result; diff --git a/src/laser.cpp b/src/laser.cpp new file mode 100644 index 0000000..f23faab --- /dev/null +++ b/src/laser.cpp @@ -0,0 +1,70 @@ +#include "laser.h" + +#include +#include +#include + +#include "macro.h" + +PwmLaser::PwmLaser(const std::string &pwmChip, const std::string &pwm) + : INIT_FIELD(pwmChip) + , INIT_FIELD(pwm) +{} + +bool PwmLaser::init() +{ + if (m_pwmChip.empty() || m_pwm.empty()) { + std::cerr << __func__ << ":\tinvalid pwm config: pwmChip == '" + << m_pwmChip << "', pwm == '" << m_pwm << "'" << std::endl; + return false; + } + + auto writeToFile = [](const auto &path, const auto &value) -> bool { + if (!std::filesystem::exists(path)) { + std::cerr << __func__ << "\tno such file: " << path << std::endl; + + return false; + } + + std::ofstream ofs(path, std::ios::out | std::ios::trunc); + + if (!ofs) { + std::cerr << __func__ << "\tcannot open" << path << "for writing" + << std::endl; + + return false; + } + + ofs << value; + + return true; + }; + + const std::filesystem::path pwmSystemRoot{"/sys/class/pwm"}; + const auto pwmChipRoot = pwmSystemRoot / m_pwmChip; + const auto pwmExportFile = pwmChipRoot / "export"; + + if (!writeToFile(pwmExportFile, m_pwm)) + return false; + + const auto pwmRoot = pwmChipRoot / m_pwm; + const auto periodFilename = pwmRoot / "period"; + constexpr unsigned periodHz{50'000}; + + if (!writeToFile(periodFilename, periodHz)) + return false; + + const auto dutyCycleFilename = pwmRoot / "duty_cycle"; + const unsigned dutyCycle{3'000}; + + if (!writeToFile(dutyCycleFilename, dutyCycle)) + return false; + + const auto enableFilename = pwmRoot / "enable"; + const int enable{1}; + + if (!writeToFile(enableFilename, enable)) + return false; + + return true; +} diff --git a/src/laser.h b/src/laser.h new file mode 100644 index 0000000..31fe7e3 --- /dev/null +++ b/src/laser.h @@ -0,0 +1,33 @@ +#pragma once + +#include + +class ILaser +{ +public: + virtual ~ILaser() = default; + +public: + virtual bool init() = 0; + virtual bool setEnabled(bool enabled) = 0; + virtual bool setLaserLevel(std::size_t level) = 0; + +public: + bool enable() { return setEnabled(true); } + bool disable() { return setEnabled(false); } +}; + +class PwmLaser : public ILaser +{ +public: + explicit PwmLaser(const std::string& pwmChip, const std::string& pwm); + ~PwmLaser() override = default; + + // ILaser +public: + bool init() override; + +private: + std::string m_pwmChip; + std::string m_pwm; +}; diff --git a/src/pixels.cpp b/src/pixels.cpp index 5aac4a6..fcdfac7 100644 --- a/src/pixels.cpp +++ b/src/pixels.cpp @@ -5,39 +5,37 @@ #include #include -Pixels& Pixels::operator+=(const Pixels& other) +Pixels& Pixels::operator+=( + const Pixels& other) { - std::transform( - std::execution::par, - pixels.begin(), - pixels.end(), - other.pixels.begin(), - pixels.begin(), - // [](auto& toAdd) { return dst += src; }); - std::plus<>() - ); + std::transform(std::execution::par, + pixels.begin(), + pixels.end(), + other.pixels.begin(), + pixels.begin(), + // [](auto& toAdd) { return dst += src; }); + std::plus<>()); return *this; } -Pixels& Pixels::operator/=(const float divider) +Pixels& Pixels::operator/=( + const float divider) { - std::for_each( - std::execution::par_unseq, - pixels.begin(), - pixels.end(), - [divider](auto& pixel) { pixel /= divider; } - ); + std::for_each(std::execution::par_unseq, + pixels.begin(), + pixels.end(), + [divider](auto& pixel) { pixel /= divider; }); return *this; } -std::optional Pixels::load(const QString& filename) +std::optional Pixels::load( + const QString& filename) { const std::filesystem::path filepath{filename.toStdString()}; - if (!std::filesystem::exists(filepath)) - { + if (!std::filesystem::exists(filepath)) { std::cerr << "no such file: " << filepath << std::endl; return {}; @@ -52,8 +50,7 @@ std::optional Pixels::load(const QString& filename) ifs.read(reinterpret_cast(&result), sizeof(Pixels)); ifs.close(); - if (!ifs) - { + if (!ifs) { std::cerr << "cannot read " << filepath << std::endl; return {}; @@ -62,14 +59,14 @@ std::optional Pixels::load(const QString& filename) return result; } -bool Pixels::save(const QString& filename) +bool Pixels::save( + const QString& filename) { const std::filesystem::path filepath{filename.toStdString()}; const auto parent_path = filepath.parent_path(); - if (!std::filesystem::exists(parent_path) && - !std::filesystem::create_directories(parent_path)) - { + if (!std::filesystem::exists(parent_path) + && !std::filesystem::create_directories(parent_path)) { std::cerr << "cannot create parent directory for file " << filepath << std::endl; @@ -84,8 +81,7 @@ bool Pixels::save(const QString& filename) ofs.write(reinterpret_cast(this), sizeof(Pixels)); ofs.close(); - if (!ofs) - { + if (!ofs) { std::cerr << "cannot write " << filepath << std::endl; return false; @@ -96,7 +92,12 @@ bool Pixels::save(const QString& filename) Pixels::operator bool() const { - return std::find_if(pixels.cbegin(), pixels.cend(), [](const auto& p) { - return !qFuzzyIsNull(p) && !std::isnan(p); - }) != pixels.cend(); + bool result = std::find_if(pixels.cbegin(), + pixels.cend(), + [](const auto& p) { + return !qFuzzyIsNull(p) && !std::isnan(p); + }) + != pixels.cend(); + // std::cout << __func__ << ":\t" << (result ? "true" : "false") << std::endl; + return result; } -- cgit v1.2.3-70-g09d2