diff options
| -rw-r--r-- | CMakeLists.txt | 13 | ||||
| -rw-r--r-- | imagealgos.cpp | 45 | ||||
| -rw-r--r-- | imagealgos.h | 11 | ||||
| -rw-r--r-- | macro.h | 9 | ||||
| -rw-r--r-- | main.cpp | 236 | ||||
| -rw-r--r-- | rotaryencoder.cpp | 107 | ||||
| -rw-r--r-- | rotaryencoder.h | 12 |
7 files changed, 346 insertions, 87 deletions
diff --git a/CMakeLists.txt b/CMakeLists.txt index a1d1b47..ccbc957 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,8 +39,11 @@ set(LIBCAMERA_LIBRARIES "${LIBCAMERA_LIBRARY}" "${LIBCAMERA_BASE_LIBRARY}") project(orpheus LANGUAGES CXX) set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX20_STANDARD_COMPILE_OPTION "-std:c++latest") +set(CMAKE_CXX20_EXTENSION_COMPILE_OPTION "-std:c++latest") -find_package(Qt6 6.4 REQUIRED COMPONENTS Quick HttpServer) +find_package(Qt6 6.4 REQUIRED COMPONENTS Quick HttpServer SerialPort) qt_standard_project_setup(REQUIRES 6.4) @@ -61,12 +64,16 @@ qt_add_executable(apporpheus LibCamera.cpp genetic_algos.h genetic_algos.cpp + rotaryencoder.h + rotaryencoder.cpp ) target_link_libraries(app${PROJECT_NAME} PRIVATE "${LIBCAMERA_LIBRARIES}" PkgConfig::Pistache Qt6::HttpServer + Qt6::SerialPort + wiringPi ) include(GNUInstallDirs) @@ -81,3 +88,7 @@ if ("${CMAKE_BUILD_TYPE}" MATCHES "Release") else() #message("it's not Debug") endif() + + +# add_compile_options(-Wall -Wextra -Wpedantic) +add_compile_options(-Ofast -fno-unroll-loops) diff --git a/imagealgos.cpp b/imagealgos.cpp index 23902d1..e22f1bd 100644 --- a/imagealgos.cpp +++ b/imagealgos.cpp @@ -95,11 +95,11 @@ void rotate(Image &image) using namespace std; - for (size_t i = 0; i < image.height; ++i) + for (size_t i = 0; i < img_height; ++i) { - for (size_t j = 0; j < image.width; ++j) + for (size_t j = 0; j < img_width; ++j) { - image.rotated_cw[j][i] = image.data[image.height - i][j]; + image.rotated_cw[j][i] = image.data[img_height - i][j]; } } @@ -109,12 +109,19 @@ void rotate(Image &image) template<class T, size_t N> constexpr size_t mysize(T (&)[N]) { return N; } +void smooth_column(uint16_t (&column)[]) { + for (size_t i = 1; i < img_height - 1; ++i) { + column[i] = median3(column[i - 1], column[i], column[i + 1]); + } +} + float process_column(uint16_t (&column)[]) { + start_timer(process_column); + float result = std::numeric_limits<float>::quiet_NaN(); - constexpr uint32_t patternSize = 16; // good - constexpr uint32_t signalThreshold = 450; // = SKO * sqrt(patternSize) + constexpr uint32_t signalThreshold = 900; // = SKO * sqrt(patternSize) static constexpr uint32_t patternOffset = patternSize - ((patternSize % 2 == 1) ? 1 : 0); const uint32_t correlationSize = img_height - patternSize + ((patternSize % 2 == 1) ? 1 : 0); @@ -125,13 +132,28 @@ float process_column(uint16_t (&column)[]) int32_t y1 = 0; int32_t y2 = 0; - memset(correlation, 0, img_height * sizeof(uint32_t)); + memset(correlation, 0, img_height * sizeof(correlation[0])); integralSum[0] = 0; for(uint32_t i = 1; i < img_height; ++i) { - if (column[i] < 100) { - column[i] = 1; - } + // if (column[i + 0] < 100) { column[i + 0] = 0; } integralSum[i + 0] = column[i + 0] / 256 + integralSum[i + 0 - 1]; + // if (column[i + 1] < 100) { column[i + 1] = 0; } integralSum[i + 1] = column[i + 1] / 256 + integralSum[i + 1 - 1]; + // if (column[i + 2] < 100) { column[i + 2] = 0; } integralSum[i + 2] = column[i + 2] / 256 + integralSum[i + 2 - 1]; + // if (column[i + 3] < 100) { column[i + 3] = 0; } integralSum[i + 3] = column[i + 3] / 256 + integralSum[i + 3 - 1]; + // if (column[i + 4] < 100) { column[i + 4] = 0; } integralSum[i + 4] = column[i + 4] / 256 + integralSum[i + 4 - 1]; + // if (column[i + 5] < 100) { column[i + 5] = 0; } integralSum[i + 5] = column[i + 5] / 256 + integralSum[i + 5 - 1]; + // if (column[i + 6] < 100) { column[i + 6] = 0; } integralSum[i + 6] = column[i + 6] / 256 + integralSum[i + 6 - 1]; + // if (column[i + 7] < 100) { column[i + 7] = 0; } integralSum[i + 7] = column[i + 7] / 256 + integralSum[i + 7 - 1]; + // if (column[i + 8] < 100) { column[i + 8] = 0; } integralSum[i + 8] = column[i + 8] / 256 + integralSum[i + 8 - 1]; + // if (column[i + 9] < 100) { column[i + 9] = 0; } integralSum[i + 9] = column[i + 9] / 256 + integralSum[i + 9 - 1]; + // if (column[i + 10] < 100) { column[i + 10] = 0; } integralSum[i + 10] = column[i + 10] / 256 + integralSum[i + 10 - 1]; + // if (column[i + 11] < 100) { column[i + 11] = 0; } integralSum[i + 11] = column[i + 11] / 256 + integralSum[i + 11 - 1]; + // if (column[i + 12] < 100) { column[i + 12] = 0; } integralSum[i + 12] = column[i + 12] / 256 + integralSum[i + 12 - 1]; + // if (column[i + 13] < 100) { column[i + 13] = 0; } integralSum[i + 13] = column[i + 13] / 256 + integralSum[i + 13 - 1]; + // if (column[i + 14] < 100) { column[i + 14] = 0; } integralSum[i + 14] = column[i + 14] / 256 + integralSum[i + 14 - 1]; + // if (column[i + 15] < 100) { column[i + 15] = 0; } integralSum[i + 15] = column[i + 15] / 256 + integralSum[i + 15 - 1]; + + if (column[i] < 100) { column[i] = 0; } integralSum[i] = column[i] / 256 + integralSum[i - 1]; } @@ -198,6 +220,8 @@ float process_column(uint16_t (&column)[]) } // std::cout << "result is '" << result << "'\n"; + // stop_timer(process_column); + return result; // center of mass @@ -227,11 +251,12 @@ float process_column(uint16_t (&column)[]) void process_columns(Image &image) { - std::cout << "here\n"; + // std::cout << "here\n"; start_timer(process_columns); for (size_t i = 0; i < image.width; i++) { + // smooth_column(image.rotated_cw[i]); image.pixels[i] = process_column(image.rotated_cw[i]); // Algo genetic(image.rotated_cw[i]); diff --git a/imagealgos.h b/imagealgos.h index b1efc87..bf5611a 100644 --- a/imagealgos.h +++ b/imagealgos.h @@ -7,6 +7,7 @@ constexpr size_t img_width = 1280; constexpr size_t img_height = 800; +constexpr uint32_t patternSize = 16; struct Image { @@ -18,12 +19,20 @@ struct Image unsigned int stride; libcamera::PixelFormat pixelFormat; float pixels[img_width]; + unsigned int measurementCounter; + uint64_t timestampNs; }; -size_t pgm_save(Image *img, FILE *outfile, bool really_save = true); +size_t pgm_save(Image *img, FILE *outfile, bool really_save = false); void unpack_10bit(uint8_t const *src, Image const &image, uint16_t *dest); void unpack_16bit(uint8_t const *src, Image const &image, uint16_t *dest); +template <typename T> +T median3(const T& a, const T& b, const T& c) { + using namespace std; + return max(min(a,b), min(max(a,b),c)); +} + void rotate(Image & image); void process_columns(Image & image); @@ -1,11 +1,18 @@ #pragma once +#include <chrono> +#include <iostream> + #define start_timer(name) \ std::chrono::steady_clock::time_point begin_ ## name = \ std::chrono::steady_clock::now(); \ \ #define stop_timer(name) \ -std::chrono::steady_clock::time_point end_ ## name = \ + std::chrono::steady_clock::time_point end_ ## name = \ std::chrono::steady_clock::now(); \ + // std::cout << #name << " (us): " \ + // << std::chrono::duration_cast<std::chrono::microseconds>( \ + // end_ ## name - begin_ ## name) \ + // << std::endl; @@ -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) diff --git a/rotaryencoder.cpp b/rotaryencoder.cpp new file mode 100644 index 0000000..41d56e8 --- /dev/null +++ b/rotaryencoder.cpp @@ -0,0 +1,107 @@ +#include "rotaryencoder.h" + +#include <cstdint> +#include <cstdio> +#include <cstdlib> +#include <vector> + +#include <QDebug> +#include <QElapsedTimer> + +#include <wiringPi.h> + +// got from https://gist.github.com/ast/a19813fce9d34c7240091db11b8190dd +// https://gist.github.com/ast +// Inspired by Paul Stoffregen's excellent Arduino library Encoder: +// https://github.com/PaulStoffregen/Encoder + +constexpr int gpioA = 17; +constexpr int gpioB = 27; +// constexpr int gpioA = 0; +// constexpr int gpioB = 2; +// constexpr int gpioA = 11; +// constexpr int gpioB = 13; + +const std::vector<int> encoderTable = { + 0, 1, -1, 0, -1, 0, 0, 1, 1, 0, 0, -1, 0, -1, 1, 0 +}; + +volatile int64_t positionSteps; +volatile uint8_t state; + +void pin_isr(void) { + uint8_t p1val = digitalRead(gpioA); + uint8_t p2val = digitalRead(gpioB); + uint8_t s = state & 3; + + if (p1val) s |= 4; + if (p2val) s |= 8; + + state = (s >> 2); + + switch (s) { + case 1: case 7: case 8: case 14: + positionSteps = positionSteps + 1; + return; + case 2: case 4: case 11: case 13: + positionSteps = positionSteps - 1; + return; + case 3: case 12: + positionSteps = positionSteps + 2; + return; + case 6: case 9: + positionSteps = positionSteps - 2; + return; + } +} + +rotaryencoder::rotaryencoder() +{ + QElapsedTimer t; + t.start(); + + if (wiringPiSetupGpio()) { + perror("wiringPiSetupGpio"); + exit(EXIT_FAILURE); + } + + qDebug() << "msecs to setup wiringPi:" << t.elapsed(); + + if ( wiringPiISR (gpioA, INT_EDGE_BOTH, &pin_isr) < 0 ) { + perror("wiringPiISR"); + exit(EXIT_FAILURE); + } + + qDebug() << "msecs to register interruption A:" << t.elapsed(); + + if ( wiringPiISR (gpioB, INT_EDGE_BOTH, &pin_isr) < 0 ) { + perror("wiringPiISR"); + exit(EXIT_FAILURE); + } + + qDebug() << "msecs to register interruption B:" << t.elapsed(); + + // pinMode (gpioA, INPUT) ; + // pinMode (gpioB, INPUT) ; + // pullUpDnControl(gpioA, PUD_UP); + // pullUpDnControl(gpioB, PUD_UP); + + qDebug() << "encoder is ok"; + + // // Show position every second + // while ( 1 ) { + // constexpr double stepsPerMm = 200; + // const double positionMm = ::positionSteps / stepsPerMm; + // qDebug() << ::positionSteps + // << '-' + // << QString::number(positionMm, 'f', 3) + // << "(mm)"; + // // printf( "%ld\n", ::position); + // usleep( 100 * 1000 ); // wait 1 second + // } +} + +int64_t rotaryencoder::position() const +{ + return ::positionSteps; +} diff --git a/rotaryencoder.h b/rotaryencoder.h new file mode 100644 index 0000000..f2f9b76 --- /dev/null +++ b/rotaryencoder.h @@ -0,0 +1,12 @@ +#pragma once + +#include <cstdint> + +class rotaryencoder +{ +public: + rotaryencoder(); + +public: + int64_t position() const; +}; |