497 lines
15 KiB
C
497 lines
15 KiB
C
#include "robot_link.h"
|
||
|
||
#include <stdio.h>
|
||
#include <stdlib.h>
|
||
#include <string.h>
|
||
#include <stdbool.h>
|
||
|
||
#include "esp_camera.h"
|
||
#include "esp_event.h"
|
||
#include "esp_http_server.h"
|
||
#include "esp_log.h"
|
||
#include "esp_netif.h"
|
||
#include "esp_timer.h"
|
||
#include "esp_wifi.h"
|
||
#include "freertos/FreeRTOS.h"
|
||
#include "freertos/event_groups.h"
|
||
#include "freertos/task.h"
|
||
#include "nvs_flash.h"
|
||
|
||
#include "camera_pins.h"
|
||
|
||
static const char *TAG = "cam_stream";
|
||
|
||
/* -------- put your Wi‑Fi credentials here -------- */
|
||
#define WIFI_SSID "Police Surveillance Van"
|
||
#define WIFI_PASS "ourpassword"
|
||
/* ------------------------------------------------ */
|
||
|
||
/* Stream tuning for navigation — lower = steadier / lower latency */
|
||
#define CAM_FRAME_SIZE FRAMESIZE_QVGA /* 320x240 */
|
||
#define CAM_JPEG_QUALITY 18 /* 0-63, higher = smaller files */
|
||
#define STREAM_FPS 10
|
||
|
||
#define WIFI_CONNECTED_BIT BIT0
|
||
#define WIFI_FAIL_BIT BIT1
|
||
#define WIFI_MAX_RETRY 10
|
||
|
||
#define PART_BOUNDARY "123456789000000000000987654321"
|
||
#define _STR(x) #x
|
||
#define STR(x) _STR(x)
|
||
|
||
static EventGroupHandle_t s_wifi_event_group;
|
||
static int s_retry_num = 0;
|
||
|
||
static const char *STREAM_CONTENT_TYPE =
|
||
"multipart/x-mixed-replace;boundary=" PART_BOUNDARY;
|
||
static const char *STREAM_BOUNDARY = "\r\n--" PART_BOUNDARY "\r\n";
|
||
static const char *STREAM_PART =
|
||
"Content-Type: image/jpeg\r\nContent-Length: %u\r\n\r\n";
|
||
|
||
/*
|
||
* Browser MJPEG (<img src=/stream>) buffers heavily.
|
||
* Polling /capture with JS is much closer to real-time for viewing.
|
||
* For robot code, prefer /capture or /stream from OpenCV/Python (not a browser).
|
||
*/
|
||
static const char INDEX_HTML[] =
|
||
"<!DOCTYPE html><html><head><meta name=\"viewport\" "
|
||
"content=\"width=device-width,initial-scale=1\">"
|
||
"<title>linefollower-eye</title>"
|
||
"<style>body{margin:0;background:#111;color:#eee;font-family:sans-serif;"
|
||
"display:flex;flex-direction:column;align-items:center}"
|
||
"img{max-width:100%;height:auto}</style></head>"
|
||
"<body><h1>OV3660 (low-latency)</h1>"
|
||
"<img id=\"v\" alt=\"stream\">"
|
||
"<p style=\"opacity:.6;font-size:12px\">polling /capture — /stream still available</p>"
|
||
"<script>"
|
||
"const img=document.getElementById('v');"
|
||
"let busy=false,prev=null;"
|
||
"async function tick(){"
|
||
" if(busy)return; busy=true;"
|
||
" try{"
|
||
" const r=await fetch('/capture?t='+Date.now(),{cache:'no-store'});"
|
||
" if(!r.ok)throw 0;"
|
||
" const b=await r.blob();"
|
||
" const u=URL.createObjectURL(b);"
|
||
" img.src=u;"
|
||
" if(prev)URL.revokeObjectURL(prev);"
|
||
" prev=u;"
|
||
" }catch(e){}"
|
||
" busy=false;"
|
||
"}"
|
||
"setInterval(tick," STR(1000 / STREAM_FPS) ");"
|
||
"tick();"
|
||
"</script></body></html>";
|
||
|
||
static esp_err_t init_camera(void)
|
||
{
|
||
camera_config_t config = {
|
||
.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 = CAM_FRAME_SIZE,
|
||
.jpeg_quality = CAM_JPEG_QUALITY,
|
||
.fb_count = 2,
|
||
.fb_location = CAMERA_FB_IN_PSRAM,
|
||
.grab_mode = CAMERA_GRAB_LATEST, /* always newest frame */
|
||
};
|
||
|
||
esp_err_t err = esp_camera_init(&config);
|
||
if (err != ESP_OK) {
|
||
ESP_LOGE(TAG, "Camera init failed: %s", esp_err_to_name(err));
|
||
return err;
|
||
}
|
||
|
||
sensor_t *sensor = esp_camera_sensor_get();
|
||
if (sensor) {
|
||
ESP_LOGI(TAG, "Camera PID=0x%04x", sensor->id.PID);
|
||
sensor->set_vflip(sensor, 1);
|
||
sensor->set_hmirror(sensor, 0);
|
||
}
|
||
|
||
ESP_LOGI(TAG, "Camera ready");
|
||
return ESP_OK;
|
||
}
|
||
|
||
static void wifi_event_handler(void *arg, esp_event_base_t event_base,
|
||
int32_t event_id, void *event_data)
|
||
{
|
||
if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) {
|
||
esp_wifi_connect();
|
||
} else if (event_base == WIFI_EVENT &&
|
||
event_id == WIFI_EVENT_STA_DISCONNECTED) {
|
||
if (s_retry_num < WIFI_MAX_RETRY) {
|
||
esp_wifi_connect();
|
||
s_retry_num++;
|
||
ESP_LOGW(TAG, "Retry Wi‑Fi connect (%d/%d)", s_retry_num,
|
||
WIFI_MAX_RETRY);
|
||
} else {
|
||
xEventGroupSetBits(s_wifi_event_group, WIFI_FAIL_BIT);
|
||
}
|
||
} else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
|
||
ip_event_got_ip_t *event = (ip_event_got_ip_t *)event_data;
|
||
ESP_LOGI(TAG, "Got IP: " IPSTR, IP2STR(&event->ip_info.ip));
|
||
ESP_LOGI(TAG, "Open http://" IPSTR "/", IP2STR(&event->ip_info.ip));
|
||
s_retry_num = 0;
|
||
xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
|
||
}
|
||
}
|
||
|
||
static void init_wifi_sta(void)
|
||
{
|
||
s_wifi_event_group = xEventGroupCreate();
|
||
|
||
ESP_ERROR_CHECK(esp_netif_init());
|
||
ESP_ERROR_CHECK(esp_event_loop_create_default());
|
||
esp_netif_create_default_wifi_sta();
|
||
|
||
wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
|
||
ESP_ERROR_CHECK(esp_wifi_init(&cfg));
|
||
|
||
ESP_ERROR_CHECK(esp_event_handler_instance_register(
|
||
WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler, NULL, NULL));
|
||
ESP_ERROR_CHECK(esp_event_handler_instance_register(
|
||
IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler, NULL, NULL));
|
||
|
||
wifi_config_t wifi_config = {0};
|
||
strncpy((char *)wifi_config.sta.ssid, WIFI_SSID,
|
||
sizeof(wifi_config.sta.ssid));
|
||
strncpy((char *)wifi_config.sta.password, WIFI_PASS,
|
||
sizeof(wifi_config.sta.password));
|
||
wifi_config.sta.threshold.authmode = WIFI_AUTH_WPA2_PSK;
|
||
|
||
ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
|
||
ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
|
||
ESP_ERROR_CHECK(esp_wifi_start());
|
||
/* Modem sleep adds tens–hundreds of ms of jitter */
|
||
ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE));
|
||
|
||
ESP_LOGI(TAG, "Connecting to Wi‑Fi SSID:%s ...", WIFI_SSID);
|
||
|
||
EventBits_t bits = xEventGroupWaitBits(
|
||
s_wifi_event_group, WIFI_CONNECTED_BIT | WIFI_FAIL_BIT, pdFALSE,
|
||
pdFALSE, portMAX_DELAY);
|
||
|
||
if (bits & WIFI_FAIL_BIT) {
|
||
ESP_LOGE(TAG, "Failed to connect to Wi‑Fi — check WIFI_SSID / WIFI_PASS");
|
||
}
|
||
}
|
||
|
||
static void set_no_cache(httpd_req_t *req)
|
||
{
|
||
httpd_resp_set_hdr(req, "Cache-Control", "no-store, no-cache, must-revalidate");
|
||
httpd_resp_set_hdr(req, "Pragma", "no-cache");
|
||
httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");
|
||
}
|
||
|
||
static esp_err_t index_handler(httpd_req_t *req)
|
||
{
|
||
set_no_cache(req);
|
||
httpd_resp_set_type(req, "text/html");
|
||
return httpd_resp_send(req, INDEX_HTML, HTTPD_RESP_USE_STRLEN);
|
||
}
|
||
|
||
static esp_err_t capture_handler(httpd_req_t *req)
|
||
{
|
||
camera_fb_t *fb = esp_camera_fb_get();
|
||
if (!fb) {
|
||
httpd_resp_send_500(req);
|
||
return ESP_FAIL;
|
||
}
|
||
|
||
set_no_cache(req);
|
||
httpd_resp_set_type(req, "image/jpeg");
|
||
esp_err_t res = httpd_resp_send(req, (const char *)fb->buf, fb->len);
|
||
esp_camera_fb_return(fb);
|
||
return res;
|
||
}
|
||
|
||
static esp_err_t stream_handler(httpd_req_t *req)
|
||
{
|
||
esp_err_t res = ESP_OK;
|
||
char part_buf[64];
|
||
const int64_t frame_period_us = 1000000 / STREAM_FPS;
|
||
|
||
res = httpd_resp_set_type(req, STREAM_CONTENT_TYPE);
|
||
if (res != ESP_OK) {
|
||
return res;
|
||
}
|
||
set_no_cache(req);
|
||
|
||
while (true) {
|
||
int64_t frame_start = esp_timer_get_time();
|
||
|
||
camera_fb_t *fb = esp_camera_fb_get();
|
||
if (!fb) {
|
||
ESP_LOGE(TAG, "Camera capture failed");
|
||
res = ESP_FAIL;
|
||
break;
|
||
}
|
||
|
||
if (fb->format != PIXFORMAT_JPEG) {
|
||
esp_camera_fb_return(fb);
|
||
ESP_LOGE(TAG, "Non-JPEG frame");
|
||
res = ESP_FAIL;
|
||
break;
|
||
}
|
||
|
||
res = httpd_resp_send_chunk(req, STREAM_BOUNDARY, strlen(STREAM_BOUNDARY));
|
||
if (res == ESP_OK) {
|
||
int hlen = snprintf(part_buf, sizeof(part_buf), STREAM_PART, fb->len);
|
||
if (hlen < 0 || hlen >= (int)sizeof(part_buf)) {
|
||
res = ESP_FAIL;
|
||
} else {
|
||
res = httpd_resp_send_chunk(req, part_buf, (size_t)hlen);
|
||
}
|
||
}
|
||
if (res == ESP_OK) {
|
||
res = httpd_resp_send_chunk(req, (const char *)fb->buf, fb->len);
|
||
}
|
||
|
||
esp_camera_fb_return(fb);
|
||
|
||
if (res != ESP_OK) {
|
||
break;
|
||
}
|
||
|
||
int64_t elapsed = esp_timer_get_time() - frame_start;
|
||
int64_t sleep_us = frame_period_us - elapsed;
|
||
if (sleep_us > 1000) {
|
||
vTaskDelay(pdMS_TO_TICKS((uint32_t)(sleep_us / 1000)));
|
||
}
|
||
}
|
||
|
||
return res;
|
||
}
|
||
|
||
static esp_err_t send_json_ack(httpd_req_t *req, const robot_ack_t *ack, bool as_error)
|
||
{
|
||
char json[384];
|
||
int n = robot_link_format_ack(ack, json, sizeof(json));
|
||
if (n < 0 || n >= (int)sizeof(json)) {
|
||
httpd_resp_send_500(req);
|
||
return ESP_FAIL;
|
||
}
|
||
|
||
set_no_cache(req);
|
||
httpd_resp_set_type(req, "application/json");
|
||
if (as_error) {
|
||
httpd_resp_set_status(req, HTTPD_400);
|
||
}
|
||
return httpd_resp_send(req, json, n);
|
||
}
|
||
|
||
static bool json_get_string(const char *json, const char *key, char *out, size_t outlen)
|
||
{
|
||
char pattern[32];
|
||
snprintf(pattern, sizeof(pattern), "\"%s\"", key);
|
||
const char *p = strstr(json, pattern);
|
||
if (!p) {
|
||
return false;
|
||
}
|
||
p = strchr(p + strlen(pattern), ':');
|
||
if (!p) {
|
||
return false;
|
||
}
|
||
p++;
|
||
while (*p == ' ' || *p == '\t') {
|
||
p++;
|
||
}
|
||
if (*p != '"') {
|
||
return false;
|
||
}
|
||
p++;
|
||
size_t i = 0;
|
||
while (*p && *p != '"' && i + 1 < outlen) {
|
||
if (*p == '\\' && p[1]) {
|
||
p++;
|
||
}
|
||
out[i++] = *p++;
|
||
}
|
||
out[i] = '\0';
|
||
return i > 0 || (p && *p == '"');
|
||
}
|
||
|
||
static esp_err_t ws_handler(httpd_req_t *req)
|
||
{
|
||
if (req->method == HTTP_GET) {
|
||
ESP_LOGI(TAG, "WebSocket /ws connected");
|
||
return ESP_OK;
|
||
}
|
||
|
||
httpd_ws_frame_t ws_pkt;
|
||
memset(&ws_pkt, 0, sizeof(ws_pkt));
|
||
ws_pkt.type = HTTPD_WS_TYPE_TEXT;
|
||
|
||
esp_err_t ret = httpd_ws_recv_frame(req, &ws_pkt, 0);
|
||
if (ret != ESP_OK) {
|
||
return ret;
|
||
}
|
||
if (ws_pkt.len == 0 || ws_pkt.len > 512) {
|
||
return ESP_OK;
|
||
}
|
||
|
||
uint8_t *buf = calloc(1, ws_pkt.len + 1);
|
||
if (!buf) {
|
||
return ESP_ERR_NO_MEM;
|
||
}
|
||
ws_pkt.payload = buf;
|
||
ret = httpd_ws_recv_frame(req, &ws_pkt, ws_pkt.len);
|
||
if (ret != ESP_OK) {
|
||
free(buf);
|
||
return ret;
|
||
}
|
||
|
||
char type[16] = {0};
|
||
char dir[16] = {0};
|
||
char text[256] = {0};
|
||
robot_ack_t ack;
|
||
memset(&ack, 0, sizeof(ack));
|
||
|
||
json_get_string((const char *)buf, "type", type, sizeof(type));
|
||
if (strcmp(type, "cmd") == 0) {
|
||
json_get_string((const char *)buf, "dir", dir, sizeof(dir));
|
||
robot_link_handle_cmd(dir, &ack);
|
||
} else if (strcmp(type, "msg") == 0) {
|
||
json_get_string((const char *)buf, "text", text, sizeof(text));
|
||
robot_link_handle_msg(text, strlen(text), &ack);
|
||
} else {
|
||
ack.ok = false;
|
||
ack.seq = 0;
|
||
ack.type = "cmd";
|
||
ack.fwd = "none";
|
||
ack.error = "bad_type";
|
||
}
|
||
|
||
char json[384];
|
||
int n = robot_link_format_ack(&ack, json, sizeof(json));
|
||
free(buf);
|
||
if (n < 0) {
|
||
return ESP_FAIL;
|
||
}
|
||
|
||
httpd_ws_frame_t reply = {
|
||
.final = true,
|
||
.fragmented = false,
|
||
.type = HTTPD_WS_TYPE_TEXT,
|
||
.payload = (uint8_t *)json,
|
||
.len = (size_t)n,
|
||
};
|
||
return httpd_ws_send_frame(req, &reply);
|
||
}
|
||
|
||
static esp_err_t cmd_handler(httpd_req_t *req)
|
||
{
|
||
char query[64] = {0};
|
||
char dir[16] = {0};
|
||
|
||
if (httpd_req_get_url_query_str(req, query, sizeof(query)) == ESP_OK) {
|
||
httpd_query_key_value(query, "dir", dir, sizeof(dir));
|
||
}
|
||
|
||
robot_ack_t ack;
|
||
esp_err_t err = robot_link_handle_cmd(dir, &ack);
|
||
return send_json_ack(req, &ack, err != ESP_OK);
|
||
}
|
||
|
||
static esp_err_t msg_handler(httpd_req_t *req)
|
||
{
|
||
if (req->content_len <= 0 || req->content_len > 512) {
|
||
robot_ack_t ack = {
|
||
.ok = false,
|
||
.seq = 0,
|
||
.type = "msg",
|
||
.fwd = "none",
|
||
.error = "bad_length",
|
||
};
|
||
return send_json_ack(req, &ack, true);
|
||
}
|
||
|
||
char buf[513];
|
||
int received = httpd_req_recv(req, buf, req->content_len);
|
||
if (received <= 0) {
|
||
httpd_resp_send_500(req);
|
||
return ESP_FAIL;
|
||
}
|
||
buf[received] = '\0';
|
||
|
||
robot_ack_t ack;
|
||
esp_err_t err = robot_link_handle_msg(buf, (size_t)received, &ack);
|
||
return send_json_ack(req, &ack, err != ESP_OK);
|
||
}
|
||
|
||
static httpd_handle_t start_webserver(void)
|
||
{
|
||
httpd_config_t config = HTTPD_DEFAULT_CONFIG();
|
||
config.server_port = 80;
|
||
config.lru_purge_enable = true;
|
||
config.stack_size = 8192;
|
||
config.max_uri_handlers = 12;
|
||
|
||
httpd_handle_t server = NULL;
|
||
if (httpd_start(&server, &config) != ESP_OK) {
|
||
ESP_LOGE(TAG, "Failed to start HTTP server");
|
||
return NULL;
|
||
}
|
||
|
||
const httpd_uri_t index_uri = {
|
||
.uri = "/", .method = HTTP_GET, .handler = index_handler};
|
||
const httpd_uri_t capture_uri = {
|
||
.uri = "/capture", .method = HTTP_GET, .handler = capture_handler};
|
||
const httpd_uri_t stream_uri = {
|
||
.uri = "/stream", .method = HTTP_GET, .handler = stream_handler};
|
||
const httpd_uri_t cmd_uri = {
|
||
.uri = "/cmd", .method = HTTP_GET, .handler = cmd_handler};
|
||
const httpd_uri_t msg_uri = {
|
||
.uri = "/msg", .method = HTTP_POST, .handler = msg_handler};
|
||
const httpd_uri_t ws_uri = {
|
||
.uri = "/ws",
|
||
.method = HTTP_GET,
|
||
.handler = ws_handler,
|
||
.is_websocket = true,
|
||
};
|
||
|
||
httpd_register_uri_handler(server, &index_uri);
|
||
httpd_register_uri_handler(server, &capture_uri);
|
||
httpd_register_uri_handler(server, &stream_uri);
|
||
httpd_register_uri_handler(server, &cmd_uri);
|
||
httpd_register_uri_handler(server, &msg_uri);
|
||
httpd_register_uri_handler(server, &ws_uri);
|
||
ESP_LOGI(TAG, "HTTP+WS server started (/ws for commands)");
|
||
return server;
|
||
}
|
||
|
||
void app_main(void)
|
||
{
|
||
esp_err_t ret = nvs_flash_init();
|
||
if (ret == ESP_ERR_NVS_NO_FREE_PAGES ||
|
||
ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
|
||
ESP_ERROR_CHECK(nvs_flash_erase());
|
||
ESP_ERROR_CHECK(nvs_flash_init());
|
||
}
|
||
|
||
if (init_camera() != ESP_OK) {
|
||
ESP_LOGW(TAG, "Continuing without camera so Wi‑Fi stays up for debugging");
|
||
}
|
||
robot_link_init();
|
||
init_wifi_sta();
|
||
start_webserver();
|
||
}
|