ESP32S3_vt/components/cam_wrapper/cam_wrapper.c

/* Implementation of the cam_wrapper shim around esp32-camera. See cam_wrapper.h. */

#include "cam_wrapper.h"
#include "driver/temperature_sensor.h"

#include <string.h>

#include "esp_log.h"
#include "esp_timer.h"

static float cam_wrapper_measure_fps(cam_wrapper_sensor_t sensor);

static const char *TAG = "cam_wrapper";

static sensor_t *s_sensor;
static char s_sensor_name[16] = "unknown";
static temperature_sensor_handle_t s_temperature;

static void fill_config(const cam_wrapper_config_t *in, camera_config_t *out)
{
    memset(out, 0, sizeof(*out));

    out->pin_pwdn = in->pin_pwdn;
    out->pin_reset = in->pin_reset;
    out->pin_xclk = in->pin_xclk;
    out->pin_sccb_sda = in->pin_sccb_sda;
    out->pin_sccb_scl = in->pin_sccb_scl;

    out->pin_d0 = in->pin_d[0];
    out->pin_d1 = in->pin_d[1];
    out->pin_d2 = in->pin_d[2];
    out->pin_d3 = in->pin_d[3];
    out->pin_d4 = in->pin_d[4];
    out->pin_d5 = in->pin_d[5];
    out->pin_d6 = in->pin_d[6];
    out->pin_d7 = in->pin_d[7];

    out->pin_vsync = in->pin_vsync;
    out->pin_href = in->pin_href;
    out->pin_pclk = in->pin_pclk;

    out->xclk_freq_hz = in->xclk_freq_hz;
    out->ledc_timer = (ledc_timer_t)in->ledc_timer;
    out->ledc_channel = (ledc_channel_t)in->ledc_channel;

    out->pixel_format = (pixformat_t)in->pixel_format;
    out->frame_size = (framesize_t)in->frame_size;
    out->jpeg_quality = in->jpeg_quality;
    out->fb_count = in->fb_count;
    out->fb_location = (camera_fb_location_t)in->fb_location;
    out->grab_mode = (camera_grab_mode_t)in->grab_mode;
    out->sccb_i2c_port = in->sccb_i2c_port;
}

esp_err_t cam_wrapper_init(const cam_wrapper_config_t *config, cam_wrapper_sensor_t sensor)
{
    camera_config_t cfg;

    (void)sensor;

    if (config == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    fill_config(config, &cfg);

    ESP_LOGI(TAG, "config: pwdn=%d reset=%d xclk=%d sda=%d scl=%d "
                  "d=[%d %d %d %d %d %d %d %d] vsync=%d href=%d pclk=%d "
                  "fmt=%d size=%d q=%d fb=%u loc=%d grab=%d i2c=%d vsync_inv=%d",
             cfg.pin_pwdn, cfg.pin_reset, cfg.pin_xclk,
             cfg.pin_sccb_sda, cfg.pin_sccb_scl,
             cfg.pin_d0, cfg.pin_d1, cfg.pin_d2, cfg.pin_d3,
             cfg.pin_d4, cfg.pin_d5, cfg.pin_d6, cfg.pin_d7,
             cfg.pin_vsync, cfg.pin_href, cfg.pin_pclk,
             (int)cfg.pixel_format, (int)cfg.frame_size, cfg.jpeg_quality,
             (unsigned)cfg.fb_count, (int)cfg.fb_location, (int)cfg.grab_mode,
             cfg.sccb_i2c_port, config->vsync_invert);

    /* Must be set before init: cam_init copies it into the driver object, and
     * ll_cam_set_pin applies it to the GPIO matrix for the VSYNC input. */
    cam_set_vsync_invert(config->vsync_invert != 0);

    esp_err_t err = esp_camera_init(&cfg);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "esp_camera_init failed: 0x%x", err);
        return err;
    }

    /* The driver probes the SCCB bus and binds a driver by the sensor's
     * PID/ID, so name the sensor from the matched entry of its table rather
     * than reporting something the driver does not actually know. */
    s_sensor = esp_camera_sensor_get();

    if (s_sensor != NULL) {
        sensor_id_t id = s_sensor->id;
        const camera_sensor_info_t *info = esp_camera_sensor_get_info(&id);

        if (info != NULL && info->name != NULL) {
            strncpy(s_sensor_name, info->name, sizeof(s_sensor_name) - 1);
            s_sensor_name[sizeof(s_sensor_name) - 1] = '\0';
        }

        ESP_LOGI(TAG, "sensor detected: %s (pid=0x%04x, ver=0x%02x)",
                 s_sensor_name, id.PID, id.VER);
    } else {
        ESP_LOGW(TAG, "camera initialized but no sensor handle is available");
    }

    /* Measure what the sensor actually produces, here, with no WiFi and no HTTP
     * in the picture. Everything downstream (socket writes, TCP, the browser) is
     * downstream of this number, so when the stream is slow this is what says
     * whether the problem is capture or transport. */
    cam_wrapper_fb_t probe;
    memset(&probe, 0, sizeof(probe));

    if (cam_wrapper_fb_get(&probe, sensor) == ESP_OK) {
        ESP_LOGI(TAG, "first frame: %ux%u, %u bytes, format %d",
                 (unsigned)probe.width, (unsigned)probe.height,
                 (unsigned)probe.len, probe.format);
        cam_wrapper_fb_release(&probe);

        cam_wrapper_measure_fps(sensor);
    } else {
        ESP_LOGW(TAG, "camera initialized but returned no frame");
    }

    return ESP_OK;
}

