M5StackとM5CameraでWiFi, UDPによる動画転送を長時間連続安定動作させる実験

M5StackとM5CameraでWiFi,UDPで長時間安定して動画転送できた M5Stack

M5StackとM5Cameraのブラッシュアップした長時間WiFi UDP画像転送スケッチ

では、前回記事からブラッシュアップして、長時間稼働でも安定して動作する WiFi UDP 画像転送スケッチを紹介します。

事前に前回記事を参照して、Arduino core for the ESP32のインストールや、自作ライブラリのインストール、およびフォントファイルのセットアップを済ませておいてください。

M5Camera側のスケッチ(ソースコード)

M5Camera側のスケッチです。
1000行を超えています。

【ソースコード】 (※無保証 ※PCの場合、ダブルクリックすればコード全体を選択できます)

/* This was modified by mgo-tec with the following esp32-camera library files:
 * The MIT License (MIT)
 * License URL: https://opensource.org/licenses/mit-license.php
 * Copyright (c) 2019 Mgo-tec. All rights reserved.
 * 
 * Use Arduino core for the ESP32 stable v1.0.4
 * 
 * esp32-camera library ( Copyright 2015-2016 Espressif Systems (Shanghai) PTE LTD)
 *  Licensed under the Apache License, Version 2.0 (the "License").
 *  URL:https://github.com/espressif/esp32-camera
 * 
 * sccb.c file is part of the OpenMV project. 
 *  Copyright (c) 2013/2014 Ibrahim Abdelkader.
 *  This work is licensed under the MIT license. 
 */

#include <rom/lldesc.h>
#include <driver/rtc_io.h>
#include <driver/i2s.h>
#include <driver/i2c.h>
#include <esp_err.h>
#include <driver/ledc.h>
#include <WiFi.h>
#include <WiFiUdp.h>

const char * to_udp_address = "192.168.4.2"; //M5StackのIPアドレス
const int to_server_udp_port = 55556; //送信相手のポート番号
const int my_server_udp_port = 55555; //開放する自ポート

const char* ssid = "xxxxxxxxx"; //9文字以上。ご自分好きなSSIDに書き換えてください
const char* password = "xxxxxxxxx"; //9文字以上。ご自分好きなパスワードに書き換えてください

boolean connected = false;
WiFiUDP udp;

const uint8_t ov2640_i2c_addrs = 0x30;

const int8_t cam_pin_PWDN = -1; //power down is not used
const int8_t cam_pin_RESET = 15; //software reset will be performed
const int8_t cam_pin_XVCLK = 27;
const int8_t cam_pin_SIOD = 22;
const int8_t cam_pin_SIOC = 23;
const int8_t cam_pin_D7 = 19;
const int8_t cam_pin_D6 = 36;
const int8_t cam_pin_D5 = 18;
const int8_t cam_pin_D4 = 39;
const int8_t cam_pin_D3 = 5;
const int8_t cam_pin_D2 = 34;
const int8_t cam_pin_D1 = 35;
const int8_t cam_pin_D0 = 32;
const int8_t cam_pin_VSYNC = 25;
const int8_t cam_pin_HREF = 26;
const int8_t cam_pin_PCLK = 21;

uint8_t camera_pid = 0;
const uint16_t sensor_resolution_h = 400, sensor_resolution_v = 296; //CIF mode
const uint16_t disp_width_pix = 200, disp_height_pix = 148;
const uint16_t out_camera_w = sensor_resolution_h/2;
const uint16_t out_camera_h = sensor_resolution_v/2;
const uint16_t max_w_pix_buf = out_camera_w * 2;
const uint16_t max_w_disp_buf = disp_width_pix * 2;
uint8_t send_udp_buf[disp_height_pix][max_w_pix_buf + 1] = {};
uint8_t send_temp_buf[2] = {0xff, 0};
boolean goDrawing_disp = false;
const uint8_t fix_div = 3;

const uint8_t ledc_duty = 1; //1bit value:1 = duty 50%
const double ledc_base_freq = 20000000.0;

const uint32_t sccb_freq = 200000; // I2C master frequency
const uint8_t i2c_write_bit = 0; // I2C master write
const uint8_t i2c_read_bit = 1;  // I2C master read
const uint8_t ack_check_en = 1; // I2C master will check ack from slave
//const uint8_t ack_check_dis = 0;
//const uint8_t ack_val = 0; // I2C ack value
const uint8_t nack_val = 1; // I2C nack value
const int sccb_i2c_port = 1;
uint8_t scan_i2c_addrs = 0;

typedef enum {
    SM_0A0B_0B0C = 0,
    SM_0A0B_0C0D = 1,
    SM_0A00_0B00 = 3,
} i2s_sampling_mode_t;

uint16_t dma_buf_size = out_camera_w * 2 * 4;
lldesc_t *dma_desc;

intr_handle_t i2s_intr_handle;
uint8_t y_pix_count = 0;

esp_err_t err = ESP_OK;
TaskHandle_t taskDMA_handl, taskUDP_handl;

static inline void resetI2Sconf();
static void IRAM_ATTR i2s_isr(void* arg);

uint8_t send_ok = 0;
uint8_t receive_auto_white_balance = 0;
uint8_t receive_auto_exposure_control = 0;
uint8_t receive_exposure[2] = {};
uint8_t receive_reset = 0;
//---------------------------
uint32_t calc_heap_time = 0;
uint32_t camera_cooling_time = 0;
boolean isStart_timer_camera = false;
boolean isCooling_camera = false;
uint32_t time_send_ping = 0;
//---------------------------
uint32_t send_fault_time = 0;
boolean isFault_send = false;
//---------------------------
static void IRAM_ATTR vsync_intr_disable();
static void IRAM_ATTR vsync_isr(void* arg);
//---------------------------
boolean isI2S_isr = false;

//*********************************************
void setup() {
  Serial.begin(115200);
  Serial.println();
  delay(1000);

  send_fault_time = millis();

  if (!xTaskCreatePinnedToCore(&taskUDP, "taskUDP", 8192, NULL, 24, &taskDMA_handl, 0)) {
    Serial.println("Failed to create UDP task");
    err = ESP_ERR_NO_MEM;
  }

  esp_err_t err = initCamera();
  if (err != ESP_OK) {
    Serial.printf("Camera init failed with error 0x%x", err);
    return;
  }
  Serial.printf("Camera:%d x %d pix, Display:%d y %d pix\r\n", out_camera_w, out_camera_h, disp_width_pix, disp_height_pix);

  initTaskDMA();
}

void loop() {
  receiveDMA();
}

//****************************************
void initTaskDMA(){
  Serial.println("taskDMA Start!!");
  initI2S();
  err = initDMAdesc();
  if (err != ESP_OK) {
    Serial.println("Failed to initialize I2S and DMA");
  }

  vsync_intr_disable();
  gpio_install_isr_service(ESP_INTR_FLAG_LEVEL1 | ESP_INTR_FLAG_IRAM);
  err = gpio_isr_handler_add((gpio_num_t)cam_pin_VSYNC, &vsync_isr, NULL);
  if (err != ESP_OK) {
      Serial.printf("vsync_isr_handler_add failed (%x)\r\n", err);
  }

  I2S0.rx_eof_num = dma_buf_size;
  I2S0.in_link.addr = (uint32_t)dma_desc;
  I2S0.in_link.start = 1;
  I2S0.int_clr.val = I2S0.int_raw.val;
  I2S0.int_ena.val = 0;
  I2S0.int_ena.in_done = 1;

  esp_intr_enable(i2s_intr_handle);
  I2S0.conf.rx_start = 1;
  vsync_intr_enable();
}

