dj1711572002/Core2_STA22_ESP-NOW_for-rev10279_rev04.ino Secret

## Core2_STA22_ESP-NOW_for-rev10279_rev04.ino

#include <M5Core2.h>
#include <esp_now.h>
#include <WiFi.h>
#include "SD.h"
#include "FS.h"
#include "SPI.h"
esp_now_peer_info_t slave;
File file;
File f;
File root;
//--general  parameters--
int i,j,k;
uint8_t f9pdata[172];
uint8_t f9pdata_1[172];
//------data value -------
int itow,itow_1;//4byte
int flags,flags_1;//1byte
int numSV,numSV_1;//1byte
int hacc,hacc_1;//2byte
int fixtime,fixtime_1;//2byte
//----SD parameters----
int readflag;
String  stime;//filename
uint8_t  count;
int epochN;
//------------------------------
//---------------------swipe--------------
// Defines gestures
Gesture swipeRight("swipe right", 160, DIR_RIGHT, 30, true);
Gesture swipeDown("swipe down", 120, DIR_DOWN, 30, true);
Gesture swipeLeft("swipe left", 160, DIR_LEFT, 30, true);
Gesture swipeUp("swipe up", 120, DIR_UP, 30, true);
String objNm;
//----------------------------------------------------------------------------------------

//--Sprite--
TFT_eSprite tftSprite = TFT_eSprite(&M5.Lcd);
//TFT_eSprite tftSprite =  TFT_eSprite(&M5.Lcd);
//TFT_eSprite DisIMUbuff =  TFT_eSprite(&M5.Lcd);
#define MAGENTA             0xF81F
#define GREENYELLOW         0xAFE5      /* 173, 255,  47 */
#define YELLOW              0xFFE0      /* 255, 255,   0 */
#define WHITE               0xFFFF      /* 255, 255, 255 */
#define FORMAT_SPIFFS_IF_FAILED true
uint16_t Col1,Col2;


// 送信コールバック
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  char macStr[18];
  snprintf(macStr, sizeof(macStr), "%02X:%02X:%02X:%02X:%02X:%02X",
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  Serial.print("Last Packet Sent to: ");
  Serial.println(macStr);
  Serial.print("Last Packet Send Status: ");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");

  // 画面にも描画

  Serial.print("Last Packet Sent to: \n  ");
  Serial.println(macStr);
  Serial.print("Last Packet Send Status: \n  ");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}

