XIAO IA vision 2 support

examples/xiao-ai-vision-sender/xiao-ai-vision-sender.cpp

@@ -0,0 +1,72 @@
+/**************************************************
+ * ESPNowCam video Transmitter.
+ * ----------------------------
+ * 
+ * XIAO FPV ESPNow transmitter uses some extra features of
+ * this board to have some power consumption improvements 
+ *
+ * by @hpsaturn Copyright (C) 2024
+ * This file is part ESPNowCam project:
+ * https://github.com/hpsaturn/ESPNowCam
+**************************************************/
+
+#include <Arduino.h>
+#include <OneButton.h>
+#include <ESPNowCam.h>
+#include <drivers/CamAIVision2.h>
+#include <Utils.h>
+
+CamAIVision2 Camera;  
+ESPNowCam radio;
+OneButton btnB(GPIO_NUM_0, true);
+
+void processFrame() {
+  if (Camera.get()) {
+    uint8_t *out_jpg = NULL;
+    size_t out_jpg_len = 0;
+    frame2jpg(Camera.fb, 12, &out_jpg, &out_jpg_len);
+    radio.sendData(out_jpg, out_jpg_len);
+    printFPS("CAM:");
+    free(out_jpg);
+    Camera.free();
+  }
+}
+
+void shutdown() {
+  Serial.println("shutdown..");
+  esp_sleep_enable_ext0_wakeup(GPIO_NUM_0,0);
+  delay(1000);
+  esp_deep_sleep_start();
+}
+
+void setup() {
+  Serial.begin(115200);
+
+  delay(1000); // only for debugging 
+
+  if(psramFound()){
+    size_t psram_size = esp_spiram_get_size() / 1048576;
+    Serial.printf("PSRAM size: %dMb\r\n", psram_size);
+  }
+
+  // Optional M5Core2 receiver B8:F0:09:C6:0E:CC
+  // const uint8_t macRecv[6] = {0xB8,0xF0,0x09,0xC6,0x0E,0xCC};
+  // radio.setTarget(macRecv);
+  radio.init();
+
+  // You are able to change the Camera config E.g:
+  // Camera.config.frame_size = FRAMESIZE_QQVGA;
+
+  if (!Camera.begin()) {
+    Serial.println("Camera Init Fail");
+    delay(1000);
+  }
+
+  btnB.attachClick([]() { shutdown(); });
+  delay(100);
+}
+
+void loop() {
+  processFrame();
+  btnB.tick();
+}

platformio.ini

@@ -70,6 +70,19 @@ lib_deps =
   ${esp32common.lib_deps}
   mathertel/OneButton@^2.0.3
 
+[env:xiao-ai-vision-sender]
+extends = xiao-common
+build_src_filter = -<*> -<*common*> +<xiao-ai-vision-sender/>
+build_flags =
+  ${env.build_flags}
+  -DAIVISION2=1
+lib_deps = 
+  ${esp32common.lib_deps}
+  hideakitai/ArduinoEigen@^0.3.2
+  bblanchon/ArduinoJson@^7.2.1
+  https://github.com/Seeed-Studio/Seeed_Arduino_SSCMA.git
+  mathertel/OneButton@^2.0.3
+
 [env:xiao-internal-jpg-sender]
 extends = xiao-common
 build_src_filter = -<*> -<*common*> +<xiao-internal-jpg-sender/>

src/drivers/CamAIVision2.cpp