void receiveDMA(){
  if(isI2S_isr){
    lldesc_t *d = dma_desc;
    int i, pix_cnt = 0;
    for(i = 0; i < disp_height_pix; i++){
      send_udp_buf[i][0] = i;
    }
    pix_cnt = 1;

    for (i = 2; i < dma_buf_size; i += 4) {
      send_udp_buf[y_pix_count][pix_cnt++] = (uint8_t) * (d->buf + i);
    }

    if(y_pix_count >= (disp_height_pix - 1)){
      y_pix_count = 0;
      goDrawing_disp = true;
      isI2S_isr = false;
      return;
    }
    y_pix_count = (y_pix_count + 1) % disp_height_pix;
    isI2S_isr = false;
  }
}
//****************************************
void taskUDP(void *pvParameters){
  connectToWiFi(ssid, password);
  if(connected){
    Serial.println("connected!");
  }
  while(true){
    if(millis() - calc_heap_time > 180000){
      Serial.printf("Free Heap Size = %d\r\n", esp_get_free_heap_size());
      Serial.printf("CPU temp = %fl\r\n", temperatureRead());
      calc_heap_time = millis();
    }

    sendUDP();
    receiveUDP();
    if(receive_reset == 1){
      ESP.restart();
    }
    delay(1); //ウォッチドッグタイマ動作に必ず必要
  }
}
//****************************************
static void IRAM_ATTR vsync_isr(void* arg){
  GPIO.status1_w1tc.val = GPIO.status1.val;
  GPIO.status_w1tc = GPIO.status;

  esp_intr_disable(i2s_intr_handle);
  I2S0.conf.rx_start = 0;
  I2S0.in_link.start = 0;
  I2S0.int_clr.val = I2S0.int_raw.val;
  resetI2Sconf();
  I2S0.rx_eof_num = dma_buf_size;
  I2S0.in_link.addr = (uint32_t)dma_desc;
  I2S0.in_link.start = 1;
  I2S0.conf.rx_start = 1;
  I2S0.int_clr.val = I2S0.int_raw.val;
  I2S0.int_ena.val = 0;
  I2S0.int_ena.in_done = 1;
  esp_intr_enable(i2s_intr_handle);
}

static void IRAM_ATTR vsync_intr_disable(){
  gpio_set_intr_type((gpio_num_t)cam_pin_VSYNC, GPIO_INTR_DISABLE);
}

static void vsync_intr_enable(){
  gpio_set_intr_type((gpio_num_t)cam_pin_VSYNC, GPIO_INTR_NEGEDGE);
}

static void IRAM_ATTR i2s_isr(void* arg){
  I2S0.int_clr.val = I2S0.int_raw.val;
  isI2S_isr = true;
}
//**********************************
void receiveUDP(){
  int receive_packetSize = udp.parsePacket();
  if(receive_packetSize > 0){
    send_ok = udp.read();
    receive_auto_white_balance = udp.read();
    receive_auto_exposure_control = udp.read();
    udp.read(&receive_exposure[0], 2);
    receive_reset = udp.read();
    uint16_t exposure = (receive_exposure[0] << 8) | receive_exposure[1];
    Serial.printf("Receive UDP send_ok=%d, AWB=%d, AEC=%d\r\n", send_ok, receive_auto_white_balance, receive_auto_exposure_control);
    Serial.printf("Receive Exposure=%d\r\n", exposure);
    Serial.printf("Receive Reset=%d\r\n", receive_reset);
    changeAutoWhiteBalance(receive_auto_white_balance);
    changeAutoExposureControl(receive_auto_exposure_control, receive_exposure);
    if(send_ok) {
      if(!isStart_timer_camera){
        isStart_timer_camera = true;
        camera_cooling_time = millis();
      }
    }else{
      isStart_timer_camera = false;
    }
  }
  udp.flush();
}

void sendUDP(){
  if(send_ok){
    if(goDrawing_disp){
      esp_intr_disable(i2s_intr_handle);
      if(connected){
        send_temp_buf[1] = (uint8_t)floor(temperatureRead());
        udp.beginPacket(to_udp_address, to_server_udp_port);
        udp.write(send_temp_buf, 2);
        udp.endPacket();
        udp.beginPacket(to_udp_address, to_server_udp_port);
        for(int i = 0; i < disp_height_pix; i++){
          if(i != 0 && (i % fix_div) == 0){
            udp.endPacket();

            if(errno ==12) {
              delay(1);
              Serial.println("error");
              break;
            }else{
              delayMicroseconds(200);
            }

            udp.beginPacket(to_udp_address, to_server_udp_port);
          }
          udp.write((const uint8_t *)&send_udp_buf[i][0], max_w_disp_buf + 1);          //Serial.printf("max_w_disp_buf=%d\r\n", max_w_disp_buf);
        }
        if(errno != 12) {
          udp.endPacket();
          delay(1);
        }else{
          udp.endPacket();
        }
        if(millis() - time_send_ping > 5000){
          time_send_ping = millis();
        }
      }else{
        Serial.print('?');
      }
      goDrawing_disp = false;
      send_fault_time = millis();
    }else{
      if(!isFault_send && (millis() - send_fault_time > 3000)){
        Serial.printf("2CPU temp = %fl\r\n", temperatureRead());
        isFault_send = true;
      }
    }
  }
}
//*********************************************
void connectToWiFi(const char * ssid, const char * pwd){
  Serial.println("Connecting to WiFi network: " + String(ssid));
  WiFi.disconnect(true, true); //WiFi config設定リセット
  WiFi.softAP(ssid, password);
  IPAddress myIP = WiFi.softAPIP();
  Serial.println("WiFi connected!");
  Serial.print("My IP address: ");
  Serial.println(myIP);
  udp.begin(myIP, my_server_udp_port);
  delay(1000);
  connected = true;
}
//*********************************************
int setFramesize(uint16_t out_width, uint16_t out_height){
  int ret = 0;
  ret = writeSCCB(0xFF, 0x00);//bank dsp
  if (!ret) {
    ret = writeSCCB(0x05, 0x00); //R_BYPASS:0x00 DSP use. 
    if(ret) return ret;
  }
  delay(5);

  uint8_t pclk_div2 = 0x01; //PCLK clock divider 2
  uint16_t window_h_start = 137;
  uint16_t window_h_end = window_h_start + sensor_resolution_h;
  uint16_t window_v_start = 2;
  uint16_t window_v_end = window_v_start + sensor_resolution_v;
  uint8_t win_h_st_bit10_3 = (uint8_t)((window_h_start >> 3) & 0x00ff);
  uint8_t win_h_end_bit10_3 = (uint8_t)((window_h_end >> 3) & 0x00ff);
  uint8_t win_v_st_bit9_2 = (uint8_t)((window_v_start >> 2) & 0x00ff);
  uint8_t win_v_end_bit9_2 = (uint8_t)((window_v_end >> 2) & 0x00ff);
  uint8_t win_h_st_bit2_0 = (uint8_t)(window_h_start & 0x0007);
  uint8_t win_h_end_bit2_0 = (uint8_t)(window_h_end & 0x0007);
  uint8_t win_v_st_bit1_0 = (uint8_t)(window_v_start & 0x0003);
  uint8_t win_v_end_bit1_0 = (uint8_t)(window_v_end & 0x0003);

  writeSCCB(0xFF, 0x01); //BANK:sensor
  delay(5);
  writeSCCB(0x12, 0b00100000); //COM7 [6:4]CIF mode
  writeSCCB(0x17, win_h_st_bit10_3); //HREFST(default:0x11)
  writeSCCB(0x18, win_h_end_bit10_3); //HREFEND(SVGA,CIF default:0x43)
  writeSCCB(0x19, win_v_st_bit9_2); //VSTRT
  writeSCCB(0x1A, win_v_end_bit9_2); //VEND
  writeSCCB(0x32, 0x00 | (pclk_div2 << 7) | (win_h_end_bit2_0 << 3) | win_h_st_bit2_0); //REG32,[7:6]10:PCLK frequency devide by 2, [5:0]0x09:CIF
  writeSCCB(0x03, 0x00 | (win_v_end_bit1_0 << 2) | win_v_st_bit1_0); //COM1 0x0A:CIF?

  writeSCCB(0x11, pclk_div2); //CLKRC 1/2 clock divider
  delay(5);

  uint16_t HSIZE = sensor_resolution_h; //Image Horizontal Size
  uint16_t VSIZE = sensor_resolution_v; //Image Vertical Size
  uint16_t H_SIZE = HSIZE / 4;
  uint16_t V_SIZE = VSIZE / 4;

  uint8_t H_SIZE_bit7_0 = (uint8_t)(H_SIZE & 0x00ff);
  uint8_t V_SIZE_bit7_0 = (uint8_t)(V_SIZE & 0x00ff);
  uint8_t H_SIZE_bit8 = (uint8_t)((H_SIZE >> 8) & 0x0001);
  uint8_t H_SIZE_bit9 = (uint8_t)((H_SIZE >> 9) & 0x0001);
  uint8_t V_SIZE_bit8 = (uint8_t)((V_SIZE >> 8) & 0x0001);

  uint8_t zoom_speed = 0;
  uint16_t OUTW = (uint16_t)floor((double)out_width / 4.0);
  uint16_t OUTH = (uint16_t)floor((double)out_height / 4.0);
  uint8_t OUTW_bit7_0 = (uint8_t)(OUTW & 0x00ff);
  uint8_t OUTH_bit7_0 = (uint8_t)(OUTH & 0x00ff);
  uint8_t OUTW_bit9_8 = (uint8_t)((OUTW >> 8) & 0x0003);
  uint8_t OUTH_bit8 = (uint8_t)((OUTH >> 8) & 0x0001);

  uint8_t HSIZE_bit10_3 = (uint8_t)((HSIZE >> 3) & 0x00ff);
  uint8_t VSIZE_bit10_3 = (uint8_t)((VSIZE >> 3) & 0x00ff);
  Serial.printf("HSIZE=%d, VSIZE=%d, H_SIZE=%d, V_SIZE=%d, OUTW=%d, OUTH=%d\r\n", HSIZE, VSIZE, H_SIZE, V_SIZE, OUTW, OUTH);

  writeSCCB(0xFF, 0x00); //BANK:DSP
  delay(5);
  writeSCCB(0xE0, 0b00000100); //RESET bit[2]:DVP ※DVPはパラレルデジタルフォーマット
  delay(5);
  writeSCCB(0xC0, HSIZE_bit10_3); //HSIZE8は11bitのうちの上位8bit。これはh_pixel 400
  writeSCCB(0xC1, VSIZE_bit10_3); //VSIZE8は11bitのうちの上位8bit。これはv_pixel 296
  writeSCCB(0x8C, 0x00); //SIZEL 使い方は不明

  writeSCCB(0x86, 0b00111101); //CTRL2
  writeSCCB(0x50, 0b10000000); //CTRLl LP_DP EN (※CTRL1と混同し易いので注意)

  writeSCCB(0x51, H_SIZE_bit7_0); //H_SIZE
  writeSCCB(0x52, V_SIZE_bit7_0); //V_SIZE
  writeSCCB(0x53, 0x00); //XOFFL
  writeSCCB(0x54, 0x00); //YOFFL
  writeSCCB(0x55, 0x00 | (V_SIZE_bit8 << 7) | (H_SIZE_bit8 << 3)); //VHYX
  writeSCCB(0x57, (H_SIZE_bit9 << 7) | 0x00); //TEST
  writeSCCB(0x5A, OUTW_bit7_0); //ZMOW[7:0] (real/4)
  writeSCCB(0x5B, OUTH_bit7_0); //ZMOH[7:0] (real/4)
  writeSCCB(0x5C, 0x00 | (zoom_speed << 4) | (OUTH_bit8 << 2) | (OUTW_bit9_8)); //ZMHH

  writeSCCB(0xD3, 0b10000010); //R_DVP_SP

  writeSCCB(0xE0, 0x00); //RESET
  writeSCCB(0x05, 0x00); //R_BYPASS:0x00 DSP use.

  delay(10); //解像度を変更する場合、これは必要らしい
  return ret;
}

