M5Atom_RTCMRecieve_ESP-NOW_to_F9P_debug_rev03.ino

M5Atom_RTCMRecieve_ESP-NOW_to_F9P_debug_rev03.ino

//M5Atom-RTCM_MovingBaseSend_Serial2_ESP-NOW_Add_Serial1_rev01.ino
//#include <M5StickC.h>
#include <M5Atom.h>
//#include <BluetoothSerial.h>
#include <esp_now.h>
#include <WiFi.h>

int startPin = 19;
volatile int t, t_1,t_2,trcv;
int x = 1;
int i = 0;
int j, k;
int stime;
int waitFlag;
int rt1;
int bufdataN,bufdataN_1;
volatile int frameNo,frameNo_1;
volatile int fsum=0;
uint8_t rtcmdata[800];//RTCM data
volatile int rcount=0;
volatile int rcount_1;
volatile uint8_t framdata[6][250];//ESP-NOW data
volatile int framdataN[6];
volatile int rtcm_end=0; 
volatile int rcvFlag = 0;
int firstFlag;
int sendFlag = 0;
uint8_t  macTarget[6];
int count, ci ;
String sdata;
String pdata;//NAV-PVT data
//============================================================================================
//=========================ESP-NOW Functions==================================================
//==============slave:PC master:SCP,AD7194=================================================================
// マルチキャスト用 MAC登録
// PC  M5ATOM COM7=D8:A0:1D:50:54:E8
// SCP 1ch  M5Stick COM13=D8:A0:1D:58:56:CC
// AD7194  M5ATOM  COM9=24:0A:C4:A5:30:DC

esp_now_peer_info_t slave; //slave　peer定義
uint8_t  data[100];//12byte  4*value0,value1,value2,value3 //送信データ配列
char cdata[100];
//char  *cdata;//シリアル受信したstring　
//uint8_t  *data;//シリアル受信stiringを文字列配列にしてそれをuint8_t配列に変換する、ポインタで定義しておく　配列数が決まってからnewで再定義

int  dnum;//data配列の要素数
volatile int comst = 0;
volatile int comtr = 0;
// 送信コールバック*************************************************************************************
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");

}
//***********************************************************************************************

// 受信コールバック**************************************************************************
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]);

  macTarget[0] = mac_addr[0];
  macTarget[1] = mac_addr[1];
  macTarget[2] = mac_addr[2];
  macTarget[3] = mac_addr[3];
  macTarget[4] = mac_addr[4];
  macTarget[5] = mac_addr[5];
  rcvFlag = 1;
  //---------------------------------
  if (data_len<10)
  {
    for(i=0;i<data_len;i++)
    {
      Serial.print(data[i]);
    }
  }
  /*//debug
  Serial.println();
  Serial.print("RCV:mac_addr[5]=");
  Serial.print( mac_addr[5],HEX);
  Serial.print(",data_len=");
  Serial.println(data_len);
  */
  //---debug print----------------------
  fsum=fsum+data_len;
  trcv=millis();//received time
  Serial.println();
  Serial.print("frameNo,");
  Serial.print(frameNo);
  Serial.print(",data_len,");
  Serial.print(data_len);
  Serial.print(",frameSum,");
  Serial.print(fsum);
  Serial.print(",trcv,");
  Serial.println (trcv);
  //--------debug print end------------------
  for ( int i = 0 ; i < data_len ; i++ ) {
    framdata[frameNo%6][i]=data[i];
     Serial.print(data[i],HEX);
     Serial.print(",");
  }
  framdataN[frameNo%6]=data_len;
  /*
  Serial.println();
  Serial.print("RCV:frameNo=");
  Serial.print(frameNo);
  Serial.print("RCV:framdataN=");
  Serial.println(framdataN[frameNo%10]);
  */
  frameNo++;

}// dataRcv end
//*************************************************************************************************
//+++++++++++++++i_to_char+++++++++++++++++
// i=IntegerValueData,*d=Array pointer, n=Array start No
void  i_to_char(int i, uint8_t *d, int n)
{
  d[n] = i & 0x000000ff;
  d[n + 1] = (i & 0x0000ff00) >> 8;
  d[n + 2] = (i & 0x00ff0000) >> 16;
  d[n + 3] = (i & 0xff000000) >> 24;
}

//========================================================================

