realrobotshk
/
vision
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
vision/main/main.c

497 lines
15 KiB
C
Raw Normal View History

streaming, comms, and basic image processing app
2026-07-17 06:52:46 +00:00
#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();
}