M5Atom_RTCMsend_ESP-NOW_continuous_rev02.ino

M5Atom_RTCMsend_ESP-NOW_continuous_rev02.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;
int t, t_1,t_2;
int x = 1;
int i = 0;
int j, k;
int stime;
int waitFlag;
int rt1;
int bufdataN,bufdataN_1;
int frameNo;
char rtcmdata[600];//RTCM data
uint8_t framdata[250];//ESP-NOW data
int rtcm_end=0; 
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[0] == 0x14)
  {
    sendFlag = 1;
  }

  /*
    if (data[0] == 0x73 && data[1] == 0x74) //when data="ST" data_sending_STart
    {
               comst=1;
               stime=data[3]|data[4]<<8|data[5]<<16|data[6]<<24; //Left Shift  OR
               rt1=millis();
                //Serial.print("********stRcv=");
                //Serial.println(millis());
    }
    if (data[0] == 0x54 && data[1] == 0x52) //when data="TR" TimerReset
    {
         comtr=1;
    }
  */
  Serial.print( macStr);
  Serial.print(",");
  Serial.print(data_len);
  for ( int i = 0 ; i < data_len ; i++ ) {

    Serial.print(data[i]);
    Serial.print(" ");

  }
  Serial.println("");

}
//*************************************************************************************************
//+++++++++++++++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,32,26);//M5 Atom Groove(baudrate,config,yellow=TX26,white=RX32) M5StickC:yellow=32=RX, white=33=TX
  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();
  
  while (Serial2.available()>0)
     {
            char cdata=Serial2.read();
              rtcmdata[count%600]=cdata;
              Serial.print(cdata,HEX);
              count++;
              t_2=t_1;
              t_1=millis();//Received time
         }
    /*     
   //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("===========Recived 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");
    }
  }
}