@@ -0,0 +1,170 @@
+#ifdef AIVISION2
+#include "CamAIVision2.h"
+
+PtrBuffer PB;
+StatInfo SI;
+SSCMA AI;
+
+inline uint16_t getMsgType(const char* resp, size_t len) {
+  uint16_t type = MSG_TYPE_UNKNOWN;
+
+  if (strnstr(resp, MSG_REPLY_STR, len) != NULL) {
+    type |= MSG_TYPE_REPLY;
+  } else if (strnstr(resp, MSG_EVENT_STR, len) != NULL) {
+    type |= MSG_TYPE_EVENT;
+  } else if (strnstr(resp, MSG_LOGGI_STR, len) != NULL) {
+    type |= MSG_TYPE_LOGGI;
+  } else {
+    log_w("Unknown message type...");
+  }
+
+  return type;
+}
+
+inline uint16_t getCmdType(const char* resp, size_t len) {
+  uint16_t type = CMD_TYPE_UNKNOWN;
+
+  if (strnstr(resp, CMD_SAMPLE_STR, len) != NULL) {
+    type |= CMD_TYPE_SAMPLE;
+  } else if (strnstr(resp, CMD_INVOKE_STR, len) != NULL) {
+    type |= CMD_TYPE_INVOKE;
+  }
+
+  return type;
+}
+static void proxyCallback(const char* resp, size_t len) {
+  log_i("proxy_callback");
+  static timeval timestamp;
+  TickType_t ticks = xTaskGetTickCount();
+  timestamp.tv_sec = ticks / configTICK_RATE_HZ;
+  timestamp.tv_usec = (ticks % configTICK_RATE_HZ) * 1e6 / configTICK_RATE_HZ;
+
+  if (!len) {
+    log_i("Response is empty...");
+    return;
+  }
+
+  uint16_t type = 0;
+  type |= getMsgType(resp, len);
+  if (type == MSG_TYPE_UNKNOWN) {
+    return;
+  }
+  type |= getCmdType(resp, len);
+
+  char* copy = (char*)malloc(len);
+  if (copy == NULL) {
+    log_e("Failed to allocate resp copy...");
+    return;
+  }
+  memcpy(copy, resp, len);
+
+  size_t limit = PB.limit;
+  PtrBuffer::Slot* p_slot = (PtrBuffer::Slot*)malloc(sizeof(PtrBuffer::Slot));
+  if (p_slot == NULL) {
+    log_e("Failed to allocate slot...");
+    return;
+  }
+
+  p_slot->id = PB.id;
+  p_slot->type = type;
+  p_slot->data = copy;
+  p_slot->size = len;
+  p_slot->timestamp = timestamp;
+
+  const char* slice = strnstr((const char*)p_slot->data, MSG_IMAGE_KEY MSG_QUOTE_STR, p_slot->size);
+  if (slice == NULL) {
+    log_w("No image data found...");
+    vTaskDelay(5 / portTICK_PERIOD_MS);
+    return;
+  }
+  size_t offset = (slice - (const char*)p_slot->data) + strlen(MSG_IMAGE_KEY MSG_QUOTE_STR);
+  const char* data = (const char*)p_slot->data + offset;
+  const char* quote = strnstr(data, MSG_QUOTE_STR, p_slot->size - offset);
+  if (quote == NULL) {
+    log_w("Invalid image data size...");
+    vTaskDelay(5 / portTICK_PERIOD_MS);
+    return;
+  }
+  size_t ilen = quote - data;
+  if (ilen == 0) {
+    log_w("Empty image data...");
+    vTaskDelay(5 / portTICK_PERIOD_MS);
+    return;
+  }
+
+  size_t jpeg_size = 0;
+  char* jpeg_buf = NULL;
+  jpeg_buf = (char*)malloc(JPG_BUFFER_SIZE);
+  if (jpeg_buf == NULL) {
+    log_e("Failed to allocate jpeg buffer...");
+    return;
+  }
+  memset(jpeg_buf, 0, JPG_BUFFER_SIZE);
+  if (mbedtls_base64_decode((unsigned char*)jpeg_buf, JPG_BUFFER_SIZE, &jpeg_size,
+                            (const unsigned char*)data, len) != 0) {
+    log_e("Failed to decode image data...");
+    vTaskDelay(5 / portTICK_PERIOD_MS);
+    return;
+  }
+  log_i("decoded JPG!");
+
+  size_t discarded = 0;
+  xSemaphoreTake(PB.mutex, portMAX_DELAY);
+  while (PB.slots.size() >= limit) {
+    PB.slots.pop_front();
+    discarded += 1;
+  }
+  PB.slots.emplace_back(std::shared_ptr<PtrBuffer::Slot>(p_slot, [](PtrBuffer::Slot* p) {
+    if (p == NULL) {
+      return;
+    }
+    if (p->data != NULL) {
+      free(p->data);
+      p->data = NULL;
+    }
+    free(p);
+  }));
+  xSemaphoreGive(PB.mutex);
+  PB.id += 1;
+
+  if (discarded > 0) {
+    log_i("Discarded %u old responses...", discarded);
+  }
+
+  log_i("Received %u bytes...", len);
+}
+
+bool CamAIVision2::get() {
+  if (!AI.invoke(-1,0,0)) {
+    AI.fetch(proxyCallback);
+    return true;
+  }
+  else return false;
+}
+
+bool CamAIVision2::sendCmd(const char * command, int len) {
+
+  TickType_t ticks = xTaskGetTickCount();
+  char cmd_tag_buf[32] = {0};
+  size_t cmd_tag_size = snprintf(cmd_tag_buf, sizeof(cmd_tag_buf), CMD_TAG_FMT_STR, ticks);
+  // INVOKE=-1,0,0
+  log_i("cmd_buf: [%i] %.*s\r\n",command, len, command);
+  AI.write(CMD_PREFIX, strlen(CMD_PREFIX));
+  AI.write(cmd_tag_buf, cmd_tag_size);
+  AI.write(command, len);
+  AI.write(CMD_SUFFIX, strlen(CMD_SUFFIX));
+  return true;
+}
+
+bool CamAIVision2::begin() {
+  static HardwareSerial atSerial(0);
+  // the esp32 arduino library may have a bug in setRxBufferSize
+  // we cannot set the buffer size larger than uint16_t max value
+  // a workaround is to modify uartBegin() in
+  //     esp32/hardware/esp32/2.0.14/cores/esp32/esp32-hal-uart.c
+  atSerial.setRxBufferSize(COM_BUFFER_SIZE);
+  log_i("starting AI vision Serial interface..");
+  return AI.begin(&atSerial);
+}
+
+#endif

