diff options
| author | Nikita Kostovsky <nikita@kostovsky.me> | 2025-11-07 23:10:25 +0100 |
|---|---|---|
| committer | Nikita Kostovsky <nikita@kostovsky.me> | 2025-11-07 23:10:25 +0100 |
| commit | c38edfbd30aae121ebdaaac8e8e25f8784da318d (patch) | |
| tree | b67604615c6feac45e503da9a28473c7ed7f8316 /src | |
| parent | a9af2d69c0a4d46dc98b6e0adaf13a7f23a24bce (diff) | |
speedup; add some i2c commands
Diffstat (limited to 'src')
| -rw-r--r-- | src/calibration.cpp | 4 | ||||
| -rw-r--r-- | src/camera/veye_i2c.cpp | 106 | ||||
| -rw-r--r-- | src/camera/veye_i2c.h | 12 | ||||
| -rw-r--r-- | src/camera/veyeimx287m.cpp | 13 | ||||
| -rw-r--r-- | src/camera/veyeimx287m.h | 4 | ||||
| -rw-r--r-- | src/camera/veyeimx287m_types.h | 135 | ||||
| -rw-r--r-- | src/image.cpp | 153 | ||||
| -rw-r--r-- | src/image.h | 1 | ||||
| -rw-r--r-- | src/main.cpp | 14 | ||||
| -rw-r--r-- | src/pixels.cpp | 5 | ||||
| -rw-r--r-- | src/pixels.h | 20 |
11 files changed, 400 insertions, 67 deletions
diff --git a/src/calibration.cpp b/src/calibration.cpp index 3bc0ba5..6cc85a1 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -209,8 +209,8 @@ QList<Pixels> filter( size_t count{1}; ++it; - while (it != rawProfiles.constEnd() && - it->counters.encoderPosition == sum.counters.encoderPosition) { + while (it != rawProfiles.constEnd() + && it->counters.encoderPosition == sum.counters.encoderPosition) { sum += *it; ++count; ++it; diff --git a/src/camera/veye_i2c.cpp b/src/camera/veye_i2c.cpp new file mode 100644 index 0000000..9d04d9b --- /dev/null +++ b/src/camera/veye_i2c.cpp @@ -0,0 +1,106 @@ +#include "veye_i2c.h" + +#include <arpa/inet.h> +#include <errno.h> +#include <fcntl.h> +#include <iostream> +#include <linux/i2c-dev.h> +#include <linux/i2c.h> +#include <string.h> +#include <sys/ioctl.h> +#include <unistd.h> + +#include "veyeimx287m_types.h" + +bool veye::imx287m::i2cRead(int fd, uint8_t i2c_addr, uint16_t reg, uint32_t &value) +{ + int err; + int i = 0; + uint8_t buf[2] = {reg >> 8, reg & 0xff}; + uint8_t bufout[4] = {0}; + struct i2c_msg msgs[2] = { + {.addr = i2c_addr, + .flags = 0, + .len = 2, + // .buf = (uint8_t *) (®), + .buf = buf}, + { + .addr = i2c_addr, + .flags = I2C_M_RD, + .len = 4, + .buf = bufout, + }, + }; + + struct i2c_rdwr_ioctl_data msgset; + msgset.msgs = msgs; + msgset.nmsgs = 2; + + err = ioctl(fd, I2C_RDWR, &msgset); + //printf("Read i2c addr %02X\n", i2c_addr); + if (err != msgset.nmsgs) { + std::cerr << "Read i2c err " << err << std::endl; + return false; + } + + value = ntohl(*(uint32_t *) bufout); + // fprintf(stderr, "addr %04x : value %08x \n", reg + i, value); + + return true; +} + +bool veye::imx287m::test(uint32_t value) +{ + const std::string i2cDevName{"/dev/i2c-10"}; + constexpr uint32_t i2cDevAddr{0x3b}; + int fd = open(i2cDevName.c_str(), O_RDWR); + + if (!fd) { + std::cerr << "cannot open i2c device " << i2cDevName << ", error: " << strerror(errno) + << std::endl; + return false; + } + + if (ioctl(fd, I2C_SLAVE_FORCE, i2cDevAddr) < 0) { + std::cerr << "cannot set i2c slave. dev: " << i2cDevName << ", addr: " << i2cDevAddr + << ", error: " << strerror(errno) << std::endl; + + return false; + } + + uint32_t expTime{0}; + // if (!i2cRead(fd, i2cDevAddr, (uint16_t) veye::imx287m::Register::ME_Time, expTime)) { + // return false; + // } + + // std::cout << "exp time is: " << expTime << std::endl; + std::cout << "set exp time to " << value; + + if (!i2cWrite(fd, (uint16_t) veye::imx287m::Register::ME_Time, value)) { + return false; + } + + if (!i2cRead(fd, i2cDevAddr, (uint16_t) veye::imx287m::Register::ME_Time, expTime)) { + return false; + } + std::cout << "\texp time is: " << expTime << std::endl; + + return true; +} + +bool veye::imx287m::i2cWrite(int fd, uint16_t reg, const uint32_t value) +{ + uint8_t msg[] = {reg >> 8, reg & 0xff, value >> 24, value >> 16, value >> 8, value >> 0}; + int len = sizeof(msg) / sizeof(msg[0]); + + if (write(fd, msg, len) != len) { + std::cerr << "cannot write value. reg: " << reg << ", error: " << strerror(errno) + << std::endl; + return false; + } + + // TODO: find a best way to get up-to-date values + usleep(100); + + return true; +} diff --git a/src/camera/veye_i2c.h b/src/camera/veye_i2c.h new file mode 100644 index 0000000..1638976 --- /dev/null +++ b/src/camera/veye_i2c.h @@ -0,0 +1,12 @@ +#pragma once + +#include <cstdint> + +namespace veye { +namespace imx287m { +bool i2cRead(int fd, uint8_t i2c_addr, uint16_t reg, uint32_t &value); +bool i2cWrite(int fd, uint16_t reg, const uint32_t value); + +bool test(uint32_t value); +} // namespace imx287m +} // namespace veye diff --git a/src/camera/veyeimx287m.cpp b/src/camera/veyeimx287m.cpp index dee3c6a..c7e519a 100644 --- a/src/camera/veyeimx287m.cpp +++ b/src/camera/veyeimx287m.cpp @@ -15,6 +15,7 @@ #include <QElapsedTimer> #include "constants.h" +#include "imagealgos.h" #include "pixels.h" // #include "rotaryencoder.h" @@ -141,7 +142,7 @@ bool VeyeIMX287m::init() bool VeyeIMX287m::setExposureTimeUs(int valueUs) { - return true; + //return true; std::cout << __func__ << ": " << valueUs << std::endl << std::flush; /* @@ -378,7 +379,15 @@ void VeyeIMX287m::calcFrameLoop(std::stop_token stopToken) } image.rotate(); - image.pixels(); + // const auto pixels = image.pixels(); + const auto pixels = image.sharedPixels(); +#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); } diff --git a/src/camera/veyeimx287m.h b/src/camera/veyeimx287m.h index 9986e59..37da5b0 100644 --- a/src/camera/veyeimx287m.h +++ b/src/camera/veyeimx287m.h @@ -42,8 +42,8 @@ public: // Image &getImage(); public: - libcamera::Signal<std::shared_ptr<Pixels>> newPixels; - libcamera::Signal<std::shared_ptr<Image>> newImage; + // libcamera::Signal<std::shared_ptr<Pixels>> newPixels; + // libcamera::Signal<std::shared_ptr<Image>> newImage; private: bool setCamParam(unsigned int v4l2controlId, int value); diff --git a/src/camera/veyeimx287m_types.h b/src/camera/veyeimx287m_types.h new file mode 100644 index 0000000..640a0e1 --- /dev/null +++ b/src/camera/veyeimx287m_types.h @@ -0,0 +1,135 @@ +#pragma once + +#include <QObject> + +namespace veye { +namespace imx287m { + +enum class Register { + ImageAcqu = 0x0400, + ImageFeature = 0x0800, + ImageSigProc = 0x0c00, + IOCtrl = 0x1000, + FPGACmd = 0x1400, + FPGABlock = 0x1800, + + Manufacturer_Name = 0x0000, + Model_Name = 0x0004, + Sensor_Name = 0x0008, + Product_Info = 0x000C, + Device_Version = 0x0010, + System_reset = 0x0014, + Param_save_to_flash = 0x0018, + System_reboot = 0x001C, + Time_stamp = 0x0020, + Error_code = 0x0024, + Format_Cap = 0x0028, + TriggerMode_Cap = 0x0030, + LaneNum_Cap = 0x0034, + Temp_K = 0x0058, + + Image_Acquisition = 0x400, + Trigger_Mode = 0x404, + Trigger_Source = 0x408, + Trigger_Num = 0x40C, + Trigger_Inerval = 0x410, + Trigger_Software = 0x414, + Trigger_Count = 0x418, + I2C_Addr = 0x41C, + I2C_Port_Sel = 0x420, + + User_overlay_enable = 0x428, + User_overlay_zone0 = 0x42C, + User_overlay_zone1 = 0x430, + User_overlay_zone2 = 0x434, + User_overlay_zone3 = 0x438, + User_define_zone0 = 0x43C, + User_define_zone1 = 0x440, + User_define_zone2 = 0x444, + User_define_zone3 = 0x448, + Nondiscontinuous_mode = 0x44C, + Sensor_Reg_Addr = 0x0450, + Sensor_Reg_Val = 0x454, + Slave_mode = 0x460, + Sensor_Frame_Count = 0x464, + Out_Frame_Count = 0x468, + Trigger_Cycle_Min = 0x46C, + Trigger_Cycle_Max = 0x470, + + Test_Image_Selector = 0x800, + Pixel_Format = 0x804, + Sensor_Width = 0x808, + Sensor_Height = 0x80C, + MaxFrame_Rate = 0x810, + Framerate = 0x814, + ROI_Width = 0x818, + ROI_Height = 0x81C, + ROI_Offset_X = 0x820, + ROI_Offset_Y = 0x824, + Image_Direction = 0x828, + Data_shift = 0x82C, + Black_Level = 0x830, + BLC_Mode = 0x834, + ReadOut_Mode = 0x838, + Lane_Num = 0x83C, + MIPI_DataRate = 0x840, + MIN_ROI_Width = 0x844, + MIN_ROI_Height = 0x848, + FrameRate_Ex = 0x850, + OSD_Mode = 0x854, + + ISP_module_ctrl = 0xC00, + Exposure_Mode = 0xC04, + Target_Brightness = 0xC08, + Exposure_Time_Source = 0xC0C, + ME_Time = 0xC10, + AE_MAX_Time = 0xC14, + Exp_Time = 0xC18, + Gain_Mode = 0xC1C, + Manual_Gain = 0xC20, + AG_Max_Gain = 0xC24, + Cur_Gain = 0xC28, + AAROIOffsetX = 0xC2C, + AAROIOffsetY = 0xC30, + AAROIWidth = 0xC34, + AAROIHeight = 0xC38, + WB_Mode = 0xC3C, + MWB_Rgain = 0xC40, + MWB_Bgain = 0xC44, + AWBROIOffsetX = 0xC48, + AWBROIOffsetY = 0xC4C, + AWBROIWidth = 0xC50, + AWBROIHeight = 0xC54, + AWB_Rgain = 0xC58, + AWB_Bgain = 0xC5C, + Gamma = 0xC60, + DPC_Start = 0xC64, + DPC_Status = 0xC68, + DPC_Count = 0xC6C, + AAROI_enable = 0xC80, + Max_Exp_time = 0xC8C, + Min_Exp_time = 0xC90, + + Trigger_Delay = 0x1000, + Trigger_Activation = 0x1004, + Trigger_Filter_Enable = 0x1008, + Trigger_Filter_Width = 0x100C, + Trigger_Exp_Delay = 0x1010, + GPIOS_Status = 0x1014, + + GPIO1_OutSelect = 0x1020, + GPIO1_Useroutput = 0x1024, + GPIO1_Reverse = 0x1028, + + GPIO2_OutSelect = 0x1030, + GPIO2_Useroutput = 0x1034, + GPIO2_Reverse = 0x1038 +}; + +class Params : public QObject +{ + // Q_OBJECT +}; + +} // namespace imx287m +} // namespace veye diff --git a/src/image.cpp b/src/image.cpp index f0c16aa..9c258c6 100644 --- a/src/image.cpp +++ b/src/image.cpp @@ -24,17 +24,19 @@ uint64_t dropped_count = 0; float process_column(const Image::column_t &column) { // Image::column_t c = column; - start_timer(process_column); - QElapsedTimer t; - t.start(); + // start_timer(process_column); + // QElapsedTimer t; + // t.start(); float result = std::numeric_limits<float>::quiet_NaN(); 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); + constexpr uint32_t correlationSize = img_height - patternSize + + ((patternSize % 2 == 1) ? 1 : 0); // constexpr uint32_t correlationSize = img_height - patternSize; - uint32_t correlation[img_height]; + uint32_t correlation[img_height] = {0}; + // memset(correlation, 0, img_height * sizeof(correlation[0])); uint32_t integralSum[img_height]; uint32_t maxTripleSum = signalThreshold * 50; uint32_t x1 = 0; @@ -42,90 +44,103 @@ float process_column(const Image::column_t &column) int32_t y2 = 0; static_assert((img_height % patternSize) == 0, "img_height % patternSize should be 0"); - std::array<uint16_t, img_height / patternSize> sums; - uint16_t maxLALASum{0}; - size_t maxLALAIdx{0}; + // std::array<uint16_t, img_height / patternSize> sums; + // uint16_t maxLALASum{0}; + // size_t maxLALAIdx{0}; // uint16_t sum{0}; - for (size_t i = 0; i < img_height; i += patternSize) { - // const auto sum = std::accumulate(column.cbegin() + i, - // column.cbegin() + i + patternSize, - // 0, - // std::plus<uint16_t>()); - uint16_t sum{0}; + // for (size_t i = 0; i < img_height; i += patternSize) { + // // const auto sum = std::accumulate(column.cbegin() + i, + // // column.cbegin() + i + patternSize, + // // 0, + // // std::plus<uint16_t>()); + // uint16_t sum{0}; - for (size_t j = i; j < i + patternSize; ++j) - sum += column[j]; - // if ((i % patternSize) == 0) - // sum = 0; - // sum += column[i]; + // for (size_t j = i; j < i + patternSize; ++j) + // sum += column[j]; + // // if ((i % patternSize) == 0) + // // sum = 0; + // // sum += column[i]; - // if (sum > 0xff) - // std::cout << sum << ' '; + // // if (sum > 0xff) + // // std::cout << sum << ' '; - if (sum > maxLALASum) { - maxLALASum = sum; - maxLALAIdx = i; - } - } + // if (sum > maxLALASum) { + // maxLALASum = sum; + // maxLALAIdx = i; + // } + // } // maxLALAIdx = patternSize * 32 - 14; // crash // maxLALAIdx = patternSize * 32 - 14; // no crash - maxLALAIdx = std::clamp(uint32_t(maxLALAIdx), - uint32_t(patternSize), - uint32_t(img_height - patternSize * 2 - 14)); + // maxLALAIdx = std::clamp(uint32_t(maxLALAIdx), + // uint32_t(patternSize), + // uint32_t(img_height - patternSize * 2 - 14)); // std::cout << maxLALAIdx << ' '; - memset(correlation, 0, img_height * sizeof(correlation[0])); - // memset(correlation, 0, patternSize / 2); - // integralSum[0] = 0; + // memset(correlation, 0, img_height * sizeof(correlation[0])); + // memset(correlation, 0, patternSize); + // memset(correlation + correlationSize - 1, 0, patternSize); + integralSum[0] = 0; // integralSum[maxLALAIdx] = 0; - memset(integralSum, 0, img_height * sizeof(integralSum[0])); + // memset(integralSum, 0, img_height * sizeof(integralSum[0])); - // for (uint32_t i = 1; i < img_height; ++i) { - for (uint32_t i = maxLALAIdx + 1; i < maxLALAIdx + patternSize * 3; ++i) { + for (uint32_t i = 1; i < img_height; ++i) { + // for (uint32_t i = maxLALAIdx + 1; i < maxLALAIdx + patternSize * 3; ++i) { integralSum[i] = column[i] + integralSum[i - 1]; } - sum_elapsed_ns += t.nsecsElapsed(); - t.restart(); + // sum_elapsed_ns += t.nsecsElapsed(); + // t.restart(); // pixel * <sum of neighbours> - // for (uint32_t i = 0; i < correlationSize; ++i) - for (uint32_t i = maxLALAIdx; i < maxLALAIdx + patternSize * 2; ++i) + for (uint32_t i = 0; i < correlationSize; ++i) + // for (uint32_t i = maxLALAIdx; i < maxLALAIdx + patternSize * 2; ++i) correlation[i + patternSize / 2] = column[i + patternSize / 2] * (integralSum[i + patternOffset] - integralSum[i]); // * (integralSum[i + patternSize] - integralSum[i]); - corr_elapsed_ns += t.nsecsElapsed(); - t.restart(); + // corr_elapsed_ns += t.nsecsElapsed(); + // t.restart(); + + uint32_t cPPP = correlation[0]; + uint32_t cPP = correlation[1]; + uint32_t cP = correlation[2]; + + // uint32_t c = correlation[3]; + // uint32_t cN = correlation[4]; + // uint32_t cNN = correlation[5]; - // for (uint32_t i = 3; i < img_height - 2; ++i) { - for (uint32_t i = maxLALAIdx + 3; i < maxLALAIdx + patternSize * 3 - 2; ++i) { + for (uint32_t i = 3; i < img_height - 2; ++i) { + // for (uint32_t i = maxLALAIdx + 3; i < maxLALAIdx + patternSize * 3 - 2; ++i) { // p - pixel, n - neighbour // P - pixel used in sum, N - neighbour used in sum // [N P N] - const auto sum = correlation[i - 1] + correlation[i] + - correlation[i + 1]; + const uint32_t c = correlation[i]; + const uint32_t cN = correlation[i + 1]; + const uint32_t cNN = correlation[i + 2]; + const auto sum = cP + c + cN; + + // const int32_t rioux0 = int32_t(cPPP + cPP) - int32_t(c + cN); + // const int32_t rioux1 = int32_t(cPP + cP) - int32_t(cN + cNN); + + // if (sum > maxTripleSum && rioux0 < 0 && rioux1 >= 0) { + // x1 = i - 1; + // y1 = rioux0; + // y2 = rioux1; + // maxTripleSum = sum; + // } if (sum > maxTripleSum) { // [N N n p] - [P N] - const int32_t rioux0 = int32_t(correlation[i - 2 - 1] + - correlation[i - 1 - 1]) - - int32_t(correlation[i + 1 - 1] + - correlation[i + 2 - 1]); + const int32_t rioux0 = int32_t(cPPP + cPP) - int32_t(c + cN); - if (rioux0 < 0) - { + if (rioux0 < 0) { // [N N p] - [p N N] - const int32_t rioux1 = int32_t(correlation[i - 2] + - correlation[i - 1]) - - int32_t(correlation[i + 1] + - correlation[i + 2]); + const int32_t rioux1 = int32_t(cPP + cP) - int32_t(cN + cNN); - if (rioux1 >= 0) - { + if (rioux1 >= 0) { x1 = i - 1; y1 = rioux0; y2 = rioux1; @@ -133,10 +148,17 @@ float process_column(const Image::column_t &column) } } } + + cPPP = cPP; + cPP = cP; + cP = c; + // c = cN; + // cN = cNN; + // cNN = correlation[i + 1]; } - value_elapsed_ns += t.nsecsElapsed(); - t.restart(); + // value_elapsed_ns += t.nsecsElapsed(); + // t.restart(); result = (y2 != y1) ? (float(x1) - (float(y1) / (y2 - y1))) : std::numeric_limits<float>::quiet_NaN(); @@ -177,6 +199,19 @@ Pixels Image::pixels() return result; } +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); + + return result; +} + void Image::copyFromData(const void *src, size_t size) { if (Q_UNLIKELY(size % sizeof(data) != 0 || size < sizeof(data))) { diff --git a/src/image.h b/src/image.h index 739c3fb..a3c4476 100644 --- a/src/image.h +++ b/src/image.h @@ -42,6 +42,7 @@ struct Image void rotate(); Pixels pixels(); + std::shared_ptr<Pixels> sharedPixels(); void copyFromData(const void* src, size_t size); private: diff --git a/src/main.cpp b/src/main.cpp index 9548757..ca2da6f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -12,6 +12,7 @@ #include "calibration.h" #include "camera/innomakerov9281.h" #include "camera/ov9281.h" +#include "camera/veye_i2c.h" #include "camera/veyeimx287m.h" #include "dumps.h" #include "fuck_intel.h" @@ -139,9 +140,18 @@ int main(int argc, char* argv[]) auto sigHandler = [](int s) { std::cout << "got signal " << s << std::endl; std::signal(s, SIG_DFL); - qApp->quit(); + // QTimer::singleShot(0, qApp, &QCoreApplication::quit); + // qApp->quit(); }; + // for (int i = 2000; i >= 0; i -= 50) { + // for (int i = 0; i < 2000; i += 50) { + // if (!veye::imx287m::test(i)) { + // return EXIT_FAILURE; + // } + // } + // return EXIT_SUCCESS; + std::signal(SIGINT, sigHandler); std::signal(SIGTERM, sigHandler); @@ -399,7 +409,7 @@ int main(int argc, char* argv[]) #endif std::cout << "connect everything" << std::endl; - // camera->newPixels.connect(&onNewPixels); + camera->newPixels.connect(&onNewPixels); // camera->newImage.connect(&onNewImage); camera->newImageCallback = &onNewImage; camera->newPixelsCallback = &onNewPixels; diff --git a/src/pixels.cpp b/src/pixels.cpp index fcdfac7..3b69c99 100644 --- a/src/pixels.cpp +++ b/src/pixels.cpp @@ -101,3 +101,8 @@ Pixels::operator bool() const // std::cout << __func__ << ":\t" << (result ? "true" : "false") << std::endl; return result; } + +// std::lock_guard<std::mutex> Pixels::lock() +// { +// return std::move(std::lock_guard{m_mtx}); +// } diff --git a/src/pixels.h b/src/pixels.h index 1d5effb..06a8654 100644 --- a/src/pixels.h +++ b/src/pixels.h @@ -1,6 +1,7 @@ #pragma once #include <array> +#include <mutex> #include "fuck_intel.h" @@ -32,4 +33,23 @@ struct Pixels [[nodiscard]] bool save(const QString& filename); operator bool() const; + + // TODO: forbid copying + // Pixels() = default; + // Pixels(Pixels &&) = default; + // Pixels &operator=(Pixels &&) = default; + + // Pixels(Pixels &) = delete; + // Pixels(const Pixels &) = delete; + // Pixels &operator=(Pixels &) = delete; + // Pixels &operator=(const Pixels &) = delete; + + /*! + * \brief lock - lock any write operations on this `Pixels` + * \return lock guard + */ + // std::lock_guard<std::mutex> lock(); + + // private: + // std::mutex m_mtx; }; |
