espilon-source/espilon_bot/components/mod_recon/mod_cam.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>

#include "esp_camera.h"
#include "esp_log.h"

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>

#include "command.h"
#include "utils.h"

/* ============================================================
 * CONFIG
 * ============================================================ */
#define TAG "CAMERA"
#define MAX_UDP_SIZE 2034

#if defined(CONFIG_MODULE_RECON) && defined(CONFIG_RECON_MODE_CAMERA)
/* ================= CAMERA PINS ================= */
#define CAM_PIN_PWDN    32
#define CAM_PIN_RESET   -1
#define CAM_PIN_XCLK    0
#define CAM_PIN_SIOD    26
#define CAM_PIN_SIOC    27
#define CAM_PIN_D7      35
#define CAM_PIN_D6      34
#define CAM_PIN_D5      39
#define CAM_PIN_D4      36
#define CAM_PIN_D3      21
#define CAM_PIN_D2      19
#define CAM_PIN_D1      18
#define CAM_PIN_D0      5
#define CAM_PIN_VSYNC   25
#define CAM_PIN_HREF    23
#define CAM_PIN_PCLK    22

/* ============================================================
 * STATE
 * ============================================================ */
static volatile bool streaming_active = false;
static bool camera_initialized = false;

static int udp_sock = -1;
static struct sockaddr_in dest_addr;

/* ⚠️ à passer en Kconfig plus tard */
static const char *token = "Sup3rS3cretT0k3n";

/* ============================================================
 * CAMERA INIT
 * ============================================================ */
static bool init_camera(void)
{
    camera_config_t cfg = {
        .pin_pwdn       = CAM_PIN_PWDN,
        .pin_reset      = CAM_PIN_RESET,
        .pin_xclk       = CAM_PIN_XCLK,
        .pin_sccb_sda   = CAM_PIN_SIOD,
        .pin_sccb_scl   = CAM_PIN_SIOC,
        .pin_d7         = CAM_PIN_D7,
        .pin_d6         = CAM_PIN_D6,
        .pin_d5         = CAM_PIN_D5,
        .pin_d4         = CAM_PIN_D4,
        .pin_d3         = CAM_PIN_D3,
        .pin_d2         = CAM_PIN_D2,
        .pin_d1         = CAM_PIN_D1,
        .pin_d0         = CAM_PIN_D0,
        .pin_vsync      = CAM_PIN_VSYNC,
        .pin_href       = CAM_PIN_HREF,
        .pin_pclk       = CAM_PIN_PCLK,
        .xclk_freq_hz   = 20000000,
        .ledc_timer     = LEDC_TIMER_0,
        .ledc_channel   = LEDC_CHANNEL_0,
        .pixel_format   = PIXFORMAT_JPEG,
        .frame_size     = FRAMESIZE_QQVGA,
        .jpeg_quality   = 20,
        .fb_count       = 2,
        .fb_location    = CAMERA_FB_IN_PSRAM,
        .grab_mode      = CAMERA_GRAB_LATEST
    };

    if (esp_camera_init(&cfg) != ESP_OK) {
        msg_error(TAG, "camera init failed", NULL);
        return false;
    }

    msg_info(TAG, "camera initialized", NULL);
    vTaskDelay(pdMS_TO_TICKS(200));
    return true;
}

/* ============================================================
 * STREAM TASK
 * ============================================================ */
static void udp_stream_task(void *arg)
{
    (void)arg;

    msg_info(TAG, "stream started", NULL);

    const size_t token_len = strlen(token);
    uint8_t buf[MAX_UDP_SIZE + 32];

    while (streaming_active) {

        camera_fb_t *fb = esp_camera_fb_get();
        if (!fb) {
            msg_error(TAG, "frame capture failed", NULL);
            vTaskDelay(pdMS_TO_TICKS(50));
            continue;
        }

        /* START */
        memcpy(buf, token, token_len);
        memcpy(buf + token_len, "START", 5);
        sendto(udp_sock, buf, token_len + 5, 0,
               (struct sockaddr *)&dest_addr, sizeof(dest_addr));

        size_t off = 0;
        size_t rem = fb->len;

        while (rem > 0 && streaming_active) {
            size_t chunk = rem > MAX_UDP_SIZE ? MAX_UDP_SIZE : rem;

            memcpy(buf, token, token_len);
            memcpy(buf + token_len, fb->buf + off, chunk);

            if (sendto(udp_sock, buf, token_len + chunk, 0,
                       (struct sockaddr *)&dest_addr,
                       sizeof(dest_addr)) < 0) {
                msg_error(TAG, "udp send failed", NULL);
                break;
            }

            off += chunk;
            rem -= chunk;
            vTaskDelay(1);
        }

        /* END */
        memcpy(buf, token, token_len);
        memcpy(buf + token_len, "END", 3);
        sendto(udp_sock, buf, token_len + 3, 0,
               (struct sockaddr *)&dest_addr, sizeof(dest_addr));

        esp_camera_fb_return(fb);
        vTaskDelay(pdMS_TO_TICKS(140)); /* ~7 FPS */
    }

    if (udp_sock >= 0) {
        close(udp_sock);
        udp_sock = -1;
    }

    msg_info(TAG, "stream stopped", NULL);
    vTaskDelete(NULL);
}

/* ============================================================
 * STREAM CONTROL
 * ============================================================ */
static void start_stream(const char *ip, uint16_t port)
{
    if (streaming_active) {
        msg_error(TAG, "stream already active", NULL);
        return;
    }

    if (!camera_initialized) {
        if (!init_camera())
            return;
        camera_initialized = true;
    }

    udp_sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP);
    if (udp_sock < 0) {
        msg_error(TAG, "udp socket failed", NULL);
        return;
    }

    memset(&dest_addr, 0, sizeof(dest_addr));
    dest_addr.sin_family = AF_INET;
    dest_addr.sin_port = htons(port);
    dest_addr.sin_addr.s_addr = inet_addr(ip);

    streaming_active = true;

    xTaskCreatePinnedToCore(
        udp_stream_task,
        "cam_stream",
        8192,
        NULL,
        5,
        NULL,
        0
    );
}

static void stop_stream(void)
{
    if (!streaming_active) {
        msg_error(TAG, "no active stream", NULL);
        return;
    }

    streaming_active = false;
}

/* ============================================================
 * COMMAND HANDLERS
 * ============================================================ */
static int cmd_cam_start(int argc,
                         char **argv,
                         const char *req,
                         void *ctx)
{
    (void)ctx;

    if (argc != 2) {
        msg_error(TAG, "usage: cam_start <ip> <port>", req);
        return -1;
    }

    start_stream(argv[0], (uint16_t)atoi(argv[1]));
    return 0;
}

static int cmd_cam_stop(int argc,
                        char **argv,
                        const char *req,
                        void *ctx)
{
    (void)argc;
    (void)argv;
    (void)ctx;

    stop_stream();
    return 0;
}

/* ============================================================
 * REGISTER COMMANDS
 * ============================================================ */
static const command_t cmd_cam_start_def = {
    .name = "cam_start",
    .min_args = 2,
    .max_args = 2,
    .handler = cmd_cam_start,
    .ctx = NULL,
    .async = false
};

static const command_t cmd_cam_stop_def = {
    .name = "cam_stop",
    .min_args = 0,
    .max_args = 0,
    .handler = cmd_cam_stop,
    .ctx = NULL,
    .async = false
};

void mod_camera_register_commands(void)
{
    command_register(&cmd_cam_start_def);
    command_register(&cmd_cam_stop_def);
}

#endif