void initI2S(){
  gpio_num_t pins[] = {
    (gpio_num_t)cam_pin_D7,
    (gpio_num_t)cam_pin_D6,
    (gpio_num_t)cam_pin_D5,
    (gpio_num_t)cam_pin_D4,
    (gpio_num_t)cam_pin_D3,
    (gpio_num_t)cam_pin_D2,
    (gpio_num_t)cam_pin_D1,
    (gpio_num_t)cam_pin_D0,
    (gpio_num_t)cam_pin_VSYNC,
    (gpio_num_t)cam_pin_HREF,
    (gpio_num_t)cam_pin_PCLK
  };
  gpio_config_t conf;
  conf.mode = GPIO_MODE_INPUT;
  conf.pull_up_en = GPIO_PULLUP_ENABLE;
  conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
  conf.intr_type = GPIO_INTR_DISABLE;

  for (int i = 0; i < sizeof(pins) / sizeof(gpio_num_t); ++i) {
    if (rtc_gpio_is_valid_gpio(pins[i])) {
      rtc_gpio_deinit(pins[i]);
    }
    conf.pin_bit_mask = 1LL << pins[i];
    gpio_config(&conf);
  }

  gpio_matrix_in(cam_pin_D0, I2S0I_DATA_IN0_IDX, false);
  gpio_matrix_in(cam_pin_D1, I2S0I_DATA_IN1_IDX, false);
  gpio_matrix_in(cam_pin_D2, I2S0I_DATA_IN2_IDX, false);
  gpio_matrix_in(cam_pin_D3, I2S0I_DATA_IN3_IDX, false);
  gpio_matrix_in(cam_pin_D4, I2S0I_DATA_IN4_IDX, false);
  gpio_matrix_in(cam_pin_D5, I2S0I_DATA_IN5_IDX, false);
  gpio_matrix_in(cam_pin_D6, I2S0I_DATA_IN6_IDX, false);
  gpio_matrix_in(cam_pin_D7, I2S0I_DATA_IN7_IDX, false);
  gpio_matrix_in(cam_pin_VSYNC, I2S0I_V_SYNC_IDX, false);
  gpio_matrix_in(0x38, I2S0I_H_SYNC_IDX, false);
  gpio_matrix_in(cam_pin_HREF, I2S0I_H_ENABLE_IDX, false);
  gpio_matrix_in(cam_pin_PCLK, I2S0I_WS_IN_IDX, false);

  // Enable and configure I2S peripheral
  periph_module_enable(PERIPH_I2S0_MODULE);
  // Toggle some reset bits in LC_CONF register
  // Toggle some reset bits in CONF register
  resetI2Sconf();
  // Enable slave mode (sampling clock is external)
  I2S0.conf.rx_slave_mod = 1;
  // Enable parallel mode
  I2S0.conf2.lcd_en = 1;
  // Use HSYNC/VSYNC/HREF to control sampling
  I2S0.conf2.camera_en = 1;
  // Configure clock divider
  I2S0.clkm_conf.clkm_div_a = 1;
  I2S0.clkm_conf.clkm_div_b = 0;
  I2S0.clkm_conf.clkm_div_num = 2;
  // FIFO will sink data to DMA
  I2S0.fifo_conf.dscr_en = 1;
  // FIFO configuration
  I2S0.fifo_conf.rx_fifo_mod = SM_0A00_0B00; //fifo mode = 3
  I2S0.fifo_conf.rx_fifo_mod_force_en = 1;
  I2S0.conf_chan.rx_chan_mod = 1;
  // Clear flags which are used in I2S serial mode
  I2S0.sample_rate_conf.rx_bits_mod = 0;
  I2S0.conf.rx_right_first = 0;
  I2S0.conf.rx_msb_right = 0;
  I2S0.conf.rx_msb_shift = 0;
  I2S0.conf.rx_mono = 0;
  I2S0.conf.rx_short_sync = 0;
  I2S0.timing.val = 0;
  I2S0.timing.rx_dsync_sw = 1;

  // Allocate I2S interrupt, keep it disabled
  esp_intr_alloc(ETS_I2S0_INTR_SOURCE,
                 ESP_INTR_FLAG_INTRDISABLED | ESP_INTR_FLAG_LEVEL1 | ESP_INTR_FLAG_IRAM,
                 &i2s_isr, NULL, &i2s_intr_handle);
}

static inline void resetI2Sconf(){
  const uint32_t lc_conf_reset_flags = I2S_IN_RST_M | I2S_AHBM_RST_M
                                       | I2S_AHBM_FIFO_RST_M;
  I2S0.lc_conf.val |= lc_conf_reset_flags;
  I2S0.lc_conf.val &= ~lc_conf_reset_flags;

  const uint32_t conf_reset_flags = I2S_RX_RESET_M | I2S_RX_FIFO_RESET_M
                                    | I2S_TX_RESET_M | I2S_TX_FIFO_RESET_M;
  I2S0.conf.val |= conf_reset_flags;
  I2S0.conf.val &= ~conf_reset_flags;
  while (I2S0.state.rx_fifo_reset_back) {
    ;
  }
}

esp_err_t initDMAdesc(){
  Serial.println("initDMAdesc");
  assert(out_camera_w % 4 == 0); //幅pixelが4で割り切れるかどうかを事前判定

  Serial.printf("DMA buffer size: %d\r\n", dma_buf_size);

  dma_desc = (lldesc_t*) malloc(sizeof(lldesc_t));
  if (dma_desc == NULL) {
    return ESP_ERR_NO_MEM;
  }

  Serial.printf("Allocating DMA buffer size=%d\r\n", dma_buf_size);

  lldesc_t* pd = dma_desc;
  pd->length = dma_buf_size;
  pd->size = pd->length;
  pd->owner = 1;
  pd->sosf = 1;
  pd->buf = (uint8_t*) malloc(dma_buf_size);
  if (pd->buf == NULL) {
    Serial.println("pd->buf NULL");
    return ESP_ERR_NO_MEM;
  }
  pd->offset = 0;
  pd->empty = 0;
  pd->eof = 1;
  pd->qe.stqe_next = dma_desc;

  return ESP_OK;
}

