ESP32 UDP Sender F9P-BNo085 data UDP Sending

WifiUDP_ESP32e_ATERM_BNOK_RELPOK_rev09.ino

//* Test sketch for Adafruit BNO08x sensor in UART-RVC mode */

//#include "Adafruit_BNO08x_RVC.h"

//Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC();
//--------------------------------Version histroy-------------------------------------------------------------
//rev01:イレギュラーデータを検出する機能、BNOのデータ配列を１２０個に拡張
//rev02:F9Pの受信データが細切れで頭がでたらめで入力される場合多発したので、ヘッダー検索してから読み込む機能追加2024/1/6
//-------------------------------------------------------------------------------------------------------------
#include <WiFi.h>
#include <WiFiUdp.h>


const char ssid[] = ""; // SSID
const char pass[] = "";  // password
//Wifi set up
static WiFiUDP wifiUdp; 
static const char *kRemoteIpadr = "192.168.179.4";//送信先PCのIPアドレス　同じポケットWiFiに接続のこと
static const int kRmoteUdpPort = 10000; //送信先のポート
static byte c[120];//12byte x10times  100msec(10msecx10data)
char b0,b1,b2;
static uint8_t f9pdata[172];
static uint8_t bnodata[172];
static int bnoitow[10];
static unsigned int itow,itow0;
uint8_t  head[4];
uint8_t  irgdata[100];
static int tp,sn,tpflag;
int i,n,m,ti,ti_1;
int tt,tin,t0,t1,t2,t3;

//===============================================
static void WiFi_setup()
{
  Serial.println("==========WiFi SetUp ========-");
  static const int kLocalPort = 5000;  //自身のポート
  WiFi.begin(ssid, pass);
  while( WiFi.status() != WL_CONNECTED) {
    delay(500);  
  } 
  Serial.print("myIP="); 
  Serial.println(WiFi.localIP());
  wifiUdp.begin(kLocalPort);
}
//=================================================
void setup() {
  // Wait for serial monitor to open
  Serial.begin(115200);
  Serial.println("Serial0 baud 115200");
  pinMode(4, INPUT_PULLUP);//TIME PULSE GPIO4
  attachInterrupt(4,tpin, RISING);
  WiFi_setup();
  //while (!Serial)
    //delay(10);

 // Serial.println("Adafruit BNO08x IMU - UART-RVC mode");
  Serial1.begin(115200,SERIAL_8N1,33,32);// ESP32  RX33白　 TX32緑　　F9P  RX緑　TX白　
  Serial2.begin(115200,SERIAL_8N1,16,17); // Default RX16,TX17 This is the baud rate specified by the datasheet
  //while (!Serial2)
    //delay(10);
    for(i=0;i<172;i++)
    {
      f9pdata[i]=0;
      bnodata[i]=0;
    }

}//set up end
//----------------------------
void tpin(){
   sn=0;//bnodata counter reset
   tp=millis();
   itow0=itow+100;
}
int  bin2int(byte c0,byte c1){
   int yawi=c0+c1*256;
      if(c1>=128){
        yawi=yawi-65535;
      }
      return yawi;
}
//----------------------------

