camera.cpp

/*
 * Copyright 2025 Arduino SA
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 *
 * Camera driver.
 */
#include "Arduino.h"
#include "camera.h"

#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/drivers/video.h>
#include <zephyr/drivers/video-controls.h>

FrameBuffer::FrameBuffer() : vbuf(NULL) {

}

uint32_t FrameBuffer::getBufferSize() {
    if (this->vbuf) {
        return this->vbuf->bytesused;
    }
}

uint8_t* FrameBuffer::getBuffer() {
    if (this->vbuf) {
        return this->vbuf->buffer;
    }
}

Camera::Camera() : vdev(NULL), byte_swap(false), yuv_to_gray(false) {
	for (size_t i = 0; i < ARRAY_SIZE(this->vbuf); i++) {
		this->vbuf[i] = NULL;
    }
}

bool Camera::begin(uint32_t width, uint32_t height, uint32_t pixformat, bool byte_swap) {
    #if DT_HAS_CHOSEN(zephyr_camera)
    this->vdev = DEVICE_DT_GET(DT_CHOSEN(zephyr_camera));
    #endif

    if (!this->vdev || !device_is_ready(this->vdev)) {
        return false;
    }

    switch (pixformat) {
        case CAMERA_RGB565:
            this->byte_swap = byte_swap;
            pixformat = VIDEO_PIX_FMT_RGB565;
            break;
        case CAMERA_GRAYSCALE:
            // There's no support for mono sensors.
            this->yuv_to_gray = true;
            pixformat = VIDEO_PIX_FMT_YUYV;
            break;
        default:
            break;
    }

    // Get capabilities
    struct video_caps caps = {0};
    if (video_get_caps(this->vdev, VIDEO_EP_OUT, &caps)) {
        return false;
    }

    for (size_t i=0; caps.format_caps[i].pixelformat != NULL; i++) {
        const struct video_format_cap *fcap = &caps.format_caps[i];
        if (fcap->width_min == width &&
            fcap->height_min == height &&
            fcap->pixelformat == pixformat) {
            break;
        }
        if (caps.format_caps[i+1].pixelformat == NULL) {
            Serial.println("The specified format is not supported");
            return false;
        }
    }

    // Set format.
    static struct video_format fmt = {
        .pixelformat = pixformat,
        .width = width,
        .height = height,
        .pitch = width * 2,
    };

	if (video_set_format(this->vdev, VIDEO_EP_OUT, &fmt)) {
        Serial.println("Failed to set video format");
		return false;
	}

	// Allocate video buffers.
	for (size_t i = 0; i < ARRAY_SIZE(this->vbuf); i++) {
		this->vbuf[i] = video_buffer_aligned_alloc(fmt.pitch * fmt.height,
                                                   CONFIG_VIDEO_BUFFER_POOL_ALIGN,
                                                   K_FOREVER);
		if (this->vbuf[i] == NULL) {
            Serial.println("Failed to allocate video buffers");
			return false;
		}
		video_enqueue(this->vdev, VIDEO_EP_OUT, this->vbuf[i]);
	}

	// Start video capture
	if (video_stream_start(this->vdev)) {
        Serial.println("Failed to start capture");
		return false;
	}

    return true;
}

bool Camera::grabFrame(FrameBuffer &fb, uint32_t timeout) {
    if (this->vdev == NULL) {
        return false;
    }

    if (video_dequeue(this->vdev, VIDEO_EP_OUT, &fb.vbuf, K_MSEC(timeout))) {
        return false;
    }

    if (this->byte_swap) {
        uint16_t *pixels = (uint16_t *) fb.vbuf->buffer;
        for (size_t i=0; i<fb.vbuf->bytesused / 2; i++) {
            pixels[i] = __REVSH(pixels[i]);
        }
    }

    if (this->yuv_to_gray) {
        uint8_t *pixels = (uint8_t *) fb.vbuf->buffer;
        for (size_t i=0; i<fb.vbuf->bytesused / 2; i++) {
            pixels[i] = pixels[i*2];
        }
        fb.vbuf->bytesused /= 2;
    }

    return true;
}

bool Camera::releaseFrame(FrameBuffer &fb) {
    if (this->vdev == NULL) {
        return false;
    }

    if (video_enqueue(this->vdev, VIDEO_EP_OUT, fb.vbuf)) {
        return false;
	}

    return true;
}

bool Camera::setVerticalFlip(bool flip_enable) {
    return video_set_ctrl(this->vdev, VIDEO_CID_VFLIP, (void *) flip_enable) == 0;
}

bool Camera::setHorizontalMirror(bool mirror_enable) {
    return video_set_ctrl(this->vdev, VIDEO_CID_HFLIP, (void *) mirror_enable) == 0;
}