/* Grabs PROBE_FRAMES frames back to back and logs the achieved rate.
 *
 * This runs on the capture path with nothing else contending for the sensor, so
 * it is the ceiling the stream can ever reach. Comparing it against the rate the
 * stream actually achieves localises the bottleneck: equal means the transport is
 * keeping up and the sensor is the limit, much lower means something between the
 * two is dropping frames. */
#define PROBE_FRAMES 20

static float cam_wrapper_measure_fps(cam_wrapper_sensor_t sensor)
{
    uint32_t total = 0;
    int64_t start = esp_timer_get_time();
    int taken = 0;

    for (int i = 0; i < PROBE_FRAMES; i++) {
        cam_wrapper_fb_t fb;
        memset(&fb, 0, sizeof(fb));

        if (cam_wrapper_fb_get(&fb, sensor) != ESP_OK) {
            ESP_LOGW(TAG, "fps probe: no frame at index %d", i);
            continue;
        }

        total += (uint32_t)fb.len;
        taken++;
        cam_wrapper_fb_release(&fb);
    }

    int64_t elapsed_us = esp_timer_get_time() - start;

    if (taken == 0 || elapsed_us <= 0) {
        ESP_LOGW(TAG, "fps probe: got no frames");
        return 0;
    }

    double fps = (double)taken * 1000000.0 / (double)elapsed_us;

    ESP_LOGI(TAG, "fps probe: %d frames in %lld ms -> %.1f fps, avg %u bytes "
                  "(sensor ceiling, no network involved)",
             taken, (long long)(elapsed_us / 1000), fps,
             (unsigned)(total / (uint32_t)taken));
    return (float)fps;
}

esp_err_t cam_wrapper_set_grayscale(int enabled)
{
    if (!s_sensor || !s_sensor->set_special_effect) return ESP_ERR_NOT_SUPPORTED;
    int rc = s_sensor->set_special_effect(s_sensor, enabled ? 2 : 0);
    if (rc != 0) return ESP_FAIL;
    ESP_LOGI(TAG, "sensor JPEG colour: %s", enabled ? "grayscale" : "colour");
    return ESP_OK;
}

esp_err_t cam_wrapper_set_quality(unsigned quality)
{
    if (quality > 63) return ESP_ERR_INVALID_ARG;
    if (!s_sensor || !s_sensor->set_quality) return ESP_ERR_NOT_SUPPORTED;
    return s_sensor->set_quality(s_sensor, (int)quality) == 0 ? ESP_OK : ESP_FAIL;
}

esp_err_t cam_wrapper_set_exposure_bias(int ev)
{
    if (ev < -2 || ev > 0) return ESP_ERR_INVALID_ARG;
    if (!s_sensor || s_sensor->id.PID != OV5640_PID)
        return ev == 0 ? ESP_OK : ESP_ERR_NOT_SUPPORTED;
    /* Driver target = (level+5)*10+5, default 55. Targets 25 and 15
     * are ~-1.14 and ~-1.87 EV, not literal driver levels -1 and -2. */
    int level = ev == -1 ? -3 : ev == -2 ? -4 : 0;
    if (s_sensor->set_exposure_ctrl(s_sensor, 1) != 0
        || s_sensor->set_gain_ctrl(s_sensor, 1) != 0
        || s_sensor->set_ae_level(s_sensor, level) != 0) return ESP_FAIL;
    ESP_LOGI(TAG, "OV5640 auto exposure bias: requested %d EV, driver level %d, target %d / 55",
             ev, level, (level + 5) * 10 + 5);
    return ESP_OK;
}

int cam_wrapper_exposure_lines(void)
{
    if (!s_sensor || s_sensor->id.PID != OV5640_PID) return -1;
    int raw = s_sensor->get_reg(s_sensor, 0x3500, 0xfffff);
    return raw < 0 ? -1 : raw >> 4;
}