int reset(){
  int ret = 0;
  ret = writeSCCB(0xFF, 0x01);//bank sensor
  if (!ret) {
    Serial.println("OV2640 System Resister Reset (COM7)");
    ret = writeSCCB(0x12, 0b10100000); //COM7:SRST System Reset & CIF mode
    if(ret) return ret;
  }
  delay(10);
  writeRegisterCIF();
  delay(100);
  return ret;
}

esp_err_t probeCamera(){
  enableOutClockToCamera();

  sccbInit(cam_pin_SIOD, cam_pin_SIOC);

  Serial.println("Resetting camera");
  gpio_config_t conf = { 0 };
  conf.pin_bit_mask = 1LL << cam_pin_RESET;
  conf.mode = GPIO_MODE_OUTPUT;
  gpio_config(&conf);

  gpio_set_level((gpio_num_t)cam_pin_RESET, 0);
  delay(10);
  gpio_set_level((gpio_num_t)cam_pin_RESET, 1);
  delay(10);

  Serial.println("Searching for camera address");
  delay(10);
  uint8_t slv_addr = sccbProbe();

  if (slv_addr == ov2640_i2c_addrs) {
    Serial.println("Detected camera OV2640");
  }else{
    disableOutClockToCamera();
    return ESP_FAIL;
  }

  uint8_t reg_PIDH = 0x0A; //Product ID Number MSB
  uint8_t reg_PIDL = 0x0B; //Product ID Number LSB
  uint8_t reg_MIDH = 0x1C; //Manufacture ID Byte MSB
  uint8_t reg_MIDL = 0x1D; //Manufacture ID Byte LSB

  writeSCCB(0xFF, 0x01);//bank sensor

  camera_pid = readSCCB(reg_PIDH);
  uint8_t ver = readSCCB(reg_PIDL);
  uint8_t midh = readSCCB(reg_MIDH);
  uint8_t midl = readSCCB(reg_MIDL);
  delay(10);

  Serial.printf("Product ID=0x%02X\r\n", camera_pid);
  Serial.printf("Product Ver=0x%02X\r\n", ver);
  Serial.printf("Manufacture ID High=0x%02X\r\n", midh);
  Serial.printf("Manufacture ID Low=0x%02X\r\n", midl);

  if(camera_pid == 0x26){
      Serial.println("camera_model = CAMERA_OV2640");
  }else{
      disableOutClockToCamera();
      Serial.println("Detected camera not supported.");
      return ESP_FAIL;
  }

  reset();
  return ESP_OK;
}

void enableOutClockToCamera(){
  ledcSetup(LEDC_CHANNEL_0, ledc_base_freq, LEDC_TIMER_1_BIT); //40MHzにする場合、bit=1
  ledcAttachPin(cam_pin_XVCLK, LEDC_CHANNEL_0);
  ledcWrite(LEDC_CHANNEL_0, ledc_duty); //duty:50%
}

void disableOutClockToCamera(){
  periph_module_disable(PERIPH_LEDC_MODULE);
}

esp_err_t initCamera()
{
  esp_err_t err = probeCamera();
  if (err != ESP_OK) {
    Serial.printf("Camera probe failed with error 0x%x", err);
    goto fail;
  }

  if (setFramesize(out_camera_w, out_camera_h) != 0) {
    Serial.println("Failed to set frame size");
    err = ESP_FAIL;
    goto fail;
  }

  writeRegisterRGB565();

  writeSCCB(0xFF, 0x01);//bank sensor
  Serial.printf("Now Gain ceiling=%d\r\n", readSCCB(0x14));

  if (camera_pid == 0x26) {
    uint8_t s_value1, s_value2;
    //set AGC(Auto Gain Ceiling)
    writeSCCB(0xFF, 0x01); //Bank Sensor
    s_value1 = readSCCB(0x14); //COM9
    delay(5);
    writeSCCB(0xFF, 0x01); //readの後のwriteは必ずbank設定必要
    writeSCCB(0x14, s_value1 | 0b01000000); //COM9 AGC(Auto Gain Ceiling) 8x
    delay(5);
    //set BPC(Black Point Corrections?),WPC(White Point Corrections?), LENC(Lens Corrections?)
    writeSCCB(0xFF, 0x00); //Bank DSP
    s_value1 = readSCCB(0x87); //CTRL3
    s_value2 = readSCCB(0xC3); //CTRL1
    delay(5);
    writeSCCB(0xFF, 0x00); //Bank DSP
    writeSCCB(0x87, 0b10010000 | s_value1); //CTRL3 [7]BPC:1, [6]WPC:1
    writeSCCB(0xC3, 0b00000000 | s_value2); //CTRL1 [3]AWB, [1]LENC
    writeSCCB(0xC2, 0b10001100); //CTRL0 [7]AEC_EN
    delay(10);

    writeSCCB(0xFF, 0x01);
    Serial.printf("After set Gain ceiling=%d\r\n", readSCCB(0x14));
    writeSCCB(0xFF, 0x00); //Bank DSP
    Serial.print("After set CTRL3= ");
    Serial.println(readSCCB(0x87), BIN);
  }

  return ESP_OK;

fail:
  disableOutClockToCamera();
  Serial.println("ERROR camera init");
  return err;
}

void sccbInit(int pin_sda, int pin_scl){
  i2c_config_t conf;
  conf.mode = I2C_MODE_MASTER;
  conf.sda_io_num = (gpio_num_t)pin_sda;
  conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
  conf.scl_io_num = (gpio_num_t)pin_scl;
  conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
  conf.master.clk_speed = sccb_freq;

  i2c_param_config((i2c_port_t)sccb_i2c_port, &conf);
  i2c_driver_install((i2c_port_t)sccb_i2c_port, conf.mode, 0, 0, 0);
}

uint8_t sccbProbe(){
  uint8_t slave_addr = 0x0;
  while (slave_addr < 0x7f) {
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, ( slave_addr << 1 ) | i2c_write_bit, ack_check_en);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin((i2c_port_t)sccb_i2c_port, cmd, 1000 / portTICK_RATE_MS);
    i2c_cmd_link_delete(cmd);
    if ( ret == ESP_OK) {
      scan_i2c_addrs = slave_addr;
      Serial.printf("Detected Slave Address=%02X\r\n", scan_i2c_addrs);
      return scan_i2c_addrs;
    }
    slave_addr++;
  }
  return scan_i2c_addrs;
}

uint8_t readSCCB(uint8_t reg){
  uint8_t data = 0;
  esp_err_t ret = ESP_FAIL;
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, ( scan_i2c_addrs << 1 ) | i2c_write_bit, ack_check_en);
  i2c_master_write_byte(cmd, reg, ack_check_en);
  i2c_master_stop(cmd);
  ret = i2c_master_cmd_begin((i2c_port_t)sccb_i2c_port, cmd, 1000 / portTICK_RATE_MS);
  i2c_cmd_link_delete(cmd);

  if (ret != ESP_OK) {
    Serial.println(ret);
    Serial.println("fail");
    return -1;
  }
  cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, ( scan_i2c_addrs << 1 ) | i2c_read_bit, ack_check_en);
  i2c_master_read_byte(cmd, &data, (i2c_ack_type_t)nack_val);
  i2c_master_stop(cmd);
  ret = i2c_master_cmd_begin((i2c_port_t)sccb_i2c_port, cmd, 1000 / portTICK_RATE_MS);
  i2c_cmd_link_delete(cmd);
  if (ret != ESP_OK) {
    Serial.printf("readSCCB Failed addr:0x%02x, reg:0x%02x, data:0x%02x, ret:%d", scan_i2c_addrs, reg, data, ret);
  }
  return data;
}

uint8_t writeSCCB(uint8_t reg, uint8_t data){
  esp_err_t ret = ESP_FAIL;
  i2c_cmd_handle_t cmd = i2c_cmd_link_create();
  i2c_master_start(cmd);
  i2c_master_write_byte(cmd, ( scan_i2c_addrs << 1 ) | i2c_write_bit, ack_check_en);
  i2c_master_write_byte(cmd, reg, ack_check_en);
  i2c_master_write_byte(cmd, data, ack_check_en);
  i2c_master_stop(cmd);
  ret = i2c_master_cmd_begin((i2c_port_t)sccb_i2c_port, cmd, 1000 / portTICK_RATE_MS);
  i2c_cmd_link_delete(cmd);
  //Serial.printf("writeSCCB ret=%d\r\n", ret);
  if (ret != ESP_OK) {
    Serial.printf("writeSCCB Failed addr:0x%02x, reg:0x%02x, data:0x%02x, ret:%d\r\n", scan_i2c_addrs, reg, data, ret);
  }
  return ret == ESP_OK ? 0 : -1;
}

