Initial commit

1 files changed, 267 insertions, 0 deletions

diff --git a/LibCamera.cpp b/LibCamera.cpp
new file mode 100644
index 0000000..945a8ae
--- /dev/null
+++ b/LibCamera.cpp
@@ -0,0 +1,267 @@
+#include "LibCamera.h"

+

+using namespace std::placeholders;

+

+int LibCamera::initCamera() {

+    int ret;

+    cm = std::make_unique<CameraManager>();

+    ret = cm->start();

+    if (ret){

+        std::cout << "Failed to start camera manager: "

+              << ret << std::endl;

+        return ret;

+    }

+    cameraId = cm->cameras()[0]->id();

+    camera_ = cm->get(cameraId);

+    if (!camera_) {

+        std::cerr << "Camera " << cameraId << " not found" << std::endl;

+        return 1;

+    }

+

+    if (camera_->acquire()) {

+        std::cerr << "Failed to acquire camera " << cameraId

+              << std::endl;

+        return 1;

+    }

+    camera_acquired_ = true;

+    return 0;

+}

+

+char * LibCamera::getCameraId(){

+    return cameraId.data();

+}

+

+void LibCamera::configureStill(int width, int height, PixelFormat format, int buffercount, int rotation) {

+    printf("Configuring still capture...\n");

+    config_ = camera_->generateConfiguration({ StreamRole::StillCapture });

+    if (width && height) {

+        libcamera::Size size(width, height);

+        config_->at(0).size = size;

+    }

+    config_->at(0).pixelFormat = format;

+    if (buffercount)

+        config_->at(0).bufferCount = buffercount;

+    Transform transform = Transform::Identity;

+    bool ok;

+    Transform rot = transformFromRotation(rotation, &ok);

+    if (!ok)

+        throw std::runtime_error("illegal rotation value, Please use 0 or 180");

+    transform = rot * transform;

+    if (!!(transform & Transform::Transpose))

+        throw std::runtime_error("transforms requiring transpose not supported");

+    // FIXME: update

+    // config_->transform = transform;

+

+    CameraConfiguration::Status validation = config_->validate();

+	if (validation == CameraConfiguration::Invalid)

+		throw std::runtime_error("failed to valid stream configurations");

+	else if (validation == CameraConfiguration::Adjusted)

+        std::cout << "Stream configuration adjusted" << std::endl;

+

+    printf("Still capture setup complete\n");

+}

+

+int LibCamera::startCamera() {

+    int ret;

+    ret = camera_->configure(config_.get());

+    if (ret < 0) {

+        std::cout << "Failed to configure camera" << std::endl;

+        return ret;

+    }

+

+    camera_->requestCompleted.connect(this, &LibCamera::requestComplete);

+

+    allocator_ = std::make_unique<FrameBufferAllocator>(camera_);

+

+    return startCapture();

+}

+

+int LibCamera::startCapture() {

+    int ret;

+    unsigned int nbuffers = UINT_MAX;

+    for (StreamConfiguration &cfg : *config_) {

+        ret = allocator_->allocate(cfg.stream());

+        if (ret < 0) {

+            std::cerr << "Can't allocate buffers" << std::endl;

+            return -ENOMEM;

+        }

+

+        unsigned int allocated = allocator_->buffers(cfg.stream()).size();

+        nbuffers = std::min(nbuffers, allocated);

+    }

+

+    for (unsigned int i = 0; i < nbuffers; i++) {

+        std::unique_ptr<Request> request = camera_->createRequest();

+        if (!request) {

+            std::cerr << "Can't create request" << std::endl;

+            return -ENOMEM;

+        }

+

+        for (StreamConfiguration &cfg : *config_) {

+            Stream *stream = cfg.stream();

+            const std::vector<std::unique_ptr<FrameBuffer>> &buffers =

+                allocator_->buffers(stream);

+            const std::unique_ptr<FrameBuffer> &buffer = buffers[i];

+

+            ret = request->addBuffer(stream, buffer.get());

+            if (ret < 0) {

+                std::cerr << "Can't set buffer for request"

+                      << std::endl;

+                return ret;

+            }

+            for (const FrameBuffer::Plane &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);

+            }

+        }

+

+        requests_.push_back(std::move(request));

+    }

+

+    ret = camera_->start(&this->controls_);

+    // ret = camera_->start();

+    if (ret) {

+        std::cout << "Failed to start capture" << std::endl;

+        return ret;

+    }

+    controls_.clear();

