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

617 lines
18 KiB
C
Raw Blame History

#include "eye_i2c.h"
#include <string.h>
#include "board_svc.h"
#include "driver/i2c_slave.h"
#include "esp_camera.h"
#include "esp_check.h"
#include "esp_log.h"
#include "esp_netif.h"
#include "esp_timer.h"
#include "esp_wifi.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "freertos/task.h"
#include "hal/i2c_ll.h"
#include "hal/i2c_types.h"
#include "nvs.h"
#include "nvs_flash.h"
#include "robot_link.h"
#include "soc/soc_caps.h"
#include "status_led.h"
#include "vision.h"
static const char *TAG = "eye_i2c";
#define REG_WHOAMI 0x00
#define REG_VERSION 0x01
#define REG_STATUS 0x02
#define REG_FLAGS 0x03
#define REG_METHOD 0x04
#define REG_CMD 0x05
#define REG_STRIDE 0x06
#define REG_MIN_AREA_L 0x07
#define REG_MIN_AREA_H 0x08
#define REG_H 0x09
#define REG_S 0x0A
#define REG_V 0x0B
#define REG_H_TOL 0x0C
#define REG_S_TOL 0x0D
#define REG_V_TOL 0x0E
#define REG_APPLY 0x0F
#define REG_FOUND 0x10
#define REG_CX_L 0x11
#define REG_CY_L 0x13
#define REG_RADIUS_L 0x15
#define REG_AREA_L 0x17
#define REG_TOTAL_MS_L 0x19
#define REG_DETECT_MS_L 0x1B
#define REG_SCRATCH0 0x1D
#define REG_SCRATCH1 0x1E
#define REG_SCRATCH2 0x1F
#define REG_WIFI_CRED 0x20
#define REG_IP0 0x21
#define REG_IP1 0x22
#define REG_IP2 0x23
#define REG_IP3 0x24
#define REG_RGB_R 0x25
#define REG_RGB_G 0x26
#define REG_RGB_B 0x27
#define WHOAMI_VALUE 0xA5
#define VERSION_VALUE 0x08
#define FLAG_LOCAL (1u << 0)
#define FLAG_SHOW_MASK (1u << 1)
#define FLAG_MASK_FULL (1u << 2)
#define FLAG_MORPH_ROI (1u << 3)
#define STATUS_WIFI (1u << 0)
#define STATUS_CAM (1u << 1)
#define STATUS_LOCAL (1u << 2)
#define STATUS_FOUND (1u << 3)
#define APPLY_BGR (1u << 0)
#define APPLY_PICK (1u << 1)
#define CMD_STOP 0
#define CMD_UP 1
#define CMD_DOWN 2
#define CMD_LEFT 3
#define CMD_RIGHT 4
#define CMD_CAM_ON 5
#define CMD_CAM_OFF 6
#define CMD_WIFI_ON 7
#define CMD_WIFI_OFF 8
#define MAX_BLOB_AREA 20000
#define MAX_BLOB_RADIUS 100
#define EYE_I2C_RX_MAX 128
#if !CONFIG_I2C_ENABLE_SLAVE_DRIVER_VERSION_2
#error "eye_i2c requires CONFIG_I2C_ENABLE_SLAVE_DRIVER_VERSION_2 (FIFO callbacks)."
#endif
typedef enum {
EVT_RX = 1,
EVT_PTR = 2,
} eye_evt_t;
typedef struct {
uint8_t data[EYE_I2C_RX_MAX];
uint32_t len;
} eye_rx_pkt_t;
typedef struct {
i2c_slave_dev_handle_t handle;
uint8_t wr[EYE_I2C_RAM_SIZE];
QueueHandle_t evt_q;
QueueHandle_t rx_q;
bool device_up;
} eye_i2c_ctx_t;
static eye_i2c_ctx_t s_ctx;
static SemaphoreHandle_t s_cam_mutex_ext;
/* Double-buffered publish image; cursor set in on_receive ISR. */
static uint8_t s_snap[2][EYE_I2C_RAM_SIZE];
static volatile uint8_t s_snap_idx;
static volatile uint8_t s_cursor;
static i2c_dev_t *s_i2c_hw;
static volatile uint32_t s_ptr_count;
static volatile uint32_t s_req_count;
static volatile uint8_t s_last_ptr;
static volatile uint8_t s_last_preload[4];
void eye_i2c_set_cam_mutex(void *mutex)
{
s_cam_mutex_ext = (SemaphoreHandle_t)mutex;
}
void eye_i2c_load_wifi_bufs(const char *ssid, const char *pass)
{
(void)ssid;
(void)pass;
}
static void put_u16(uint8_t *p, uint16_t v)
{
p[0] = (uint8_t)(v & 0xFF);
p[1] = (uint8_t)((v >> 8) & 0xFF);
}
static uint16_t get_u16(const uint8_t *p)
{
return (uint16_t)p[0] | ((uint16_t)p[1] << 8);
}
static bool wifi_has_ip(void)
{
esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
if (!netif) {
return false;
}
esp_netif_ip_info_t info;
if (esp_netif_get_ip_info(netif, &info) != ESP_OK) {
return false;
}
return info.ip.addr != 0;
}
static void wifi_fill_ip(uint8_t out[4])
{
out[0] = out[1] = out[2] = out[3] = 0;
esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
if (!netif) {
return;
}
esp_netif_ip_info_t info;
if (esp_netif_get_ip_info(netif, &info) != ESP_OK || info.ip.addr == 0) {
return;
}
uint32_t addr = info.ip.addr; /* network byte order on ESP */
out[0] = (uint8_t)(addr & 0xFF);
out[1] = (uint8_t)((addr >> 8) & 0xFF);
out[2] = (uint8_t)((addr >> 16) & 0xFF);
out[3] = (uint8_t)((addr >> 24) & 0xFF);
}
static void publish_regs(const uint8_t *tmp)
{
uint8_t next = (uint8_t)(1u - s_snap_idx);
memcpy(s_snap[next], tmp, EYE_I2C_RAM_SIZE);
s_snap_idx = next;
}
static void refresh_regs(void)
{
uint8_t tmp[EYE_I2C_RAM_SIZE];
memset(tmp, 0, sizeof(tmp));
tmp[REG_WHOAMI] = WHOAMI_VALUE;
tmp[REG_VERSION] = VERSION_VALUE;
uint8_t flags = 0;
if (vision_local_enabled()) {
flags |= FLAG_LOCAL;
}
if (vision_show_mask()) {
flags |= FLAG_SHOW_MASK;
}
if (vision_get_mask_full()) {
flags |= FLAG_MASK_FULL;
}
if (vision_get_morph_roi()) {
flags |= FLAG_MORPH_ROI;
}
tmp[REG_FLAGS] = flags;
tmp[REG_METHOD] = (vision_get_method() == VISION_METHOD_RECT) ? 1 : 0;
tmp[REG_STRIDE] = (uint8_t)vision_get_coarse_stride();
put_u16(&tmp[REG_MIN_AREA_L], (uint16_t)vision_get_min_area());
uint8_t h, s, v, ht, st, vt;
vision_get_hsv(&h, &s, &v);
vision_get_tolerance(&ht, &st, &vt);
tmp[REG_H] = h;
tmp[REG_S] = s;
tmp[REG_V] = v;
tmp[REG_H_TOL] = ht;
tmp[REG_S_TOL] = st;
tmp[REG_V_TOL] = vt;
tmp[REG_CMD] = 0xFF;
tmp[REG_APPLY] = 0;
vision_stats_t stt;
vision_get_stats(&stt);
bool found = stt.found;
if (found && (stt.area > MAX_BLOB_AREA || stt.radius > MAX_BLOB_RADIUS)) {
found = false;
}
tmp[REG_FOUND] = found ? 1 : 0;
if (found) {
put_u16(&tmp[REG_CX_L], (uint16_t)(int16_t)(stt.cx + (stt.cx >= 0 ? 0.5f : -0.5f)));
put_u16(&tmp[REG_CY_L], (uint16_t)(int16_t)(stt.cy + (stt.cy >= 0 ? 0.5f : -0.5f)));
put_u16(&tmp[REG_RADIUS_L], (uint16_t)(stt.radius + 0.5f));
uint16_t area = stt.area > 65535 ? 65535 : (uint16_t)stt.area;
put_u16(&tmp[REG_AREA_L], area);
}
put_u16(&tmp[REG_TOTAL_MS_L], (uint16_t)(stt.total_us / 1000u));
put_u16(&tmp[REG_DETECT_MS_L], (uint16_t)(stt.detect_us / 1000u));
uint8_t status = 0;
if (wifi_has_ip()) {
status |= STATUS_WIFI;
}
if (board_camera_is_on()) {
status |= STATUS_CAM;
}
if (vision_local_enabled()) {
status |= STATUS_LOCAL;
}
if (found) {
status |= STATUS_FOUND;
}
tmp[REG_STATUS] = status;
uint8_t ip[4];
wifi_fill_ip(ip);
tmp[REG_IP0] = ip[0];
tmp[REG_IP1] = ip[1];
tmp[REG_IP2] = ip[2];
tmp[REG_IP3] = ip[3];
/* Preserve writable scratch from host */
tmp[REG_SCRATCH0] = s_ctx.wr[REG_SCRATCH0];
tmp[REG_SCRATCH1] = s_ctx.wr[REG_SCRATCH1];
tmp[REG_SCRATCH2] = s_ctx.wr[REG_SCRATCH2];
publish_regs(tmp);
}
static void do_cmd(uint8_t code)
{
const char *dir = NULL;
switch (code) {
case CMD_STOP:
dir = "stop";
break;
case CMD_UP:
dir = "up";
break;
case CMD_DOWN:
dir = "down";
break;
case CMD_LEFT:
dir = "left";
break;
case CMD_RIGHT:
dir = "right";
break;
case CMD_CAM_ON:
ESP_LOGI(TAG, "CMD cam on");
(void)board_camera_set(true);
return;
case CMD_CAM_OFF:
ESP_LOGI(TAG, "CMD cam off");
(void)board_camera_set(false);
return;
case CMD_WIFI_ON:
ESP_LOGI(TAG, "CMD wifi on");
(void)board_wifi_set(true);
return;
case CMD_WIFI_OFF:
ESP_LOGI(TAG, "CMD wifi off");
(void)board_wifi_set(false);
return;
default:
return;
}
robot_ack_t ack;
robot_link_handle_cmd(dir, &ack);
}
static esp_err_t do_pick(int x, int y)
{
if (s_cam_mutex_ext &&
xSemaphoreTake(s_cam_mutex_ext, pdMS_TO_TICKS(200)) != pdTRUE) {
return ESP_ERR_TIMEOUT;
}
camera_fb_t *fb = esp_camera_fb_get();
esp_err_t err = ESP_FAIL;
if (fb) {
err = vision_sample_hsv_from_frame(fb, x, y);
esp_camera_fb_return(fb);
}
if (s_cam_mutex_ext) {
xSemaphoreGive(s_cam_mutex_ext);
}
return err;
}
static void apply_write(uint8_t reg, const uint8_t *data, uint32_t len)
{
if (reg >= EYE_I2C_RAM_SIZE || len == 0) {
return;
}
if (reg + len > EYE_I2C_RAM_SIZE) {
len = EYE_I2C_RAM_SIZE - reg;
}
uint8_t before[EYE_I2C_RAM_SIZE];
memcpy(before, s_ctx.wr, EYE_I2C_RAM_SIZE);
for (uint32_t i = 0; i < len; i++) {
s_ctx.wr[reg + i] = data[i];
}
const uint8_t *rx = s_ctx.wr;
const uint8_t *prev = before;
if (reg <= REG_FLAGS && reg + len > REG_FLAGS && rx[REG_FLAGS] != prev[REG_FLAGS]) {
uint8_t f = rx[REG_FLAGS];
vision_set_local_enabled((f & FLAG_LOCAL) != 0);
vision_set_show_mask((f & FLAG_SHOW_MASK) != 0);
vision_set_mask_full((f & FLAG_MASK_FULL) != 0);
vision_set_morph_roi((f & FLAG_MORPH_ROI) != 0);
(void)board_camera_sync_format();
}
if (reg <= REG_METHOD && reg + len > REG_METHOD && rx[REG_METHOD] != prev[REG_METHOD]) {
vision_set_method(rx[REG_METHOD] == 1 ? VISION_METHOD_RECT : VISION_METHOD_CONTOUR);
}
if (reg <= REG_CMD && reg + len > REG_CMD && rx[REG_CMD] != prev[REG_CMD] &&
rx[REG_CMD] != 0xFF) {
do_cmd(rx[REG_CMD]);
}
if (reg <= REG_STRIDE && reg + len > REG_STRIDE && rx[REG_STRIDE] != prev[REG_STRIDE]) {
vision_set_coarse_stride(rx[REG_STRIDE]);
}
if ((reg <= REG_MIN_AREA_H && reg + len > REG_MIN_AREA_L) &&
(rx[REG_MIN_AREA_L] != prev[REG_MIN_AREA_L] || rx[REG_MIN_AREA_H] != prev[REG_MIN_AREA_H])) {
vision_set_min_area((int)get_u16(&rx[REG_MIN_AREA_L]));
}
if ((reg <= REG_V && reg + len > REG_H) &&
(rx[REG_H] != prev[REG_H] || rx[REG_S] != prev[REG_S] || rx[REG_V] != prev[REG_V])) {
vision_set_hsv(rx[REG_H], rx[REG_S], rx[REG_V]);
}
if ((reg <= REG_V_TOL && reg + len > REG_H_TOL) &&
(rx[REG_H_TOL] != prev[REG_H_TOL] || rx[REG_S_TOL] != prev[REG_S_TOL] ||
rx[REG_V_TOL] != prev[REG_V_TOL])) {
vision_set_tolerance(rx[REG_H_TOL], rx[REG_S_TOL], rx[REG_V_TOL]);
}
if (reg <= REG_APPLY && reg + len > REG_APPLY && rx[REG_APPLY] != 0 &&
rx[REG_APPLY] != prev[REG_APPLY]) {
if (rx[REG_APPLY] & APPLY_BGR) {
vision_set_color_bgr(rx[REG_SCRATCH0], rx[REG_SCRATCH1], rx[REG_SCRATCH2]);
}
if (rx[REG_APPLY] & APPLY_PICK) {
int x = (int)get_u16(&rx[REG_SCRATCH0]);
int y = rx[REG_SCRATCH2];
(void)do_pick(x, y);
}
s_ctx.wr[REG_APPLY] = 0;
}
if ((reg <= REG_RGB_B && reg + len > REG_RGB_R) &&
(rx[REG_RGB_R] != prev[REG_RGB_R] || rx[REG_RGB_G] != prev[REG_RGB_G] ||
rx[REG_RGB_B] != prev[REG_RGB_B])) {
(void)status_led_set_rgb(rx[REG_RGB_R], rx[REG_RGB_G], rx[REG_RGB_B]);
}
ESP_LOGI(TAG, "WR reg=0x%02x len=%lu", reg, (unsigned long)len);
refresh_regs();
}
static void apply_wifi_cred(const uint8_t *data, uint32_t len)
{
/* Payload: [ssid_len][ssid…][pass_len][pass…] */
if (!data || len < 2) {
ESP_LOGW(TAG, "WIFI_CRED short (%lu)", (unsigned long)len);
return;
}
uint8_t ssid_len = data[0];
if (ssid_len == 0 || ssid_len > 32 || (uint32_t)(1 + ssid_len) >= len) {
ESP_LOGW(TAG, "WIFI_CRED bad ssid_len=%u len=%lu", ssid_len, (unsigned long)len);
return;
}
uint8_t pass_len = data[1 + ssid_len];
if ((uint32_t)(1 + ssid_len + 1 + pass_len) > len || pass_len > 64) {
ESP_LOGW(TAG, "WIFI_CRED bad pass_len=%u len=%lu", pass_len, (unsigned long)len);
return;
}
char ssid[33];
char pass[65];
memset(ssid, 0, sizeof(ssid));
memset(pass, 0, sizeof(pass));
memcpy(ssid, &data[1], ssid_len);
memcpy(pass, &data[1 + ssid_len + 1], pass_len);
ESP_LOGI(TAG, "WIFI_CRED ssid='%s' (pass %u bytes) — enabling", ssid, pass_len);
(void)board_wifi_set_credentials(ssid, pass);
(void)board_wifi_set(true);
}
static void IRAM_ATTR preload_tx_fifo(void)
{
if (!s_i2c_hw) {
return;
}
uint8_t c = s_cursor;
if (c >= EYE_I2C_RAM_SIZE) {
c = 0;
}
uint8_t n = (uint8_t)(EYE_I2C_RAM_SIZE - c);
if (n > SOC_I2C_FIFO_LEN) {
n = SOC_I2C_FIFO_LEN;
}
uint8_t idx = s_snap_idx;
i2c_ll_slave_disable_tx_it(s_i2c_hw);
i2c_ll_txfifo_rst(s_i2c_hw);
i2c_ll_write_txfifo(s_i2c_hw, &s_snap[idx][c], n);
/* Snapshot what we loaded for task-context logging. */
s_last_preload[0] = s_snap[idx][c];
s_last_preload[1] = (n > 1) ? s_snap[idx][c + 1] : 0;
s_last_preload[2] = (n > 2) ? s_snap[idx][c + 2] : 0;
s_last_preload[3] = (n > 3) ? s_snap[idx][c + 3] : 0;
}
static bool IRAM_ATTR on_receive_cb(i2c_slave_dev_handle_t i2c_slave,
const i2c_slave_rx_done_event_data_t *evt, void *arg)
{
(void)i2c_slave;
(void)arg;
if (!evt || !evt->buffer || evt->length == 0 || evt->length > EYE_I2C_RX_MAX) {
return false;
}
s_cursor = evt->buffer[0];
s_last_ptr = s_cursor;
if (evt->length == 1) {
/*
* Register pointer write (start of a split read). RP2040 cannot clock-
* stretch, so the TX FIFO must already hold the reply before readfrom().
*/
preload_tx_fifo();
s_ptr_count++;
BaseType_t woke = pdFALSE;
eye_evt_t ev = EVT_PTR;
xQueueSendFromISR(s_ctx.evt_q, &ev, &woke);
return woke == pdTRUE;
}
eye_rx_pkt_t pkt = {0};
pkt.len = evt->length;
memcpy(pkt.data, evt->buffer, evt->length);
BaseType_t woke = pdFALSE;
if (xQueueSendFromISR(s_ctx.rx_q, &pkt, &woke) != pdTRUE) {
return false;
}
eye_evt_t ev = EVT_RX;
xQueueSendFromISR(s_ctx.evt_q, &ev, &woke);
return woke == pdTRUE;
}
static bool IRAM_ATTR on_request_cb(i2c_slave_dev_handle_t i2c_slave,
const i2c_slave_request_event_data_t *evt, void *arg)
{
(void)i2c_slave;
(void)evt;
(void)arg;
s_req_count++;
preload_tx_fifo();
if (s_i2c_hw) {
i2c_ll_slave_clear_stretch(s_i2c_hw);
}
return false;
}
static esp_err_t slave_hw_start(void)
{
s_i2c_hw = I2C_LL_GET_HW(I2C_NUM_1);
i2c_slave_config_t cfg = {
.i2c_port = I2C_NUM_1,
.sda_io_num = EYE_I2C_SDA_GPIO,
.scl_io_num = EYE_I2C_SCL_GPIO,
.clk_source = I2C_CLK_SRC_DEFAULT,
.send_buf_depth = 256,
.receive_buf_depth = 256,
.slave_addr = EYE_I2C_ADDR,
.addr_bit_len = I2C_ADDR_BIT_LEN_7,
.intr_priority = 0,
.flags =
{
.enable_internal_pullup = false,
},
};
esp_err_t err = i2c_new_slave_device(&cfg, &s_ctx.handle);
if (err != ESP_OK) {
ESP_LOGE(TAG, "i2c_new_slave_device: %s", esp_err_to_name(err));
return err;
}
/* Pico/RP2040 master I2C does not reliably support slave clock stretching. */
i2c_ll_slave_enable_scl_stretch(s_i2c_hw, false);
i2c_ll_slave_clear_stretch(s_i2c_hw);
i2c_slave_event_callbacks_t cbs = {
.on_receive = on_receive_cb,
.on_request = on_request_cb,
};
err = i2c_slave_register_event_callbacks(s_ctx.handle, &cbs, NULL);
if (err != ESP_OK) {
i2c_del_slave_device(s_ctx.handle);
s_ctx.handle = NULL;
return err;
}
s_ctx.device_up = true;
refresh_regs();
ESP_LOGI(TAG, "v2 slave up addr=0x%02X SDA=%d SCL=%d (no-stretch, preload TX)", EYE_I2C_ADDR,
EYE_I2C_SDA_GPIO, EYE_I2C_SCL_GPIO);
return ESP_OK;
}
static void eye_i2c_task(void *arg)
{
(void)arg;
while (true) {
eye_evt_t ev;
if (xQueueReceive(s_ctx.evt_q, &ev, pdMS_TO_TICKS(40)) != pdTRUE) {
refresh_regs();
continue;
}
do {
if (ev == EVT_RX) {
eye_rx_pkt_t pkt;
while (xQueueReceive(s_ctx.rx_q, &pkt, 0) == pdTRUE) {
if (pkt.len >= 2 && pkt.data[0] == REG_WIFI_CRED) {
apply_wifi_cred(&pkt.data[1], pkt.len - 1);
} else if (pkt.len >= 2) {
apply_write(pkt.data[0], &pkt.data[1], pkt.len - 1);
}
}
} else if (ev == EVT_PTR) {
ESP_LOGI(TAG, "PTR=0x%02x preload=%02x %02x %02x %02x (ptr_n=%lu req_n=%lu)",
s_last_ptr, s_last_preload[0], s_last_preload[1], s_last_preload[2],
s_last_preload[3], (unsigned long)s_ptr_count, (unsigned long)s_req_count);
}
} while (xQueueReceive(s_ctx.evt_q, &ev, 0) == pdTRUE);
}
}
esp_err_t eye_i2c_init(void)
{
memset(&s_ctx, 0, sizeof(s_ctx));
memset(s_snap, 0, sizeof(s_snap));
s_ctx.wr[REG_FLAGS] = FLAG_MORPH_ROI;
s_ctx.wr[REG_STRIDE] = 4;
s_ctx.wr[REG_CMD] = 0xFF;
put_u16(&s_ctx.wr[REG_MIN_AREA_L], 80);
uint8_t h, s, v, ht, st, vt;
vision_get_hsv(&h, &s, &v);
vision_get_tolerance(&ht, &st, &vt);
s_ctx.wr[REG_H] = h;
s_ctx.wr[REG_S] = s;
s_ctx.wr[REG_V] = v;
s_ctx.wr[REG_H_TOL] = ht;
s_ctx.wr[REG_S_TOL] = st;
s_ctx.wr[REG_V_TOL] = vt;
s_ctx.evt_q = xQueueCreate(16, sizeof(eye_evt_t));
s_ctx.rx_q = xQueueCreate(8, sizeof(eye_rx_pkt_t));
if (!s_ctx.evt_q || !s_ctx.rx_q) {
return ESP_ERR_NO_MEM;
}
ESP_RETURN_ON_ERROR(slave_hw_start(), TAG, "slave_hw_start failed");
BaseType_t ok = xTaskCreate(eye_i2c_task, "eye_i2c", 6144, NULL, 12, NULL);
if (ok != pdPASS) {
i2c_del_slave_device(s_ctx.handle);
return ESP_ERR_NO_MEM;
}
return ESP_OK;
}
Reference in New Issue View Git Blame Copy Permalink