void writeRegisterCIF(){
  Serial.println("writeRegisterCIF");
  writeSCCB(0xff, 0x00);
  delay(5);
  writeSCCB(0x2c, 0xff);
  writeSCCB(0x2e, 0xdf);
  delay(5);
  writeSCCB(0xff, 0x01);
  delay(5);
  writeSCCB(0x3c, 0x32);
  //
  writeSCCB(0x11, 0x00); //CLKRC: none clock divide
  writeSCCB(0x09, 0x02);
  writeSCCB(0x04, 0x28);
  writeSCCB(0x13, 0b11100111); //COM8:default C7, [2]AGC 0:manual 1:auto [0]AEC(自動露出) 0:manual 1:auto
  writeSCCB(0x14, 0x48);
  writeSCCB(0x2c, 0x0c);
  writeSCCB(0x33, 0x78);
  writeSCCB(0x3a, 0x33);
  writeSCCB(0x3b, 0xfB);
  //
  writeSCCB(0x3e, 0x00);
  writeSCCB(0x43, 0x11);
  writeSCCB(0x16, 0x10);
  //
  writeSCCB(0x39, 0x92);
  //
  writeSCCB(0x35, 0xda);
  writeSCCB(0x22, 0x1a);
  writeSCCB(0x37, 0xc3);
  writeSCCB(0x23, 0x00);
  writeSCCB(0x34, 0xc0); //ARCOM2
  writeSCCB(0x36, 0x1a);
  writeSCCB(0x06, 0x88);
  writeSCCB(0x07, 0xc0);
  writeSCCB(0x0d, 0x87);
  writeSCCB(0x0e, 0x41);
  writeSCCB(0x4c, 0x00);
  writeSCCB(0x48, 0x00);
  writeSCCB(0x5B, 0x00);
  writeSCCB(0x42, 0x03);
  //
  writeSCCB(0x4a, 0x81);
  writeSCCB(0x21, 0x99);
  //
  writeSCCB(0x24, 0x40);
  writeSCCB(0x25, 0x38);
  writeSCCB(0x26, 0b10000010); //VV Fast Mode Large Step Threshold. [7:4]High threshold, [3:0]Low threshold
  writeSCCB(0x5c, 0x00);
  writeSCCB(0x63, 0x00);
  writeSCCB(0x46, 0x22);
  writeSCCB(0x0c, 0x3c);
  //
  writeSCCB(0x61, 0x70);
  writeSCCB(0x62, 0x80);
  writeSCCB(0x7c, 0x05);
  //
  writeSCCB(0x20, 0x80);
  writeSCCB(0x28, 0x30);
  writeSCCB(0x6c, 0x00);
  writeSCCB(0x6d, 0x80);
  writeSCCB(0x6e, 0x00);
  writeSCCB(0x70, 0x02);
  writeSCCB(0x71, 0x94);
  writeSCCB(0x73, 0xc1);
  //
  writeSCCB(0x12, 0x40); //COM7
  writeSCCB(0x17, 0x11);
  writeSCCB(0x18, 0x43);
  writeSCCB(0x19, 0x00);
  writeSCCB(0x1a, 0x4b);
  writeSCCB(0x32, 0x09);
  writeSCCB(0x37, 0xc0);
  writeSCCB(0x4f, 0xca); //BD50
  writeSCCB(0x50, 0xa8); //BD60
  writeSCCB(0x5a, 0x23);
  writeSCCB(0x6d, 0x00);
  writeSCCB(0x3d, 0x38);
  //
  delay(5);
  writeSCCB(0xff, 0x00);
  delay(5);
  writeSCCB(0xe5, 0x7f);
  writeSCCB(0xf9, 0xc0);
  writeSCCB(0x41, 0x24);
  writeSCCB(0xe0, 0x14);
  writeSCCB(0x76, 0xff);
  writeSCCB(0x33, 0xa0);
  writeSCCB(0x42, 0x20);
  writeSCCB(0x43, 0x18);
  writeSCCB(0x4c, 0x00);
  writeSCCB(0x87, 0xd5);
  writeSCCB(0x88, 0x3f);
  writeSCCB(0xd7, 0x03);
  writeSCCB(0xd9, 0x10);
  writeSCCB(0xd3, 0x82);
  //
  writeSCCB(0xc8, 0x08);
  writeSCCB(0xc9, 0x80);
  //
  writeSCCB(0x7c, 0x00);
  writeSCCB(0x7d, 0x00);
  writeSCCB(0x7c, 0x03);
  writeSCCB(0x7d, 0x48);
  writeSCCB(0x7d, 0x48);
  writeSCCB(0x7c, 0x08);
  writeSCCB(0x7d, 0x20);
  writeSCCB(0x7d, 0x10);
  writeSCCB(0x7d, 0x0e);
  //
  writeSCCB(0x90, 0x00);
  writeSCCB(0x91, 0x0e);
  writeSCCB(0x91, 0x1a);
  writeSCCB(0x91, 0x31);
  writeSCCB(0x91, 0x5a);
  writeSCCB(0x91, 0x69);
  writeSCCB(0x91, 0x75);
  writeSCCB(0x91, 0x7e);
  writeSCCB(0x91, 0x88);
  writeSCCB(0x91, 0x8f);
  writeSCCB(0x91, 0x96);
  writeSCCB(0x91, 0xa3);
  writeSCCB(0x91, 0xaf);
  writeSCCB(0x91, 0xc4);
  writeSCCB(0x91, 0xd7);
  writeSCCB(0x91, 0xe8);
  writeSCCB(0x91, 0x20);
  //
  writeSCCB(0x92, 0x00);
  writeSCCB(0x93, 0x06);
  writeSCCB(0x93, 0xe3);
  writeSCCB(0x93, 0x05);
  writeSCCB(0x93, 0x05);
  writeSCCB(0x93, 0x00);
  writeSCCB(0x93, 0x04);
  writeSCCB(0x93, 0x00);
  writeSCCB(0x93, 0x00);
  writeSCCB(0x93, 0x00);
  writeSCCB(0x93, 0x00);
  writeSCCB(0x93, 0x00);
  writeSCCB(0x93, 0x00);
  writeSCCB(0x93, 0x00);
  //
  writeSCCB(0x96, 0x00);
  writeSCCB(0x97, 0x08);
  writeSCCB(0x97, 0x19);
  writeSCCB(0x97, 0x02);
  writeSCCB(0x97, 0x0c);
  writeSCCB(0x97, 0x24);
  writeSCCB(0x97, 0x30);
  writeSCCB(0x97, 0x28);
  writeSCCB(0x97, 0x26);
  writeSCCB(0x97, 0x02);
  writeSCCB(0x97, 0x98);
  writeSCCB(0x97, 0x80);
  writeSCCB(0x97, 0x00);
  writeSCCB(0x97, 0x00);
  //
  writeSCCB(0xc3, 0b11100001); //CTRL1
  writeSCCB(0xa4, 0x00);
  writeSCCB(0xa8, 0x00);
  writeSCCB(0xc5, 0x11);
  writeSCCB(0xc6, 0x51);
  writeSCCB(0xbf, 0x80);
  writeSCCB(0xc7, 0x10);
  writeSCCB(0xb6, 0x66);
  writeSCCB(0xb8, 0xA5);
  writeSCCB(0xb7, 0x64);
  writeSCCB(0xb9, 0x7C);
  writeSCCB(0xb3, 0xaf);
  writeSCCB(0xb4, 0x97);
  writeSCCB(0xb5, 0xFF);
  writeSCCB(0xb0, 0xC5);
  writeSCCB(0xb1, 0x94);
  writeSCCB(0xb2, 0x0f);
  writeSCCB(0xc4, 0x5c);
  //
  writeSCCB(0xc0, 0x64);
  writeSCCB(0xc1, 0x4B);
  writeSCCB(0x8c, 0x00);
  writeSCCB(0x86, 0x3D);
  writeSCCB(0x50, 0x00); //bank00, CTRl
  writeSCCB(0x51, 0xC8);
  writeSCCB(0x52, 0x96);
  writeSCCB(0x53, 0x00);
  writeSCCB(0x54, 0x00);
  writeSCCB(0x55, 0x00);
  writeSCCB(0x5a, 0xC8); //ZMOW(Zoom Output Width) 200px
  writeSCCB(0x5b, 0x96); //ZMOH(Zoom Output Height) 150px
  writeSCCB(0x5c, 0x00);
  writeSCCB(0xd3, 0x82);
  //
  writeSCCB(0xc3, 0b11100001); //CTRL1
  writeSCCB(0x7f, 0x00);
  //
  writeSCCB(0xda, 0x08); //IMAGE_MODE RGB565
  //
  writeSCCB(0xe5, 0x1f);
  writeSCCB(0xe1, 0x67);
  writeSCCB(0xe0, 0x00);
  writeSCCB(0xdd, 0x7f);
  writeSCCB(0x05, 0x00);
}

