summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/calibration.cpp28
-rw-r--r--src/calibration.h2
-rw-r--r--src/camera/ov9281.cpp385
-rw-r--r--src/camera/ov9281.h68
-rw-r--r--src/image.cpp110
-rw-r--r--src/image.h21
-rw-r--r--src/pixels.cpp7
-rw-r--r--src/pixels.h3
-rw-r--r--src/typedefs.h18
9 files changed, 626 insertions, 16 deletions
diff --git a/src/calibration.cpp b/src/calibration.cpp
index 73bd786..ff37e73 100644
--- a/src/calibration.cpp
+++ b/src/calibration.cpp
@@ -191,6 +191,34 @@ QImage calibrationTableToImage(const CalibrationTablePtr& calibrationTable)
return result;
}
+QList<Pixels> filter(const QList<Pixels>& rawProfiles)
+{
+ QList<Pixels> result;
+
+ QList<Pixels>::const_iterator it = rawProfiles.constBegin();
+
+ while (it != rawProfiles.constEnd())
+ {
+ Pixels sum = *it;
+
+ size_t count{1};
+ ++it;
+
+ while (it != rawProfiles.constEnd() &&
+ it->counters.encoderPosition == sum.counters.encoderPosition)
+ {
+ sum += *it;
+ ++count;
+ ++it;
+ }
+
+ sum /= float(count);
+ result << sum;
+ }
+
+ return result;
+}
+
CalibrationTablePtr calibrateZ(const QList<Pixels>& rawProfiles,
const uint32_t& stepsPerMm)
{
diff --git a/src/calibration.h b/src/calibration.h
index ddb66a6..c2e5d73 100644
--- a/src/calibration.h
+++ b/src/calibration.h
@@ -21,6 +21,8 @@ bool openCalibrationTable(const QString &filename, CalibrationTablePtr &table);
void dumpCalibrationPixels(std::vector<Pixels> &&calibrationPixels);
bool dump(const CalibrationTablePtr &table, const QString &filename);
+QList<Pixels> filter(const QList<Pixels> &rawProfiles);
+
CalibrationTablePtr calibrateX(const QList<Pixels> &rawProfiles);
CalibrationTablePtr calibrateZ(const QList<Pixels> &rawProfiles,
const uint32_t &stepsPerMm);
diff --git a/src/camera/ov9281.cpp b/src/camera/ov9281.cpp
new file mode 100644
index 0000000..bdb9f89
--- /dev/null
+++ b/src/camera/ov9281.cpp
@@ -0,0 +1,385 @@
+#include "ov9281.h"
+
+#include <iostream>
+#include <thread>
+
+#include <string.h>
+#include <sys/mman.h>
+
+#include <libcamera/camera.h>
+#include <libcamera/camera_manager.h>
+#include <libcamera/control_ids.h>
+#include <libcamera/framebuffer_allocator.h>
+#include <libcamera/request.h>
+
+#include "image.h"
+#include "macro.h"
+#include "pixels.h"
+#include "rotaryencoder.h"
+#include "typedefs.h"
+
+OV9281::OV9281(const std::shared_ptr<libcamera::Camera> &camera)
+ : INIT_FIELD(camera)
+{
+ std::cout << __func__ << ":\tid: " << m_camera->id();
+}
+
+OV9281::~OV9281()
+{
+ for (auto &[fd, mem] : m_mappedBuffers)
+ {
+ munmap(mem.first, mem.second);
+ }
+
+ m_camera->release();
+}
+
+bool OV9281::init()
+{
+ if (m_camera->acquire() != EXIT_SUCCESS)
+ {
+ std::cerr << __func__ << ": " << m_camera->id()
+ << ": cannot acquire camera." << std::endl;
+
+ return false;
+ }
+
+ m_config = m_camera->generateConfiguration({libcamera::StreamRole::Raw});
+
+ if (m_config->empty())
+ {
+ std::cerr << __func__ << ": " << m_camera->id()
+ << ": cannot generate configuration" << std::endl;
+
+ return false;
+ }
+
+ 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;
+ streamConfig.bufferCount = OV9281::bufferCount;
+
+ if (!validateConfig())
+ {
+ std::cerr << __func__ << ": " << m_camera->id()
+ << ": cannot apply default config" << std::endl;
+
+ return false;
+ }
+
+ return true;
+}
+
+bool OV9281::validateConfig()
+{
+ using namespace libcamera;
+
+ auto status = m_config->validate();
+
+ // WARNING: unsafe
+
+ switch (status)
+ {
+ case CameraConfiguration::Status::Valid: {
+ std::cout << __func__ << ": " << "config is valid" << std::endl;
+ break;
+ }
+ case CameraConfiguration::Status::Adjusted: {
+ if (m_config->empty())
+ {
+ std::cerr << __func__ << ": " << "config is adjusted, but empty"
+ << std::endl;
+ return false;
+ }
+
+ libcamera::StreamConfiguration &streamConfig = m_config->at(0);
+ std::cout << __func__
+ << ":\tpixelFormat: " << streamConfig.pixelFormat.toString()
+ << std::endl;
+ std::cout << __func__ << ":\tbufferCount: " << streamConfig.bufferCount
+ << std::endl;
+ std::cout << __func__ << ":\torientation: " << m_config->orientation
+ << std::endl;
+ break;
+ }
+ case CameraConfiguration::Status::Invalid: {
+ std::cerr << __func__ << ":\tconfig is invalid" << std::endl;
+
+ return false;
+ }
+ }
+
+ return true;
+}
+
+bool OV9281::applyConfig()
+{
+ // FIXME: may crassh even on success (e.g. by setting pixelFormat to "8")
+ if (m_camera->configure(m_config.get()) != EXIT_SUCCESS)
+ {
+ std::cerr << __func__ << ":\tcannot apply config" << std::endl;
+
+ return false;
+ }
+
+ return true;
+}
+
+void OV9281::onRequestCompleted(libcamera::Request *completed_request)
+{
+ using namespace libcamera;
+
+ if (completed_request->status() == Request::RequestCancelled)
+ {
+ std::cerr << __func__ << ":\trequest canceled" << std::endl;
+
+ return;
+ }
+
+ const auto &buffers = completed_request->buffers();
+
+ 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 = m_mappedBuffers[plane.fd.get()].first;
+
+ auto img = std::make_shared<Image>();
+
+ img->width = imageSize.width;
+ img->height = imageSize.height;
+
+ 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();
+
+ img->rotate();
+
+ auto pixels = img->pixels();
+#ifdef emit
+#undef emit
+ newPixels.emit(pixels);
+#define emit
+#endif
+ }
+ }
+
+ const libcamera::ControlList &metadata = completed_request->metadata();
+ const ControlInfoMap &control_map = m_camera->controls();
+ // const ControlIdMap & ctrlIdMap = control_map.idmap();
+
+ auto frameDurationCtrl = control_map.find(&controls::FrameDurationLimits);
+ double fps = frameDurationCtrl == control_map.end() ?
+ std::numeric_limits<double>::quiet_NaN() :
+ (1e6 / frameDurationCtrl->second.min().get<int64_t>());
+
+ auto exp = metadata.get(controls::ExposureTime);
+ auto ag = metadata.get(controls::AnalogueGain);
+ auto ae = metadata.get(controls::AeEnable);
+ // auto br= metadata.get(controls::Brightness);
+ static auto lastControls = completed_request->controls();
+
+ 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,
+ m_exposureTime);
+
+ m_camera->queueRequest(completed_request);
+}
+
+std::vector<std::shared_ptr<OV9281>> OV9281::search(
+ std::unique_ptr<libcamera::CameraManager> &manager)
+{
+ std::vector<std::shared_ptr<OV9281>> result;
+
+ for (const auto &camera : manager->cameras())
+ {
+ auto ov9281 = std::shared_ptr<OV9281>(new OV9281(camera));
+
+ if (!ov9281->init())
+ {
+ continue;
+ }
+
+ result.push_back(ov9281);
+ }
+
+ return result;
+}
+
+bool OV9281::startStream()
+{
+ if (m_config->empty())
+ {
+ std::cerr << __func__ << ":\tconfig is empty" << std::endl;
+ return false;
+ }
+
+ auto &streamConfig = m_config->at(0);
+ m_allocator = std::make_unique<libcamera::FrameBufferAllocator>(m_camera);
+ auto stream = streamConfig.stream();
+ auto ret = m_allocator->allocate(stream);
+
+ // TODO: check if zero
+ if (ret < 0)
+ {
+ std::cerr << __func__ << ":\tcan't allocate buffers: " << strerror(ret)
+ << std::endl;
+ return false;
+ }
+
+ auto allocatedCount = ret;
+ std::cout << __func__ << ":\tallocated " << allocatedCount
+ << " buffers for stream" << std::endl;
+
+ const auto &buffers = m_allocator->buffers(stream);
+
+ for (const auto &buffer : buffers)
+ {
+ auto request = m_camera->createRequest();
+
+ if (!request)
+ {
+ std::cerr << __func__ << ":\tcan't create request" << std::endl;
+
+ return false;
+ }
+
+ // TODO: try multiple buffers per request and compare performance
+ ret = request->addBuffer(stream, buffer.get());
+
+ if (ret < 0)
+ {
+ std::cerr << __func__
+ << ":\tcan't set buffer for request: " << strerror(ret)
+ << std::endl;
+
+ return false;
+ }
+
+ for (const auto &plane : buffer->planes())
+ {
+ void *memory = mmap(NULL,
+ plane.length,
+ PROT_READ,
+ MAP_SHARED,
+ plane.fd.get(),
+ 0);
+ m_mappedBuffers[plane.fd.get()] = std::make_pair(memory,
+ plane.length);
+ }
+
+ 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<const std::int64_t, 2>(
+ value_pair));
+
+ m_requests.push_back(std::move(request));
+ }
+
+ m_camera->requestCompleted.connect(this, &OV9281::onRequestCompleted);
+
+ // FIXME: memleak
+ std::unique_ptr<libcamera::ControlList> camcontrols{
+ new libcamera::ControlList()};
+
+ {
+ using namespace std::chrono_literals;
+ std::this_thread::sleep_for(500ms);
+ }
+
+ ret = m_camera->start(camcontrols.get());
+
+ if (ret)
+ {
+ std::cerr << __func__ << ":\tfailed to start camera: " << strerror(ret)
+ << std::endl;
+
+ return false;
+ }
+
+ for (auto &request : m_requests)
+ {
+ ret = m_camera->queueRequest(request.get());
+
+ if (ret)
+ {
+ std::cerr << __func__
+ << ":\tfailed to queue request: " << strerror(ret)
+ << std::endl;
+
+ return false;
+ }
+ }
+
+ return true;
+}
+
+void OV9281::printControls()
+{
+ using namespace libcamera;
+ const libcamera::ControlInfoMap &control_map = m_camera->controls();
+
+ // for (const auto & [id, info]: control_map)
+ for (const std::pair<const ControlId *, ControlInfo> &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";
+ }
+}
diff --git a/src/camera/ov9281.h b/src/camera/ov9281.h
new file mode 100644
index 0000000..f70db2f
--- /dev/null
+++ b/src/camera/ov9281.h
@@ -0,0 +1,68 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <vector>
+
+#include <libcamera/base/signal.h>
+#include <libcamera/formats.h>
+
+namespace libcamera {
+class Camera;
+class CameraConfiguration;
+class CameraManager;
+class FrameBufferAllocator;
+class Request;
+} // namespace libcamera
+
+class Image;
+class Pixels;
+
+class OV9281
+{
+public:
+ ~OV9281();
+
+public:
+ static std::vector<std::shared_ptr<OV9281>> search(
+ std::unique_ptr<libcamera::CameraManager> &manager);
+
+ // public functions
+public:
+ bool startStream();
+ void printControls();
+
+ // signals
+public:
+ // TODO: image->pixels in separate thread
+ // TODO: respect sender/receiver threads
+ libcamera::Signal<std::shared_ptr<Pixels>> newPixels;
+
+private:
+ explicit OV9281(const std::shared_ptr<libcamera::Camera> &camera);
+
+ // private functions
+private:
+ bool init();
+ bool validateConfig();
+ bool applyConfig();
+
+ void onRequestCompleted(libcamera::Request *completed_request);
+
+ // constants
+private:
+ static inline constexpr auto pixelFormat{libcamera::formats::R16};
+ static inline constexpr unsigned int bufferCount{2};
+ static inline constexpr size_t desiredFPS{144};
+
+ // member variables
+private:
+ std::shared_ptr<libcamera::Camera> m_camera{nullptr};
+ std::unique_ptr<libcamera::CameraConfiguration> m_config{nullptr};
+ std::map<int, std::pair<void *, unsigned int>> m_mappedBuffers;
+ std::vector<std::unique_ptr<libcamera::Request>> m_requests;
+ std::unique_ptr<libcamera::FrameBufferAllocator> m_allocator{nullptr};
+
+ // TODO: set exposureTime from outside
+ int32_t m_exposureTime{1000};
+};
diff --git a/src/image.cpp b/src/image.cpp
new file mode 100644
index 0000000..a9280a4
--- /dev/null
+++ b/src/image.cpp
@@ -0,0 +1,110 @@
+#include "image.h"
+
+#include "macro.h"
+#include "pixels.h"
+
+float process_column(const uint16_t (&column)[])
+{
+ start_timer(process_column);
+
+ 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);
+ uint32_t correlation[img_height];
+ uint32_t integralSum[img_height];
+ uint32_t maxSum = signalThreshold * 50;
+ uint32_t x1 = 0;
+ int32_t y1 = 0;
+ int32_t y2 = 0;
+
+ 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] = 0;
+ // }
+
+ integralSum[i] = column[i] / 256 + integralSum[i - 1];
+ }
+
+ for (uint32_t i = 0; i < correlationSize; ++i)
+ correlation[i + patternSize / 2] = column[i + patternSize / 2] / 256 *
+ (integralSum[i + patternOffset] -
+ integralSum[i]);
+
+ for (uint32_t i = 3; i < img_height - 2; ++i)
+ {
+ const auto sum = correlation[i - 1] + correlation[i] +
+ correlation[i + 1];
+
+ if (sum > maxSum)
+ {
+ const int32_t rioux0 = int32_t(correlation[i - 2 - 1] +
+ correlation[i - 1 - 1]) -
+ int32_t(correlation[i + 1 - 1] +
+ correlation[i + 2 - 1]);
+
+ if (rioux0 < 0)
+ {
+ const int32_t rioux1 = int32_t(correlation[i - 2] +
+ correlation[i - 1]) -
+ int32_t(correlation[i + 1] +
+ correlation[i + 2]);
+
+ if (rioux1 >= 0)
+ {
+ x1 = i - 1;
+ y1 = rioux0;
+ y2 = rioux1;
+ maxSum = sum;
+ }
+ }
+ }
+ }
+
+ result = (y2 != y1) ? (float(x1) - (float(y1) / (y2 - y1)))
+ : std::numeric_limits<float>::quiet_NaN();
+
+ return result;
+}
+
+void Image::rotate()
+{
+ start_timer(rotate);
+
+ using namespace std;
+
+ for (size_t i = 0; i < img_height; ++i)
+ {
+ for (size_t j = 0; j < img_width; ++j)
+ {
+ rotated_cw[j][i] = data[img_height - i][j];
+ }
+ }
+
+ stop_timer(rotate);
+}
+
+std::shared_ptr<Pixels> Image::pixels() const
+{
+ auto result = std::make_shared<Pixels>();
+ result->counters = counters;
+
+ start_timer(process_columns);
+
+ for (size_t i = 0; i < width; i++)
+ {
+ result->pixels[i] = process_column(rotated_cw[i]);
+ }
+
+ stop_timer(process_columns);
+
+ return result;
+}
diff --git a/src/image.h b/src/image.h
new file mode 100644
index 0000000..2fff020
--- /dev/null
+++ b/src/image.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "constants.h"
+#include "typedefs.h"
+
+class Pixels;
+
+struct Image
+{
+ int width{0};
+ int height{0};
+ uint16_t data[img_height][img_width] = {{0}};
+ uint16_t rotated_cw[img_width][img_height] = {{0}};
+ size_t dataSize{0};
+ unsigned int stride{0};
+ libcamera::PixelFormat pixelFormat{0};
+ Counters counters{};
+
+ void rotate();
+ std::shared_ptr<Pixels> pixels() const;
+};
diff --git a/src/pixels.cpp b/src/pixels.cpp
index b314e6a..5aac4a6 100644
--- a/src/pixels.cpp
+++ b/src/pixels.cpp
@@ -93,3 +93,10 @@ bool Pixels::save(const QString& filename)
return true;
}
+
+Pixels::operator bool() const
+{
+ return std::find_if(pixels.cbegin(), pixels.cend(), [](const auto& p) {
+ return !qFuzzyIsNull(p) && !std::isnan(p);
+ }) != pixels.cend();
+}
diff --git a/src/pixels.h b/src/pixels.h
index 6095220..1d5effb 100644
--- a/src/pixels.h
+++ b/src/pixels.h
@@ -6,6 +6,7 @@
#include <QString>
+#include "constants.h"
#include "typedefs.h"
struct Pixels
@@ -29,4 +30,6 @@ struct Pixels
* \return - true on success, false otherwise
*/
[[nodiscard]] bool save(const QString& filename);
+
+ operator bool() const;
};
diff --git a/src/typedefs.h b/src/typedefs.h
index 2af2921..7ed8411 100644
--- a/src/typedefs.h
+++ b/src/typedefs.h
@@ -2,8 +2,6 @@
#include <libcamera/pixel_format.h>
-#include "constants.h"
-
struct Counters
{
uint32_t timestampUs{0};
@@ -11,22 +9,10 @@ struct Counters
int32_t encoderPosition{0};
};
-struct Image
-{
- int width;
- int height;
- uint16_t data[img_height][img_width];
- uint16_t rotated_cw[img_width][img_height];
- size_t dataSize;
- unsigned int stride;
- libcamera::PixelFormat pixelFormat;
- Counters counters{};
-};
-
struct requested_params_t
{
- int32_t exposureTime = {1000};
- int32_t laserLevel = {3000};
+ int32_t exposureTime{1000};
+ int32_t laserLevel{3000};
uint32_t stepsPerMm{200};
};
generated by cgit v1.2.3-70-g09d2 (git 2.52.0) at 2025-12-08 13:00:50 +0000