M5StickC as a SPP Wifi module for Teensy.1 RTK data to a Monitor with ESP-NOW

M5StickC-STA22_Teensy-Serial_ESP-NOW_toM5StickC_Reset_rev07.ino

//#include <M5Atom.h>
#include <M5StickC.h>
#include <BluetoothSerial.h>
#include <esp_now.h>
#include <WiFi.h>
int startPin=19;
int t,t_1;
int x=1;
int i=0;
int j,k;
int stime;
int rt1;
uint8_t  f9pHead[4];
uint8_t  f9pdata[172];
uint32_t itow;
uint8_t llitow,litow,uitow,uuitow;
int  rcvt,rcvt_1;               
int count,ci ;
String sdata;
volatile uint8_t iniflag;
uint8_t  com[20];
//============================================================================================
//=========================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]);

  Serial.print( macStr);
    Serial.print(",");
    Serial.println(data_len);
    for ( int i = 0 ; i < data_len ; i++ ) {
    Serial.print(data[i],HEX);
    Serial.print("-");
   }
   Serial.println("");

   if(data[0]=='R'){
      iniflag=1;
      //M5.Power.reset();
      //ESP.restart();
    }
}//Rcv 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();
  pinMode(startPin,OUTPUT);//startPin Start DataSending
  //Serial2.begin(115200,SERIAL_8N1,32,26);//*******M5 Atom lite Grove(baudrate,config,RX32,TX26)*******
  Serial2.begin(115200,SERIAL_8N1,33,32);//*******M5 StickC Grove(baudrate,config,RX32,TX26)*******
  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();
  }
//-----M5StickC --------------------------------------------------
         memset(&slave, 0, sizeof(slave));
        slave.peer_addr[0] = (uint8_t)0xD8;//0x24;//0x24;
        slave.peer_addr[1] = (uint8_t)0xA0;//0x0A;//0x0A;
        slave.peer_addr[2] = (uint8_t)0x1D;//0xC4;//0xC4;
        slave.peer_addr[3] = (uint8_t)0x58;//0xA5;//0xF8;
        slave.peer_addr[4] = (uint8_t)0x47;//0x30;//0x5D;
        slave.peer_addr[5] = (uint8_t)0x30;//0xDC;//0x78;
        esp_err_t addStatus = esp_now_add_peer(&slave);//slave address set
 //---=TEST SEND-----------------
        com[0]='T';
        com[1]='E';
        com[2]='S';
        com[3]='T';
    esp_err_t result = esp_now_send(slave.peer_addr, com, 4);
     Serial.print("Send Status: ");
    if (result == ESP_OK) {
      Serial.println("Success");
    } else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
      Serial.println("ESPNOW not Init.");
       ESP.restart();
    } else if (result == ESP_ERR_ESPNOW_ARG) {
      Serial.println("Invalid Argument");
       ESP.restart();
    } else if (result == ESP_ERR_ESPNOW_INTERNAL) {
      Serial.println("Internal Error");
       ESP.restart();
    } else if (result == ESP_ERR_ESPNOW_NO_MEM) {
      Serial.println("ESP_ERR_ESPNOW_NO_MEM");
       ESP.restart();
    } else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
      Serial.println("Peer not found.");
       ESP.restart();
    } else {
      Serial.println("Not sure what happened");
       ESP.restart();
    }
 
  // ESP-NOWコールバック登録 
  esp_now_register_send_cb(OnDataSent);
  esp_now_register_recv_cb(OnDataRecv);

  iniflag=0;// Input'R' from M5StickC then iniflag=1

//--------------------------------------------------
//--------------------------------------------------
 
}
void loop()
{
      
          t=millis();   
      //===============Serial2 REceiving================================== 
     if(Serial2.available() ){  
        while(Serial2.available()>0)//Binary recieving 
         {
           f9pdata[i]=Serial2.read();
           i++;
         }
        count=i;
        i=0;
     //--------sending ----------------------------------------------   
     
        esp_err_t result = esp_now_send(slave.peer_addr, f9pdata, count);
     
    Serial.print("Send Status: ");
    if (result == ESP_OK) {
      Serial.println("Success");
    } else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
      Serial.println("ESPNOW not Init.");
       //ESP.restart();
    } else if (result == ESP_ERR_ESPNOW_ARG) {
      Serial.println("Invalid Argument");
       //ESP.restart();
    } else if (result == ESP_ERR_ESPNOW_INTERNAL) {
      Serial.println("Internal Error");
       //ESP.restart();
    } else if (result == ESP_ERR_ESPNOW_NO_MEM) {
      Serial.println("ESP_ERR_ESPNOW_NO_MEM");
       //ESP.restart();
    } else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
      Serial.println("Peer not found.");
       //ESP.restart();
    } else {
      Serial.println("Not sure what happened");
       //ESP.restart();
    }
     
   }//if serial2.available()
    delay(10);     
}//loop end