void writeRegisterRGB565(){
  Serial.println("writeRegisterRGB565");
  writeSCCB(0xff, 0x00); //bank dsp
  writeSCCB(0xE0, 0x04); //RESET DVP
  writeSCCB(0xDA, 0x08); //IMAGE_MODE, RGB565
  writeSCCB(0xE0, 0x00); //RESET
}

void dispOnColorBar(){
  writeSCCB(0xff, 0x01); //bank sensor
  writeSCCB(0x12, 0b00100010); //COM7 CIF mode, colorbar en
}

void dispOffColorBar(){
  writeSCCB(0xff, 0x01); //bank sensor
  writeSCCB(0x12, 0b00100000); //COM7 CIF mode, colorbar en
}

void changeAutoWhiteBalance(uint8_t receive_data){
  Serial.println("AWB change!");
  uint8_t CTRL1;
  writeSCCB(0xff, 0x00);
  CTRL1 = readSCCB(0xC3);
  Serial.print("Old CTRL1=");
  Serial.println(CTRL1, BIN);
  uint8_t CTRL1_bit3_2 = 0, awb_bit = 0, awb_gain_bit = 0;
  awb_bit = receive_data << 3;
  awb_gain_bit = receive_data << 2;
  CTRL1_bit3_2 = awb_bit | awb_gain_bit;
  uint8_t msb, lsb;
  msb = CTRL1 & 0b11110000;
  lsb = CTRL1 & 0b00000011;
  CTRL1 = (lsb | CTRL1_bit3_2) | msb;
  writeSCCB(0xff, 0x00);
  writeSCCB(0xC3, CTRL1);
  Serial.print("New CTRL1=");
  Serial.println(CTRL1, BIN);
}

void changeAutoExposureControl(uint8_t change, uint8_t exposure[2]){
  uint8_t COM8 = 0;
  writeSCCB(0xff, 0x01);
  COM8 = readSCCB(0x13);
  Serial.print("Old COM8=");
  Serial.println(COM8, BIN);
  uint8_t change_aec = 0;
  if(change) {
    change_aec = 0b11100111;
  }else{
    change_aec = 0b11000110;
    exposureResister(exposure);
  }
  if(COM8 != change_aec){
    COM8 = change_aec;
    writeSCCB(0xff, 0x01);
    writeSCCB(0x13, COM8);
    Serial.print("New COM8=");
    Serial.println(COM8, BIN);
  }
}

void exposureResister(uint8_t exposure[2]){
  writeSCCB(0xff, 0x01);
  uint8_t old_REG45 = readSCCB(0x45);
  uint8_t old_AEC = readSCCB(0x10);
  uint8_t old_REG04 = readSCCB(0x04);

  uint16_t old_ex[3] = {};
  old_ex[0] = (((uint16_t)old_REG45 & 0x003f)) << 10;
  old_ex[1] = ((uint16_t)old_AEC) << 2;
  old_ex[2] = ((uint16_t)old_REG04) & 0x0003;
  uint16_t old_exposure = old_ex[0] | old_ex[1] | old_ex[1];
  Serial.printf("old_exposure=%d\r\n", old_exposure);
  uint8_t new_REG45 = 0;
  uint8_t new_AEC = 0;
  uint8_t new_REG04 = 0;

  new_REG45 = (old_REG45 & 0b11000000);
  new_REG45 = new_REG45 | (exposure[0] >> 2);
  new_AEC = (exposure[0] << 6) | (exposure[1] >> 6);
  new_REG04 = (new_REG04 & 0b11111100);
  new_REG04 = new_REG04 | (exposure[1] & 0b00000011);
  writeSCCB(0xff, 0x01);
  writeSCCB(0x45, new_REG45);
  writeSCCB(0x10, new_AEC);
  writeSCCB(0x04, new_REG04);
  uint16_t new_exposure = (exposure[0] << 8) | exposure[1];
  Serial.printf("new_exposure=%d\r\n", new_exposure);
}

【ザッと解説】

63行目では、M5CameraのCPU温度をM5Stackへ送信するためのバッファです。
1byte目を0xffとしているのは、M5Stack側でそのデータが温度データであることを判断するためのフラグみたいなもんです。

281行目で、M5CameraのCPU温度を取得し、283行でUDPの2byte目に温度データを入れ込んで送信しています。
284行のendPacketで、2byteを1パケットとして送信しています。
M5Stack側で受信した時、1パケットが2byteデータならば、温度データと判別できます。

そして、先ほど述べたように、イメージセンサ OV2640 からのDMAデータ受信は、VSYNC信号も割り込み制御にした為、whileループが不要になりました。
DMAによるイメージセンサからの画像データ取得はCPU core 1で動作させ、WiFi UDPの送受信はCPU core 0で動作させています。

そして、CPU温度も取得してWiFi UDPの画像データの前に付加して、M5Stackへ送信しています。
今までArduinoプログラミングをしていて、無限ループが無いのにカメラからの画像データを取得できるというのは、何か不思議な感じがしますね。
改めて、「割り込み」制御は素晴らしい!!!

あとは、過去の記事からの一連のシリーズにより解説は割愛しますが、前回記事のDMA制御部分がシンプルになりました。

M5Stack側のスケッチ(ソースコード)

M5Stack側のスケッチも前回記事からブラッシュアップして、LCDディスプレイ表示はCPU core 1のメインloop内で動作させ、WiFi UDP送受信はCPU core 0で動作させています。

【ソースコード】 (※無保証 ※PCの場合、ダブルクリックすればコード全体を選択できます)

#define MGO_TEC_BV1_M5STACK_SD_SKETCH
#include <mgo_tec_bv1_m5stack_sd_simple1.h> //ESP32_mgo_tec library. beta ver 1.0.69

#include <WiFi.h>
#include <WiFiUdp.h>

const char* utf8sjis_file = "/font/Utf8Sjis.tbl"; //UTF8 Shift_JIS 変換テーブルファイル名を記載しておく
const char* shino_half_font_file = "/font/shnm8x16.bdf"; //半角フォントファイル名を定義
const char* shino_full_font_file = "/font/MYshnmk16.bdf"; //自作改変全角東雲フォントファイル

const char * to_udp_address = "192.168.4.1";
const int to_server_udp_port = 55555; //送信相手のポート番号
const int my_server_udp_port = 55556; //開放する自ポート

const char* ssid = "xxxxxxxxx"; //9文字以上。ご自分好きなSSIDに書き換えてください
const char* password = "xxxxxxxxx"; //9文字以上。ご自分好きなパスワードに書き換えてください

WiFiUDP udp;
boolean connected = false;

const uint16_t disp_w_pix = 200;
const uint16_t disp_h_pix = 148;
const uint8_t fix_div = 3;
const uint8_t div_h = disp_h_pix / fix_div;
uint16_t udp_line = 0;
boolean isDisp_ok = false;
const uint16_t line_max_bytes = disp_w_pix * 2;
unsigned char line_num_buf[fix_div] = {};
unsigned char pix_buf[disp_h_pix][line_max_bytes] = {};
TaskHandle_t task_handl;

enum UdpSendStatus {
  udp_stop = 0,
  udp_start = 1
} UdpSendState = udp_stop;

enum UdpSendAwb {
  awb_off = 0,
  awb_on = 1
} UdpSendAWB = awb_off;

enum UdpSendAec {
  aec_off = 0,
  aec_on = 1
} UdpSendAEC = aec_on;

uint8_t udp_send_reset = 0;

int16_t exposure[4] = {0, 192, 256, 8192};
int8_t exposure_count = 0;
String aec_str[4] = {"Auto ",
                     " - 1 ",
                     " + 1 ",
                     " + 2 "};
