M5Cameraの動画をM5StackへWiFi, UDPで送信する実験

M5Stack側のスケッチ入力

M5Stack側のESP32はSTAモードに設定します。

【ソースコード】 (※無保証　※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"; //M5Camera側のSoftAPモードのSSID
const char* password = "xxxxxxxxx"; //M5Camera側のSoftAPモードのパスワード

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;

//********CPU core 1 task********************
void setup(){
  Serial.begin(115200);
  delay(1000);
  xTaskCreatePinnedToCore(&taskDisplay, "taskDisplay", 8192, NULL, 10, &task_handl, 0);
  connectToWiFi();
  while(!connected){
    delay(1);
  }
  send_data[2] = (uint8_t)UdpSendAEC;
}

void loop(){
  receiveUDP();
  sendUDP(send_data);
}

void receiveUDP(){
  if(!isDisp_ok){
    if(connected){
      int packetSize = udp.parsePacket();
      if(packetSize > 0){
        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);
        }
        udp_line = (uint16_t)floor((double)line_num_buf[0] / (double)fix_div);
        //Serial.printf("udp_line=%2d, packet_size =%d, (", udp_line, packetSize);
        if(udp_line == (div_h - 1)) isDisp_ok = true;
      }
      udp.flush();
      delayMicroseconds(500);
    }
  }
}

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;
  }
}
//********CPU core 0 task********************
void taskDisplay(void *pvParameters){
  uint16_t y;
  initDisplay();
  while(!connected){
    Serial.print('.');
    delay(500);
  }
  disableCore0WDT(); //ウォッチドッグ無効
  connectedWifiDisp();
  while(true){
    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;
    }
    delayMicroseconds(500); //ウォッチドッグ無効の場合
    button_action(); //ボタン操作
    //delay(1); //ウォッチドッグ有効の場合
  }
}

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[1].x0 = 106; mM5.font[1].y0 = 160;
  mM5.font[1].htmlColorCode( "#FFFFFF" );
  mM5.font[1].Xsize = 1, mM5.font[1].Ysize = 2;
  mM5.disp_fnt[1].dispText( mM5.font[1], "Auto WhiteB" );
  mM5.font[2].x0 = 212; mM5.font[2].y0 = 160;
  mM5.font[2].htmlColorCode( "#FFFFFF" );
  mM5.font[2].Xsize = 2, mM5.font[2].Ysize = 2;
  mM5.disp_fnt[2].dispText( mM5.font[2], " 露出 " );

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

