ESP32 WiFi UDP GNSS RTK Ublox F9P +IMU BNO085

WifiUDP_ESP32_BNo085_SimpleRTK2B_rev03.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[] = "yourID"; // あなたのwifiルーターSSID
const char pass[] = "your pass";  // あなたのwifiルータの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[12];
uint8_t  head[4];
uint8_t  irgdata[100];
static int tp,sn;
int i,n,m;
int t0,t1,t2,t3;
long itow;
//===============================================
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);
  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
//----------------------------
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()){
      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
      {
          
          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++;
          sn=0;//bno data array counter reset
          t2=micros();
          wifiUdp.beginPacket(kRemoteIpadr, kRmoteUdpPort);
          wifiUdp.write(f9pdata,172);  //送信される
          wifiUdp.endPacket(); 
          t3=micros();
          Serial.println();
          Serial.printf("----F9P epochN=%d,head=%x,%x,itow=%d,time=%d,F9PRcvTim=%d,UDPSendTim=%d\n\r",n,f9pdata[0],f9pdata[1],itow,millis(),t1-t0,t3-t2);
      } 
        // wifi udp send
      
  }
       
  }

 //----BNO085----
 if(Serial2.available())
 {
   b0=Serial2.read();
   b1=Serial2.read();
   b2=Serial2.read();
    if(b0==0xaa && b1==0xaa){
      for(i=0;i<11;i++){
       c[i+sn]=Serial2.read();
       //Serial.print(c[i],HEX);
       //Serial.print(",");
      }
     // c[0]=Serial2.read();
     // c[1]=Serial2.read();
      int yawi=bin2int(c[sn+0],c[sn+1]);
      int pitchi=bin2int(c[sn+2],c[sn+3]);
      int rolli=bin2int(c[sn+4],c[sn+5]);
      int ax=bin2int(c[sn+6],c[sn+7]);
      int ay=bin2int(c[sn+8],c[sn+9]);
      int az=bin2int(c[sn+10],c[sn+11]);
      sn++;
      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,%d\n\r",sn,yawf,pitchf,rollf,axf,ayf,azf,millis());
      
    }
    /*
    if(sn==9)
    {
      wifiUdp.beginPacket(kRemoteIpadr, kRmoteUdpPort);
      wifiUdp.write(c,120);  //送信される
      wifiUdp.endPacket(); 
    }
    */
  }
 }//loop end