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

1048 lines
31 KiB
C
Raw Normal View History

basically working, i2c slave, wifi testing
2026-07-20 02:08:23 +00:00
#include "vision.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "esp_heap_caps.h"
#include "esp_http_server.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "img_converters.h"
static const char *TAG = "vision";
static SemaphoreHandle_t s_lock;
static bool s_local_enabled;
static bool s_show_mask;
static vision_method_t s_method;
static uint8_t s_h;
static uint8_t s_s;
static uint8_t s_v;
static uint8_t s_h_tol;
static uint8_t s_s_tol;
static uint8_t s_v_tol;
static uint8_t s_disp_b;
static uint8_t s_disp_g;
static uint8_t s_disp_r;
static int s_coarse_stride;
static int s_min_area;
static bool s_mask_full;
static bool s_morph_roi;
static vision_stats_t s_stats;
#define DEFAULT_H 15
#define DEFAULT_S 180
#define DEFAULT_V 200
#define DEFAULT_H_TOL 10
#define DEFAULT_S_TOL 80
#define DEFAULT_V_TOL 80
#define DEFAULT_STRIDE 4
#define DEFAULT_MIN_AREA 80
#define REFINE_MARGIN 16
typedef struct {
uint8_t h_c;
uint8_t s_c;
uint8_t v_c;
uint8_t h_tol;
uint8_t s_tol;
uint8_t v_tol;
} hsv_params_t;
typedef struct {
int min_gx;
int min_gy;
int max_gx;
int max_gy;
int area;
} coarse_blob_t;
static coarse_blob_t s_last_coarse;
static bool s_have_coarse;
static void hsv_to_bgr_opencv(uint8_t h, uint8_t s, uint8_t v,
uint8_t *b, uint8_t *g, uint8_t *r);
void vision_init(void)
{
s_lock = xSemaphoreCreateMutex();
s_local_enabled = false;
s_show_mask = false;
s_method = VISION_METHOD_CONTOUR;
s_h = DEFAULT_H;
s_s = DEFAULT_S;
s_v = DEFAULT_V;
s_h_tol = DEFAULT_H_TOL;
s_s_tol = DEFAULT_S_TOL;
s_v_tol = DEFAULT_V_TOL;
hsv_to_bgr_opencv(s_h, s_s, s_v, &s_disp_b, &s_disp_g, &s_disp_r);
s_coarse_stride = DEFAULT_STRIDE;
s_min_area = DEFAULT_MIN_AREA;
s_mask_full = false;
s_morph_roi = true;
memset(&s_stats, 0, sizeof(s_stats));
ESP_LOGI(TAG, "ready (local vision off)");
}
bool vision_local_enabled(void)
{
bool v = false;
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
v = s_local_enabled;
xSemaphoreGive(s_lock);
}
return v;
}
bool vision_show_mask(void)
{
bool v = false;
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
v = s_show_mask;
xSemaphoreGive(s_lock);
}
return v;
}
vision_method_t vision_get_method(void)
{
vision_method_t m = VISION_METHOD_CONTOUR;
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
m = s_method;
xSemaphoreGive(s_lock);
}
return m;
}
void vision_set_local_enabled(bool enabled)
{
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
s_local_enabled = enabled;
xSemaphoreGive(s_lock);
}
ESP_LOGI(TAG, "local vision %s", enabled ? "ON" : "OFF");
}
void vision_set_show_mask(bool show)
{
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
s_show_mask = show;
xSemaphoreGive(s_lock);
}
}
void vision_set_method(vision_method_t method)
{
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
s_method = method;
xSemaphoreGive(s_lock);
}
}
void vision_set_coarse_stride(int stride)
{
if (stride < 2) {
stride = 2;
}
if (stride > 8) {
stride = 8;
}
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
s_coarse_stride = stride;
xSemaphoreGive(s_lock);
}
}
void vision_set_min_area(int area)
{
if (area < 1) {
area = 1;
}
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
s_min_area = area;
xSemaphoreGive(s_lock);
}
}
void vision_set_mask_full(bool full)
{
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
s_mask_full = full;
xSemaphoreGive(s_lock);
}
}
void vision_set_morph_roi(bool enable)
{
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
s_morph_roi = enable;
xSemaphoreGive(s_lock);
}
}
int vision_get_coarse_stride(void)
{
int v = DEFAULT_STRIDE;
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
v = s_coarse_stride;
xSemaphoreGive(s_lock);
}
return v;
}
int vision_get_min_area(void)
{
int v = DEFAULT_MIN_AREA;
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
v = s_min_area;
xSemaphoreGive(s_lock);
}
return v;
}
bool vision_get_mask_full(void)
{
bool v = false;
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
v = s_mask_full;
xSemaphoreGive(s_lock);
}
return v;
}
bool vision_get_morph_roi(void)
{
bool v = true;
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
v = s_morph_roi;
xSemaphoreGive(s_lock);
}
return v;
}
void vision_get_stats(vision_stats_t *out)
{
if (!out) {
return;
}
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
*out = s_stats;
xSemaphoreGive(s_lock);
} else {
*out = s_stats;
}
}
void vision_set_hsv(uint8_t h, uint8_t s, uint8_t v)
{
uint8_t db, dg, dr;
hsv_to_bgr_opencv(h, s, v, &db, &dg, &dr);
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
s_h = h;
s_s = s;
s_v = v;
s_disp_b = db;
s_disp_g = dg;
s_disp_r = dr;
xSemaphoreGive(s_lock);
}
ESP_LOGI(TAG, "HSV centre -> (%u,%u,%u)", h, s, v);
}
void vision_get_hsv(uint8_t *h, uint8_t *s, uint8_t *v)
{
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
if (h) {
*h = s_h;
}
if (s) {
*s = s_s;
}
if (v) {
*v = s_v;
}
xSemaphoreGive(s_lock);
}
}
void vision_set_tolerance(uint8_t h_tol, uint8_t s_tol, uint8_t v_tol)
{
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
if (h_tol < 1) {
h_tol = 1;
}
if (s_tol < 1) {
s_tol = 1;
}
if (v_tol < 1) {
v_tol = 1;
}
s_h_tol = h_tol;
s_s_tol = s_tol;
s_v_tol = v_tol;
xSemaphoreGive(s_lock);
}
}
void vision_get_tolerance(uint8_t *h_tol, uint8_t *s_tol, uint8_t *v_tol)
{
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
if (h_tol) {
*h_tol = s_h_tol;
}
if (s_tol) {
*s_tol = s_s_tol;
}
if (v_tol) {
*v_tol = s_v_tol;
}
xSemaphoreGive(s_lock);
}
}
static void bgr_to_hsv_opencv(uint8_t b, uint8_t g, uint8_t r,
uint8_t *h, uint8_t *s, uint8_t *v)
{
uint8_t v_max = b;
if (g > v_max) {
v_max = g;
}
if (r > v_max) {
v_max = r;
}
uint8_t v_min = b;
if (g < v_min) {
v_min = g;
}
if (r < v_min) {
v_min = r;
}
*v = v_max;
int diff = (int)v_max - (int)v_min;
if (diff == 0) {
*h = 0;
*s = 0;
return;
}
*s = (uint8_t)((diff * 255) / v_max);
float hf;
if (v_max == r) {
hf = 60.f * (float)((int)g - (int)b) / (float)diff;
} else if (v_max == g) {
hf = 60.f * (float)((int)b - (int)r) / (float)diff + 120.f;
} else {
hf = 60.f * (float)((int)r - (int)g) / (float)diff + 240.f;
}
if (hf < 0.f) {
hf += 360.f;
}
int hi = (int)(hf * 0.5f + 0.5f);
if (hi > 179) {
hi = 179;
}
*h = (uint8_t)hi;
}
/* Inverse of OpenCV-style HSV (H 0-179) used above — for UI swatch. */
static void hsv_to_bgr_opencv(uint8_t h, uint8_t s, uint8_t v,
uint8_t *b, uint8_t *g, uint8_t *r)
{
if (s == 0) {
*b = *g = *r = v;
return;
}
int region = h / 30;
int remainder = (h - (region * 30)) * 6;
int p = (v * (255 - s)) / 255;
int q = (v * (255 - ((s * remainder) / 255))) / 255;
int t = (v * (255 - ((s * (255 - remainder)) / 255))) / 255;
switch (region) {
case 0:
*r = v;
*g = (uint8_t)t;
*b = (uint8_t)p;
break;
case 1:
*r = (uint8_t)q;
*g = v;
*b = (uint8_t)p;
break;
case 2:
*r = (uint8_t)p;
*g = v;
*b = (uint8_t)t;
break;
case 3:
*r = (uint8_t)p;
*g = (uint8_t)q;
*b = v;
break;
case 4:
*r = (uint8_t)t;
*g = (uint8_t)p;
*b = v;
break;
default:
*r = v;
*g = (uint8_t)p;
*b = (uint8_t)q;
break;
}
}
static void cam_bytes_to_bgr(const uint8_t *p, uint8_t *b, uint8_t *g, uint8_t *r)
{
uint8_t hb = p[0];
uint8_t lb = p[1];
*r = (uint8_t)((hb & 0xF8) | ((hb & 0xF8) >> 5));
*g = (uint8_t)(((hb & 0x07) << 5) | ((lb & 0xE0) >> 3));
*b = (uint8_t)(((lb & 0x1F) << 3) | ((lb & 0x1F) >> 2));
}
static bool hsv_in_range(uint8_t h, uint8_t s, uint8_t v, const hsv_params_t *p)
{
int s_lo = (int)p->s_c - (int)p->s_tol;
int s_hi = (int)p->s_c + (int)p->s_tol;
int v_lo = (int)p->v_c - (int)p->v_tol;
int v_hi = (int)p->v_c + (int)p->v_tol;
if (s < s_lo || s > s_hi) {
return false;
}
if (v < v_lo || v > v_hi) {
return false;
}
if (p->h_c - p->h_tol < 0) {
int wrap_lo = 179 + (p->h_c - p->h_tol);
return (h <= p->h_c + p->h_tol) || (h >= wrap_lo);
}
if (p->h_c + p->h_tol > 179) {
int wrap_hi = p->h_c + p->h_tol - 179;
return (h >= p->h_c - p->h_tol) || (h <= wrap_hi);
}
return h >= p->h_c - p->h_tol && h <= p->h_c + p->h_tol;
}
static bool pixel_matches(const uint8_t *raw, int w, int x, int y, const hsv_params_t *p)
{
uint8_t b, g, r, hh, ss, vv;
cam_bytes_to_bgr(raw + ((size_t)y * (size_t)w + (size_t)x) * 2, &b, &g, &r);
bgr_to_hsv_opencv(b, g, r, &hh, &ss, &vv);
return hsv_in_range(hh, ss, vv, p);
}
static inline uint8_t clamp_u8(int v, int lo, int hi)
{
if (v < lo) {
return (uint8_t)lo;
}
if (v > hi) {
return (uint8_t)hi;
}
return (uint8_t)v;
}
static inline int clamp_i(int v, int lo, int hi)
{
if (v < lo) {
return lo;
}
if (v > hi) {
return hi;
}
return v;
}
static void snapshot_params(hsv_params_t *p, int *stride, int *min_area,
bool *mask_full, bool *morph_roi)
{
if (xSemaphoreTake(s_lock, portMAX_DELAY) != pdTRUE) {
return;
}
p->h_c = s_h;
p->s_c = s_s;
p->v_c = s_v;
p->h_tol = s_h_tol;
p->s_tol = s_s_tol;
p->v_tol = s_v_tol;
*stride = s_coarse_stride;
*min_area = s_min_area;
*mask_full = s_mask_full;
*morph_roi = s_morph_roi;
xSemaphoreGive(s_lock);
}
void vision_set_color_bgr(uint8_t b, uint8_t g, uint8_t r)
{
uint8_t h, s, v;
bgr_to_hsv_opencv(b, g, r, &h, &s, &v);
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
s_h = h;
s_s = s;
s_v = v;
s_disp_b = b;
s_disp_g = g;
s_disp_r = r;
xSemaphoreGive(s_lock);
}
ESP_LOGI(TAG, "colour BGR(%u,%u,%u) -> HSV(%u,%u,%u)", b, g, r, h, s, v);
}
esp_err_t vision_sample_hsv_from_frame(const camera_fb_t *fb, int x, int y)
{
if (!fb || fb->format != PIXFORMAT_RGB565) {
return ESP_ERR_INVALID_STATE;
}
int w = (int)fb->width;
int h = (int)fb->height;
if (w <= 0 || h <= 0) {
return ESP_ERR_INVALID_SIZE;
}
x = clamp_i(x, 0, w - 1);
y = clamp_i(y, 0, h - 1);
const uint8_t *raw = (const uint8_t *)fb->buf;
const uint8_t *px = raw + (size_t)y * (size_t)w * 2 + (size_t)x * 2;
uint8_t b, g, r, hh, ss, vv;
cam_bytes_to_bgr(px, &b, &g, &r);
bgr_to_hsv_opencv(b, g, r, &hh, &ss, &vv);
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
s_h = hh;
s_s = ss;
s_v = vv;
s_disp_b = b;
s_disp_g = g;
s_disp_r = r;
xSemaphoreGive(s_lock);
}
ESP_LOGI(TAG, "live pick @ (%d,%d) BGR(%u,%u,%u) -> HSV(%u,%u,%u)", x, y, b, g, r, hh, ss, vv);
return ESP_OK;
}
static bool find_largest_coarse_blob(const uint8_t *grid, int gw, int gh, int min_coarse_area,
coarse_blob_t *best)
{
uint8_t *visited = calloc((size_t)gw * (size_t)gh, 1);
if (!visited) {
return false;
}
int *queue = heap_caps_malloc((size_t)gw * (size_t)gh * 2 * sizeof(int),
MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
if (!queue) {
queue = malloc((size_t)gw * (size_t)gh * 2 * sizeof(int));
}
if (!queue) {
free(visited);
return false;
}
bool found = false;
best->area = 0;
for (int gy = 0; gy < gh; gy++) {
for (int gx = 0; gx < gw; gx++) {
int idx = gy * gw + gx;
if (!grid[idx] || visited[idx]) {
continue;
}
int head = 0;
int tail = 0;
int area = 0;
int min_gx = gx;
int min_gy = gy;
int max_gx = gx;
int max_gy = gy;
queue[tail++] = gx;
queue[tail++] = gy;
visited[idx] = 1;
while (head < tail) {
int cx = queue[head++];
int cy = queue[head++];
area++;
if (cx < min_gx) {
min_gx = cx;
}
if (cy < min_gy) {
min_gy = cy;
}
if (cx > max_gx) {
max_gx = cx;
}
if (cy > max_gy) {
max_gy = cy;
}
static const int dx[4] = {1, -1, 0, 0};
static const int dy[4] = {0, 0, 1, -1};
for (int k = 0; k < 4; k++) {
int nx = cx + dx[k];
int ny = cy + dy[k];
if (nx < 0 || nx >= gw || ny < 0 || ny >= gh) {
continue;
}
int nidx = ny * gw + nx;
if (!grid[nidx] || visited[nidx]) {
continue;
}
visited[nidx] = 1;
queue[tail++] = nx;
queue[tail++] = ny;
}
}
if (area >= min_coarse_area && area > best->area) {
best->area = area;
best->min_gx = min_gx;
best->min_gy = min_gy;
best->max_gx = max_gx;
best->max_gy = max_gy;
found = true;
}
}
}
free(queue);
free(visited);
return found;
}
static bool refine_blob(const camera_fb_t *fb, const hsv_params_t *p,
const coarse_blob_t *coarse, int stride, int min_area,
int margin, vision_stats_t *stats)
{
int w = (int)fb->width;
int h = (int)fb->height;
const uint8_t *raw = (const uint8_t *)fb->buf;
int x0 = coarse->min_gx * stride - margin;
int y0 = coarse->min_gy * stride - margin;
int x1 = (coarse->max_gx + 1) * stride + margin;
int y1 = (coarse->max_gy + 1) * stride + margin;
x0 = clamp_i(x0, 0, w - 1);
y0 = clamp_i(y0, 0, h - 1);
x1 = clamp_i(x1, 0, w - 1);
y1 = clamp_i(y1, 0, h - 1);
int64_t sum_x = 0;
int64_t sum_y = 0;
int area = 0;
for (int y = y0; y <= y1; y++) {
for (int x = x0; x <= x1; x++) {
if (!pixel_matches(raw, w, x, y, p)) {
continue;
}
sum_x += x;
sum_y += y;
area++;
}
}
if (area < min_area) {
stats->found = false;
return false;
}
stats->found = true;
stats->area = area;
stats->cx = (float)sum_x / (float)area;
stats->cy = (float)sum_y / (float)area;
stats->radius = sqrtf((float)area / (float)M_PI);
return true;
}
static void morph_3x3_roi(uint8_t *mask, int w, int h, int x0, int y0, int x1, int y1, bool dilate)
{
uint8_t *tmp = heap_caps_malloc((size_t)w * (size_t)h, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
if (!tmp) {
return;
}
memcpy(tmp, mask, (size_t)w * (size_t)h);
x0 = clamp_i(x0, 1, w - 2);
y0 = clamp_i(y0, 1, h - 2);
x1 = clamp_i(x1, 1, w - 2);
y1 = clamp_i(y1, 1, h - 2);
for (int y = y0; y <= y1; y++) {
for (int x = x0; x <= x1; x++) {
int on = dilate ? 0 : 1;
for (int dy = -1; dy <= 1; dy++) {
for (int dx = -1; dx <= 1; dx++) {
if (dilate) {
if (tmp[(y + dy) * w + (x + dx)]) {
on = 1;
}
} else if (!tmp[(y + dy) * w + (x + dx)]) {
on = 0;
}
}
}
mask[y * w + x] = (uint8_t)(on ? 255 : 0);
}
}
heap_caps_free(tmp);
}
static void morph_open_close_roi(uint8_t *mask, int w, int h, int x0, int y0, int x1, int y1)
{
morph_3x3_roi(mask, w, h, x0, y0, x1, y1, true);
morph_3x3_roi(mask, w, h, x0, y0, x1, y1, false);
morph_3x3_roi(mask, w, h, x0, y0, x1, y1, false);
morph_3x3_roi(mask, w, h, x0, y0, x1, y1, true);
}
static void fill_mask_region(const camera_fb_t *fb, const hsv_params_t *p,
uint8_t *mask, int x0, int y0, int x1, int y1)
{
int w = (int)fb->width;
const uint8_t *raw = (const uint8_t *)fb->buf;
for (int y = y0; y <= y1; y++) {
for (int x = x0; x <= x1; x++) {
mask[y * w + x] = pixel_matches(raw, w, x, y, p) ? 255 : 0;
}
}
}
static esp_err_t build_full_mask(const camera_fb_t *fb, const hsv_params_t *p, uint8_t **out_mask)
{
int w = (int)fb->width;
int h = (int)fb->height;
uint8_t *mask = heap_caps_calloc((size_t)w * (size_t)h, 1, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
if (!mask) {
return ESP_ERR_NO_MEM;
}
fill_mask_region(fb, p, mask, 0, 0, w - 1, h - 1);
*out_mask = mask;
return ESP_OK;
}
static esp_err_t build_roi_mask(const camera_fb_t *fb, const hsv_params_t *p,
bool morph, const coarse_blob_t *coarse, int stride,
uint8_t **out_mask, uint32_t *morph_us)
{
int w = (int)fb->width;
int h = (int)fb->height;
uint8_t *mask = heap_caps_calloc((size_t)w * (size_t)h, 1, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
if (!mask) {
return ESP_ERR_NO_MEM;
}
int x0 = coarse->min_gx * stride - REFINE_MARGIN;
int y0 = coarse->min_gy * stride - REFINE_MARGIN;
int x1 = (coarse->max_gx + 1) * stride + REFINE_MARGIN;
int y1 = (coarse->max_gy + 1) * stride + REFINE_MARGIN;
x0 = clamp_i(x0, 0, w - 1);
y0 = clamp_i(y0, 0, h - 1);
x1 = clamp_i(x1, 0, w - 1);
y1 = clamp_i(y1, 0, h - 1);
fill_mask_region(fb, p, mask, x0, y0, x1, y1);
if (morph) {
int64_t t0 = esp_timer_get_time();
morph_open_close_roi(mask, w, h, x0, y0, x1, y1);
if (morph_us) {
*morph_us = (uint32_t)(esp_timer_get_time() - t0);
}
} else if (morph_us) {
*morph_us = 0;
}
*out_mask = mask;
return ESP_OK;
}
static esp_err_t mask_to_jpeg(const uint8_t *mask, int w, int h, int quality,
uint8_t **jpg_buf, size_t *jpg_len)
{
camera_fb_t fake = {
.buf = (uint8_t *)mask,
.len = (size_t)w * (size_t)h,
.width = (size_t)w,
.height = (size_t)h,
.format = PIXFORMAT_GRAYSCALE,
};
return frame2jpg(&fake, quality, jpg_buf, jpg_len) ? ESP_OK : ESP_FAIL;
}
static void run_detection(const camera_fb_t *fb, vision_stats_t *stats)
{
memset(stats, 0, sizeof(*stats));
if (fb->format != PIXFORMAT_RGB565) {
return;
}
int64_t t_detect = esp_timer_get_time();
hsv_params_t p;
int stride;
int min_area;
bool mask_full;
bool morph_roi;
snapshot_params(&p, &stride, &min_area, &mask_full, &morph_roi);
(void)mask_full;
int w = (int)fb->width;
int h = (int)fb->height;
const uint8_t *raw = (const uint8_t *)fb->buf;
int gw = (w + stride - 1) / stride;
int gh = (h + stride - 1) / stride;
int min_coarse = min_area / (stride * stride);
if (min_coarse < 1) {
min_coarse = 1;
}
int64_t t0 = esp_timer_get_time();
uint8_t *grid = heap_caps_calloc((size_t)gw * (size_t)gh, 1, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
if (!grid) {
return;
}
for (int gy = 0; gy < gh; gy++) {
int py = gy * stride + stride / 2;
if (py >= h) {
py = h - 1;
}
for (int gx = 0; gx < gw; gx++) {
int px = gx * stride + stride / 2;
if (px >= w) {
px = w - 1;
}
grid[gy * gw + gx] = pixel_matches(raw, w, px, py, &p) ? 1 : 0;
}
}
stats->coarse_us = (uint32_t)(esp_timer_get_time() - t0);
coarse_blob_t coarse = {0};
t0 = esp_timer_get_time();
if (!find_largest_coarse_blob(grid, gw, gh, min_coarse, &coarse)) {
heap_caps_free(grid);
s_have_coarse = false;
stats->detect_us = (uint32_t)(esp_timer_get_time() - t_detect);
return;
}
stats->coarse_us += (uint32_t)(esp_timer_get_time() - t0);
s_last_coarse = coarse;
s_have_coarse = true;
t0 = esp_timer_get_time();
refine_blob(fb, &p, &coarse, stride, min_area, REFINE_MARGIN, stats);
stats->refine_us = (uint32_t)(esp_timer_get_time() - t0);
heap_caps_free(grid);
stats->detect_us = (uint32_t)(esp_timer_get_time() - t_detect);
if (stats->found) {
ESP_LOGD(TAG, "blob @ (%.0f,%.0f) r=%.0f area=%d",
stats->cx, stats->cy, stats->radius, stats->area);
}
}
esp_err_t vision_encode_capture_jpeg(
const camera_fb_t *fb,
int quality,
uint8_t **jpg_buf,
size_t *jpg_len,
vision_stats_t *stats_out)
{
if (!fb || !jpg_buf || !jpg_len) {
return ESP_ERR_INVALID_ARG;
}
*jpg_buf = NULL;
*jpg_len = 0;
int64_t t_total = esp_timer_get_time();
vision_stats_t stats = {0};
if (vision_local_enabled() && fb->format == PIXFORMAT_RGB565) {
run_detection(fb, &stats);
if (vision_show_mask()) {
hsv_params_t p;
int stride;
int min_area;
bool mask_full;
bool morph_roi;
snapshot_params(&p, &stride, &min_area, &mask_full, &morph_roi);
int64_t t0 = esp_timer_get_time();
uint8_t *mask = NULL;
esp_err_t err;
if (mask_full) {
err = build_full_mask(fb, &p, &mask);
} else if (stats.found && s_have_coarse) {
err = build_roi_mask(fb, &p, morph_roi, &s_last_coarse, stride, &mask,
&stats.morph_us);
} else {
mask = heap_caps_calloc((size_t)fb->width * (size_t)fb->height, 1,
MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
err = mask ? ESP_OK : ESP_ERR_NO_MEM;
}
stats.mask_us = (uint32_t)(esp_timer_get_time() - t0);
if (err != ESP_OK || !mask) {
heap_caps_free(mask);
return err;
}
t0 = esp_timer_get_time();
err = mask_to_jpeg(mask, (int)fb->width, (int)fb->height, quality, jpg_buf, jpg_len);
stats.jpeg_us = (uint32_t)(esp_timer_get_time() - t0);
heap_caps_free(mask);
if (err != ESP_OK) {
return err;
}
} else {
int64_t t0 = esp_timer_get_time();
camera_fb_t fb_copy = *fb;
esp_err_t err = frame2jpg(&fb_copy, quality, jpg_buf, jpg_len) ? ESP_OK : ESP_FAIL;
stats.jpeg_us = (uint32_t)(esp_timer_get_time() - t0);
if (err != ESP_OK) {
return err;
}
}
} else {
if (vision_local_enabled() && vision_show_mask() && fb->format != PIXFORMAT_RGB565) {
ESP_LOGW(TAG, "mask requested but frame is not RGB565 - passthrough");
}
if (fb->format == PIXFORMAT_JPEG) {
*jpg_buf = (uint8_t *)malloc(fb->len);
if (!*jpg_buf) {
return ESP_ERR_NO_MEM;
}
memcpy(*jpg_buf, fb->buf, fb->len);
*jpg_len = fb->len;
} else if (fb->format == PIXFORMAT_RGB565) {
int64_t t0 = esp_timer_get_time();
camera_fb_t fb_copy = *fb;
esp_err_t err = frame2jpg(&fb_copy, quality, jpg_buf, jpg_len) ? ESP_OK : ESP_FAIL;
stats.jpeg_us = (uint32_t)(esp_timer_get_time() - t0);
if (err != ESP_OK) {
return err;
}
} else {
return ESP_ERR_NOT_SUPPORTED;
}
}
stats.total_us = (uint32_t)(esp_timer_get_time() - t_total);
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
s_stats = stats;
xSemaphoreGive(s_lock);
}
if (stats_out) {
*stats_out = stats;
}
return ESP_OK;
}
esp_err_t vision_detect_on_frame(const camera_fb_t *fb, vision_stats_t *stats_out)
{
if (!fb || !stats_out) {
return ESP_ERR_INVALID_ARG;
}
int64_t t0 = esp_timer_get_time();
run_detection(fb, stats_out);
stats_out->total_us = (uint32_t)(esp_timer_get_time() - t0);
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
s_stats = *stats_out;
xSemaphoreGive(s_lock);
}
return ESP_OK;
}
size_t vision_format_stats_json(const vision_stats_t *stats, char *buf, size_t len)
{
if (!stats || !buf || len == 0) {
return 0;
}
return (size_t)snprintf(
buf, len,
"{\"found\":%s,\"cx\":%.1f,\"cy\":%.1f,\"radius\":%.0f,\"area\":%d,"
"\"total_ms\":%.1f,\"detect_ms\":%.1f,\"coarse_ms\":%.1f,\"refine_ms\":%.1f,"
"\"morph_ms\":%.1f,\"mask_ms\":%.1f,\"jpeg_ms\":%.1f}",
stats->found ? "true" : "false",
stats->cx, stats->cy, stats->radius, stats->area,
stats->total_us / 1000.0f, stats->detect_us / 1000.0f, stats->coarse_us / 1000.0f,
stats->refine_us / 1000.0f, stats->morph_us / 1000.0f, stats->mask_us / 1000.0f,
stats->jpeg_us / 1000.0f);
}
void vision_add_capture_headers(httpd_req_t *req, const vision_stats_t *s)
{
if (!req || !s) {
return;
}
static char data[256];
if (xSemaphoreTake(s_lock, portMAX_DELAY) != pdTRUE) {
return;
}
if (s->found) {
snprintf(data, sizeof(data),
"found=1;cx=%.1f;cy=%.1f;r=%.0f;area=%d;total=%.1f;detect=%.1f;coarse=%.1f;"
"refine=%.1f;morph=%.1f;mask=%.1f;jpeg=%.1f",
s->cx, s->cy, s->radius, s->area,
s->total_us / 1000.0f, s->detect_us / 1000.0f, s->coarse_us / 1000.0f,
s->refine_us / 1000.0f, s->morph_us / 1000.0f, s->mask_us / 1000.0f,
s->jpeg_us / 1000.0f);
} else {
snprintf(data, sizeof(data),
"found=0;total=%.1f;detect=%.1f;coarse=%.1f;refine=%.1f;morph=%.1f;mask=%.1f;"
"jpeg=%.1f",
s->total_us / 1000.0f, s->detect_us / 1000.0f, s->coarse_us / 1000.0f,
s->refine_us / 1000.0f, s->morph_us / 1000.0f, s->mask_us / 1000.0f,
s->jpeg_us / 1000.0f);
}
esp_err_t err = httpd_resp_set_hdr(req, "X-Vision-Data", data);
xSemaphoreGive(s_lock);
if (err != ESP_OK) {
ESP_LOGW(TAG, "X-Vision-Data header rejected: %s", esp_err_to_name(err));
}
}
static const char *method_name(vision_method_t m)
{
return m == VISION_METHOD_RECT ? "rect" : "contour";
}
void vision_format_ack(char *ack_buf, size_t ack_len)
{
uint8_t hh, ss, vv, ht, st, vt, db, dg, dr;
int stride;
int min_area;
bool mask_full;
bool morph_roi;
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
hh = s_h;
ss = s_s;
vv = s_v;
ht = s_h_tol;
st = s_s_tol;
vt = s_v_tol;
db = s_disp_b;
dg = s_disp_g;
dr = s_disp_r;
stride = s_coarse_stride;
min_area = s_min_area;
mask_full = s_mask_full;
morph_roi = s_morph_roi;
xSemaphoreGive(s_lock);
} else {
hh = ss = vv = ht = st = vt = 0;
db = dg = dr = 0;
stride = DEFAULT_STRIDE;
min_area = DEFAULT_MIN_AREA;
mask_full = false;
morph_roi = true;
}
snprintf(
ack_buf, ack_len,
"{\"ok\":true,\"type\":\"vision\",\"local\":%s,\"show_mask\":%s,"
"\"method\":\"%s\",\"h\":%u,\"s\":%u,\"v\":%u,"
"\"b\":%u,\"g\":%u,\"r\":%u,"
"\"h_tol\":%u,\"s_tol\":%u,\"v_tol\":%u,"
"\"coarse_stride\":%d,\"min_area\":%d,"
"\"mask_full\":%s,\"morph_roi\":%s}",
vision_local_enabled() ? "true" : "false",
vision_show_mask() ? "true" : "false",
method_name(vision_get_method()),
hh, ss, vv,
db, dg, dr,
ht, st, vt,
stride, min_area,
mask_full ? "true" : "false",
morph_roi ? "true" : "false");
}
esp_err_t vision_handle_update(
bool has_local, bool local,
bool has_show_mask, bool show_mask,
bool has_method, const char *method,
bool has_h, int h,
bool has_s, int s,
bool has_v, int v,
bool has_h_tol, int h_tol,
bool has_s_tol, int s_tol,
bool has_v_tol, int v_tol,
bool has_b, int b,
bool has_g, int g,
bool has_r, int r,
bool has_coarse_stride, int coarse_stride,
bool has_min_area, int min_area,
bool has_mask_full, bool mask_full,
bool has_morph_roi, bool morph_roi,
char *ack_buf, size_t ack_len)
{
if (has_local) {
vision_set_local_enabled(local);
}
if (has_show_mask) {
vision_set_show_mask(show_mask);
}
if (has_method && method) {
if (strcmp(method, "rect") == 0) {
vision_set_method(VISION_METHOD_RECT);
} else {
vision_set_method(VISION_METHOD_CONTOUR);
}
}
if (has_coarse_stride) {
vision_set_coarse_stride(coarse_stride);
}
if (has_min_area) {
vision_set_min_area(min_area);
}
if (has_mask_full) {
vision_set_mask_full(mask_full);
}
if (has_morph_roi) {
vision_set_morph_roi(morph_roi);
}
if (has_h_tol || has_s_tol || has_v_tol) {
uint8_t ht = s_h_tol, st = s_s_tol, vt = s_v_tol;
if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
ht = s_h_tol;
st = s_s_tol;
vt = s_v_tol;
xSemaphoreGive(s_lock);
}
if (has_h_tol) {
ht = clamp_u8(h_tol, 1, 90);
}
if (has_s_tol) {
st = clamp_u8(s_tol, 1, 255);
}
if (has_v_tol) {
vt = clamp_u8(v_tol, 1, 255);
}
vision_set_tolerance(ht, st, vt);
}
if (has_b && has_g && has_r && !(has_h && has_s && has_v)) {
vision_set_color_bgr(clamp_u8(b, 0, 255), clamp_u8(g, 0, 255), clamp_u8(r, 0, 255));
}
if (has_h && has_s && has_v) {
vision_set_hsv(clamp_u8(h, 0, 179), clamp_u8(s, 0, 255), clamp_u8(v, 0, 255));
}
vision_format_ack(ack_buf, ack_len);
return ESP_OK;
}