void connectedWifiDisp(){
  mM5.font[0].x0 = 205; mM5.font[0].y0 = 0;
  mM5.font[0].htmlColorCode( "#FFFFFF" );
  mM5.font[0].Xsize = 2, mM5.font[0].Ysize = 2;
  mM5.disp_fnt[0].dispText( mM5.font[0], " Wi-Fi" );
  mM5.font[0].x0 = 205; mM5.font[0].y0 = 32;
  mM5.disp_fnt[0].dispText( mM5.font[0], "  UDP" );
  mM5.font[0].x0 = 205; mM5.font[0].y0 = 64;
  mM5.disp_fnt[0].dispText( mM5.font[0], " Video" );
  mM5.font[0].x0 = 205; mM5.font[0].y0 = 96;
  mM5.disp_fnt[0].dispText( mM5.font[0], "Receive" );
}
//*********************************************
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[5].htmlColorCode( "#ff00ff" );
    Serial.println("exposure=AUTO");
  }else{
    UdpSendAEC = aec_off;
    mM5.font[5].htmlColorCode( "yellow" );
    Serial.printf("exposure=%d\r\n", exposure[exposure_count]);
  }
  mM5.disp_fnt[5].dispText( mM5.font[5], 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){
  IPAddress 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 button_action(){
  mM5.btnA.buttonAction();
  switch( mM5.btnA.ButtonStatus ){
    case mM5.btnA.MomentPress:
      Serial.println("\r\nButton A Moment Press");
      if(UdpSendState == udp_start){
        UdpSendState = udp_stop;
        mM5.font[3].htmlColorCode( "red" );
      }else{
        UdpSendState = udp_start;
        mM5.font[3].htmlColorCode( "green" );
      }
      send_data[0] = (uint8_t)UdpSendState;
      isSend_ok = true;
      Serial.printf("UdpSendState=%d\r\n", (uint8_t)UdpSendState);
      mM5.disp_fnt[3].dispText( mM5.font[3], stream_str[(int)UdpSendState] );
      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[3].htmlColorCode( "white" );
      mM5.font[3].htmlBgColorCode( "red" );
      LCD.drawRectangleFill(btn1_x0+1, btn_y0+1, btn1_x1-1, btn_y1-1, "red");
      mM5.disp_fnt[3].dispText( mM5.font[3], 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[4].htmlColorCode( "white" );
      }else{
        UdpSendAWB = awb_off;
        mM5.font[4].htmlColorCode( "blue" );
      }
      send_data[1] = (uint8_t)UdpSendAWB;
      isSend_ok = true;
      Serial.printf("AWB=%d\r\n", (uint8_t)UdpSendAWB);
      mM5.disp_fnt[4].dispText( mM5.font[4], 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;
  }
}

【解説】

●1-2行目：
私の自作ライブラリESP32_mgo_tecを使う場合の設定です。

●7-9行目：
M5Stackのmicro SDカードに保存してあるフォントファイルを定義します。

●11-16行：
前回記事とほぼ同じ設定です。
ssid とパスワードはM5CameraのSoftAPモードのssidとパスワードにします。

●23行：
先ほどのM5Camera側のスケッチ解説でも述べているように、
fix_div=3
という数値は、M5Cameraから送られてきた１パケットデータに画像データが３行分ふくまれているので、それを分割して受信するための数値です。

●28行目：
M5Cameraから受信したパケットの1byte目のデータを格納します。
画像のライン番号を収納します。
１パケットに3行分のライン番号が送られてきます。

●29行目：
画像のpixelデータを格納します。
画像データはRGB565フォーマットで送られてくるので、1pixelは2byte分のデータになります。

●32-56行：
M5Stackのボタン操作の状態定義です。
49行目では、M5Cameraに内蔵されているイメージセンサOV2640の露出時間を16bit値で４段階で定義しています。
数値がゼロの場合は自動露出設定にするように、先に紹介したM5Camera側のスケッチで処理します。
露出時間の値に正直よくわかりません。
いろいろ実験した結果の代表的な数値を使いました。

●64-67行：
M5StackのLCDディスプレイにボタン操作表示を表示するための座標位置です。

●70-112行：
M5Stack側ESP32のCPU core 1タスクで、WiFi, UDP でボタン操作のデータをM5Cameraへ送信して、そして、M5Cameraからの大量の画像データを常時受信しています。
ボタン操作した場合、
isSend_ok == true
となると、1パケット6byte分のデータが送信されます。
M5Cameraからの画像データを受信すると、1byte目が画像のライン番号とRGB565画素データに分けて受信します。
100行目のdelayMicroseconds関数で適度に間隔を空けないとうまくデータを受信できませんでした。

●114-185行：
M5Stack側ESP32のCPU core 0タスクでLCDディスプレイ表示しています。
初期化はsetup関数でやらないことに注目してください。
マルチコア、マルチタスクで動作させる場合、同じcoreタスクで実行させないと思わぬトラブルになるので要注意です。
ですから、116行目のようにLCDの初期化はcore 0 で行うようにしています。
121行目でESP32の動作を監視するウォッチドッグタイマを無効にしています。
その場合、131行目のdelayMicroseconds関数で500usの間を空けてLCDディスプレイ表示を更新しています。
ウォッチドッグタイマを無効にした場合、長時間動作し続けるとフリーズした場合強制リセット動作がしなくなるそうです。
ウォッチドッグタイマについて、私はほとんど無知なので良く分かりません。
ウォッチドッグタイマを有効にした場合は、131行目を削除して、133行目のコメントを削除してdelay(1)を実行させると、ウォッチドッグタイマが動作し続けます。
その場合、M5StackのLCDディスプレイ表示が若干遅くなります。

また、先ほど述べましたが、今回、私の自作ライブラリbeta ver 1.0.69で127行目にあるように、
drawPixel65kColRGB565Bytes
という関数を追加しています。
この関数を使えば、RGB565フォーマットデータで一気にLCD表示できて、しかも高速です。
それ以前のライブラリでは動作しませんので、必ずbeta ver 1.0.69を使って下さい。

また、137-172行ではディスプレイに枠線や文字を表示させる初期化設定です。
174-185行では、M5CameraのSoftAPに接続出来た場合、M5Stackのディスプレイの上半分右側に
Wi-Fi UDP Video Receive
という文字を表示させます。

●196-217行：
M5StackのCボタンが押された場合、16bitの露出時間数値を8bitに分割してsend_dataバッファに格納します。
Ｃボタンが瞬時押しされた場合はステップアップし、長押しされた場合はステップダウンします。
aec_onになった場合、自動露出設定がONになります。

●240-310行：
M5Stackのボタン操作で行う処理です。
このボタン操作関数群や定数は私の自作ライブラリESP32_mgo_tecで独自に作っています。
M5Stack標準ライブラリとは使い方が異なるので注意してください。

以上、M5Stack側のスケッチのザッとした解説でした。

コンパイル書込み実行

では、M5Camera側のスケッチ、およびM5Stack側のスケッチをそれぞれコンパイル書き込み実行してみてください。

書込みする場合のボード設定は以下のようにしてください。

書込みが終わったら、まずM5Cameraから起動します。
その後、M5Stackを起動します。

そうすると、M5Cameraに接続できれば、下図の様に画面の上半分右側に
Wi-Fi UDP Video Receive
と表示されます。
あと、ボタン操作は以下の通りです。

Ａボタンを瞬時押しする毎にM5Cameraからの画像データ受信Start-Stopが切り替わります。
Ａボタンを長押しすると、M5Cameraが強制リセットされます。

Ｂボタンを瞬時押しする毎に、自動ホワイトバランス調整ON-OFFが切り替わります。

Ｃボタンを瞬時押しする毎に露出時間が３段階で切り替わります。
デフォルトが自動露出調整（AEC）です。
Ｃボタンを長押しすると、露出時間設定がステップダウンします。

あとは、最初に紹介した動画のように動作すればＯＫです。

では、次の節では、モバイルバッテリーについてと、パケットロスについてを述べてみます。