M5StackBasic_SKI_F9P-MB_PC-Slave_PVTREL_rev02.ino

//M5StackBasic_SKI_F9P-MB_PC-Slave_PVTREL_rev02.ino
#include <M5Stack.h>

//#include <M5StickC.h>
#include <esp_now.h>
#include <WiFi.h>
#include "time.h"
#define GREENYELLOW         0xAFE5  
//===DEF:Receive data =======
int idata[172];//uint8_t=>int
//char hexdata[1000];//int =>HEX strings
String datahex[172];
String sdstr;//HEXStrings Array=>String
char buf[1000];
//===DEF:WiFi====================
const char* ssid       = "******";  
const char* password   = "XXXXXXX";

//===DEF:NTP Def====================
const char* ntpServer =  "ntp.jst.mfeed.ad.jp";
const long  gmtOffset_sec = 9 * 3600;
const int   daylightOffset_sec = 0;
static uint8_t conv2d(const char* p); // Forward declaration needed for IDE 1.6.x
uint8_t hh = conv2d(__TIME__), mm = conv2d(__TIME__ + 3), ss = conv2d(__TIME__ + 6); // Get H, M, S from compile time
byte xcolon = 0, xsecs = 0;
 
//===DEF:SD FILE============
File file;
char* fname;//保存するファイル名
String timeStr;  
int logFlag=0;

//===DEF:SD CARD WRITE===================
void writeData(char *paramStr) {
  // SDカードへの書き込み処理（ファイル追加モード）
  // SD.beginはM5.begin内で処理されているので不要
  file = SD.open(fname, FILE_APPEND);
  file.println(paramStr);
  //file.close();
}
//==============================================================
//==================UBX_DEC parameters==========================
int FIX;
String sFix;
int JST;
int SEC;
long relN;
long relE;
long relD;
long Length;
long Head;
long Long;
long Lati;
long Seah;
long Acc2d;
char numSV;//satellite number
String timstmp;//PVT MMDDHHMM
long iTow;
int iTows;
long su;
int sNo;//pvt start
int rNo;//relposned start
//===================================================================
esp_now_peer_info_t slave;