String stream_str[3] = {"Stop ", "Start", "RESET"};
String awb_str[2] = {" OFF ", " O N "};

uint8_t send_auto_white_balance = 0;
uint8_t send_auto_exposure_control = 0;
uint8_t send_data[6] = {};
uint8_t offset = 1;
boolean isSend_ok = false;

uint16_t btn1_x0 = 0, btn1_x1 = 107;
uint16_t btn2_x0 = 107, btn2_x1 = 212;
uint16_t btn3_x0 = 212, btn3_x1 = 319;
uint16_t btn_y0 = 197, btn_y1 = 233;

//-------------------
int16_t check_sum = 0;
uint32_t calc_heap_time = 0;
boolean isCooling_cpu = false;
uint8_t cpu_temp = 0;
boolean isDisp_cpu_temp = false;
boolean isDisp_first_cpu_temp = true;
boolean onButton_A = false;
boolean isDisp_cooling = false;
IPAddress myIP;
//-------------------------
uint8_t receive_temp_buf = 0;
uint8_t old_recv_temp = 0;
uint16_t y;

//********CPU core 1 task********************
void setup(){
  Serial.begin(115200);
  delay(1000);
  initDisplay();
  send_data[2] = (uint8_t)UdpSendAEC;
  xTaskCreatePinnedToCore(&taskUDP, "taskUDP", 8192, NULL, 24, &task_handl, 0);
  while(!connected){
    Serial.print('.');
    delay(500);
  }
  connectedWifiDisp();
}

void loop(){
  if(!isCooling_cpu){
    if(isDisp_ok){
      for(int k = 0; k < disp_h_pix; k++){
        y = k + offset;
        LCD.drawPixel65kColRGB565Bytes(offset, y, disp_w_pix + offset, y, (uint8_t*)&pix_buf[k][0], line_max_bytes);
      }
      isDisp_ok = false;
    }
  }
  actionButton(); //ボタン操作
  decideUdpStarStop();

  if(isDisp_cpu_temp){
    mM5.disp_fnt[5].dispText( mM5.font[5], String(cpu_temp) );
    isDisp_cpu_temp = false;
  }
  if(receive_temp_buf != old_recv_temp){
    mM5.disp_fnt[4].dispText( mM5.font[4], String(receive_temp_buf) );
    old_recv_temp = receive_temp_buf;
  }

  if(isDisp_cooling){
    mM5.disp_fnt[6].dispText( mM5.font[6], "CPU Cooling!" );
    isDisp_cooling = false;
  }
}
//***********************************
void taskUDP(void *pvParameters){
  connectToWiFi();
  while(!connected){
    delay(1);
  }
  while(true){
    receiveUDP();
    sendUDP(send_data);
    if(isDisp_first_cpu_temp || millis() - calc_heap_time > 10000){
      cpu_temp = (uint8_t)floor(temperatureRead());
      isDisp_cpu_temp = true;
      //Serial.printf("Free Heap Size = %d\r\n", esp_get_free_heap_size());
      //Serial.printf("CPU temp = %d\r\n", cpu_temp);
      if(!isCooling_cpu){
        if(UdpSendState == udp_start && cpu_temp > 64.0){
          onButton_A = true;
          isCooling_cpu = true;
          isDisp_cooling = true;
          //udp.stop();
        }
      }else{
        if(cpu_temp < 58.0){
          //udp.begin(myIP, my_server_udp_port);
          onButton_A = true;
          isCooling_cpu = false;
          isDisp_cooling = false;
        }
      }

      calc_heap_time = millis();
      isDisp_first_cpu_temp = false;
    }
    delay(1); //ウォッチドッグタイマ動作のために必ず必要
  }
}

void receiveUDP(){
  if(!isCooling_cpu){
    if(!isDisp_ok){
      if(connected){
        int packetSize = udp.parsePacket();
        if(packetSize == 2){
          if(udp.read() == 0xff){
            receive_temp_buf = udp.read();
          }
        }else if(packetSize > 2){
          for(int i = 0; i < fix_div; i++){
            udp.read(&line_num_buf[i], 1);
            udp.read(&pix_buf[line_num_buf[i]][0], line_max_bytes);
          }
          if((line_num_buf[0] == 0) || (line_num_buf[0] == (check_sum + 1))) {
            udp_line = (uint16_t)floor((double)line_num_buf[0] / (double)fix_div);
            if(udp_line == (div_h - 1)) isDisp_ok = true;
            check_sum = line_num_buf[fix_div - 1];
          }
        }
        udp.flush();
      }
    }
  }
}

void sendUDP(uint8_t send_data[5]){
  if(isSend_ok){
    udp.beginPacket(to_udp_address, to_server_udp_port);
    udp.write(send_data, 6);
    udp.endPacket();
    isSend_ok = false;
  }
}

void initDisplay(){
  mM5.init( utf8sjis_file, shino_half_font_file, shino_full_font_file );
  LCD.ILI9341init();
  LCD.brightness( 255 );  
  LCD.displayClear();
  LCD.drawRectangleLine(0, 0, disp_w_pix + offset, disp_h_pix + offset, 31, 63, 31);
  LCD.drawRectangleLine(btn1_x0, btn_y0, btn1_x1, btn_y1, "#ffffff");
  LCD.drawRectangleLine(btn2_x0, btn_y0, btn2_x1, btn_y1, "#ffffff");
  LCD.drawRectangleLine(btn3_x0, btn_y0, btn3_x1, btn_y1, "#ffffff");

  mM5.font[0].x0 = 10; mM5.font[0].y0 = 160;
  mM5.font[0].htmlColorCode( "#FFFFFF" );
  mM5.font[0].Xsize = 1, mM5.font[0].Ysize = 2;
  mM5.disp_fnt[0].dispText( mM5.font[0], "  Stream" );
  mM5.font[0].x0 = 106; mM5.font[0].y0 = 160;
  mM5.font[0].htmlColorCode( "#FFFFFF" );
  mM5.font[0].Xsize = 1, mM5.font[0].Ysize = 2;
  mM5.disp_fnt[0].dispText( mM5.font[0], "Auto WhiteB" );
  mM5.font[0].x0 = 212; mM5.font[0].y0 = 160;
  mM5.font[0].htmlColorCode( "#FFFFFF" );
  mM5.font[0].Xsize = 2, mM5.font[0].Ysize = 2;
  mM5.disp_fnt[0].dispText( mM5.font[0], " 露出 " );

  mM5.font[1].x0 = 10; mM5.font[1].y0 = 200;
  mM5.font[1].htmlColorCode( "red" );
  mM5.font[1].Xsize = 2, mM5.font[1].Ysize = 2;
  mM5.disp_fnt[1].dispText( mM5.font[1], stream_str[0] );
  mM5.font[2].x0 = 120; mM5.font[2].y0 = 200;
  mM5.font[2].htmlColorCode( "blue" );
  mM5.font[2].Xsize = 2, mM5.font[2].Ysize = 2;
  mM5.disp_fnt[2].dispText( mM5.font[2], awb_str[0] );
  mM5.font[3].x0 = 230; mM5.font[3].y0 = 200;
  mM5.font[3].htmlColorCode( "#ff00ff" );
  mM5.font[3].Xsize = 2, mM5.font[3].Ysize = 2;
  mM5.disp_fnt[3].dispText( mM5.font[3], aec_str[0] );
}

void connectedWifiDisp(){
  mM5.font[0].x0 = 205; mM5.font[0].y0 = 0;
  mM5.font[0].htmlColorCode( "#FFFFFF" );
  mM5.font[0].Xsize = 1, mM5.font[0].Ysize = 2;
  mM5.disp_fnt[0].dispText( mM5.font[0], "   Wi-Fi UDP" );
  mM5.font[0].x0 = 205; mM5.font[0].y0 = 32;
  mM5.disp_fnt[0].dispText( mM5.font[0], "     Video" );
  mM5.font[0].x0 = 205; mM5.font[0].y0 = 64;
  mM5.disp_fnt[0].dispText( mM5.font[0], "    Receive" );

  mM5.font[4].x0 = 205; mM5.font[4].y0 = 96;
  mM5.font[4].htmlColorCode( "#00ff00" );
  mM5.font[4].Xsize = 1, mM5.font[4].Ysize = 2;
  String cam_temp_str = "Camera: -- ℃";
  mM5.disp_fnt[4].dispText( mM5.font[4], cam_temp_str );
  mM5.font[4].x0 = 269;

  mM5.font[5].x0 = 205; mM5.font[5].y0 = 128;
  mM5.font[5].htmlColorCode( "#ffff00" );
  mM5.font[5].Xsize = 1, mM5.font[5].Ysize = 2;
  String cpu_temp_str = "C P U: " + String(cpu_temp) + " ℃";
  mM5.disp_fnt[5].dispText( mM5.font[5], cpu_temp_str );
  mM5.font[5].x0 = 261;

  mM5.font[6].x0 = 5; mM5.font[6].y0 = 50;
  mM5.font[6].htmlColorCode( "#ff00ff" );
  mM5.font[6].Xsize = 2, mM5.font[6].Ysize = 2;
}
//*********************************************
void connectToWiFi(){
  Serial.println("Connecting to WiFi network: " + String(ssid));
  WiFi.disconnect(true, true);
  delay(1000);
  WiFi.onEvent(WiFiEvent);
  WiFi.begin(ssid, password);
  Serial.println("Waiting for WIFI connection...");
}