// 受信コールバック
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {
  char macStr[18];
  snprintf(macStr, sizeof(macStr), "%02X:%02X:%02X:%02X:%02X:%02X",
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  //Serial.printf("Last Packet Recv from: %s\n", macStr);
  //Serial.printf("Last Packet Recv Data(%d): ", data_len);
  //LCD SET

                 // M5.Lcd.fillRect(0,0,80,15,BLACK);
                 // M5.Lcd.setTextSize(2);
                 // M5.Lcd.setCursor(0,00);
     count=data_len;

     //Serial.print("f9pdata[]=");
     for ( int i = 0 ; i < data_len ; i++ ) {
             f9pdata[i]=data[i];
             //Serial.print(f9pdata[i]);
             //Serial.print(",");
          }
          //Serial.println(millis());

     itow=f9pdata[0]+f9pdata[1]*256+f9pdata[2]*256*256+f9pdata[3]*256*256*256;
    //Serial.printf("---------itow=%d,ESP-RCV=%dbyte,millis=%d\n\r",itow,data_len,millis());
    /*
      //********************temp.txt READ********************************
        stime="/tmp.txt";
        file = SD.open(stime);//read
        j=0;
        while(file.available()){
          f9pdata_1[j]=file.read();
          //Serial.print(f9pdata_1[j]);
          // Serial.print(",");
        j++;
        }
      file.close();
      //Serial.println(millis());
       itow_1=f9pdata_1[0]+f9pdata_1[1]*256+f9pdata_1[2]*256*256+f9pdata_1[3]*256*256*256;
           flags_1=f9pdata_1[4];
           numSV_1=f9pdata_1[5];
           hacc_1=f9pdata_1[6]+f9pdata_1[7]*256;
           fixtime_1=f9pdata_1[8]+f9pdata_1[9];

 //--------Start SD card
      stime="/tmp.txt";
      //file = SD.open(stime, FILE_APPEND);//追記
      file = SD.open(stime, FILE_WRITE);//上書き
      //file.println(str);
      file.write(data,data_len);
      file.close();
       //listDir(SD, "/", 0);
      //Serial.println("--------ESP-NOW Recived SD Write FINISHED----------------");
      */
           flags=f9pdata[4];
           numSV=f9pdata[5];
           hacc=f9pdata[6]+f9pdata[7]*256;
           fixtime=f9pdata[8]+f9pdata[9]*256;
          double fixpercent=(double)(((double)f9pdata[10]+f9pdata[11]*256)/10);
          int epNo=f9pdata[12]+f9pdata[13]*256;
          Serial.printf("---epNo=%d---\n\r",epNo);
          int miscount1=f9pdata[14];
          double pDOP=((double)f9pdata[15]+f9pdata[16]*256)/100;

       if(epNo%8==0){
       //---------------現在相対座標-------------------------
           double rrN=by2Todbl(f9pdata[17],f9pdata[18])/100;
          //Serial.printf("epNo=%d,rrN=%3.2f,[17]=%x,[18]=%x\n\r",epNo,rrN,f9pdata[17],f9pdata[18]);
          double rrE=by2Todbl(f9pdata[19],f9pdata[20])/100;
          double rrD=by2Todbl(f9pdata[21],f9pdata[22])/100;
           double rrL=sqrt(pow(rrN,2)+pow(rrE,2)+pow(rrD,2));
          //----------------MovingBase Head-------------------------
          //double mHead=((double)f9pdata[23]+f9pdata[24]*256+f9pdata[25]*256*256+f9pdata[26]*256*256*256)*0.00001;
           double mHead=by4Todbl(f9pdata[23],f9pdata[24],f9pdata[25],f9pdata[26])*0.00001;
          //------------Calibration Average ----------------
          double rNaveN=by4Todbl(f9pdata[27],f9pdata[28],f9pdata[29],f9pdata[30])*0.01;
          double rEaveN=by4Todbl(f9pdata[31],f9pdata[32],f9pdata[33],f9pdata[34])*0.01;
          double rDaveN=by4Todbl(f9pdata[35],f9pdata[36],f9pdata[37],f9pdata[38])*0.01;
          double rLaveN=by4Todbl(f9pdata[39],f9pdata[40],f9pdata[41],f9pdata[42])*0.01;
          //------------Calibration standard Deviation-----------------------------
          double rNstd=by2Todbl(f9pdata[43],f9pdata[44])*0.01;
          double rEstd=by2Todbl(f9pdata[45],f9pdata[46])*0.01;
          double rDstd=by2Todbl(f9pdata[47],f9pdata[48])*0.01;
          double rLstd=by2Todbl(f9pdata[49],f9pdata[50])*0.01;

          //------------Calibration Counter-----------------------------
          int avecount=(int)f9pdata[51]+f9pdata[52]*256;

         Serial.printf("------------------------------itow=%d---------------------------------------\n\r",itow);
        Serial.printf("it=%d,flg=%d,nSV=%d,acc=%d,tim=%d,F%%=%3.1f,epN=%d,mis=%d,Dop=%2.2f\n\r",itow,flags,numSV,hacc,fixtime,fixpercent,epNo,miscount1,pDOP);
        Serial.printf("rrN=%5.2f,rrE=%5.2f,rrD=%5.2f,Hed=%3.5f,Nav=%5.2f,Eav=%5.2f,Dav=%5.2f,Lav=%5.2f\n\r",rrN,rrE,rrD,mHead,rNaveN,rEaveN,rDaveN,rLaveN);
        Serial.printf(",Nstd=%5.2f,Estd=%5.2f,Dstd=%5.2f,Lstd=%5.2f,avecount=%d \n\r",rNstd,rEstd,rDstd,rLstd,avecount);


  //=====Sprite draw====================
         int  coff=0;
            tftSprite.fillRect(0,0,320,240,BLACK);
            tftSprite.setTextSize(2);
            Col1=0xF81F;//MAGENTA
            Col2=0xFFE0;//YELLOW
            tftSprite.setTextColor(Col2);//YELLOW Text
              tftSprite.setCursor(0,0);
              tftSprite.printf("%d:nSV/Dop=%d/%4.2f",itow,numSV,pDOP);//RTK Acc/Dop表示
              tftSprite.setCursor(0,20+coff);
              tftSprite.printf("flags-acc=%d-%dmm",flags,hacc);//
              tftSprite.setCursor(0,40+coff);
              tftSprite.printf("FixTm/%%=%dsec,%3.1f%%",fixtime,fixpercent);//fixtime/fixpercent
              tftSprite.setCursor(0,60+coff);
              tftSprite.printf("misN/epN=%d/%d",miscount1,epNo);//Misscount/epNo
              tftSprite.setCursor(0,80+coff);
              tftSprite.printf("BLen=%2.1fkm/%3.2fcm",rLaveN/100000,rLstd*3);//基線長＋３シグマ
              tftSprite.setCursor(0,100+coff);
              tftSprite.printf("Head=%3.5fdeg",mHead);//Heading
              tftSprite.setCursor(0,120+coff);
              tftSprite.printf("rN=% 3.2f/%3.2fcm",rrN,rNstd*3);//N相対座標＋３シグマ
              tftSprite.setCursor(0,140+coff);
              tftSprite.printf("rE=% 3.2f/%3.2fcm",rrE,rEstd*3);//E相対座標＋３シグマ
              tftSprite.setCursor(0,160+coff);
              tftSprite.printf("rD=% 3.2f/%3.2fcm",rrD,rDstd*3);//D相対座標＋３シグマ
              tftSprite.setCursor(0,180+coff);
              tftSprite.printf("rL=%5.2fcm",rrL);//基線長＋３シグマ

               tftSprite.pushSprite(0, 0);  //PUSH SPLITE To LCD

       }//if itow%1000==500 end

}
// ===========BYTE TO DOUBLE=========================
double by2Todbl(uint8_t l,uint8_t u){
        double result;
       if( u & 0x80){
         int i=(l+u *256)-65536;
         result=(double)i;
       }
       else{
         result=(double)(l+u*256);
       }
      return result;
}
double by4Todbl(uint8_t ll,uint8_t l,uint8_t u,uint8_t uu){
        double result;
       if( uu & 0x80){
         int i=(ll+l*256+u*256*256+uu*256*256*256)-4294967296;
         result=(double)i;
       }
       else{
         result=(double)(ll+l*256+u*256*256+uu*256*256*256);
       }
      return result;
}


void setup() {
  M5.begin();
  //---LCD SET UP---
      M5.Lcd.fillScreen(BLACK);
     M5.Lcd.setRotation(1); //  tftSprite.print("ESP-NOW Test\n");
      M5.Lcd.print("----SetUP-----\n");
      tftSprite.createSprite(M5.Lcd.width(),210);// M5.Lcd.height());
  ///---SD initialize------------
      // Start SD card
      if (!SD.begin())
      {
        // tftSprite.println("ERROR: SD CARD.");
         Serial.print("SD  begin ERROR ");
      }
      stime="/ESPdata.txt";
      file = SD.open(stime, FILE_APPEND);
      String strTotal="1234567890";
      file.println(strTotal);
      file.close();
      //Serial.println(strTotal);
      //listDir(SD, "/", 0);
  Serial.print("ESP-NOW Test\n");
 //MAC address GET-----------
  uint8_t mac3[6];
  esp_read_mac(mac3, ESP_MAC_WIFI_STA);
  Serial.printf("[Wi-Fi Station] Mac Address = %02X:%02X:%02X:%02X:%02X:%02X\r\n", mac3[0], mac3[1], mac3[2], mac3[3], mac3[4], mac3[5]);
  // ESP-NOW初期化
  WiFi.mode(WIFI_STA);
  WiFi.disconnect();
  if (esp_now_init() == ESP_OK) {
    Serial.println("ESPNow Init Success");
    Serial.print("ESPNow Init Success\n");
  } else {
    Serial.println("ESPNow Init Failed");
    Serial.print("ESPNow Init Failed\n");
    ESP.restart();
  }

  // マルチキャスト用Slave登録
  memset(&slave, 0, sizeof(slave));
  for (int i = 0; i < 6; ++i) {
    slave.peer_addr[i] = (uint8_t)0xff;
  }
  esp_err_t addStatus = esp_now_add_peer(&slave);
  if (addStatus == ESP_OK) {
    // Pair success
    Serial.println("Pair success");
  }

  // ESP-NOWコールバック登録
  esp_now_register_send_cb(OnDataSent);
  esp_now_register_recv_cb(OnDataRecv);
 count=0;
 epochN=0;
}
//============LOOP========================================
void loop() {
  M5.update();
   /*
      stime="/tmp.txt";
      //file = SD.open(stime, FILE_APPEND);//追記
      file = SD.open(stime);//read
      //String rstr="";
      //Serial.println();
      //Serial.printf("====Loop SD read:itow=");
      j=0;
      while(file.available()){
        f9pdata[j]=file.read();
        //Serial.print(f9pdata[j],HEX);
        j++;
      }
      delay(5);
      file.close();
      delay(5);
      itow=f9pdata[0]+f9pdata[1]*256+f9pdata[2]*256*256+f9pdata[3]*256*256*256;
      //Serial.printf("itow=%d:",itow);
      // epoch change
      if(itow-itow_1>=500 && j>0){
           epochN++;
           flags=f9pdata[4];
           numSV=f9pdata[5];
           hacc=f9pdata[6]+f9pdata[7]*256;
           fixtime=f9pdata[8]+f9pdata[9];
           //Serial.printf("epochN=%d:itow=%d,flags=%d,numSV=%d,hacc=%d,fixtime=%d,time=%d\n\r",epochN,itow,flags,numSV,hacc,fixtime,millis());
          //Serial.print(":f9pdata[]=");
          //for(k=0;k<j;k++){
           // Serial.print(f9pdata[k],HEX);
           // Serial.print(",");
          //}
          itow_1=itow;
          Serial.println();
        } //itow>itow_1 end


      //listDir(SD, "/", 0);

  // ボタンを押したら送信

  if ( M5.BtnA.wasPressed() ) {//For Atom HomeButton=GPIO39=> M5.BtnB.wasPressed()   For M5StickC GPIO37=> M5.BtnA.wasPressed()
    //Core2 Send============MPC Slave AP Mac  target:Core2======================
        slave.peer_addr[0] = (uint8_t)0xff;//0x24;//0xD8;//0x50;
        slave.peer_addr[1] = (uint8_t)0xff;//0x0A;//0xA0;//0x02;
        slave.peer_addr[2] = (uint8_t)0xff;//0xC4;//0x1D;//0x91;
        slave.peer_addr[3] = (uint8_t)0xff;//0xF8;//0x58;//0x87;
        slave.peer_addr[4] = (uint8_t)0xff;//0x5D;//0x47;//0x21;
        slave.peer_addr[5] = (uint8_t)0xff;//0x78;//0x30;//0x84;
        esp_err_t addStatus = esp_now_add_peer(&slave);//slave address set
        if (addStatus == ESP_OK) {
              // Pair success
               Serial.println("Pair success");
         }
    uint8_t data[2] = {123, 234};
    esp_err_t result = esp_now_send(slave.peer_addr, data, sizeof(data));
    Serial.print("Send Status: ");
    if (result == ESP_OK) {
      Serial.println("Success");
    } else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
      Serial.println("ESPNOW not Init.");
    } else if (result == ESP_ERR_ESPNOW_ARG) {
      Serial.println("Invalid Argument");
    } else if (result == ESP_ERR_ESPNOW_INTERNAL) {
      Serial.println("Internal Error");
    } else if (result == ESP_ERR_ESPNOW_NO_MEM) {
      Serial.println("ESP_ERR_ESPNOW_NO_MEM");
    } else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
      Serial.println("Peer not found.");
    } else {
      Serial.println("Not sure what happened");
    }
  }
  delay(1);

  */
}
//===========================================================================================
//===========LOOP END========================================================================
//===========================================================================================
/*
void listDir(fs::FS &fs, const char * dirname, uint8_t levels){
    Serial.printf("Listing directory: %s\n", dirname);

    File root = fs.open(dirname);
    if(!root){
        Serial.println("Failed to open directory");
        return;
    }
    if(!root.isDirectory()){
        Serial.println("Not a directory");
        return;
    }

    File file = root.openNextFile();
    while(file){
        if(file.isDirectory()){
            Serial.print("  DIR : ");
            Serial.println(file.name());
            if(levels){
                listDir(fs, file.path(), levels -1);
            }
        } else {
            Serial.print("  FILE: ");
            Serial.print(file.name());
            Serial.print("  SIZE: ");
            Serial.println(file.size());
        }
        file = root.openNextFile();
    }
}

void createDir(fs::FS &fs, const char * path){
    Serial.printf("Creating Dir: %s\n", path);
    if(fs.mkdir(path)){
        Serial.println("Dir created");
    } else {
        Serial.println("mkdir failed");
    }
}
*/
void removeDir(fs::FS &fs, const char * path){
    Serial.printf("Removing Dir: %s\n", path);
    if(fs.rmdir(path)){
        Serial.println("Dir removed");
    } else {
        Serial.println("rmdir failed");
    }
}

void readFile(fs::FS &fs, const char * path){
    Serial.printf("Reading file: %s\n", path);

    File file = fs.open(path);
    if(!file){
        Serial.println("Failed to open file for reading");
        return;
    }

    Serial.print("Read from file: ");
    while(file.available()){
        Serial.write(file.read());
    }
    file.close();
}

void writeFile(fs::FS &fs, const char * path, const char * message){
    Serial.printf("Writing file: %s\n", path);

    File file = fs.open(path, FILE_WRITE);
    if(!file){
        Serial.println("Failed to open file for writing");
        return;
    }
    if(file.print(message)){
        Serial.println("File written");
    } else {
        Serial.println("Write failed");
    }
    file.close();
}

void appendFile(fs::FS &fs, const char * path, const char * message){
    Serial.printf("Appending to file: %s\n", path);

    File file = fs.open(path, FILE_APPEND);
    if(!file){
        Serial.println("Failed to open file for appending");
        return;
    }
    if(file.print(message)){
        Serial.println("Message appended");
    } else {
        Serial.println("Append failed");
    }
    file.close();
}

void renameFile(fs::FS &fs, const char * path1, const char * path2){
    Serial.printf("Renaming file %s to %s\n", path1, path2);
    if (fs.rename(path1, path2)) {
        Serial.println("File renamed");
    } else {
        Serial.println("Rename failed");
    }
}

void deleteFile(fs::FS &fs, const char * path){
    Serial.printf("Deleting file: %s\n", path);
    if(fs.remove(path)){
        Serial.println("File deleted");
    } else {
        Serial.println("Delete failed");
    }
}

void testFileIO(fs::FS &fs, const char * path){
    File file = fs.open(path);
    static uint8_t buf[512];
    size_t len = 0;
    uint32_t start = millis();
    uint32_t end = start;
    if(file){
        len = file.size();
        size_t flen = len;
        start = millis();
        while(len){
            size_t toRead = len;
            if(toRead > 512){
                toRead = 512;
            }
            file.read(buf, toRead);
            len -= toRead;
        }
        end = millis() - start;
        Serial.printf("%u bytes read for %u ms\n", flen, end);
        file.close();
    } else {
        Serial.println("Failed to open file for reading");
    }


    file = fs.open(path, FILE_WRITE);
    if(!file){
        Serial.println("Failed to open file for writing");
        return;
    }

    size_t i;
    start = millis();
    for(i=0; i<2048; i++){
        file.write(buf, 512);
    }
    end = millis() - start;
    Serial.printf("%u bytes written for %u ms\n", 2048 * 512, end);
    file.close();
}

	#include <M5Core2.h>
	#include <esp_now.h>
	#include <WiFi.h>
	#include "SD.h"
	#include "FS.h"
	#include "SPI.h"
	esp_now_peer_info_t slave;
	File file;
	File f;
	File root;
	//--general parameters--
	int i,j,k;
	uint8_t f9pdata[172];
	uint8_t f9pdata_1[172];
	//------data value -------
	int itow,itow_1;//4byte
	int flags,flags_1;//1byte
	int numSV,numSV_1;//1byte
	int hacc,hacc_1;//2byte
	int fixtime,fixtime_1;//2byte
	//----SD parameters----
	int readflag;
	String stime;//filename
	uint8_t count;
	int epochN;
	//------------------------------
	//---------------------swipe--------------
	// Defines gestures
	Gesture swipeRight("swipe right", 160, DIR_RIGHT, 30, true);
	Gesture swipeDown("swipe down", 120, DIR_DOWN, 30, true);
	Gesture swipeLeft("swipe left", 160, DIR_LEFT, 30, true);
	Gesture swipeUp("swipe up", 120, DIR_UP, 30, true);
	String objNm;
	//----------------------------------------------------------------------------------------

	//--Sprite--
	TFT_eSprite tftSprite = TFT_eSprite(&M5.Lcd);
	//TFT_eSprite tftSprite = TFT_eSprite(&M5.Lcd);
	//TFT_eSprite DisIMUbuff = TFT_eSprite(&M5.Lcd);
	#define MAGENTA 0xF81F
	#define GREENYELLOW 0xAFE5 /* 173, 255, 47 */
	#define YELLOW 0xFFE0 /* 255, 255, 0 */
	#define WHITE 0xFFFF /* 255, 255, 255 */
	#define FORMAT_SPIFFS_IF_FAILED true
	uint16_t Col1,Col2;


	// 送信コールバック
	void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
	char macStr[18];
	snprintf(macStr, sizeof(macStr), "%02X:%02X:%02X:%02X:%02X:%02X",
	mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
	Serial.print("Last Packet Sent to: ");
	Serial.println(macStr);
	Serial.print("Last Packet Send Status: ");
	Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");

	// 画面にも描画

	Serial.print("Last Packet Sent to: \n ");
	Serial.println(macStr);
	Serial.print("Last Packet Send Status: \n ");
	Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
	}

	// 受信コールバック
	void OnDataRecv(const uint8_t mac_addr, const uint8_t data, int data_len) {
	char macStr[18];
	snprintf(macStr, sizeof(macStr), "%02X:%02X:%02X:%02X:%02X:%02X",
	mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
	//Serial.printf("Last Packet Recv from: %s\n", macStr);
	//Serial.printf("Last Packet Recv Data(%d): ", data_len);
	//LCD SET

	// M5.Lcd.fillRect(0,0,80,15,BLACK);
	// M5.Lcd.setTextSize(2);
	// M5.Lcd.setCursor(0,00);
	count=data_len;

	//Serial.print("f9pdata[]=");
	for ( int i = 0 ; i < data_len ; i++ ) {
	f9pdata[i]=data[i];
	//Serial.print(f9pdata[i]);
	//Serial.print(",");
	}
	//Serial.println(millis());

	itow=f9pdata[0]+f9pdata[1]256+f9pdata[2]256256+f9pdata[3]256256256;
	//Serial.printf("---------itow=%d,ESP-RCV=%dbyte,millis=%d\n\r",itow,data_len,millis());
	/*
	//******************temp.txt READ******************************
	stime="/tmp.txt";
	file = SD.open(stime);//read
	j=0;
	while(file.available()){
	f9pdata_1[j]=file.read();
	//Serial.print(f9pdata_1[j]);
	// Serial.print(",");
	j++;
	}
	file.close();
	//Serial.println(millis());
	itow_1=f9pdata_1[0]+f9pdata_1[1]256+f9pdata_1[2]256256+f9pdata_1[3]256256256;
	flags_1=f9pdata_1[4];
	numSV_1=f9pdata_1[5];
	hacc_1=f9pdata_1[6]+f9pdata_1[7]*256;
	fixtime_1=f9pdata_1[8]+f9pdata_1[9];

	//--------Start SD card
	stime="/tmp.txt";
	//file = SD.open(stime, FILE_APPEND);//追記
	file = SD.open(stime, FILE_WRITE);//上書き
	//file.println(str);
	file.write(data,data_len);
	file.close();
	//listDir(SD, "/", 0);
	//Serial.println("--------ESP-NOW Recived SD Write FINISHED----------------");
	*/
	flags=f9pdata[4];
	numSV=f9pdata[5];
	hacc=f9pdata[6]+f9pdata[7]*256;
	fixtime=f9pdata[8]+f9pdata[9]*256;
	double fixpercent=(double)(((double)f9pdata[10]+f9pdata[11]*256)/10);
	int epNo=f9pdata[12]+f9pdata[13]*256;
	Serial.printf("---epNo=%d---\n\r",epNo);
	int miscount1=f9pdata[14];
	double pDOP=((double)f9pdata[15]+f9pdata[16]*256)/100;

	if(epNo%8==0){
	//---------------現在相対座標-------------------------
	double rrN=by2Todbl(f9pdata[17],f9pdata[18])/100;
	//Serial.printf("epNo=%d,rrN=%3.2f,[17]=%x,[18]=%x\n\r",epNo,rrN,f9pdata[17],f9pdata[18]);
	double rrE=by2Todbl(f9pdata[19],f9pdata[20])/100;
	double rrD=by2Todbl(f9pdata[21],f9pdata[22])/100;
	double rrL=sqrt(pow(rrN,2)+pow(rrE,2)+pow(rrD,2));
	//----------------MovingBase Head-------------------------
	//double mHead=((double)f9pdata[23]+f9pdata[24]256+f9pdata[25]256256+f9pdata[26]256256256)*0.00001;
	double mHead=by4Todbl(f9pdata[23],f9pdata[24],f9pdata[25],f9pdata[26])*0.00001;
	//------------Calibration Average ----------------
	double rNaveN=by4Todbl(f9pdata[27],f9pdata[28],f9pdata[29],f9pdata[30])*0.01;
	double rEaveN=by4Todbl(f9pdata[31],f9pdata[32],f9pdata[33],f9pdata[34])*0.01;
	double rDaveN=by4Todbl(f9pdata[35],f9pdata[36],f9pdata[37],f9pdata[38])*0.01;
	double rLaveN=by4Todbl(f9pdata[39],f9pdata[40],f9pdata[41],f9pdata[42])*0.01;
	//------------Calibration standard Deviation-----------------------------
	double rNstd=by2Todbl(f9pdata[43],f9pdata[44])*0.01;
	double rEstd=by2Todbl(f9pdata[45],f9pdata[46])*0.01;
	double rDstd=by2Todbl(f9pdata[47],f9pdata[48])*0.01;
	double rLstd=by2Todbl(f9pdata[49],f9pdata[50])*0.01;

	//------------Calibration Counter-----------------------------
	int avecount=(int)f9pdata[51]+f9pdata[52]*256;

	Serial.printf("------------------------------itow=%d---------------------------------------\n\r",itow);
	Serial.printf("it=%d,flg=%d,nSV=%d,acc=%d,tim=%d,F%%=%3.1f,epN=%d,mis=%d,Dop=%2.2f\n\r",itow,flags,numSV,hacc,fixtime,fixpercent,epNo,miscount1,pDOP);
	Serial.printf("rrN=%5.2f,rrE=%5.2f,rrD=%5.2f,Hed=%3.5f,Nav=%5.2f,Eav=%5.2f,Dav=%5.2f,Lav=%5.2f\n\r",rrN,rrE,rrD,mHead,rNaveN,rEaveN,rDaveN,rLaveN);
	Serial.printf(",Nstd=%5.2f,Estd=%5.2f,Dstd=%5.2f,Lstd=%5.2f,avecount=%d \n\r",rNstd,rEstd,rDstd,rLstd,avecount);





	//=====Sprite draw====================
	int coff=0;
	tftSprite.fillRect(0,0,320,240,BLACK);
	tftSprite.setTextSize(2);
	Col1=0xF81F;//MAGENTA
	Col2=0xFFE0;//YELLOW
	tftSprite.setTextColor(Col2);//YELLOW Text
	tftSprite.setCursor(0,0);
	tftSprite.printf("%d:nSV/Dop=%d/%4.2f",itow,numSV,pDOP);//RTK Acc/Dop表示
	tftSprite.setCursor(0,20+coff);
	tftSprite.printf("flags-acc=%d-%dmm",flags,hacc);//
	tftSprite.setCursor(0,40+coff);
	tftSprite.printf("FixTm/%%=%dsec,%3.1f%%",fixtime,fixpercent);//fixtime/fixpercent
	tftSprite.setCursor(0,60+coff);
	tftSprite.printf("misN/epN=%d/%d",miscount1,epNo);//Misscount/epNo
	tftSprite.setCursor(0,80+coff);
	tftSprite.printf("BLen=%2.1fkm/%3.2fcm",rLaveN/100000,rLstd*3);//基線長＋３シグマ
	tftSprite.setCursor(0,100+coff);
	tftSprite.printf("Head=%3.5fdeg",mHead);//Heading
	tftSprite.setCursor(0,120+coff);
	tftSprite.printf("rN=% 3.2f/%3.2fcm",rrN,rNstd*3);//N相対座標＋３シグマ
	tftSprite.setCursor(0,140+coff);
	tftSprite.printf("rE=% 3.2f/%3.2fcm",rrE,rEstd*3);//E相対座標＋３シグマ
	tftSprite.setCursor(0,160+coff);
	tftSprite.printf("rD=% 3.2f/%3.2fcm",rrD,rDstd*3);//D相対座標＋３シグマ
	tftSprite.setCursor(0,180+coff);
	tftSprite.printf("rL=%5.2fcm",rrL);//基線長＋３シグマ

	tftSprite.pushSprite(0, 0); //PUSH SPLITE To LCD

	}//if itow%1000==500 end

	}
	// ===========BYTE TO DOUBLE=========================
	double by2Todbl(uint8_t l,uint8_t u){
	double result;
	if( u & 0x80){
	int i=(l+u *256)-65536;
	result=(double)i;
	}
	else{
	result=(double)(l+u*256);
	}
	return result;
	}
	double by4Todbl(uint8_t ll,uint8_t l,uint8_t u,uint8_t uu){
	double result;
	if( uu & 0x80){
	int i=(ll+l256+u256256+uu256256256)-4294967296;
	result=(double)i;
	}
	else{
	result=(double)(ll+l256+u256256+uu256256256);
	}
	return result;
	}


	void setup() {
	M5.begin();
	//---LCD SET UP---
	M5.Lcd.fillScreen(BLACK);
	M5.Lcd.setRotation(1); // tftSprite.print("ESP-NOW Test\n");
	M5.Lcd.print("----SetUP-----\n");
	tftSprite.createSprite(M5.Lcd.width(),210);// M5.Lcd.height());
	///---SD initialize------------
	// Start SD card
	if (!SD.begin())
	{
	// tftSprite.println("ERROR: SD CARD.");
	Serial.print("SD begin ERROR ");
	}
	stime="/ESPdata.txt";
	file = SD.open(stime, FILE_APPEND);
	String strTotal="1234567890";
	file.println(strTotal);
	file.close();
	//Serial.println(strTotal);
	//listDir(SD, "/", 0);
	Serial.print("ESP-NOW Test\n");
	//MAC address GET-----------
	uint8_t mac3[6];
	esp_read_mac(mac3, ESP_MAC_WIFI_STA);
	Serial.printf("[Wi-Fi Station] Mac Address = %02X:%02X:%02X:%02X:%02X:%02X\r\n", mac3[0], mac3[1], mac3[2], mac3[3], mac3[4], mac3[5]);
	// ESP-NOW初期化
	WiFi.mode(WIFI_STA);
	WiFi.disconnect();
	if (esp_now_init() == ESP_OK) {
	Serial.println("ESPNow Init Success");
	Serial.print("ESPNow Init Success\n");
	} else {
	Serial.println("ESPNow Init Failed");
	Serial.print("ESPNow Init Failed\n");
	ESP.restart();
	}

	// マルチキャスト用Slave登録
	memset(&slave, 0, sizeof(slave));
	for (int i = 0; i < 6; ++i) {
	slave.peer_addr[i] = (uint8_t)0xff;
	}
	esp_err_t addStatus = esp_now_add_peer(&slave);
	if (addStatus == ESP_OK) {
	// Pair success
	Serial.println("Pair success");
	}

	// ESP-NOWコールバック登録
	esp_now_register_send_cb(OnDataSent);
	esp_now_register_recv_cb(OnDataRecv);
	count=0;
	epochN=0;
	}
	//============LOOP========================================
	void loop() {
	M5.update();
	/*
	stime="/tmp.txt";
	//file = SD.open(stime, FILE_APPEND);//追記
	file = SD.open(stime);//read
	//String rstr="";
	//Serial.println();
	//Serial.printf("====Loop SD read:itow=");
	j=0;
	while(file.available()){
	f9pdata[j]=file.read();
	//Serial.print(f9pdata[j],HEX);
	j++;
	}
	delay(5);
	file.close();
	delay(5);
	itow=f9pdata[0]+f9pdata[1]256+f9pdata[2]256256+f9pdata[3]256256256;
	//Serial.printf("itow=%d:",itow);
	// epoch change
	if(itow-itow_1>=500 && j>0){
	epochN++;
	flags=f9pdata[4];
	numSV=f9pdata[5];
	hacc=f9pdata[6]+f9pdata[7]*256;
	fixtime=f9pdata[8]+f9pdata[9];
	//Serial.printf("epochN=%d:itow=%d,flags=%d,numSV=%d,hacc=%d,fixtime=%d,time=%d\n\r",epochN,itow,flags,numSV,hacc,fixtime,millis());
	//Serial.print(":f9pdata[]=");
	//for(k=0;k<j;k++){
	// Serial.print(f9pdata[k],HEX);
	// Serial.print(",");
	//}
	itow_1=itow;
	Serial.println();
	} //itow>itow_1 end


	//listDir(SD, "/", 0);

	// ボタンを押したら送信

	if ( M5.BtnA.wasPressed() ) {//For Atom HomeButton=GPIO39=> M5.BtnB.wasPressed() For M5StickC GPIO37=> M5.BtnA.wasPressed()
	//Core2 Send============MPC Slave AP Mac target:Core2======================
	slave.peer_addr[0] = (uint8_t)0xff;//0x24;//0xD8;//0x50;
	slave.peer_addr[1] = (uint8_t)0xff;//0x0A;//0xA0;//0x02;
	slave.peer_addr[2] = (uint8_t)0xff;//0xC4;//0x1D;//0x91;
	slave.peer_addr[3] = (uint8_t)0xff;//0xF8;//0x58;//0x87;
	slave.peer_addr[4] = (uint8_t)0xff;//0x5D;//0x47;//0x21;
	slave.peer_addr[5] = (uint8_t)0xff;//0x78;//0x30;//0x84;
	esp_err_t addStatus = esp_now_add_peer(&slave);//slave address set
	if (addStatus == ESP_OK) {
	// Pair success
	Serial.println("Pair success");
	}
	uint8_t data[2] = {123, 234};
	esp_err_t result = esp_now_send(slave.peer_addr, data, sizeof(data));
	Serial.print("Send Status: ");
	if (result == ESP_OK) {
	Serial.println("Success");
	} else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
	Serial.println("ESPNOW not Init.");
	} else if (result == ESP_ERR_ESPNOW_ARG) {
	Serial.println("Invalid Argument");
	} else if (result == ESP_ERR_ESPNOW_INTERNAL) {
	Serial.println("Internal Error");
	} else if (result == ESP_ERR_ESPNOW_NO_MEM) {
	Serial.println("ESP_ERR_ESPNOW_NO_MEM");
	} else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
	Serial.println("Peer not found.");
	} else {
	Serial.println("Not sure what happened");
	}
	}
	delay(1);

	*/
	}
	//===========================================================================================
	//===========LOOP END========================================================================
	//===========================================================================================
	/*
	void listDir(fs::FS &fs, const char * dirname, uint8_t levels){
	Serial.printf("Listing directory: %s\n", dirname);

	File root = fs.open(dirname);
	if(!root){
	Serial.println("Failed to open directory");
	return;
	}
	if(!root.isDirectory()){
	Serial.println("Not a directory");
	return;
	}

	File file = root.openNextFile();
	while(file){
	if(file.isDirectory()){
	Serial.print(" DIR : ");
	Serial.println(file.name());
	if(levels){
	listDir(fs, file.path(), levels -1);
	}
	} else {
	Serial.print(" FILE: ");
	Serial.print(file.name());
	Serial.print(" SIZE: ");
	Serial.println(file.size());
	}
	file = root.openNextFile();
	}
	}

	void createDir(fs::FS &fs, const char * path){
	Serial.printf("Creating Dir: %s\n", path);
	if(fs.mkdir(path)){
	Serial.println("Dir created");
	} else {
	Serial.println("mkdir failed");
	}
	}
	*/
	void removeDir(fs::FS &fs, const char * path){
	Serial.printf("Removing Dir: %s\n", path);
	if(fs.rmdir(path)){
	Serial.println("Dir removed");
	} else {
	Serial.println("rmdir failed");
	}
	}

	void readFile(fs::FS &fs, const char * path){
	Serial.printf("Reading file: %s\n", path);

	File file = fs.open(path);
	if(!file){
	Serial.println("Failed to open file for reading");
	return;
	}

	Serial.print("Read from file: ");
	while(file.available()){
	Serial.write(file.read());
	}
	file.close();
	}

	void writeFile(fs::FS &fs, const char * path, const char * message){
	Serial.printf("Writing file: %s\n", path);

	File file = fs.open(path, FILE_WRITE);
	if(!file){
	Serial.println("Failed to open file for writing");
	return;
	}
	if(file.print(message)){
	Serial.println("File written");
	} else {
	Serial.println("Write failed");
	}
	file.close();
	}

	void appendFile(fs::FS &fs, const char * path, const char * message){
	Serial.printf("Appending to file: %s\n", path);

	File file = fs.open(path, FILE_APPEND);
	if(!file){
	Serial.println("Failed to open file for appending");
	return;
	}
	if(file.print(message)){
	Serial.println("Message appended");
	} else {
	Serial.println("Append failed");
	}
	file.close();
	}

	void renameFile(fs::FS &fs, const char * path1, const char * path2){
	Serial.printf("Renaming file %s to %s\n", path1, path2);
	if (fs.rename(path1, path2)) {
	Serial.println("File renamed");
	} else {
	Serial.println("Rename failed");
	}
	}

	void deleteFile(fs::FS &fs, const char * path){
	Serial.printf("Deleting file: %s\n", path);
	if(fs.remove(path)){
	Serial.println("File deleted");
	} else {
	Serial.println("Delete failed");
	}
	}

	void testFileIO(fs::FS &fs, const char * path){
	File file = fs.open(path);
	static uint8_t buf[512];
	size_t len = 0;
	uint32_t start = millis();
	uint32_t end = start;
	if(file){
	len = file.size();
	size_t flen = len;
	start = millis();
	while(len){
	size_t toRead = len;
	if(toRead > 512){
	toRead = 512;
	}
	file.read(buf, toRead);
	len -= toRead;
	}
	end = millis() - start;
	Serial.printf("%u bytes read for %u ms\n", flen, end);
	file.close();
	} else {
	Serial.println("Failed to open file for reading");
	}


	file = fs.open(path, FILE_WRITE);
	if(!file){
	Serial.println("Failed to open file for writing");
	return;
	}

	size_t i;
	start = millis();
	for(i=0; i<2048; i++){
	file.write(buf, 512);
	}
	end = millis() - start;
	Serial.printf("%u bytes written for %u ms\n", 2048 * 512, end);
	file.close();
	}