diff options
| -rw-r--r-- | src/camera/veyeimx287m.cpp | 104 | ||||
| -rw-r--r-- | src/camera/veyeimx287m.h | 3 | ||||
| -rw-r--r-- | src/image.cpp | 12 | ||||
| -rw-r--r-- | src/image.h | 8 | ||||
| -rw-r--r-- | src/imagealgos.cpp | 12 | ||||
| -rw-r--r-- | src/main.cpp | 259 |
6 files changed, 117 insertions, 281 deletions
diff --git a/src/camera/veyeimx287m.cpp b/src/camera/veyeimx287m.cpp index 958c985..5b745aa 100644 --- a/src/camera/veyeimx287m.cpp +++ b/src/camera/veyeimx287m.cpp @@ -9,7 +9,9 @@ #include <sys/mman.h> #include <unistd.h> +#include <execution> #include <iostream> +#include <ranges> #include <span> #include <QElapsedTimer> @@ -650,47 +652,37 @@ void VeyeIMX287m::calcFrameLoop(std::stop_token stopToken) { t.start(); - // std::lock_guard buffer_lock{m_bufferMutexes[bufferIdx]}; - // get: 4100-4500 - // memcpy(&image.data, m_videoBuffers[bufferIdx], img_size); - memcpy(&image.data, m_videoBuffers[bufferIdx], radxa_raw_img_size); - // memcpy(&image.data, buffers[bufferIdx].mem[0], radxa_raw_img_size); - // get: 5000-5100 - // memcpy_1by1<img_size / sizeof(ARRAY_TYPE)>((ARRAY_TYPE *) &image.data, - // (ARRAY_TYPE *) m_videoBuffers[bufferIdx]); - // memcpy_1by1<img_size>((std::byte *) &image.data, - // (std::byte *) m_videoBuffers[bufferIdx]); + // auto &src = *(Image::data_t *) m_videoBuffers[bufferIdx]; + auto &src = *(Image::radxa_data_t *) m_videoBuffers[bufferIdx]; + auto &dst = image.data; + Image::copy(dst, src); - // memcpy_neon<img_size / sizeof(ARRAY_TYPE)>((ARRAY_TYPE *) &image.data, - // (ARRAY_TYPE *) m_videoBuffers[bufferIdx]); - // std::cerr << "size: " << img_size / sizeof(ARRAY_TYPE) << std::endl; - // Image::data_t native; - // Image::data_t neon; - // memcpy(&native, m_videoBuffers[bufferIdx], img_size); - // memcpy_neon<img_size / sizeof(ARRAY_TYPE)>((ARRAY_TYPE *) &neon, - // (ARRAY_TYPE *) m_videoBuffers[bufferIdx]); - - // if (memcmp(&native, &neon, img_size) != 0) { - // std::cerr << "different: " << img_size / sizeof(ARRAY_TYPE) << std::endl; + // #pragma omp parallel for num_threads(4) + // for (size_t i = 0; i < img_height; ++i) { + // memcpy(dst[i].data(), src[i].data(), img_width); // } + + // for (std::tuple<Image::row_t, Image::row_t> dst_src : std::views::zip()) + + // auto &dst = *(Image::data_t *) m_videoBuffers[bufferIdx]; + // std::transform(std::execution::unseq, + // src.begin(), + // src.end(), + // dst.begin(), + // src.begin(), + // [](auto &srcRow, auto &dstRow) -> Image::row_t { + // // memcpy(dstRow.data(), srcRow.data(), img_width); + // return dstRow; + // // return Image::row_t(dst.begin(), dst.begin() + img_width); + // }); + // memcpy(&image.data, m_videoBuffers[bufferIdx], radxa_raw_img_size); get_elapsed_ns += t.nsecsElapsed(); } image.rotate(); - // const auto pixels = image.pixels(); const auto pixels = image.sharedPixels(); ++processedCounter; - -#pragma push_macro("emit") -#undef emit - // std::cout << "emit pixels" << std::endl; - newPixels.emit(pixels); -#pragma pop_macro("emit") - // const auto lines = pixelsToLines(*pixels); - // continue; - - // setCamParam(V4L2_CID_EXPOSURE, 18); } } @@ -706,11 +698,11 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex) double elapsedTime = (curr.tv_sec - prev.tv_sec) * 1000.0; // sec to ms elapsedTime += (curr.tv_usec - prev.tv_usec) / 1000.0; // us to ms - if (elapsedTime > 1000.) { + if (elapsedTime > 1000. && processedCounter != 0) { fprintf(stderr, - "fps: %d\tdropped: %d sec: %d " - "dq: %d get: %d rot: %d pix: %d sum: %d corr: " - "%d val: %d\n", + "fps: %d\tdropped: %lu sec: %ld " + "dq: %lu get: %lu rot: %lu pix: %lu sum: %lu corr: " + "%lu val: %lu\n", counter, dropped_count, curr.tv_sec % 1000, @@ -753,25 +745,20 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex) buf.m.planes = planes; static uint16_t requestIdx{0}; // buffer.index = BUFFER_COUNT; + const auto tmpIdx = requestIdx++ % BUFFER_COUNT; buf.index = requestIdx++ % BUFFER_COUNT; { QElapsedTimer t; t.start(); - // { - // std::lock_guard<std::mutex> lock(m_queueMtx); - - // if (m_buffersQueue.size() >= BUFFER_COUNT) { - // // std::cout << "frame skip" << std::endl; - // dq_elapsed_ns += t.nsecsElapsed(); - // return false; - // } - // } auto oldIdx = buf.index; // m_bufferMutexes[buffer.index].lock(); - std::lock_guard<std::mutex> lock(m_camMtx); - ret = ioctl(m_cam_fd, VIDIOC_DQBUF, &buf); + { + std::lock_guard<std::mutex> lock(m_camMtx); + ret = ioctl(m_cam_fd, VIDIOC_DQBUF, &buf); + } + // m_bufferMutexes[buffer.index].unlock(); auto newIdx = buf.index; @@ -798,7 +785,7 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex) // auto &image = video_buffer_ptr[buffer.index]; imageIndex = buf.index; - // std::cout << "index: " << imageIndex << std::endl; + // std::cout << "index: " << imageIndex << '\t' << tmpIdx << std::endl; auto &image = m_images[buf.index]; image.height = img_height; image.width = img_width; @@ -812,21 +799,14 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex) image.counters.timestampUs = buf.timestamp.tv_sec * 1000 * 1000 + buf.timestamp.tv_usec; - // { - // QElapsedTimer t; - // t.start(); - // memcpy(&image.data, m_videoBuffers[buffer.index], img_size); - // // std::cout << (void *) video_buffer_ptr[buffer.index] << std::endl; - // get_elapsed_ns += t.nsecsElapsed(); - // } - { std::lock_guard<std::mutex> lock(m_camMtx); ret = ioctl(m_cam_fd, VIDIOC_QBUF, &buf); } if (ret != 0) { - DBG("ioctl(VIDIOC_QBUF) failed %d(%s)", errno, strerror(errno)); + std::cerr << "ioctl(VIDIOC_QBUF) failed: " << errno << " (" << strerror(errno) << ")" + << std::endl; return false; } @@ -846,7 +826,15 @@ bool VeyeIMX287m::getImage(Image &image) { QElapsedTimer t; t.start(); - memcpy(&image.data, m_videoBuffers[bufferIdx], img_size); + // memcpy(&image.data, m_videoBuffers[bufferIdx], img_size); + auto &src = *(Image::radxa_data_t *) m_videoBuffers[bufferIdx]; + auto &dst = image.data; + Image::copy(dst, src); + + // #pragma omp parallel for num_threads(4) + // for (size_t i = 0; i < img_height; ++i) { + // memcpy(dst[i].data(), src[i].data(), img_width); + // } // memcpy(&image.data, buffers[bufferIdx].mem[0], img_size); get_elapsed_ns += t.nsecsElapsed(); } diff --git a/src/camera/veyeimx287m.h b/src/camera/veyeimx287m.h index e1eb76d..b28ce96 100644 --- a/src/camera/veyeimx287m.h +++ b/src/camera/veyeimx287m.h @@ -44,6 +44,7 @@ public: public: // libcamera::Signal<std::shared_ptr<Pixels>> newPixels; // libcamera::Signal<std::shared_ptr<Image>> newImage; + uint32_t processedCounter{0}; private: bool setCamParam(unsigned int v4l2controlId, int value); @@ -81,6 +82,4 @@ private: std::jthread m_streamThread; // std::jthread m_calcThreads[1]; std::jthread m_calcThreads[4]; - - uint32_t processedCounter{0}; }; diff --git a/src/image.cpp b/src/image.cpp index e1f7731..2962947 100644 --- a/src/image.cpp +++ b/src/image.cpp @@ -16,6 +16,7 @@ uint64_t corr_elapsed_ns = 0; uint64_t max_elapsed_ns = 0; uint64_t value_elapsed_ns = 0; uint64_t rot_elapsed_ns = 0; +// time spent for conversion of image to pixels uint64_t pix_elapsed_ns = 0; uint64_t dropped_count = 0; @@ -178,8 +179,10 @@ void Image::rotate() #ifdef RADXA_ZERO_3E for (size_t i = 0; i < img_height; ++i) { + // const Image::row_t row = data[img_height - i - 1]; for (size_t j = 0; j < img_width; ++j) { rotated_cw[j][i] = data[img_height - i - 1][j]; + // rotated_cw[j][i] = row[j]; } } #else @@ -212,10 +215,10 @@ std::shared_ptr<Pixels> Image::sharedPixels() t.start(); static auto result = std::make_shared<Pixels>(); - pix_elapsed_ns += t.nsecsElapsed(); result->counters = counters; std::transform(rotated_cw.cbegin(), rotated_cw.cend(), result->pixels.begin(), process_column); + pix_elapsed_ns += t.nsecsElapsed(); return result; } @@ -255,3 +258,10 @@ void Image::copyFromData(const void *src, size_t size) break; } } + +void Image::copy(data_t &dst, const radxa_data_t &src) +{ + for (size_t i = 0; i < img_height; ++i) { + memcpy(dst[i].data(), src[i].data(), img_width); + } +} diff --git a/src/image.h b/src/image.h index a59e209..178f0ee 100644 --- a/src/image.h +++ b/src/image.h @@ -20,8 +20,10 @@ struct Image // uint8_t data[img_height][img_width] = {{0}}; #ifdef RADXA_ZERO_3E - using row_t = std::array<uint8_t, radxa_raw_img_stride>; - // using row_t = std::array<uint8_t, img_width>; + using radxa_row_t = std::array<uint8_t, radxa_raw_img_stride>; + using radxa_data_t = std::array<radxa_row_t, img_height>; + // using row_t = std::array<uint8_t, radxa_raw_img_stride>; + using row_t = std::array<uint8_t, img_width>; using data_t = std::array<row_t, img_height>; #else using row_t = std::array<uint8_t, img_width>; @@ -52,6 +54,8 @@ struct Image std::shared_ptr<Pixels> sharedPixels(); void copyFromData(const void* src, size_t size); + static void copy(data_t &dst, const radxa_data_t &src); + private: Image &operator=(Image &other) = delete; }; diff --git a/src/imagealgos.cpp b/src/imagealgos.cpp index 9228749..a293803 100644 --- a/src/imagealgos.cpp +++ b/src/imagealgos.cpp @@ -35,13 +35,13 @@ size_t pgm_save(Image *img, FILE *outfile, bool really_save) n += sprintf((char*)pgm_image, "P5\n%d %d\n%d\n", img->width, img->height, 0xFF); -#ifdef RADXA_ZERO_3E - for (size_t row{0}; row < img_height; ++row) { - memcpy(pgm_image + n + row * img_width, img->data[row].data(), img_width); - } -#else + // #ifdef RADXA_ZERO_3E + // for (size_t row{0}; row < img_height; ++row) { + // memcpy(pgm_image + n + row * img_width, img->data[row].data(), img_width); + // } + // #else memcpy(pgm_image + n, img->data.data(), sizeof(img->data)); -#endif // RADXA_ZERO_3E + // #endif // RADXA_ZERO_3E // memcpy(pgm_image + n, img->data->data(), sizeof(img->data)); n += sizeof(img->data); pgm_image_size = n; diff --git a/src/main.cpp b/src/main.cpp index 2ec6c0c..422a476 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -444,10 +444,6 @@ int main(int argc, char* argv[]) QHttpServer qHttpServer; auto httpGetImage = [&]() -> QHttpServerResponse { - // return QHttpServerResponse::StatusCode::ServiceUnavailable; - // std::cout << "http: image" << std::endl << std::flush; - // FILE *f = fopen("/tmp/img.pgm", "w"); - // static bool save = false; auto cam = dynamic_cast<VeyeIMX287m *>(camera.get()); if (!cam) { @@ -456,28 +452,21 @@ int main(int argc, char* argv[]) } static Image img; - // yeaah - // ::img = &img; + if (!cam->getImage(img)) { qDebug() << "cannot get image"; return QHttpServerResponse::StatusCode::ServiceUnavailable; } - // std::cout << "http: got image" << std::endl << std::flush; - // pgm_save(::img); + pgm_save(&img); std::lock_guard<std::mutex> lg(pgm_image_mtx); - // save = false; - // qDebug() << "mutex locked"; - // qDebug() << "image saved to array"; + return QHttpServerResponse{QByteArray((const char *) pgm_image, pgm_image_size), QHttpServerResponse::StatusCode::Ok}; }; qHttpServer.route("/v1/sensor/image", httpGetImage); qHttpServer.route("/v1/sensor/image2", httpGetImage); - // qHttpServer.route("/v1/sensor/exposureTimeUs", [&]() { - // // std::lock_guard<std::mutex> lg(pgm_image_mtx); - // return "123"; - // }); + qHttpServer.route("/v1/pixels", [&]() -> QHttpServerResponse { // std::cout << "http: pixels" << std::endl; // return QHttpServerResponse::StatusCode::ServiceUnavailable; @@ -498,99 +487,51 @@ int main(int argc, char* argv[]) } ::pixels = std::move(img.pixels()); - - // std::lock_guard<std::mutex> lg(pgm_image_mtx); + ++cam->processedCounter; // qt json does not allow to limit double precision - - QJsonArray pixels; - - for (size_t i = 0; i < img_width; ++i) { - // pixels << img_height - img.pixels[i]; - pixels << ::pixels.pixels[i]; - // rjPixels.PushBack(::pixels.pixels[i], jd.GetAllocator()); - // rjPixels.PushBack(rapidjson::Value(::pixels.pixels[i]), al); - } - - QJsonObject json; - json["pixels"] = pixels; - // json["encoderPosition"] = qint64{encoder.position()}; - // FIXME: get prom pixels struct - json["measurementCounter"] = qint64{img.counters.measurementCounter}; - json["timestampUs"] = qint64(img.counters.timestampUs); - const auto lines = pixelsToLines(::pixels); - // qDebug() << "lines count is " << lines.count(); + const auto nan2zero = [](const auto &value) { return qIsNaN(value) ? 0 : value; }; + rapidjson::Document jd; + jd.SetObject(); + auto &al = jd.GetAllocator(); - QJsonArray jsonLines; + rapidjson::Value rjPixels{rapidjson::kArrayType}; - for (const auto& l : lines) - { - const auto p1 = (l.p1() * 1000).toPoint().toPointF() / 1000.; - const auto p2 = (l.p2() * 1000).toPoint().toPointF() / 1000.; - // jsonLines << QJsonArray{QJsonArray{l.p1().x(), l.p1().y()}, - // QJsonArray{l.p2().x(), l.p2().y()}}; - jsonLines << QJsonArray{QJsonArray{p1.x(), p1.y()}, QJsonArray{p2.x(), p2.y()}}; + for (size_t i = 0; i < img_width; ++i) { + rjPixels.PushBack(nan2zero(::pixels.pixels[i]), al); } - json["lines"] = jsonLines; + rapidjson::Value rjLines{rapidjson::kArrayType}; - const auto result = QJsonDocument(json).toJson(); - // qDebug() << "pixels answer size is" << result.size(); - // static bool done{false}; - // if (!done) { - // qDebug().noquote() << result; - // done = true; - // } - - { - rapidjson::Document jd; - jd.SetObject(); - auto &al = jd.GetAllocator(); - rapidjson::Value rjPixels{rapidjson::kArrayType}; - for (size_t i = 0; i < img_width; ++i) { - if (qIsNaN(::pixels.pixels[i])) { - rjPixels.PushBack(double(0), al); - } else { - rjPixels.PushBack(double(::pixels.pixels[i]), al); - } - } - jd.AddMember("measurementCounter", img.counters.measurementCounter, al); - jd.AddMember("timestampUs", img.counters.timestampUs, al); - jd.AddMember("pixels", rjPixels.Move(), al); - rapidjson::StringBuffer buffer; - rapidjson::Writer<rapidjson::StringBuffer> writer(buffer); - writer.SetMaxDecimalPlaces(2); - jd.Accept(writer); - QString res{(const char *) buffer.GetString()}; - // qDebug() << "size:" << res.size(); - // qDebug().noquote() << "ret pix"; + for (const auto &l : lines) { + rapidjson::Value rjLineP1{rapidjson::kArrayType}; + rjLineP1.PushBack(nan2zero(l.p1().x()), al).PushBack(nan2zero(l.p1().y()), al); + rapidjson::Value rjLineP2{rapidjson::kArrayType}; + rjLineP2.PushBack(nan2zero(l.p2().x()), al).PushBack(nan2zero(l.p2().y()), al); + rapidjson::Value rjLinePoints{rapidjson::kArrayType}; + rjLinePoints.PushBack(rjLineP1, al).PushBack(rjLineP2, al); - return QHttpServerResponse{res}; + rjLines.PushBack(rjLinePoints, al); } - // { - // std::srand(static_cast<unsigned>(std::time(nullptr))); - // rapidjson::Document doc; - // doc.SetObject(); - // rapidjson::Document::AllocatorType &allocator = doc.GetAllocator(); - // rapidjson::Value valuesArray(rapidjson::kArrayType); - // // for (int i = 0; i < 10; ++i) { - // // valuesArray.PushBack(rapidjson::Value(rand() % 100), allocator); - // // } - // for (size_t i = 0; i < img_width; ++i) { - // valuesArray.PushBack(rapidjson::Value(::pixels.pixels[i]), allocator); - // } - // doc.AddMember("values", valuesArray, allocator); - // rapidjson::StringBuffer buffer; - // rapidjson::Writer<rapidjson::StringBuffer> writer(buffer); - // doc.Accept(writer); - // std::cout << buffer.GetString() << "\n"; - // } + // jd.AddMember("encoderPosition", qint64{encoder.position()}); + // FIXME: get prom pixels struct + jd.AddMember("measurementCounter", img.counters.measurementCounter, al); + jd.AddMember("timestampUs", img.counters.timestampUs, al); + jd.AddMember("pixels", rjPixels.Move(), al); + jd.AddMember("lines", rjLines.Move(), al); - // qDebug() << "size:" << result.size(); - return QHttpServerResponse(result); + rapidjson::StringBuffer buffer; + rapidjson::Writer<rapidjson::StringBuffer> writer(buffer); + writer.SetMaxDecimalPlaces(2); + jd.Accept(writer); + QString res{(const char *) buffer.GetString()}; + // qDebug() << "size:" << res.size(); + // qDebug().noquote() << "ret pix"; + + return QHttpServerResponse{res}; }); qHttpServer.route("/v1/profile", [&]() -> QHttpServerResponse { @@ -644,41 +585,21 @@ int main(int argc, char* argv[]) return QHttpServerResponse::StatusCode::Ok; }); - qHttpServer - .route("/v1/commands/gCode", - [&](const QHttpServerRequest& request) -> QHttpServerResponse { - std::cout << "http: gCode" << std::endl; - if (request.method() != QHttpServerRequest::Method::Post) - { - return QHttpServerResponse::StatusCode::NotFound; - } + qHttpServer.route("/v1/commands/gCode", + [&](const QHttpServerRequest &request) -> QHttpServerResponse { + std::cout << "http: gCode" << std::endl; + if (request.method() != QHttpServerRequest::Method::Post) { + return QHttpServerResponse::StatusCode::NotFound; + } - const auto command = request.body(); + const auto command = request.body(); - qDebug() << "send gCode:" << command; + qDebug() << "send gCode:" << command; - // printerClient.sendCommand(command); + // printerClient.sendCommand(command); - return QHttpServerResponse::StatusCode::Ok; - }); - - // qHttpServer - // .route("/v1/commands/startCalibration", - // [&](const QHttpServerRequest& request) -> QHttpServerResponse { - // std::cout << "http: startCalibration" << std::endl; - // 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; - // }); + return QHttpServerResponse::StatusCode::Ok; + }); qHttpServer.route( "/v1/sensor/params", @@ -690,30 +611,6 @@ int main(int argc, char* argv[]) std::lock_guard<std::mutex> lg(pgm_image_mtx); QJsonObject json; - // const libcamera::ControlIdMap& ctrlIdMap = - // camera->controls().idmap(); - - // qDebug() << "readParams:" << lastControls.size(); - // qDebug() << request.method(); - - // for (const auto& [id, value] : lastControls) - // { - // const libcamera::ControlId* controlId = ctrlIdMap.at(id); - // auto name = QString::fromStdString(controlId->name()); - // const auto valueStr = - // QString::fromStdString(value.toString()); - // qDebug() - // << "\t param:" << controlId->id() << name << valueStr; - - // name[0] = name[0].toLower(); - // json[name] = valueStr; - // } - - // json[laserLevelKey] = requested_params.laserLevel; - - // qDebug() << "response body:" << json; - - // QHttpServerResponse return QHttpServerResponse(QJsonDocument(json).toJson()); } @@ -748,9 +645,6 @@ int main(int argc, char* argv[]) return QHttpServerResponse::StatusCode:: RequestRangeNotSatisfiable; - // qDebug() << "set gain:" << value; - - // requested_params.exposureTime = value; if (!camera->setGain(value)) return QHttpServerResponse::StatusCode:: RequestRangeNotSatisfiable; @@ -760,37 +654,11 @@ int main(int argc, char* argv[]) { const int32_t value{json[laserLevelKey].toInt()}; - // if (value == 0) - // { - // return QHttpServerResponse::StatusCode::NotFound; - // } - - // qDebug() << "set new laserLevel:" << value; if (!camera->setLaserLevel(value)) return QHttpServerResponse::StatusCode:: RequestRangeNotSatisfiable; requested_params.laserLevel = value; - - // const QString laserLevelFile{ - // "/sys/class/pwm/pwmchip2/pwm1/duty_cycle"}; - // QFile f{laserLevelFile}; - - // if (!f.open(QFile::ReadWrite)) - // { - // qDebug() << "cannot open laser level file:" - // << f.errorString(); - // qDebug() << "file path is" << f.fileName(); - // return QHttpServerResponse::StatusCode::InternalServerError; - // } - - // QTextStream s{&f}; - - // s << value; - - // s >> requested_params.laserLevel; - - // qDebug() << "done with laser level"; } return QHttpServerResponse(request.body()); @@ -804,46 +672,13 @@ int main(int argc, char* argv[]) qDebug() << "listen: " << qHttpServer.listen(QHostAddress::Any, 8081); - // QFuture<void> future = QtConcurrent::run([]() { - // Port port(8080); - // Address addr(Ipv4::any(), port); - - // HttpService httpService(addr); - - // size_t threads_count = 1; - // httpService.init(threads_count); - // httpService.start(); - // }); - //////////////////////////////////////////////////////////////////////////// std::clog << std::flush; std::cerr << std::flush; std::cout << "ok for now" << std::endl << std::flush; - // camera->stop(); - // camera->release(); - // cm->stop(); - auto result = app.exec(); - // future.cancel(); - // future.waitForFinished(); - - // for (auto& [fd, mem] : mappedBuffers_) - // { - // munmap(mem.first, mem.second); - // } - - // FIXME: crash somewhere here. proper libcamera finishing needed - // requests.clear(); - // mappedBuffers_.clear(); - - // camera->stop(); - // config.reset(); - // allocator->free(stream); - // allocator.reset(); - // camera->release(); - // camera.reset(); #ifndef INNO_MAKER cm->stop(); #endif |