void setup() {
  M5.begin();
  //Serial2.begin(115200,SERIAL_8N1,22,33);//M5 Atom RX:G22 TX:G33 =>SimpleRTK2Blite Xbee  
  Serial2.begin(115200,SERIAL_8N1,26,32);//M5 Atom Groove  NA RX:G26 White TX:G32 =>SimpleRTK2Blite Xbee  
  t_1 = millis();
  //------------------------------------------------
  //--------- ESP-NOW初期化--------------------------
  //------------------------------------------------
  WiFi.mode(WIFI_STA);
  WiFi.disconnect();
  if (esp_now_init() == ESP_OK) {
    //Serial.println("ESPNow Init Success");

  } else {
    //Serial.println("ESPNow Init Failed");

    ESP.restart();
  }
  // マルチキャスト用Slave＜pnのM5ATOM＞ address 50:02:91:86:9B:CC
  memset(&slave, 0, sizeof(slave));
  //============MPC Slave AP Mac======================
  slave.peer_addr[0] = (uint8_t)0xff;
  slave.peer_addr[1] = (uint8_t)0xff;
  slave.peer_addr[2] = (uint8_t)0xff;
  slave.peer_addr[3] = (uint8_t)0xff;
  slave.peer_addr[4] = (uint8_t)0xff;
  slave.peer_addr[5] = (uint8_t)0xff;

  esp_err_t addStatus = esp_now_add_peer(&slave);


  // ESP-NOWコールバック登録
  esp_now_register_send_cb(OnDataSent);
  esp_now_register_recv_cb(OnDataRecv);

  //--------------------------------------------------
  //--------------------------------------------------
  firstFlag = 1;
  count=0;
}
void loop()
{
  M5.update();
        t = millis();
        rcount_1=rcount;
       // if (frameNo>frameNo_1)
       if(t-trcv>300 && frameNo > 0)//最後にRCVがあった時から300msec受信がなければ1パケット終了したと判断
        {
              frameNo_1=frameNo;
              Serial.println();
              Serial.print(",t-trcv>300:,");
              for (i=0;i<frameNo;i++)
              {
                 Serial.println();
                 Serial.print(",frameNo=,");
                 Serial.println(i);
                      for (j=0;j<framdataN[i];j++)
                      {
                          Serial.print(framdata[i][j],HEX);
                          Serial.print(",");
                          rtcmdata[rcount%800]=framdata[i][j];
                          rcount++; 
                      }
              }
                            // }
                            //RTCM  packet Recive finished  start to send F9P through Serial2
                            //if ( frameNo==frameNo_1 && rcount==rcount_1 && rcount>500 || t-t_1>800)
                            //{
              frameNo=0;
              frameNo_1=0;
              fsum=0;
              t_1=t;
              Serial.println();
              Serial.print("rcount,");
              Serial.print(rcount);
              Serial.println();
              for (k=0;k<rcount;k++)
              {
                    Serial2.write(rtcmdata[k%800]);
                    Serial.print(rtcmdata[k],HEX);
                    Serial.print(",");
              }
              Serial.println();
              rcount=0;
              rcount_1=0;
        } //if t-trcv>300 end
  
 

  
   
 
  
    /*     
   //debug
   Serial.println();
   Serial.print("t,t_1,t-t_1=");
   Serial.print(t);
   Serial.print(",");
   Serial.print(t_1);
   Serial.print(",");
   Serial.println(t-t_1);
   //debug
   */
     
        
          //RTCM data to packets  framdata[]   
    
       //Serial.print("data_len=");
       // Serial.println(count);
        //Serial.printf("===========Rcved Packet size count=%d,In Wait_time=%d\n\r",count,millis());
       /*  
      for(i=0;i<count;i++)
          {
                if(rtcmdata[i]==0xd3 && rtcmdata[i+1]==0x00  )
                {     
                 frameNo++;
                 Serial.printf(">>>>>>>>>>>count=%d,FrameNo=%d,Message length[%d] =%d\n\r",count,frameNo,frameNo,rtcmdata[i+2]);
                   for (k=0;k<rtcmdata[i+2]+6;k++)//header 6byte+ data 
                   {
                        framdata[k]=(uint8_t)rtcmdata[k+i];
                       //Serial.printf("frmdata[%d]=%x,rtcmfd[%d]=%x\n\r",k,frmdata[k],k+i,rtcmfd[k+i]);
                      Serial.printf("%x,",framdata[k]);
                       Serial2.write(framdata[k]);
                   }
                    //ESP-NOW send framdata[]<250
                    Serial.println();
                    Serial.print("Send data k=");
                    Serial.println(k);
                    esp_err_t result = esp_now_send(slave.peer_addr, framdata, k);  //ESP-NOW Binary Send
                   //Serial2.write(0x0d);// duplicated char
                   //Serial2.write(0x0a);// duplicated char
                   Serial.printf("\n\rFrameNo=%d,Framesize_k=%d,time=%d>>>>>>>>>>>>>\n\r",frameNo,k,millis());
                   
                }
                
          }
         frameNo=0;
         count=0;
          */
 
  // ボタンを押したら送信
  if ( M5.Btn.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");
    }
  }
}