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

1007 lines
30 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters!

This file contains ambiguous Unicode characters that may be confused with others in your current locale. If your use case is intentional and legitimate, you can safely ignore this warning. Use the Escape button to highlight these characters.

#include "board_svc.h"
#include "eye_i2c.h"
#include "robot_link.h"
#include "status_led.h"
#include "vision.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include "esp_camera.h"
#include "esp_check.h"
#include "esp_event.h"
#include "esp_heap_caps.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 camera_config_t s_cam_config;
static bool s_camera_ok = false;
static pixformat_t s_camera_pix_fmt = PIXFORMAT_JPEG;
static SemaphoreHandle_t s_cam_mutex;
static httpd_handle_t s_httpd = NULL;
static bool s_wifi_driver_up = false;
static bool s_wifi_netif_ready = false;
static volatile bool s_want_camera = false;
static volatile bool s_want_wifi = false;
static volatile bool s_wifi_restart = false;
static char s_wifi_ssid[33];
static char s_wifi_pass[65];
static bool s_wifi_cred_from_i2c = false;
static void set_no_cache(httpd_req_t *req);
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";
/* Embedded UI (main/www/index.html) — video + local vision controls */
extern const uint8_t index_html_start[] asm("_binary_index_html_start");
extern const uint8_t index_html_end[] asm("_binary_index_html_end");
static esp_err_t init_camera_with_format(pixformat_t fmt)
{
if (s_cam_mutex) {
xSemaphoreTake(s_cam_mutex, portMAX_DELAY);
}
s_cam_config = (camera_config_t){
.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 = fmt,
.frame_size = CAM_FRAME_SIZE,
.jpeg_quality = CAM_JPEG_QUALITY,
.fb_count = 2,
.fb_location = CAMERA_FB_IN_PSRAM,
.grab_mode = CAMERA_GRAB_LATEST,
};
if (s_camera_ok) {
esp_camera_deinit();
s_camera_ok = false;
}
esp_err_t err = esp_camera_init(&s_cam_config);
if (err != ESP_OK) {
ESP_LOGE(TAG, "Camera init failed (%s): %s",
fmt == PIXFORMAT_RGB565 ? "RGB565" : "JPEG", esp_err_to_name(err));
if (s_cam_mutex) {
xSemaphoreGive(s_cam_mutex);
}
return err;
}
sensor_t *sensor = esp_camera_sensor_get();
if (sensor) {
ESP_LOGI(TAG, "Camera PID=0x%04x format=%s", sensor->id.PID,
fmt == PIXFORMAT_RGB565 ? "RGB565" : "JPEG");
sensor->set_vflip(sensor, 1);
sensor->set_hmirror(sensor, 0);
}
s_camera_ok = true;
s_camera_pix_fmt = fmt;
ESP_LOGI(TAG, "Camera ready");
if (s_cam_mutex) {
xSemaphoreGive(s_cam_mutex);
}
return ESP_OK;
}
static camera_fb_t *camera_capture(void)
{
if (s_cam_mutex) {
xSemaphoreTake(s_cam_mutex, portMAX_DELAY);
}
camera_fb_t *fb = s_camera_ok ? esp_camera_fb_get() : NULL;
if (s_cam_mutex) {
xSemaphoreGive(s_cam_mutex);
}
return fb;
}
static void camera_release(camera_fb_t *fb)
{
if (!fb) {
return;
}
if (s_cam_mutex) {
xSemaphoreTake(s_cam_mutex, portMAX_DELAY);
}
esp_camera_fb_return(fb);
if (s_cam_mutex) {
xSemaphoreGive(s_cam_mutex);
}
}
static esp_err_t capture_send_jpeg(httpd_req_t *req, const camera_fb_t *fb)
{
uint8_t *jpg_buf = NULL;
size_t jpg_len = 0;
vision_stats_t stats = {0};
esp_err_t enc = vision_encode_capture_jpeg(fb, CAM_JPEG_QUALITY, &jpg_buf, &jpg_len, &stats);
if (enc != ESP_OK || !jpg_buf || jpg_len == 0) {
ESP_LOGW(TAG, "capture encode failed: %s", esp_err_to_name(enc));
free(jpg_buf);
return ESP_FAIL;
}
set_no_cache(req);
httpd_resp_set_type(req, "image/jpeg");
if (vision_local_enabled()) {
vision_add_capture_headers(req, &stats);
}
esp_err_t res = httpd_resp_send(req, (const char *)jpg_buf, jpg_len);
free(jpg_buf);
return res;
}
static camera_fb_t *camera_snapshot_copy(camera_fb_t *snap_out)
{
camera_fb_t *fb = camera_capture();
if (!fb) {
return NULL;
}
*snap_out = *fb;
if (fb->format != PIXFORMAT_RGB565 && fb->format != PIXFORMAT_JPEG) {
camera_release(fb);
return NULL;
}
uint8_t *copy = heap_caps_malloc(fb->len, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
if (!copy) {
copy = malloc(fb->len);
}
if (!copy) {
camera_release(fb);
return NULL;
}
memcpy(copy, fb->buf, fb->len);
snap_out->buf = copy;
camera_release(fb);
return snap_out;
}
static void camera_snapshot_free(camera_fb_t *snap)
{
if (snap && snap->buf) {
heap_caps_free(snap->buf);
snap->buf = NULL;
}
}
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 load_wifi_credentials(char *ssid, size_t ssid_len, char *pass, size_t pass_len)
{
if (s_wifi_cred_from_i2c && s_wifi_ssid[0] != '\0') {
strncpy(ssid, s_wifi_ssid, ssid_len - 1);
ssid[ssid_len - 1] = '\0';
strncpy(pass, s_wifi_pass, pass_len - 1);
pass[pass_len - 1] = '\0';
return;
}
strncpy(ssid, WIFI_SSID, ssid_len - 1);
ssid[ssid_len - 1] = '\0';
strncpy(pass, WIFI_PASS, pass_len - 1);
pass[pass_len - 1] = '\0';
nvs_handle_t nvs;
if (nvs_open("wifi", NVS_READONLY, &nvs) != ESP_OK) {
return;
}
size_t len = ssid_len;
if (nvs_get_str(nvs, "ssid", ssid, &len) == ESP_OK) {
ESP_LOGI(TAG, "Wi‑Fi SSID from NVS");
}
len = pass_len;
if (nvs_get_str(nvs, "pass", pass, &len) != ESP_OK) {
/* keep default pass if missing */
}
nvs_close(nvs);
}
static esp_err_t wifi_driver_init_once(void)
{
if (s_wifi_netif_ready) {
return ESP_OK;
}
s_wifi_event_group = xEventGroupCreate();
ESP_RETURN_ON_ERROR(esp_netif_init(), TAG, "netif");
ESP_RETURN_ON_ERROR(esp_event_loop_create_default(), TAG, "event loop");
esp_netif_create_default_wifi_sta();
wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
ESP_RETURN_ON_ERROR(esp_wifi_init(&cfg), TAG, "wifi_init");
ESP_RETURN_ON_ERROR(esp_event_handler_instance_register(
WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler, NULL, NULL),
TAG, "wifi event");
ESP_RETURN_ON_ERROR(esp_event_handler_instance_register(
IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler, NULL, NULL),
TAG, "ip event");
s_wifi_netif_ready = true;
return ESP_OK;
}
static esp_err_t wifi_start_sta(void)
{
ESP_RETURN_ON_ERROR(wifi_driver_init_once(), TAG, "wifi_driver_init_once");
char ssid[33];
char pass[65];
load_wifi_credentials(ssid, sizeof(ssid), pass, sizeof(pass));
wifi_config_t wifi_config = {0};
strncpy((char *)wifi_config.sta.ssid, ssid, sizeof(wifi_config.sta.ssid));
strncpy((char *)wifi_config.sta.password, pass, sizeof(wifi_config.sta.password));
wifi_config.sta.threshold.authmode = WIFI_AUTH_WPA2_PSK;
s_retry_num = 0;
xEventGroupClearBits(s_wifi_event_group, WIFI_CONNECTED_BIT | WIFI_FAIL_BIT);
ESP_RETURN_ON_ERROR(esp_wifi_set_mode(WIFI_MODE_STA), TAG, "set_mode");
ESP_RETURN_ON_ERROR(esp_wifi_set_config(WIFI_IF_STA, &wifi_config), TAG, "set_config");
ESP_RETURN_ON_ERROR(esp_wifi_start(), TAG, "wifi_start");
ESP_RETURN_ON_ERROR(esp_wifi_set_ps(WIFI_PS_NONE), TAG, "set_ps");
eye_i2c_load_wifi_bufs(ssid, pass);
ESP_LOGI(TAG, "Connecting to Wi‑Fi SSID:%s ...", ssid);
EventBits_t bits = xEventGroupWaitBits(
s_wifi_event_group, WIFI_CONNECTED_BIT | WIFI_FAIL_BIT, pdFALSE, pdFALSE,
pdMS_TO_TICKS(20000));
if (bits & WIFI_CONNECTED_BIT) {
s_wifi_driver_up = true;
return ESP_OK;
}
ESP_LOGW(TAG, "Wi‑Fi not connected (timeout/fail) — check WIFI_SSID / WIFI_PASS");
s_wifi_driver_up = true; /* driver running; STATUS_WIFI stays clear until IP */
return ESP_ERR_TIMEOUT;
}
static void wifi_stop_sta(void)
{
if (s_httpd) {
httpd_stop(s_httpd);
s_httpd = NULL;
ESP_LOGI(TAG, "HTTP server stopped");
}
if (s_wifi_netif_ready) {
esp_wifi_stop();
s_retry_num = 0;
if (s_wifi_event_group) {
xEventGroupClearBits(s_wifi_event_group, WIFI_CONNECTED_BIT | WIFI_FAIL_BIT);
}
}
s_wifi_driver_up = false;
ESP_LOGI(TAG, "Wi‑Fi stopped");
}
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");
const size_t len = (size_t)(index_html_end - index_html_start);
return httpd_resp_send(req, (const char *)index_html_start, len);
}
static esp_err_t capture_handler(httpd_req_t *req)
{
camera_fb_t snap = {0};
if (!camera_snapshot_copy(&snap)) {
httpd_resp_send_500(req);
return ESP_FAIL;
}
esp_err_t res = capture_send_jpeg(req, &snap);
camera_snapshot_free(&snap);
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 snap = {0};
if (!camera_snapshot_copy(&snap)) {
ESP_LOGE(TAG, "Camera capture failed");
res = ESP_FAIL;
break;
}
uint8_t *jpg_buf = NULL;
size_t jpg_len = 0;
if (vision_encode_capture_jpeg(&snap, CAM_JPEG_QUALITY, &jpg_buf, &jpg_len, NULL) != ESP_OK ||
!jpg_buf) {
camera_snapshot_free(&snap);
res = ESP_FAIL;
break;
}
camera_snapshot_free(&snap);
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, (unsigned)jpg_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 *)jpg_buf, jpg_len);
}
free(jpg_buf);
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);
}
/* Match "key": so short keys like "h" do not hit "h_tol". */
static const char *json_find_key(const char *json, const char *key)
{
char pattern[40];
snprintf(pattern, sizeof(pattern), "\"%s\":", key);
return strstr(json, pattern);
}
static bool json_get_string(const char *json, const char *key, char *out, size_t outlen)
{
const char *p = json_find_key(json, key);
if (!p) {
return false;
}
p = strchr(p, ':');
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 bool json_get_bool(const char *json, const char *key, bool *out)
{
const char *p = json_find_key(json, key);
if (!p) {
return false;
}
p = strchr(p, ':');
if (!p) {
return false;
}
p++;
while (*p == ' ' || *p == '\t') {
p++;
}
if (strncmp(p, "true", 4) == 0) {
*out = true;
return true;
}
if (strncmp(p, "false", 5) == 0) {
*out = false;
return true;
}
return false;
}
static bool json_get_int(const char *json, const char *key, int *out)
{
const char *p = json_find_key(json, key);
if (!p) {
return false;
}
p = strchr(p, ':');
if (!p) {
return false;
}
p++;
while (*p == ' ' || *p == '\t') {
p++;
}
char *end = NULL;
long v = strtol(p, &end, 10);
if (end == p) {
return false;
}
*out = (int)v;
return true;
}
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 if (strcmp(type, "vision") == 0) {
char json[512];
bool do_get = false;
(void)json_get_bool((const char *)buf, "get", &do_get);
if (do_get) {
/* Read-only: never touch camera format or params. */
vision_format_ack(json, sizeof(json));
free(buf);
httpd_ws_frame_t reply = {
.final = true,
.fragmented = false,
.type = HTTPD_WS_TYPE_TEXT,
.payload = (uint8_t *)json,
.len = strlen(json),
};
return httpd_ws_send_frame(req, &reply);
}
bool local = false;
bool show_mask = false;
bool has_local = json_get_bool((const char *)buf, "local", &local);
bool has_show_mask = json_get_bool((const char *)buf, "show_mask", &show_mask);
char method[16] = {0};
bool has_method = json_get_string((const char *)buf, "method", method, sizeof(method));
int h = 0, s = 0, v = 0, h_tol = 0, s_tol = 0, v_tol = 0;
int b = 0, g = 0, r = 0;
int pick_x = 0, pick_y = 0;
int coarse_stride = 0, min_area = 0;
bool mask_full = false, morph_roi = false;
bool has_h = json_get_int((const char *)buf, "h", &h);
bool has_s = json_get_int((const char *)buf, "s", &s);
bool has_v = json_get_int((const char *)buf, "v", &v);
bool has_h_tol = json_get_int((const char *)buf, "h_tol", &h_tol);
bool has_s_tol = json_get_int((const char *)buf, "s_tol", &s_tol);
bool has_v_tol = json_get_int((const char *)buf, "v_tol", &v_tol);
bool has_b = json_get_int((const char *)buf, "b", &b);
bool has_g = json_get_int((const char *)buf, "g", &g);
bool has_r = json_get_int((const char *)buf, "r", &r);
bool has_pick_x = json_get_int((const char *)buf, "pick_x", &pick_x);
bool has_pick_y = json_get_int((const char *)buf, "pick_y", &pick_y);
bool has_coarse_stride = json_get_int((const char *)buf, "coarse_stride", &coarse_stride);
bool has_min_area = json_get_int((const char *)buf, "min_area", &min_area);
bool has_mask_full = json_get_bool((const char *)buf, "mask_full", &mask_full);
bool has_morph_roi = json_get_bool((const char *)buf, "morph_roi", &morph_roi);
bool target_local = has_local ? local : vision_local_enabled();
pixformat_t want_fmt = target_local ? PIXFORMAT_RGB565 : PIXFORMAT_JPEG;
if (want_fmt != s_camera_pix_fmt) {
ESP_LOGI(TAG, "Switching camera for local vision -> %s",
want_fmt == PIXFORMAT_RGB565 ? "RGB565" : "JPEG");
esp_err_t cam_err = init_camera_with_format(want_fmt);
if (cam_err != ESP_OK) {
snprintf(
json, sizeof(json),
"{\"ok\":false,\"type\":\"vision\",\"error\":\"camera_reinit\"}");
free(buf);
httpd_ws_frame_t reply = {
.final = true,
.fragmented = false,
.type = HTTPD_WS_TYPE_TEXT,
.payload = (uint8_t *)json,
.len = strlen(json),
};
return httpd_ws_send_frame(req, &reply);
}
}
if (target_local && has_pick_x && has_pick_y) {
has_h = false;
has_s = false;
has_v = false;
}
vision_handle_update(
has_local, local,
has_show_mask, show_mask,
has_method, method,
has_h, h, has_s, s, has_v, v,
has_h_tol, h_tol, has_s_tol, s_tol, has_v_tol, v_tol,
has_b, b, has_g, g, has_r, r,
has_coarse_stride, coarse_stride,
has_min_area, min_area,
has_mask_full, mask_full,
has_morph_roi, morph_roi,
json, sizeof(json));
if (target_local && has_pick_x && has_pick_y) {
camera_fb_t snap = {0};
if (camera_snapshot_copy(&snap)) {
if (vision_sample_hsv_from_frame(&snap, pick_x, pick_y) == ESP_OK) {
vision_format_ack(json, sizeof(json));
}
camera_snapshot_free(&snap);
}
}
free(buf);
httpd_ws_frame_t reply = {
.final = true,
.fragmented = false,
.type = HTTPD_WS_TYPE_TEXT,
.payload = (uint8_t *)json,
.len = strlen(json),
};
return httpd_ws_send_frame(req, &reply);
} 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 esp_err_t vision_handler(httpd_req_t *req)
{
camera_fb_t snap = {0};
if (!camera_snapshot_copy(&snap)) {
httpd_resp_send_500(req);
return ESP_FAIL;
}
vision_stats_t stats = {0};
esp_err_t err = vision_detect_on_frame(&snap, &stats);
camera_snapshot_free(&snap);
if (err != ESP_OK) {
httpd_resp_send_500(req);
return ESP_FAIL;
}
char json[320];
vision_format_stats_json(&stats, json, sizeof(json));
set_no_cache(req);
httpd_resp_set_type(req, "application/json");
return httpd_resp_send(req, json, HTTPD_RESP_USE_STRLEN);
}
static esp_err_t vision_settings_handler(httpd_req_t *req)
{
char json[512];
vision_format_ack(json, sizeof(json));
set_no_cache(req);
httpd_resp_set_type(req, "application/json");
return httpd_resp_send(req, json, HTTPD_RESP_USE_STRLEN);
}
static httpd_handle_t start_webserver(void)
{
if (s_httpd) {
return s_httpd;
}
httpd_config_t config = HTTPD_DEFAULT_CONFIG();
config.server_port = 80;
config.lru_purge_enable = true;
config.stack_size = 20480;
config.max_uri_handlers = 12;
config.max_resp_headers = 16;
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 vision_uri = {
.uri = "/vision", .method = HTTP_GET, .handler = vision_handler};
const httpd_uri_t vision_settings_uri = {
.uri = "/vision/settings", .method = HTTP_GET, .handler = vision_settings_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, &vision_uri);
httpd_register_uri_handler(server, &vision_settings_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)");
s_httpd = server;
return server;
}
esp_err_t board_camera_set(bool enable)
{
s_want_camera = enable;
return ESP_OK;
}
esp_err_t board_wifi_set(bool enable)
{
s_want_wifi = enable;
return ESP_OK;
}
esp_err_t board_wifi_set_credentials(const char *ssid, const char *password)
{
if (!ssid || ssid[0] == '\0') {
return ESP_ERR_INVALID_ARG;
}
memset(s_wifi_ssid, 0, sizeof(s_wifi_ssid));
memset(s_wifi_pass, 0, sizeof(s_wifi_pass));
strncpy(s_wifi_ssid, ssid, sizeof(s_wifi_ssid) - 1);
if (password) {
strncpy(s_wifi_pass, password, sizeof(s_wifi_pass) - 1);
}
s_wifi_cred_from_i2c = true;
if (s_wifi_driver_up) {
s_wifi_restart = true;
}
/* Persist so a later CMD 7 / reboot path can reuse them */
nvs_handle_t nvs;
if (nvs_open("wifi", NVS_READWRITE, &nvs) == ESP_OK) {
(void)nvs_set_str(nvs, "ssid", s_wifi_ssid);
(void)nvs_set_str(nvs, "pass", s_wifi_pass);
(void)nvs_commit(nvs);
nvs_close(nvs);
}
return ESP_OK;
}
esp_err_t board_camera_sync_format(void)
{
if (!s_camera_ok) {
return ESP_ERR_INVALID_STATE;
}
pixformat_t want = vision_local_enabled() ? PIXFORMAT_RGB565 : PIXFORMAT_JPEG;
if (want == s_camera_pix_fmt) {
return ESP_OK;
}
ESP_LOGI(TAG, "Switching camera format -> %s",
want == PIXFORMAT_RGB565 ? "RGB565" : "JPEG");
return init_camera_with_format(want);
}
bool board_camera_is_on(void)
{
return s_camera_ok;
}
bool board_wifi_is_on(void)
{
return s_wifi_driver_up;
}
static void apply_camera_want(void)
{
if (s_want_camera && !s_camera_ok) {
/* RGB565 so I2C vision works without Wi‑Fi */
if (init_camera_with_format(PIXFORMAT_RGB565) != ESP_OK) {
ESP_LOGE(TAG, "Camera enable failed");
s_want_camera = false;
}
} else if (!s_want_camera && s_camera_ok) {
if (s_cam_mutex) {
xSemaphoreTake(s_cam_mutex, portMAX_DELAY);
}
esp_camera_deinit();
s_camera_ok = false;
if (s_cam_mutex) {
xSemaphoreGive(s_cam_mutex);
}
ESP_LOGI(TAG, "Camera off");
}
/* Format switches only via board_camera_sync_format() on FLAGS/WS — not every poll. */
}
static void apply_wifi_want(void)
{
if (s_wifi_restart && s_wifi_driver_up) {
ESP_LOGI(TAG, "Restarting Wi‑Fi with new credentials");
wifi_stop_sta();
s_wifi_restart = false;
}
if (s_want_wifi && !s_wifi_driver_up) {
esp_err_t err = wifi_start_sta();
if (err == ESP_OK || err == ESP_ERR_TIMEOUT) {
if (!s_httpd) {
start_webserver();
}
} else {
ESP_LOGE(TAG, "Wi‑Fi enable failed: %s", esp_err_to_name(err));
s_want_wifi = false;
}
} else if (!s_want_wifi && s_wifi_driver_up) {
wifi_stop_sta();
}
}
static void power_task(void *arg)
{
(void)arg;
while (true) {
apply_camera_want();
apply_wifi_want();
vTaskDelay(pdMS_TO_TICKS(50));
}
}
static void vision_loop_task(void *arg)
{
(void)arg;
while (true) {
if (!s_camera_ok || !vision_local_enabled()) {
vTaskDelay(pdMS_TO_TICKS(40));
continue;
}
camera_fb_t snap = {0};
if (camera_snapshot_copy(&snap)) {
vision_stats_t stats = {0};
(void)vision_detect_on_frame(&snap, &stats);
camera_snapshot_free(&snap);
}
/* Detection dominates period; short yield for I2C/HTTP */
vTaskDelay(pdMS_TO_TICKS(5));
}
}
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());
}
s_cam_mutex = xSemaphoreCreateMutex();
if (status_led_init() != ESP_OK) {
ESP_LOGW(TAG, "Status LED init failed");
}
vision_init();
robot_link_init();
eye_i2c_set_cam_mutex(s_cam_mutex);
if (eye_i2c_init() != ESP_OK) {
ESP_LOGW(TAG, "I2C slave init failed");
}
xTaskCreate(power_task, "eye_power", 8192, NULL, 5, NULL);
xTaskCreate(vision_loop_task, "eye_vision", 8192, NULL, 4, NULL);
ESP_LOGI(TAG, "Boot: camera+Wi‑Fi off — I2C CMD 5=cam on, 6=off, 7=wifi on, 8=off");
}
Reference in New Issue View Git Blame Copy Permalink