src/drivers/CamAIVision2.h

@@ -0,0 +1,114 @@
+#ifndef CAMAIVISION_H
+#define CAMAIVISION_H
+#ifdef AIVISION2
+
+#include <Seeed_Arduino_SSCMA.h>
+#include <Wire.h>
+
+#include <deque>
+#include <memory>
+
+#include "CameraBase.hpp"
+#include "esp_camera.h"
+#include <mbedtls/base64.h>
+
+#if defined(CONFIG_IDF_TARGET_ESP32S3)
+#define PTR_BUFFER_SIZE 8
+#define COM_BUFFER_SIZE (1024 * 128)
+#define RSP_BUFFER_SIZE (1024 * 196)
+#define JPG_BUFFER_SIZE (1024 * 128)
+#define RST_BUFFER_SIZE (1024 * 64)
+#define QRY_BUFFER_SIZE (1024 * 16)
+#define CMD_BUFFER_SIZE (1024 * 12)
+
+#define BYTE_TRACKER_ENABLED 1
+#else
+#warning "Server may not work properly due to resource constraints..."
+#define PTR_BUFFER_SIZE 3
+#define COM_BUFFER_SIZE (1024 * 32)
+#define RSP_BUFFER_SIZE (1024 * 32)
+#define JPG_BUFFER_SIZE (1024 * 32)
+#define RST_BUFFER_SIZE (1024 * 4)
+#define QRY_BUFFER_SIZE (1024 * 4)
+#define CMD_BUFFER_SIZE (1024 * 4)
+
+#define BYTE_TRACKER_ENABLED 0
+#endif
+
+#define CMD_TAG_FMT_STR "HTTPD%.8X@"
+#define CMD_TAG_SIZE snprintf(NULL, 0, CMD_TAG_FMT_STR, 0)
+
+#define MSG_IMAGE_KEY "\"image\": "
+#define MSG_COMMA_STR ", "
+#define MSG_QUOTE_STR "\""
+#define MSG_REPLY_STR "\"type\": 0"
+#define MSG_EVENT_STR "\"type\": 1"
+#define MSG_LOGGI_STR "\"type\": 2"
+#define MSG_TERMI_STR "\r\n"
+
+enum MsgType : uint16_t {
+  MSG_TYPE_UNKNOWN = 0,
+  MSG_TYPE_REPLY = 0xff & (1 << 1),
+  MSG_TYPE_EVENT = 0xff & (1 << 2),
+  MSG_TYPE_LOGGI = 0xff & (1 << 3),
+};
+
+#define CMD_SAMPLE_STR "SAMPLE"
+#define CMD_INVOKE_STR "INVOKE"
+
+enum CmdType : uint16_t {
+  CMD_TYPE_UNKNOWN = 0,
+  CMD_TYPE_SAMPLE = 0xff00 & (1 << 8),
+  CMD_TYPE_INVOKE = 0xff00 & (2 << 8),
+  CMD_TYPE_SENSOR = 0xff00 & (3 << 8),
+};
+
+struct PtrBuffer {
+  struct Slot {
+    size_t id = 0;
+    uint16_t type = 0;
+    void* data = NULL;
+    size_t size = 0;
+    timeval timestamp;
+  };
+
+  SemaphoreHandle_t mutex;
+  std::deque<std::shared_ptr<Slot>> slots;
+  volatile size_t id = 1;
+  const size_t limit = PTR_BUFFER_SIZE;
+};
+
+struct StatInfo {
+  size_t last_frame_id = 0;
+  timeval last_frame_timestamp;
+  SemaphoreHandle_t mutex;
+};
+
+class CamAIVision2 : public CameraBase {
+ public:
+
+  using CameraBase::free;
+  bool begin();
+  bool get(); 
+  bool sendCmd(const char * command, int len);
+
+  CamAIVision2() {
+    config.xclk_freq_hz = 20000000;
+    config.ledc_timer = LEDC_TIMER_0;
+    config.ledc_channel = LEDC_CHANNEL_0;
+    config.pixel_format = PIXFORMAT_RGB565;
+    config.frame_size = FRAMESIZE_QVGA;
+    config.jpeg_quality = 12;
+    config.fb_count = 2;
+    config.fb_location = CAMERA_FB_IN_PSRAM;
+    config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
+    fb = nullptr;
+    sensor = nullptr;
+  }
+
+ private:
+
+};
+
+#endif
+#endif