int cam_wrapper_gain_raw(void)
{
    if (!s_sensor || s_sensor->id.PID != OV5640_PID) return -1;
    return s_sensor->get_reg(s_sensor, 0x350a, 0x03ff);
}

esp_err_t cam_wrapper_set_shutter_lines(unsigned lines)
{
    if (!s_sensor || s_sensor->id.PID != OV5640_PID)
        return lines == 0 ? ESP_OK : ESP_ERR_NOT_SUPPORTED;
    int vts = s_sensor->get_reg(s_sensor, 0x380e, 0xffff);
    if (vts <= 0 || lines > (unsigned)vts) return ESP_ERR_INVALID_ARG;
    int previous = cam_wrapper_exposure_lines();
    if (s_sensor->set_exposure_ctrl(s_sensor, lines == 0) != 0
        || s_sensor->set_gain_ctrl(s_sensor, 1) != 0) return ESP_FAIL;
    if (lines && (s_sensor->set_aec_value(s_sensor, (int)lines) != 0
                  || cam_wrapper_exposure_lines() != (int)lines)) return ESP_FAIL;
    ESP_LOGI(TAG, "OV5640 shutter: previous %d lines, requested %u lines (0=auto), AGC enabled",
             previous, lines);
    return ESP_OK;
}

esp_err_t cam_wrapper_set_horizontal_timing(unsigned hts)
{
    if (!hts) return ESP_OK;
    if (!s_sensor || s_sensor->id.PID != OV5640_PID) return ESP_ERR_NOT_SUPPORTED;
    int start = s_sensor->get_reg(s_sensor, 0x3800, 0x0fff);
    int end = s_sensor->get_reg(s_sensor, 0x3804, 0x0fff);
    int previous = s_sensor->get_reg(s_sensor, 0x380c, 0x0fff);
    if (start < 0 || end < start || previous <= 0) return ESP_FAIL;
    unsigned active = (unsigned)(end - start + 1) / (s_sensor->status.binning ? 2 : 1);
    /* Preserve the active readout window and a conservative blanking margin.
     * This is a guard against invalid geometry, not a stability guarantee. */
    if ((hts & 1) || hts < active + 64 || hts > 4095) return ESP_ERR_INVALID_ARG;
    /* Use the byte order that cannot transiently shorten the line below
     * either valid endpoint while updating this live sensor. */
    int rc;
    if (hts < (unsigned)previous) {
        rc = s_sensor->set_reg(s_sensor, 0x380d, 0xff, hts & 0xff)
            || s_sensor->set_reg(s_sensor, 0x380c, 0x0f, hts >> 8);
    } else {
        rc = s_sensor->set_reg(s_sensor, 0x380c, 0x0f, hts >> 8)
            || s_sensor->set_reg(s_sensor, 0x380d, 0xff, hts & 0xff);
    }
    if (rc || s_sensor->get_reg(s_sensor, 0x380c, 0x0fff) != (int)hts) return ESP_FAIL;
    ESP_LOGI(TAG, "OV5640 horizontal timing: HTS %d -> %u, active %u, VTS %d (PLL/XCLK unchanged)",
             previous, hts, active, s_sensor->get_reg(s_sensor, 0x380e, 0xffff));
    return ESP_OK;
}

esp_err_t cam_wrapper_set_exposure_limit(unsigned lines)
{
    if (!lines) return ESP_OK;
    if (!s_sensor || s_sensor->id.PID != OV5640_PID) return ESP_ERR_NOT_SUPPORTED;
    int vts = s_sensor->get_reg(s_sensor, 0x380e, 0xffff);
    int band50 = s_sensor->get_reg(s_sensor, 0x3a08, 0x03ff);
    int band60 = s_sensor->get_reg(s_sensor, 0x3a0a, 0x03ff);
    if (vts <= 4 || lines > (unsigned)(vts - 4) || band50 <= 0 || band60 <= 0)
        return ESP_ERR_INVALID_ARG;
    unsigned max50 = lines / (unsigned)band50;
    unsigned max60 = lines / (unsigned)band60;
    if (!max50) max50 = 1;
    if (!max60) max60 = 1;
    if (max50 > 63 || max60 > 63) return ESP_ERR_INVALID_ARG;
    int rc = s_sensor->set_exposure_ctrl(s_sensor, 0)
        || s_sensor->set_aec2(s_sensor, 0)
        || s_sensor->set_reg(s_sensor, 0x3a02, 0xff, lines >> 8)
        || s_sensor->set_reg(s_sensor, 0x3a03, 0xff, lines & 0xff)
        || s_sensor->set_reg(s_sensor, 0x3a14, 0x0f, lines >> 8)
        || s_sensor->set_reg(s_sensor, 0x3a15, 0xff, lines & 0xff)
        || s_sensor->set_reg(s_sensor, 0x3a0d, 0x3f, max60)
        || s_sensor->set_reg(s_sensor, 0x3a0e, 0x3f, max50)
        || s_sensor->set_gain_ctrl(s_sensor, 1)
        || s_sensor->set_exposure_ctrl(s_sensor, 1);
    if (rc || s_sensor->get_reg(s_sensor, 0x3a02, 0xffff) != (int)lines
        || s_sensor->get_reg(s_sensor, 0x3a14, 0x0fff) != (int)lines)
        return ESP_FAIL;
    ESP_LOGI(TAG, "OV5640 auto exposure cap: %u lines, band maxima 50Hz=%u / 60Hz=%u, AEC/AGC auto, night mode off",
             lines, max50, max60);
    return ESP_OK;
}

