ESP32 UDP Log BNO085 & GNSS RTK F9P binary log

WifiUDP_ESP32_BNo085_SimpleRTK2B_rev05.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[] = "your ID"; // SSIDを入れて接続してください
const char pass[] = "your pass";  // passwordをいれてください
//Wifi set up
static WiFiUDP wifiUdp; 
static const char *kRemoteIpadr = "192.168.0.109";//送信先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;
int tt,tin,t0,t1,t2,t3;

//===============================================
static void 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(460800);
  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);

}//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() {
  //BNO08x_RVC_Data heading;

 // if (!rvc.read(&heading)) {
 //   return;
 // }
 //delay(20);
 //----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)
          {
              f9pdata[i]=Serial1.read();
              //Serial.print(f9pdata[i],HEX);
              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----
 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;
     
      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,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,tp=%d,bnoitow=%d\n\r",sn,yawf,pitchf,rollf,axf,ayf,azf,tp,bnoitow[sn]);
      if(sn%10==0)
      {
        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("bnoitowX=%x,%x,%x,%x\n\r",bnodata[150],bnodata[151],bnodata[152],bnodata[153]);
        //itow data
        bnodata[168]=f9pdata[6];//bntime[10]はつぶす
        bnodata[169]=f9pdata[7];//bntime[10]はつぶす
        bnodata[170]=f9pdata[8];
        bnodata[171]=f9pdata[9];
        //Serial.printf("UDP SEND TIMING sn=%d,itow=%d\n\r",sn,itow);
         wifiUdp.beginPacket(kRemoteIpadr, kRmoteUdpPort);
        wifiUdp.write(bnodata,172);  //送信される
        wifiUdp.endPacket(); 
       
        //for(i=0;i<172;i++){
        //  Serial.print(bnodata[i],HEX);
        //}
        //Serial.println();
        

      }
    }
   
  }
 }//loop end