// 送信コールバック
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");
/*
  // 画面にも描画
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.print("Last Packet Sent to: \n  ");
  M5.Lcd.println(macStr);
  M5.Lcd.print("Last Packet Send Status: \n  ");
  M5.Lcd.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 Time=%d,Datalength=%d \n\r", millis(),data_len);
    M5.Lcd.setCursor(1, 50);
//==================MB Monitor ==============================
        sNo=0;//PVT start No
        // Serial.printf("sec 16=%d\n\r",data[sNo+16]);
        // SEC=(int)data[sNo+16];
        FIX=data[sNo+26];
        if (FIX==3){sFix="FIXED";}
        if (FIX==0){sFix="NO FIX";}
        Serial.printf("FIX=%d,sFix=%s\n\r",FIX,sFix);
        JST=(data[sNo+14]+9)%24;
        timstmp=String(data[sNo+12],DEC)+"-"+String(data[sNo+13],DEC)+"-"+String(JST,DEC)+":"+String(data[sNo+15],DEC);//MMDDHHMM 
        Long=B2L(data[sNo+33],data[sNo+32],data[sNo+31],data[sNo+30]);//Longtitude上位ー＞下位
        Lati=B2L(data[sNo+37],data[sNo+36],data[sNo+35],data[sNo+34]);//latitude 上位ー＞下位
        Seah=B2L(data[sNo+45],data[sNo+44],data[sNo+43],data[sNo+42]);//SeaHeight 上位ー＞下位
        Acc2d=B2L(data[sNo+49],data[sNo+48],data[sNo+47],data[sNo+46]);//Accuracy 2D 上位ー＞下位
        rNo=100;//RELPOSNED Start No
        relN=B2L(data[rNo+17],data[rNo+16],data[rNo+15],data[rNo+14]);
        relE=B2L(data[rNo+21],data[rNo+20],data[rNo+19],data[rNo+18]);
        relD=B2L(data[rNo+25],data[rNo+24],data[rNo+23],data[rNo+22]);
        Length=B2L(data[rNo+29],data[rNo+28],data[rNo+27],data[rNo+26]);
        Head=B2L(data[rNo+33],data[rNo+32],data[rNo+31],data[rNo+30]);
        numSV=data[sNo+29];
        //Serial.printf("timestmp=%s,Long=%d,Lati=%d,Seaheight=%d\n\r",timstmp,Long,Lati,Seah);
        //Serial.printf("Acc2d=%d,relN=%d,relE=%d,relD=%d,Length=%d,Head=%d,NumSV=%d\n\r",Acc2d,relN,relE,relD,Length,Head,numSV);
  //-----------------LCD Monitor---------------------------------------------------
      //M5.Lcd.setCursor(230, 200);//C Button
      //M5.Lcd.print("LOG");
     // M5.Lcd.setCursor(0, 200);//C Button
      //M5.Lcd.print("RESTART");
       M5.Lcd.setTextColor(GREENYELLOW);
      M5.Lcd.fillRect(1, 1, 320, 198, BLACK);
      M5.Lcd.setCursor(1, 1);//Cursol
      M5.Lcd.setTextSize(3);
      M5.Lcd.printf("[%s]:numSV=%d\n\r",sFix,numSV);
      M5.Lcd.printf("Acc=%dmm\n\r",Acc2d);
      M5.Lcd.printf("Len=%dcm\n\r",Length);
      M5.Lcd.printf("Hed=%4.1fdeg\n\r",(float)Head/100000);
      M5.Lcd.printf("N=%d,E=%d,D=%d\n\r",relN,relE,relD);
      M5.Lcd.setTextSize(2);
       M5.Lcd.setCursor(1, 140);//Cursol
      M5.Lcd.printf("Seah=%7.2f m\n\r",(float)Seah/1000);
      M5.Lcd.printf("Long=%10.7f deg\n\r",(float)Long*0.0000001);
      M5.Lcd.printf("Lati= %10.7f deg\n\r",(float)Lati*0.0000001);
      


   //---PC USB send---------------------------------------------------------------------------------------------    
      sdstr="";
     for ( int i = 0 ; i < data_len ; i++ ) {//-------------------------------------------
        datahex[i]=String(data[i],HEX);
        //sdstr=sdstr+","+datahex[i];  NG?
         if (logFlag==1)
      {
        sdstr=","+datahex[i];//OK 
        file.print(sdstr);
      }
        //Serial.printf("datahex[%d]=%s,data=%x,sdstr=%s\n\r",i,datahex[i],data[i],sdstr);
    //Serial.write(data[i]);
        Serial.print(data[i],HEX);
       if (i < data_len-1)
          {
                Serial.print(',');
          }
      //---172byte finished------------
       if((i+1) % 172 ==0)
       {
          Serial.println();
       
       }
   //Serial.printf("data[%d]=%x\n\r",i,data[i]);
  } //172 byte Ascii Transform--------------------------------------------------------
   //strhex=buf;
  // Serial.printf("strhex=%s\n\r",strhex);
  
  if (logFlag==1)
      {
        file.println();
      }
     
  //===data Log to SD Card=============================================  
     // file.println(sdStr);
}//End Data Rcv
//====4byte Binary to Long ==================
long B2L(uint8_t b4 , uint8_t b3 ,uint8_t b2 , uint8_t b1 ) {
    //pc.printf("B2L IN=%s,%x,%x,%x,%x,b4&0x80=%d\n\r",sen,b4,b3,b2,b1,b4 &0x80);
    //pc.printf("B2L IN=b4&0x80=%d\n\r",b4 & 0x80);
    
    if ((b4 & 0x80) && 0x80){//最上位ビットたっていればマイナス
        su = -(256-(int)b1)+(255-(int)b2)*256+(255-(int)b3)*65536+(255-(int)b4)*256*256*256;
        //pc.printf("B2L-:sen=%s,%d,%d,%d,%d,%d\n\r",sen,b4,b3,b2,b1,su);
    }
    else { 
     su=(int)b1+(int)b2*256+(int)b3* 65536+(int)b4*256*256*256;
     //pc.printf("B2L+:sen=%s,%d,%d,%d,%d,%d,%d\n\r",sen,b4,b3,b2,b1,su);
    }
    
     
    return su;
}   



//===FUNCTION:NTP GET==========================
void NTPget()
{
       M5.Lcd.setCursor(1, 1);
      M5.Lcd.print("***Set Pocket WIFI***");
      WiFi.begin(ssid, password);
      while (WiFi.status() != WL_CONNECTED) 
      {
              delay(500);
              Serial.print(".");
      }
      Serial.println("CONNECTED");

    //init and get the time
      configTime(gmtOffset_sec, daylightOffset_sec, ntpServer);
      struct tm timeinfo;
       if (!getLocalTime(&timeinfo))
       {
            Serial.println("Failed to obtain time");
            return;
      }
      
      hh = timeinfo.tm_hour;
      mm = timeinfo.tm_min;
      ss = timeinfo.tm_sec;
      timeStr="/"+String(timeinfo.tm_mon)+String(timeinfo.tm_mday)+"_"+String(hh)+"_"+String(mm)+"_"+String(ss)+".csv";
      
      Serial.println(timeStr);
       M5.Lcd.setCursor(1,20);
      M5.Lcd.print("LogFile=");
       M5.Lcd.print(timeStr+"\n");
      
      //disconnect WiFi as it's no longer needed
      WiFi.disconnect(true);
      WiFi.mode(WIFI_OFF);
      WiFi.disconnect();
      //delay(3000);
 
}

void setup() {
  M5.begin();
   pinMode(21, OUTPUT);// Green LED = Grove Yellow
 M5.Lcd.setTextSize(2);
 M5.Lcd.setTextColor(YELLOW);
 //NTP get for FileName---------------------
    NTPget();//NTP Access Time Stamp Get
   //*******************SD FILE OPEN *********************************************************
   file = SD.open(timeStr, FILE_APPEND);
  // ESP-NOW初期化
  WiFi.mode(WIFI_STA);
  WiFi.disconnect();
  if (esp_now_init() == ESP_OK) {
   Serial.println("ESPNow Init Success");
   M5.Lcd.print("ESPNow Init Success\n");
  } else {
    Serial.println("ESPNow Init Failed");
    M5.Lcd.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);
 
 M5.Lcd.setCursor(230, 200);//C Button
   M5.Lcd.print("LOG");
 M5.Lcd.setCursor(0, 200);//C Button
   M5.Lcd.print("RESTART");
}

void loop() {
  M5.update();
 //button C　LOG START/STOP Toggling
 if ( M5.BtnC.wasPressed() ) 
 {// buttonA=>M5StickC  buttonB=>M5Atom lite
    if (logFlag==0)
    {
      logFlag=1;//Logging Start
      Serial.printf("logFag=%d\n\r",logFlag);
      M5.Lcd.fillRect(50, 160, 220, 21, BLACK);
      M5.Lcd.setCursor(50, 160);//C Button
      M5.Lcd.setTextSize(3);
      M5.Lcd.print("---START---");
      digitalWrite(21, HIGH);
    }
    else if (logFlag==1)
    {
      logFlag=0;//LOgging Stop
      Serial.printf("logFag=%d\n\r",logFlag);
      M5.Lcd.setCursor(50, 160);//C Button
      M5.Lcd.setTextSize(3);
      //M5.Lcd.drawRect(50, 160,220, 21, WHITE);
      M5.Lcd.fillRect(50, 160, 220, 21, BLACK);
       M5.Lcd.setCursor(50, 160);//C Button
      M5.Lcd.print("---STOP---");
      digitalWrite(21, LOW);
      file.close();
    }
  
 }

 if ( M5.BtnA.wasPressed() ) //RESET 
 {
    ESP.restart();
 }
 /*
  // ボタンを押したら送信
  if ( M5.BtnB.wasPressed() ) {// buttonA=>M5StickC  buttonB=>M5Atom lite
    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);
  */
}// end loop
// =======================================================
//===Function to extract numbers from compile time string
//========================================================
static uint8_t conv2d(const char* p) {
  uint8_t v = 0;
  if ('0' <= *p && *p <= '9')
    v = *p - '0';
  return 10 * v + *++p - '0';
}