+    camera_started_ = true;

+    for (std::unique_ptr<Request> &request : requests_) {

+        ret = queueRequest(request.get());

+        if (ret < 0) {

+            std::cerr << "Can't queue request" << std::endl;

+            camera_->stop();

+            return ret;

+        }

+    }

+    viewfinder_stream_ = config_->at(0).stream();

+    return 0;

+}

+

+void LibCamera::StreamDimensions(Stream const *stream, uint32_t *w, uint32_t *h, uint32_t *stride) const

+{

+	StreamConfiguration const &cfg = stream->configuration();

+	if (w)

+		*w = cfg.size.width;

+	if (h)

+		*h = cfg.size.height;

+	if (stride)

+		*stride = cfg.stride;

+}

+

+Stream *LibCamera::VideoStream(uint32_t *w, uint32_t *h, uint32_t *stride) const

+{

+	StreamDimensions(viewfinder_stream_, w, h, stride);

+	return viewfinder_stream_;

+}

+

+int LibCamera::queueRequest(Request *request) {

+    std::lock_guard<std::mutex> stop_lock(camera_stop_mutex_);

+    if (!camera_started_)

+        return -1;

+    {

+        std::lock_guard<std::mutex> lock(control_mutex_);

+        request->controls() = std::move(controls_);

+    }

+    return camera_->queueRequest(request);

+}

+

+void LibCamera::requestComplete(Request *request) {

+    if (request->status() == Request::RequestCancelled)

+        return;

+    processRequest(request);

+}

+

+void LibCamera::processRequest(Request *request) {

+    requestQueue.push(request);

+}

+

+void LibCamera::returnFrameBuffer(LibcameraOutData frameData) {

+    uint64_t request = frameData.request;

+    Request * req = (Request *)request;

+    req->reuse(Request::ReuseBuffers);

+    queueRequest(req);

+}

+

+bool LibCamera::readFrame(LibcameraOutData *frameData){

+    std::lock_guard<std::mutex> lock(free_requests_mutex_);

+    // int w, h, stride;

+    if (!requestQueue.empty()){

+        Request *request = this->requestQueue.front();

+

+        const Request::BufferMap &buffers = request->buffers();

+        for (auto it = buffers.begin(); it != buffers.end(); ++it) {

+            FrameBuffer *buffer = it->second;

+            for (unsigned int i = 0; i < buffer->planes().size(); ++i) {

+                const FrameBuffer::Plane &plane = buffer->planes()[i];

+                const FrameMetadata::Plane &meta = buffer->metadata().planes()[i];

+                

+                void *data = mappedBuffers_[plane.fd.get()].first;

+                int length = std::min(meta.bytesused, plane.length);

+

+                frameData->size = length;

+                frameData->imageData = (uint8_t *)data;

+            }

+        }

+        this->requestQueue.pop();

+        frameData->request = (uint64_t)request;

+        return true;

+    } else {

+        Request *request = nullptr;

+        frameData->request = (uint64_t)request;

+        return false;

+    }

+}

+

+void LibCamera::set(ControlList controls){

+    std::lock_guard<std::mutex> lock(control_mutex_);

+	this->controls_ = std::move(controls);

+}

+

+int LibCamera::resetCamera(int width, int height, PixelFormat format, int buffercount, int rotation) {

+    stopCamera();

+    configureStill(width, height, format, buffercount, rotation);

+    return startCamera();

+}

+

+void LibCamera::stopCamera() {

+    if (camera_){

+        {

+            std::lock_guard<std::mutex> lock(camera_stop_mutex_);

+            if (camera_started_){

+                if (camera_->stop())

+                    throw std::runtime_error("failed to stop camera");

+                camera_started_ = false;

+            }

+        }

+        camera_->requestCompleted.disconnect(this, &LibCamera::requestComplete);

+    }

+    while (!requestQueue.empty())

+        requestQueue.pop();

+

+    for (auto &iter : mappedBuffers_)

+	{

+        std::pair<void *, unsigned int> pair_ = iter.second;

+		munmap(std::get<0>(pair_), std::get<1>(pair_));

+	}

+

+    mappedBuffers_.clear();

+

+    requests_.clear();

+

+    allocator_.reset();

+

+    controls_.clear();

+}

+

+void LibCamera::closeCamera(){

+    if (camera_acquired_)

+        camera_->release();

+    camera_acquired_ = false;

+

+    camera_.reset();

+

+    cm.reset();

+}