float cam_wrapper_probe_frame_rate(void)
{
    /* Drain the old three-buffer pipeline after changing the colour effect. */
    for (int i = 0; i < 4; ++i) {
        cam_wrapper_fb_t fb;
        if (cam_wrapper_fb_get(&fb, CAM_WRAPPER_SENSOR_AUTO) == ESP_OK)
            cam_wrapper_fb_release(&fb);
    }
    return cam_wrapper_measure_fps(CAM_WRAPPER_SENSOR_AUTO);
}

esp_err_t cam_wrapper_temperature_init(void)
{
    if (s_temperature) return ESP_OK;
    temperature_sensor_config_t config = TEMPERATURE_SENSOR_CONFIG_DEFAULT(20, 100);
    esp_err_t rc = temperature_sensor_install(&config, &s_temperature);
    if (rc != ESP_OK) return rc;
    rc = temperature_sensor_enable(s_temperature);
    if (rc != ESP_OK) {
        temperature_sensor_uninstall(s_temperature);
        s_temperature = NULL;
    }
    return rc;
}

esp_err_t cam_wrapper_temperature_read(float *celsius)
{
    if (!s_temperature) return ESP_ERR_INVALID_STATE;
    return temperature_sensor_get_celsius(s_temperature, celsius);
}

esp_err_t cam_wrapper_fb_get(cam_wrapper_fb_t *out, cam_wrapper_sensor_t sensor)
{
    (void)sensor;

    if (out == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    memset(out, 0, sizeof(*out));

    camera_fb_t *fb = esp_camera_fb_get();
    if (fb == NULL) {
        return ESP_FAIL;
    }

    out->data = fb->buf;
    out->len = fb->len;
    out->width = fb->width;
    out->height = fb->height;
    out->format = (int)fb->format;
    out->handle = fb;
    out->capture_started_us = (int64_t)fb->timestamp.tv_sec * 1000000 + fb->timestamp.tv_usec;

    return ESP_OK;
}

esp_err_t cam_wrapper_fb_get_latest(cam_wrapper_fb_t *out, uint32_t timeout_ms)
{
    if (!out) return ESP_ERR_INVALID_ARG;
    memset(out, 0, sizeof(*out));
    uint32_t skipped = 0;
    camera_fb_t *fb = esp_camera_fb_get_latest(timeout_ms, &skipped);
    if (!fb) return ESP_FAIL;
    out->data = fb->buf;
    out->len = fb->len;
    out->width = fb->width;
    out->height = fb->height;
    out->format = (int)fb->format;
    out->handle = fb;
    out->capture_started_us = (int64_t)fb->timestamp.tv_sec * 1000000 + fb->timestamp.tv_usec;
    out->skipped_before = skipped;
    return ESP_OK;
}

void cam_wrapper_fb_release(cam_wrapper_fb_t *fb)
{
    if (fb == NULL || fb->handle == NULL) {
        return;
    }

    esp_camera_fb_return((camera_fb_t *)fb->handle);

    fb->data = NULL;
    fb->len = 0;
    fb->handle = NULL;
}

esp_err_t cam_wrapper_reconfigure(const cam_wrapper_config_t *config)
{
    camera_config_t cfg;

    if (config == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    fill_config(config, &cfg);

    return esp_camera_reconfigure(&cfg);
}

esp_err_t cam_wrapper_deinit(void)
{
    esp_err_t err = esp_camera_deinit();

    s_sensor = NULL;
    strcpy(s_sensor_name, "unknown");

    return err;
}

const char *cam_wrapper_sensor_name(void)
{
    return s_sensor_name;
}