void calcExposure(boolean isIncrease){
  if(isIncrease){
    exposure_count++;
  }else{
    exposure_count--;
  }
  if(exposure_count > 3) exposure_count = 3;
  if(exposure_count < 0) exposure_count = 0;
  if(exposure_count == 0) {
    UdpSendAEC = aec_on;
    mM5.font[3].htmlColorCode( "#ff00ff" );
    Serial.println("exposure=AUTO");
  }else{
    UdpSendAEC = aec_off;
    mM5.font[3].htmlColorCode( "yellow" );
    Serial.printf("exposure=%d\r\n", exposure[exposure_count]);
  }
  mM5.disp_fnt[3].dispText( mM5.font[3], aec_str[exposure_count] );
  send_data[2] = (uint8_t)UdpSendAEC;
  send_data[3] = (uint8_t)(exposure[exposure_count] >> 8);
  send_data[4] = (uint8_t)(exposure[exposure_count] & 0x00ff);
}

void WiFiEvent(WiFiEvent_t event){
  myIP = WiFi.localIP();
  switch(event) {
    case SYSTEM_EVENT_STA_GOT_IP:
      Serial.println("WiFi connected!");
      Serial.print("My IP address: ");
      Serial.println(myIP);
      //udp.begin関数は自サーバーの待ち受けポート開放する関数である
      udp.begin(myIP, my_server_udp_port);
      delay(1000);
      connected = true;
      break;
    case SYSTEM_EVENT_STA_DISCONNECTED:
      Serial.println("WiFi lost connection");
      connected = false;
      break;
    default:
      break;
  }
}

void decideUdpStarStop(){
  if(onButton_A){
    if(UdpSendState == udp_start){
      UdpSendState = udp_stop;
      mM5.font[1].htmlColorCode( "red" );
    }else{
      UdpSendState = udp_start;
      mM5.font[1].htmlColorCode( "green" );
    }
    send_data[0] = (uint8_t)UdpSendState;
    isSend_ok = true;
    Serial.printf("UdpSendState=%d\r\n", (uint8_t)UdpSendState);
    mM5.disp_fnt[1].dispText( mM5.font[1], stream_str[(int)UdpSendState] );
    onButton_A = false;
  }
}

void actionButton(){
  mM5.btnA.buttonAction();
  switch( mM5.btnA.ButtonStatus ){
    case mM5.btnA.MomentPress:
      Serial.println("\r\nButton A Moment Press");
      onButton_A = true;
      break;
    case mM5.btnA.ContPress:
      Serial.println("\r\n-------------Button A Cont Press");
      udp_send_reset = 1;
      send_data[5] = udp_send_reset;
      isSend_ok = true;
      mM5.font[1].htmlColorCode( "white" );
      mM5.font[1].htmlBgColorCode( "red" );
      LCD.drawRectangleFill(btn1_x0+1, btn_y0+1, btn1_x1-1, btn_y1-1, "red");
      mM5.disp_fnt[1].dispText( mM5.font[1], stream_str[2] );
      Serial.println("Send [Reset]!!!");
      break;
    default:
      break;
  }

  mM5.btnB.buttonAction();
  switch( mM5.btnB.ButtonStatus ){
    case mM5.btnB.MomentPress:
      Serial.println("\r\nButton B Moment Press");
      if(UdpSendAWB == awb_off){
        UdpSendAWB = awb_on;
        mM5.font[2].htmlColorCode( "white" );
      }else{
        UdpSendAWB = awb_off;
        mM5.font[2].htmlColorCode( "blue" );
      }
      send_data[1] = (uint8_t)UdpSendAWB;
      isSend_ok = true;
      Serial.printf("AWB=%d\r\n", (uint8_t)UdpSendAWB);
      mM5.disp_fnt[2].dispText( mM5.font[2], awb_str[(int)UdpSendAWB] );
      break;
    case mM5.btnB.ContPress:
      Serial.println("\r\n-------------Button B Cont Press");
      break;
    default:
      break;
  }

  mM5.btnC.buttonAction();
  switch( mM5.btnC.ButtonStatus ){
    case mM5.btnC.MomentPress:
      Serial.println("\r\nButton C Moment Press");
      calcExposure(true);
      isSend_ok = true;
      break;
    case mM5.btnC.ContPress:
      Serial.println("\r\n-------------Button C Cont Press");
      calcExposure(false);
      isSend_ok = true;
      break;
    default:
      break;
  }
}

【ザッと解説】

M5Stack側も135行目のようにCPU温度を取得して、LCDに表示させています。
そして、140-145行で、M5Stack側のCPU温度が64℃を超えたらM5Camera側へUDP送信をストップする信号を送信し、M5Stack側のLCD表示をストップするようにしました。
M5Camera側の64℃越えは監視していませんが、冷却ファンを使わないのなら監視した方が良いと思います。
M5Stack側が58℃以下になったら、自動的にWiFi UDP送受信するようにしました。

167-170行で、UDP受信した1パケットが2byteデータならば、M5CameraのCPU温度データと判別しています。

168行目で1byte目が0xffで、169行目にあるように2byte目がM5CameraのCPU温度です。
それをM5StackのLCDに表示するようにしました。

UDPパケットが2byte以上ならば画像データです。
176-180行で、画像の垂直位置データがパケットロスで連番にならない場合、LCDに描画せずにスルーするようにしました。
これで、パケットロスによる画面ノイズを少なくできます。

コンパイル書き込み実行

では、M5Stack、M5Camera両方をArduino IDEでコンパイル書き込み実行してみてください。
その時、M5Camera側から起動させると良いと思います。

M5StackのLCD表示の右上に下図の様な表示が出れば、M5CameraとWiFi UDP通信可能になったという合図です。

また、M5Camera側のCPU温度と、M5Stack側のCPU温度も表示するようにしました。
ボタン操作は前回記事と同様です。

これで、冷却ファンを外してみて下さい。
暖房の効いた部屋なら直ぐにM5Stack側が65℃を超えて画像が停止すると思います
それだけ、今回のWiFi UDPの連続画像転送はCPU負荷が大きいということだと思います。

編集後記

前回でもいっていたディープラーニングを始めることは、今回も先送りにしてしまいました。
年末に取り掛かりたいなぁ・・・。

でも、ESP32の「割り込み」制御って、こんなにも便利で高速処理ができるんだということが改めて分かりましたね。
GPIOの割り込みを使えば、M5Stackのボタン操作もポーリングではなく、割り込みを使った方が断然良さそうです。

それと、今回の様なCPUに長時間連続して負荷を掛ける場合は、冷却ファン必須ですね。
なぜESP32系のデバイスに冷却ファンオプションが無いのか疑問に感じてしまいました。

あと、DMAのようなシビアな高速タイミング動作はcore 1で動作させて、WiFi関連はcore 0で動作させることが安定動作に繋がるような気がします。
これらは、FreeRTOSでタスクでセマフォ等をうまく使えば、もっとスマートなプログラミングができるかもしれません。
でも、FreeRTOSは無知なので、今後の課題ですね。

ということで、もうすぐ今年も終わりです。
1年経つのが早過ぎです。
ヤバイ!

ではまた・・・。

コメント

  1. macsbug より:
    • mgo-tec mgo-tec より:

      macsbugさん

      コメントいただき、ありがとうございます。

      そのファンは以前Twitterで情報を頂いたんですが、その時は単体では売っていませんでした。
      今はもう単体で売っているんですね。
      私はAliexpressは使ったことが無いので、いつかAmazonかスイッチサイエンスさんで販売してほしいなぁ・・・と願って待っています。
      いつも有益な情報ありがとうございます。
      m(_ _)m

タイトルとURLをコピーしました