void loop() {
  
  //ti=millis();
  //if(ti-ti_1>1000)
  //{
   // ti_1=ti;
    //Serial.printf("waitin F9P ti=%d\n\r",ti);
  //}
 //----F9P----
 if(Serial1.available())
  {
    if(Serial1.available()){
      tt=millis();
      head[0]=Serial1.read();
      head[1]=Serial1.read();
      head[2]=Serial1.read();
      head[3]=Serial1.read();
      if(head[0]==0xb5 && head[1]==0x62 && head[2]==0x01 && head[3]==0x07 )//PVT Header 4byte
      {
          tin=millis();
          t0=micros();
          f9pdata[0]=head[0];
          f9pdata[1]=head[1];
          f9pdata[2]=head[2];
          f9pdata[3]=head[3];
          i=4;
          while(i<172-4)
          {
              if(Serial1.available())
              {
                f9pdata[i]=Serial1.read();
                //Serial.print(f9pdata[i],HEX);
                //if(i==99)
                //{
                 // Serial.println();
                //}
                i++;
              }
          }
         
          t1=micros();
          //itow check
          itow=f9pdata[6]+f9pdata[7]*256+f9pdata[8]*65536+f9pdata[9]*16777216;
         
          n++;
         
          t2=micros();
          wifiUdp.beginPacket(kRemoteIpadr, kRmoteUdpPort);
          wifiUdp.write(f9pdata,172);  //送信される
          wifiUdp.endPacket(); 
          t3=micros();
          //Serial.println();
          //Serial.printf("----F9P epochN=%d,itowX=%x,%x,%x,%x,itow=%d,tt=%d,tin=%d,F9PRcvTim=%d,UDPSendTim=%d\n\r",n,f9pdata[6],f9pdata[7],f9pdata[8],f9pdata[9],itow,tt,tin,t1-t0,t3-t2);
      }
      
        // wifi udp send
      
  }
       
  }

 //----BNO085----
 //BNO085  RVC mode   AA,AA,index,yawL,yawH,pitchL,pitchH,rollL,rollH,accXL,accXH,accYL,accYH,accZL,accZH 15byte
 if(Serial2.available())
 {
   b0=Serial2.read();//header 0xaa
   b1=Serial2.read();//header 0xaa
   b2=Serial2.read();//Index 0-255
  int geta=sn*15;//0,15,30,45,60,75,90,105,120,135,
   //Serial.printf("%x,%x,%x,",b0,b1,b2);
    if(b0==0xaa && b1==0xaa){
      bnodata[0+geta]=b0;
      bnodata[1+geta]=b1;
      bnodata[2+geta]=b2;
      //Serial.printf("%d,",b2);
      for(i=3;i<15;i++){
       bnodata[i+geta]=Serial2.read();
       //Serial.print(c[i],HEX);
       //Serial.print(",");
      }
      bnoitow[sn]=itow0+(millis()-tp);//bnodata[]のitow値
     // c[0]=Serial2.read();
     // c[1]=Serial2.read();
      int yawi=bin2int(bnodata[geta+3],bnodata[geta+4]);
      int pitchi=bin2int(bnodata[geta+5],bnodata[geta+6]);
      int rolli=bin2int(bnodata[geta+7],bnodata[geta+8]);
      int ax=bin2int(bnodata[geta+9],bnodata[geta+10]);
      int ay=bin2int(bnodata[geta+11],bnodata[geta+12]);
      int az=bin2int(bnodata[geta+13],bnodata[geta+14]);
      sn++;//BNO サンプリングカウンタ
      sn=sn%10;
      double yawf=(double)yawi*0.01;
      double pitchf=(double)pitchi*0.01;
      double rollf=(double)rolli*0.01;
      double axf=(double)ax*0.01;
      double ayf=(double)ay*0.01;
      double azf=(double)az*0.01;
      //Serial.println();
      //Serial.printf("c[0]=%d,c[1]=%d,yawi=%d,yawf=%4.2f\n\r",c[0],c[1],yawi,yawf);
      //Serial.printf("%d,%d,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,tp=%d,bnoitow=%d\n\r",sn,bnodata[2+geta],yawf,pitchf,rollf,axf,ayf,azf,tp,bnoitow[sn]);
      if(sn%10==0)
      {
        //BNOitowを150-153に格納
        unsigned int data=bnoitow[0];
        bnodata[153] = (uint8_t)(data >> 24) & 0xFF;
        bnodata[152]  =(uint8_t)(data >> 16) & 0xFF;
        bnodata[151]  =(uint8_t)(data >> 8) & 0xFF;
        bnodata[150]  = (uint8_t)(data& 0xFF);
        //Serial.printf("sn0:bnoitowX=%x,%x,%x,%x\n\r",bnodata[150],bnodata[151],bnodata[152],bnodata[153]);//rev09_test1で追加
        //itow dataを最後尾168-171に格納
        bnodata[168]=f9pdata[6];//bntime[10]はつぶす
        bnodata[169]=f9pdata[7];//bntime[10]はつぶす
        bnodata[170]=f9pdata[8];
        bnodata[171]=f9pdata[9];
        //Serial.printf("sn0:UDP SEND TIMING sn=%d,itow=%d\n\r",sn,itow);//rev09_test2で追加
        wifiUdp.beginPacket(kRemoteIpadr, kRmoteUdpPort);
        wifiUdp.write(bnodata,172);  //送信される
        wifiUdp.endPacket(); 
       //XXXXXXXXXXXXXXXXXXXXXXここで遅延して１０％データ落ち発生XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
       // for(i=0;i<172;i++){//rev09_test3で追加 rev09_test4で削除
       //  Serial.print(bnodata[i],HEX);
        //}
        //XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
        //Serial.println();
        

      }
    }
   
  }
 }//loop end