Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/STA22_8Hz_USBHOST_Uart4_MPU6500x2_Kalman_Stable2_rev10391_6.ino Secret

Created May 31, 2022 06:01
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save dj1711572002/5bf724f4570cec5a9c84ced0be4991c0 to your computer and use it in GitHub Desktop.
Save dj1711572002/5bf724f4570cec5a9c84ced0be4991c0 to your computer and use it in GitHub Desktop.
Download ZIP
Teensy4.1_Arduino_F9P_MovingBase+MPU6500x2_Syncronized Logger
Raw
STA22_8Hz_USBHOST_Uart4_MPU6500x2_Kalman_Stable2_rev10391_6.ino
#include <Wire.h>
#include <SPI.h>
#include <MPU6500_WE.h>
#include <Kalman.h>//
//Kalman
//x, y, zの順
float acc[3],acc_1[3];
float accOffset[3],accOffset_1[3];
float gyro[3],gyro_1[3];
float gyroOffset[3],gyroOffset_1[3];
float kalAngleX,kalAngleX_1;
float kalAngleY,kalAngleY_1;
float kalAngleZ,kalAngleZ_1;
Kalman kalmanX;
Kalman kalmanY;
Kalman kalmanZ;
Kalman kalmanX_1;
Kalman kalmanY_1;
Kalman kalmanZ_1;
long lastMs = 0;
long tick = 0;
//------------MPU6500 lib----------------
#define MPU6500_ADDR 0x68
#define MPU6500_ADDR_1 0x69
MPU6500_WE myMPU6500 = MPU6500_WE(MPU6500_ADDR);
MPU6500_WE myMPU6500_1 = MPU6500_WE(MPU6500_ADDR_1);
#define PRINT_RAW
#define PRINT_SPEED 20//250 // 20 ms between prints
static unsigned long lastPrint = 0; // Keep track of print time
// STA22_USBHOSt_Dual_Buf172_sdFAT_rev098
//RTKMovingBaseモードでのデータログシステムPgm
// Base:F9P NAV-PVT+RELPOSNED出力
//Rover:F9P　 NAV-PVT+RELPOSNED出力
//USB HOST機能：USB HUB1,2とuserialb0-3の４個接続
//HOST IDGET
char baserover0="";
char baserover1="";
uint8_t buf[2048], buf1[200], buf2[200], buf3[200];
uint8_t b0ID[200];//usrialb0 uniqueID
uint8_t b1ID[200];//usrialb0 uniqueID
uint8_t b2ID[200];//usrialb0 uniqueID
uint8_t b3ID[200];//usrialb0 uniqueID
char* b0IDname;
char* b1IDname;
char* b2IDname;
char* b3IDname;
int epNo,epNo_1,ep0No, ep0No_1, ep1No, ep1No_1; //EpochNo
long itow0, itow0_1,itow0_2;
long itow1, itow1_1,itow1_2;
long length0,length1;
float ellapsedtime, ellapsedtime_1;
int b0byteN, b1byteN, b2byteN, b3byteN;
int b0IDflag, b1IDflag, b2IDflag, b3IDflag;
int Startflag = 0;
int Usbflag0 = 0;
int Usbflag1 = 0;
int IDpoll = 0;
volatile uint8_t b0Open, b1Open, b2Open, b3Open;
//F9P UniqueID
uint8_t Lbase[] = {0x2C, 0x48, 0x82, 0xA1 , 0x9F}; //F9P Left base
uint8_t Rrover[] = {0x7F, 0x4D, 0x02, 0xD7, 0xDB}; //F9H Right rover
uint8_t Rbase[] = {0xEB, 0x4D, 0x22, 0xD2, 0x9C}; //F9P Right base
uint8_t Lrover[] = {0x7F, 0x4D, 0x72, 0x13, 0x19}; //F9H Left rover
//------------------- Monitoring Prameters------------------------
int RTKstart,RTKfix,fixtime,fixitow;//flgs=1からflags=131までの時間
uint8_t stflag,fixflag,fixed;
uint8_t PVT0flag,RELPOS0flag,PVT1flag,RELPOS1flag;
int fixepoNo,allepoch,misepoch;//epochのカウント
int sa0,sa1,miscount,miscount0,miscount1,miscount00,miscount11;//epoch飛び itow0-itow0_1の差
double fixpercent;//Fix率
double rN[10],rE[10],rD[10],rL[10],mH[10];//基準位置average initial sample
double rNm[10],rEm[10],rDm[10],rLm[10],mmH[10];//MovingBase average initial sample
double relN0,relE0,relD0,relL0,mH0;//初回静止時の基準位置
volatile double relN,relE,relD,relL;//基準局からの現在位置
volatile double relNm,relEm,relDm,relLm,mHead;//MovingBase RELPOS値
double rrN,rrE,rrD,rrL;//基準位置平均値からの現在位置relN-rNaveN
double rNstd,rEstd,rDstd,rLstd,mHstd;//標準偏差 rNdvNの平方根
double rNmstd,rEmstd,rDmstd,rLmstd,mmHstd;//MovingBase標準偏差 rNdvNの平方根
double rNaveN,rEaveN,rDaveN,rLaveN,mHaveN;//基準位置平均値
double rNmaveN,rEmaveN,rDmaveN,rLmaveN,mmHaveN;//MovingBase位置平均値
double rNaveN1,rEaveN1,rDaveN1,rLaveN1,mHaveN1;//基準位置平均値1
double rNmaveN1,rEmaveN1,rDmaveN1,rLmaveN1,mmHaveN1;//MovingBase位置平均値1
double rNsum,rEsum,rDsum,rLsum,mHsum;//基準位置総和
double rNmsum,rEmsum,rDmsum,rLmsum,mmHsum;//MovingBase位置総和
double rNsgmN,rEsgmN,rDsgmN,rLsgmN,mHsfmN;//基準位置のstandard deviation N
double rNmsgmN,rEmsgmN,rDmsgmN,rLmsgmN,mmHsfmN;//Movingbaseのstandard deviation N
double rNsgmN1,rEsgmN1,rDsgmN1,rLsgmN1,mHsgmN1;//基準位置のstandard deviation N+1
double rNmsgmN1,rEmsgmN1,rDmsgmN1,rLmsgmN1,mmHsgmN1;//MovingBaseのstandard deviation N+1
double rNdvsum,rEdvsum,rDdvsum,rLdvsum,mHdvsum;//基準位置のdeviation sum
double rNmdvsum,rEmdvsum,rDmdvsum,rLmdvsum,mmHdvsum;//MovingBaseのdeviation sum
double rNdvN,rEdvN,rDdvN,rLdvN,mHdvN;//基準位置のdeviation N
double rNmdvN,rEmdvN,rDmdvN,rLmdvN,mmHdvN;//MovingBaseのdeviation N
double rNdvN1,rEdvN1,rDdvN1,rLdvN1,mHdvN1;//基準位置のdeviation N+1
double rNmdvN1,rEmdvN1,rDmdvN1,rLmdvN1,mmHdvN1;//MovingBaseのdeviation N+1
int avecount;
int mbcount;
uint8_t califlag;
//imu
String imus[100];
String imusum="";
//-----SD log parameters----------------------
//#include <SD.h>
#include "SdFat.h"
#include <SPI.h>
// Setup for built in SD card.
#define SPI_CLOCK SD_SCK_MHZ(50)
#define SD_CONFIG SdioConfig(FIFO_SDIO)
// SdFat-beta usage
SdFs sd;
FsFile myFile;//UBX_base pvt relposned
FsFile myFileR;//UBX_rover pvt relposned
FsFile myFileT;//TXT_rtk status statics
FsFile myFileI;//TXT_imu & pvt
int epNosd,epNosd_1;
int n;//counter
int i, j,jj, k,l,m;
int starttime;
// ============GNSS RTK top terms========================
static int itow, itow_1, itowR;
static int hacc, accL,numsv;
static uint8_t flags,mflags;
uint8_t flags0,flags0_1,flags1,flags1_1;
static uint8_t fix0,fix1;
static double pdop;
static int headmoti,gspeedi;
static double headmotd,gspeedd;
static int velN,velE,velD;
int JST_day;
//timestamp file name
char fnameB[30];//UBX Binary File name
char fnameR[30];//Rover Binary
char fnameV[30];//UBX Value File name NAV-PVT+NAV-RELPOSNED
char fnameI[30];//imu TXT
char monC[200];//Monitor Text data Save
char imuchr[200];
uint8_t Sflag = 0;
char sc;
int debugP = 1;
//const int chipSelect =BUILTIN_SDCARD;// 10;
//String dataString,dataString1;
int kaisu;
int tubx0,tubx1,ttxt0,ttxt1;
//-----USB HOST
#include "USBHost_t36.h"
#define USBBAUD 460800//115200 //460800
uint32_t baud = USBBAUD;
uint32_t format = USBHOST_SERIAL_8N1;
USBHost myusb;
USBHub hub1(myusb);
//USBHub hub2(myusb);
//USBHIDParser hid1(myusb);
//USBHIDParser hid2(myusb);
//USBHIDParser hid3(myusb);
//===normal buffer=====
USBSerial userialb0(myusb);
USBSerial userialb1(myusb);
//====Big Buffer=======
//USBSerial_BigBuffer userialb0(myusb);//NG
//USBSerial_BigBuffer userialb1(myusb);//NG
uint8_t dummy;
char data0, datab, datab2;
//USBDriver *drivers[] = {&hub1, &hub2, &hid1, &hid2, &hid3, &userialb0, &userialb1, &userialb2, &userialb3};
USBDriver *drivers[] = {&hub1, &userialb0, &userialb1};
#define CNT_DEVICES (sizeof(drivers)/sizeof(drivers[0]))
//const char * driver_names[CNT_DEVICES] = {"Hub1", "Hub2", "HID1", "HID2", "HID3", "USERIALB0", "USERIALB1", "USERIALB2", "USERIALB3" };
const char * driver_names[CNT_DEVICES] = {"Hub1","USERIALB0", "USERIALB1"};
//bool driver_active[CNT_DEVICES] = {false, false, false, false};
bool driver_active[CNT_DEVICES] = {false, false, false};
//Data
//Data
uint8_t baseNo;//USb 0 or 1
uint8_t Buf172[172];
uint8_t dBuf0[172], dBuf1[172],dBuf11[172];
int PVT1startNo;
uint8_t PVTdata[172];//userialb0 PVT
uint8_t RELPOSdata[172];//userialb0 RELPOS
uint8_t PVT1data[172];//userialb1 PVT
uint8_t RELPOS1data[172];//userialb1 RELPOS
long PVTval[33];
long PVT1val[33];
long RELPOSval[21];
long RELPOS1val[21];
//USB0-USB1=> base-rover
long PVTBval[33];
long PVTRval[33];
long RELPOSBval[21];
long RELPOSRval[21];
uint8_t RELPOSBdata[172];//base line RELPOS
volatile uint8_t RELPOS1Bdata[172];//base line RELPOS
uint8_t epflag0,epflag1;
uint8_t fdata[4];
int Num0, Num0_1, Num1, Num1_1;
int flag0, flag1;
char keyin;
//int time0, time0_1, time1, time1_1;
int trcv0,trcv1,tsdbin0,tsdbin1,tsdtxt0,tsdtxt1,timu0,timu1;
//==============time pulse interrupt================
volatile int tptime,tptime0,tptimerr;
volatile int tpitow0,tpitow;
volatile uint8_t tpflag;
volatile int tpcount;
char c;
volatile uint8_t itowinitflag=0;
//IMU
int dcount;
static uint8_t serialbuffer[172];
//=================================================================
//******************************************************************
void setup()
{
//Serial.begin(460800);
Serial.begin(921600);
Serial3.begin(115200);//Rx15-Tx14 M5Atom ESP-NOW
Serial4.begin(115200);//Rx16-Tx17 Rover uart1 ubx in high speed460800NG
Serial4.addMemoryForRead(serialbuffer, 172);
while (!Serial && (millis() < 5000)) ; // wait for Arduino Serial Monitor
Serial.println("\n\nUSB Host Testing - Serial");
myusb.begin();
Serial1.begin(115200); // We will echo stuff Through Serial1...
//-----------SD CARD Initialize------------------
if (!sd.begin(SD_CONFIG)) {
Serial.println("initialization failed!");
return;
}
Serial.println("initialization done.");
Sflag = 0;
Num0 = 0;
Num1 = 0;
b0IDflag = 0;
b1IDflag = 0;
Serial.println("Waiting SerialMonitor stanby");
delay(10000);//モニター立ち上げ時間待ち
ep0No = 0;
ep1No = 0;
itow0 = 1000;
itow0_1 = 1;
itow1 = 1000;
itow1_1 = 1;
sc=' ';
avecount=0;
starttime = millis();
rNsum=0;
rEsum=0;
rDsum=0;
rLsum=0;
mHsum=0;
califlag=0;//Calibration ON=1
PVT0flag=1;
RELPOS0flag=1;
PVT1flag=1;
RELPOS1flag=1;
dcount=0;
//TIMING Check Pin
pinMode(41,OUTPUT);//Userialb0 reading High
pinMode(39,OUTPUT);//Userialb1 reading High
pinMode(40,OUTPUT);//SD Binary File Writing High
//=================time pule interrupt pin=====================
pinMode(38,INPUT_PULLUP);//
attachInterrupt(digitalPinToInterrupt(38),timepulse,FALLING);
//==============================================================
//===================IMU MPU6500x2 setup=====================================
Wire.begin();
delay(100);
if(!myMPU6500.init()){
Serial.println("MPU6500 does not respond");
}
else{
Serial.println("MPU6500 is connected");
}
delay(100);
if(!myMPU6500_1.init()){
Serial.println("MPU6500_1 does not respond");
}
else{
Serial.println("MPU6500_1 is connected");
}
// MPU6500 parameter initial setup
MPU6500setup();
//--------------Kalman set up------------------
calibration();//IMU Kalman Calibration
}//*****set up end*************************************************
//=======================================================================================
//*****************************************************************
//**************TIME PULSE INTERRUPT*******************************
void timepulse(){
if(millis()-tptime>900){// TimePulse Noise よけ900msec周期より早ければノイズ
tptime=millis();
tpflag=1;
tpcount++;
lastPrint = millis(); // IMU timing Update lastPrint time
// Serial.println("*");
}
}
//********************************************************************
//********************************************************************
//----------------IMU Kalman functions -------------------
void readGyroAcc(){//gyro &acc read
//acc read acc.x acc.y acc.z
xyzFloat Acc = myMPU6500.getGValues();
xyzFloat Acc_1 = myMPU6500_1.getGValues();
//gyro read gyr.x gyr.y gyr.z
xyzFloat gyr = myMPU6500.getGyrValues();
xyzFloat gyr_1 = myMPU6500_1.getGyrValues();
acc[0]=Acc.x;
acc[1]=Acc.y;
acc[2]=Acc.z;
acc_1[0]=Acc_1.x;
acc_1[1]=Acc_1.y;
acc_1[2]=Acc_1.z;
gyro[0]=gyr.x;
gyro[1]=gyr.y;
gyro[2]=gyr.z;
gyro_1[0]=gyr_1.x;
gyro_1[1]=gyr_1.y;
gyro_1[2]=gyr_1.z;
}// readGyroAcc end
//---CalResult apply---------
void applyCalibration(){
gyro[0] -= gyroOffset[0];
gyro[1] -= gyroOffset[1];
gyro[2] -= gyroOffset[2];
gyro_1[0] -= gyroOffset_1[0];
gyro_1[1] -= gyroOffset_1[1];
gyro_1[2] -= gyroOffset_1[2];
acc[0] -= accOffset[0];
acc[1] -= accOffset[1];
acc[2] -= accOffset[2];
acc_1[0] -= accOffset_1[0];
acc_1[1] -= accOffset_1[1];
acc_1[2] -= accOffset_1[2];
}
//-----------roll pitch yaw-----
float getRoll(){
return atan2(acc[1], acc[2]) * RAD_TO_DEG;
}
float getRoll_1(){
return atan2(acc_1[1], acc_1[2]) * RAD_TO_DEG;
}
float getPitch(){
return atan(-acc[0] / sqrt(acc[1]*acc[1] + acc[2]*acc[2])) * RAD_TO_DEG;
}
float getPitch_1(){
return atan(-acc[0] / sqrt(acc_1[1]*acc_1[1] + acc_1[2]*acc_1[2])) * RAD_TO_DEG;
}
float getYaw(){
return atan(acc[1] / acc[0]) * RAD_TO_DEG;
}
float getYaw_1(){
return atan(acc_1[1] / acc_1[0]) * RAD_TO_DEG;
}
//--------------Kalman cal--------------
void calibration(){
//補正値を求める
float gyroSum[3],gyroSum_1[3];
float accSum[3],accSum_1[3];
int COUNTER = 500;
for(int i = 0; i < COUNTER; i++){
readGyroAcc();
gyroSum[0] += gyro[0];
gyroSum[1] += gyro[1];
gyroSum[2] += gyro[2];
gyroSum_1[0] += gyro_1[0];
gyroSum_1[1] += gyro_1[1];
gyroSum_1[2] += gyro_1[2];
accSum[0] += acc[0];
accSum[1] += acc[1];
accSum[2] += acc[2];
accSum_1[0] += acc_1[0];
accSum_1[1] += acc_1[1];
accSum_1[2] += acc_1[2];
delay(10);
}
gyroOffset[0] = gyroSum[0]/COUNTER;
gyroOffset[1] = gyroSum[1]/COUNTER;
gyroOffset[2] = gyroSum[2]/COUNTER;
gyroOffset_1[0] = gyroSum_1[0]/COUNTER;
gyroOffset_1[1] = gyroSum_1[1]/COUNTER;
gyroOffset_1[2] = gyroSum_1[2]/COUNTER;
accOffset[0] = accSum[0]/COUNTER;
accOffset[1] = accSum[1]/COUNTER;
accOffset[2] = accSum[2]/COUNTER - 1.0;//重力加速度1G
accOffset_1[0] = accSum_1[0]/COUNTER;
accOffset_1[1] = accSum_1[1]/COUNTER;
accOffset_1[2] = accSum_1[2]/COUNTER - 1.0;//重力加速度1G
Serial.println("-------------Calibration Results----------------------");
Serial.println("MPU6500 X Y Z\n");
Serial.printf("%7.2f %7.2f %7.2f\n", gyroOffset[0], gyroOffset[1], gyroOffset[2]);
Serial.printf("%7.2f %7.2f %7.2f\n", accOffset[0]*1000, accOffset[1]*1000, accOffset[2]*1000);
Serial.println("MPU6500_1 X Y Z\n");
Serial.printf("%7.2f %7.2f %7.2f\n", gyroOffset_1[0], gyroOffset_1[1], gyroOffset_1[2]);
Serial.printf("%7.2f %7.2f %7.2f\n", accOffset_1[0]*1000, accOffset_1[1]*1000, accOffset_1[2]*1000);
Serial.println("-------------------------------------------------------");
readGyroAcc();
kalmanX.setAngle(getRoll());
kalmanX_1.setAngle(getRoll_1());
kalmanY.setAngle(getPitch());
kalmanY_1.setAngle(getPitch_1());
kalAngleZ = 0;
kalAngleZ_1 = 0;
lastMs = micros();
}
//=======================================================================================================================================
//==========LOOP ========================================================================================================================
//=======================================================================================================================================
void loop() {
if(tpflag==1 ){//itow millis()初期値決定
if(itowinitflag==0){//itow initできるまで
tptime0=tptime;
if(itow0%1000==875 ){//itow0がxxxxxx875msec
tpitow0=int((itow0+125)/1000)*1000;
itowinitflag=1;
Serial.printf(">>> >>>>>>>>>>>>>>>>>>>>INITIAL TimePulse Interrupted tptime0=%d, itow0=%d\n\r",tptime0,tpitow0);
}
else if(itow0%1000==125){
tpitow0=int(itow0/1000)*1000;
itowinitflag=1;
Serial.printf(">>> >>>>>>>>>>>>>>>>>>>>INITIAL TimePulse Interrupted tptime0=%d, itow0=%d\n\r",tptime0,tpitow0);
}
else if(itow0%1000==0){//itow1が1000msecなら採用
tpitow0=itow0;
itowinitflag=1;
Serial.printf(">>> >>>>>>>>>>>>>>>>>>>>INITIAL TimePulse Interrupted tptime0=%d, itow0=%d\n\r",tptime0,tpitow0);
}
else{
itowinitflag=0;
}
}//itowinit==0のあいだ
if(itowinitflag==1 && tpcount>1){
tpitow=tpitow0+1000*(tpcount-1);//1000msec毎にインクリメント
tptimerr=tptime-(tptime0+1000*(tpcount-1));
Serial.printf(">>>> >>>>>>>>>>>>>>>>>>> TimePulse Interrupted tptime=%d,err=%d, tpitow=%d\n\r",tptime,tptimerr,tpitow);
}
}//tpflag==1 timepulse ありの場合
tpflag=0; //timepulse なしにセット
myusb.Task();
updateDeviceList();
//Serial.print(b0Open);
//}
//===useralb0 read=========================================================
//@@@@@@@@@@@@@重要　USBバッファが予定数になった順に処理していくと破綻しない@@@@@@@@@@@@@@@@@@@@@@@@@
// if(userialb0.available()>170 || userialb1.available()>170 ){ //Userib0とuserialb1のどちらかが満杯になったら先に処理する同時なら同時処理する。
// if(userialb0.available()>0 || userialb1.available()>0 ){ //Userib0とuserialb1のどちらかが満杯になったら先に処理する同時なら同時処理する。
if(userialb0.available()>171){// && Serial4.available()>171 ){ //バッファが満杯になるまで読まないで、一挙によむ
trcv0=millis();//debug
digitalWrite(41, HIGH); //
i=0;
while(i<172){
while(userialb0.available()){
dBuf0[i]=userialb0.read();
//Serial.print(dBuf0[i],HEX);
i++;
}
}
}
// delay(3);
//Serial.printf("Serial4.available=%d\n\r",Serial4.available());
if(Serial4.available()){
//Serial.println();
j=0;
while(j<172){
while(Serial4.available()){
dBuf1[j]=Serial4.read();
//Serial.print(dBuf1[j],HEX);
j++;
}
}
digitalWrite(41, LOW); //
// delay(3);
//Serial.println();
}//
//@@@@@@@@@@@@@@@@@@@@@@ READ END @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//userialb0 check print
if(dBuf0[0]==0xb5 && dBuf0[1]==0x62 && dBuf0[2]==0x01 && dBuf0[3]==0x07 ){//userialb0 PVT
flags0=dBuf0[27];
if(flags0==131 && fix0==0){
miscount0=0;
fix0=1;
}
itow0_1=itow0;
itow0 = B2L(dBuf0[9], dBuf0[8], dBuf0[7], dBuf0[6]); //dBuf0 iTow Calc
if(itow0-itow0_1>127){
miscount0++;
}
else{
ep0No++;
}
if(itow0>itow0_1 && flags0>0 && Sflag==0){ //<131
Serial.printf("[%c]U_0_PVT_i=,%d,[,%d,],mis0=,%d,(,%x,%x,%x,%x),%d,flags0=,%d\n\r",baserover0,i,itow0,miscount0,dBuf0[0],dBuf0[1],dBuf0[2],dBuf0[3],millis(),flags0);
}
}
else if(dBuf0[0]==0xb5 && dBuf0[1]==0x62 && dBuf0[2]==0x01 && dBuf0[3]==0x3C && itow0>itow0_1){
itow0_1=itow1;
itow0 = B2L(dBuf0[13], dBuf0[12],dBuf0[11], dBuf0[10]); //dBuf1 iTow Calc
if(itow0-itow0_1>127){
miscount0++;
}
else{
ep0No++;
}
if(itow0>itow0_1 && flags0>0 && Sflag==0){//<131 ){
Serial.printf("[%c]U_0_RELPOS_i=,%d,[,%d,],mis0=,%d,(%x,%x,%x,%x),%d\n\r",baserover0,i,itow0,miscount0,dBuf0[0],dBuf0[1],dBuf0[2],dBuf0[3],millis());
}
}// if RELPOS head
else{
//Serial.printf("U_0_NG_i=,%d,[,%d,],%x,%x,%x,%x\n\r",i,itow0,dBuf0[0],dBuf0[1],dBuf0[2],dBuf0[3]);
}
//length0 = B2L(RELPOSdata[29], RELPOSdata[28], RELPOSdata[27], RELPOSdata[26]);
//Serial.printf("========length0=%d\n\r",length0);
// }
//===Seria4 SORT========================================================
//Serial.println("-----------------------SORT dBuf1=>dBuf11-------------------------------------");
//Serial.print(" dBuf1:");
for(j=0;j<172;j++) //Search PVT Header------------------------------------------------------
{
if(dBuf1[j]==0xb5 && dBuf1[j+1]==0x62 && dBuf1[j+2]==0x01 && dBuf1[j+3]==0x07) //Serial4 PVT header
{
PVT1startNo=j;
}
//Serial.print(dBuf1[j],HEX);
}
//Serial.println();
//Sorting--------------------------------------------------------------------------------
for(jj=0;jj<172-PVT1startNo;jj++)//Sort dBuf1[PVT1startNo]～dBuf1[171]　To dBuf11[0]～dBuf11[171-PVT1startNo]
{
dBuf11[jj]=dBuf1[PVT1startNo+jj];
}
for(jj=0;jj<PVT1startNo;jj++)//Sort
{
dBuf11[jj+172-PVT1startNo]= dBuf1[jj];
}
//Serial.print("dBuf11:");
//for(j=0;j<172;j++)
//{
// Serial.print(dBuf11[j],HEX);
//}
//Serial.println();
if(dBuf11[0]==0xb5 && dBuf11[1]==0x62 && dBuf11[2]==0x01 && dBuf11[3]==0x07){//userialb1 PVT
flags1=dBuf11[27];
if(flags1==131 && fix1==0){
miscount1=0;
fix1=1;
}
itow1_1=itow1;
itow1 = B2L(dBuf11[9], dBuf11[8], dBuf11[7], dBuf11[6]); //PVT1 iTow Calc
if(itow1-itow1_1>127){
miscount1++;
}
else{
ep1No++;
}
if(itow1>itow1_1 && flags1>0 && Sflag==0){//<131){
Serial.printf("[%c]U_1_PVT_j=,%d,[,%d,],mis1=,%d,(,%x,%x,%x,%x),%d,flags1=,%d\n\r",baserover1,j,itow1,miscount1,dBuf11[0],dBuf11[1],dBuf11[2],dBuf11[3],millis(),flags1);
}
}//if PVT head
else if(dBuf1[0]==0xb5 && dBuf1[1]==0x62 && dBuf1[2]==0x01 && dBuf1[3]==0x3C ){
itow1_1=itow1;
itow1 = B2L(dBuf1[13], dBuf1[12],dBuf1[11], dBuf1[10]); //dBuf1 iTow Calc
if(itow1-itow1_1>127){
miscount1++;
}
else{
ep1No++;
}
if(itow1>itow1_1 && flags1>0 && Sflag==0){//<131){
Serial.printf("[%c]U_1_RELPOS_j=,%d,[,%d,],mis1=,%d,(%x,%x,%x,%x),%d\n\r",baserover1,j,itow1,miscount1,dBuf1[0],dBuf1[1],dBuf1[2],dBuf1[3],millis());
}
}// if RELPOS head
else{
//Serial.printf("U_1_NG_i=%d,[%d],%x,%x,%x,%x\n\r",i,itow1,dBuf1[0],dBuf1[1],dBuf1[2],dBuf1[3]);
epflag1=0;//繰り返し表示防止
}
//length1 = B2L(RELPOS1data[29], RELPOS1data[28], RELPOS1data[27], RELPOS1data[26]);
//Serial.printf("========length1=%d\n\r",length1);
//
//digitalWrite(39, LOW); //
trcv1=millis();//debug
//=====================================================================================================
//================================1周期処理===============================================================-
//====================================================================================================
//if(itow0-itow0_1==125 || itow1-itow1_1==125){
if(itow0-itow0_1>=123 ){//&& itow0==itow1){ //8Hz=125msecのばらつき123msec以上itow差で周期完了------------------------
digitalWrite(40, HIGH); // Epoch process IN
//if(itow0==10){ //stop
itow0_1=itow0;//重複プリント防止
itow1_1=itow1;
//Serial.printf("<userial.read Finished time= %d>\n\r",millis());
//=================================================================
// if(itow0>itow0_1){
//===============dBuf=>PVTdata,RELPOSdata 代入==========================
//Serial.print("USB0data:");
for(i=0;i<100;i++){//USB0 PVT
PVTdata[i]=dBuf0[i];
//Serial.print(PVTdata[i],HEX);
}
//Serial.print("-");
for(i=0;i<72;i++){
RELPOSdata[i]=dBuf0[i+100];
//Serial.print(RELPOSdata[i],HEX);
}
//Serial.println();
//Serial.print("USB1data:");
for(i=0;i<100;i++){//USB1
PVT1data[i]=dBuf11[i];
//Serial.print(PVT1data[i],HEX);
}
//Serial.print("-");
for(i=0;i<72;i++){
RELPOS1data[i]=dBuf11[i+100];
//Serial.print(RELPOS1data[i],HEX);
}
//Serial.println();
//}
// if(itow0==10){ //stop
//================================================================
//================================================================
//***********************************************************************************
//**********************125msec Process**********************************************
//***********************************************************************************
sa0=itow0-itow0_1;
sa1=itow1-itow1_1;
//---------Base Rover 判定 代入-------------------------------
baserover0='B';
baserover1='R';
// ======================バイナリーから実数データへConversion =====================================
int result=PVTcnv(PVTdata,PVTBval);
//Serial.printf("USB0-PVTcnv=%d,",result);
int result1=PVTcnv(PVT1data,PVTRval);
//Serial.printf("USB1-PVTcnv=%d,",result1);
int resultR=RELPOScnv(RELPOSdata,RELPOSBval);
//Serial.printf("USB0-RELPOScnv=%d,",resultR);
int resultR1=RELPOScnv(RELPOS1data,RELPOSRval);
//========================================================================
//Serial.printf("USB1-RELPOScnv=%d\n\r",resultR1);
// Serial.printf("----USB0 Length=RELPOSval[7]=%4.3f,USB1 Length=RELPOS1val[7]=%4.3f\n\r",(float)RELPOSval[7]/100,(float)RELPOS1val[7]/100);
////////////////////////////////////////////////////////////////////////////////
//--------- 基本パラメータ産出--------------------------------------
itow=PVTBval[0];
flags=PVTBval[11];
hacc=PVTBval[18];
pdop=PVTBval[27]/100;//((double)PVTdata[82] +PVTdata[83] * 256)/100;
numsv=PVTBval[13];//(int)PVTdata[29];
// ===============other parameters========================
headmoti=PVTval[24];//long
gspeedi=PVTval[23];//long
headmotd=(double)headmoti*0.00001;//deg 10e-5
gspeedd=(double)gspeedi*0.001;//m/sec
/////////////////////////////////////////////////////////////////////////////
//---In PVT-RELPOSBdata PRINT COPY --------------
// for(m=0;m<72;m++){
/// RELPOSBdata[m]=PVTdata[m+100];//PVTdata[100-171]=RELPOSBdata[0-71
// }
//------------------------------------------------------------------
//---------BaseLine Analysys---------------------------------------
//result=RELPOScnv(RELPOSBdata,RELPOSBval);
relN=(double)RELPOSBval[4]+(double)RELPOSBval[10]*0.01;
relE=(double)RELPOSBval[5]+(double)RELPOSBval[11]*0.01;
relD=(double)RELPOSBval[6]+(double)RELPOSBval[12]*0.01;
relL=(double)RELPOSBval[7]+(double)RELPOSBval[13]*0.01;
//--------------------MovingBase Rover Length RealTime averaging----------------------------------------------------
//---------MovingBase Rover RELPOSNED Analysys---------------------------
mHead=(double)RELPOSRval[8]*0.00001;//Heading deg x10-5
mflags=RELPOSRval[20];
//Serial.printf("mHead=%5.5f,RELPOSdata[8]=%d,mflags=%d\n\r",mHead,RELPOSval[8],mflags);
relNm=(double)RELPOSRval[4]+(double)RELPOSRval[10]*0.01;
relEm=(double)RELPOSRval[5]+(double)RELPOSRval[11]*0.01;
relDm=(double)RELPOSRval[6]+(double)RELPOSRval[12]*0.01;
relLm=(double)RELPOSRval[7]+(double)RELPOSRval[13]*0.01;//固定されているので一定値なのでLengthのばらつきを統計処理
//--------------------------------------------------------------------------------------------------------
//****************flags0 flags1　合算************************************************************************
if (flags0<flags1){
flags=flags0;
}
else{
flags=flags1;
}
//*****************FIX 判定*********************************************************
if(flags0==1 && stflag==0){
RTKstart=millis();
stflag=1;
}
else if(flags0==131 && flags1==131 && fixflag==0){//fixed
RTKfix=millis();
fixflag=1;
}
if(stflag==1 && fixflag==1 && fixed==0){
fixtime=int(((double)RTKfix-RTKstart)/1000);//sec
fixed=1;
fixepoNo=epNo;//FixしたときのEpochNo
fixitow=itow0;
miscount0=0;//Fix後のカウント値
miscount1=0;//Fix後のカウント値
}
miscount=max(miscount0,miscount1);
//fix率計算 allepoch,misepoch;/
if(fixed==1 && epNo>fixepoNo){
allepoch=epNo-fixepoNo;
if (flags<131){
misepoch++;
}
fixpercent=((double)(allepoch-misepoch)/allepoch)*100;
}
//---------------------------------------------------------------------
//---------------------------------------------------------------------
//CALIBRATION MODE================BaseLine Averaging Standard Deviation =================================================
//Serial.printf("if--califlag=%d\n\r",califlag);
if(fixflag==1 && califlag==0 ){
avecount++;
if(avecount<11){//[1-10] 10count
rN[avecount]=relN;
rNsum=rNsum+relN;
rE[avecount]=relE;
rEsum=rEsum+relE;
rD[avecount]=relD;
rDsum=rDsum+relD;
rL[avecount]=relL;
rLsum=rLsum+relL;
rLm[avecount]=relLm; //MovingBase length
rLmsum=rLmsum+relLm; //MovingBase length sum
//mH[avecount]=mHead;
//mHsum=mHsum+mHead;
}
if(avecount==10){//10count ave stdev
rNaveN=rNsum/10;
rEaveN=rEsum/10;
rDaveN=rDsum/10;
rLaveN=rLsum/10;
rLmaveN=rLmsum/10; //MovingBase Length ave
//mHaveN=mHsum/10;
int cn;
for(cn=1;cn<11;cn++){
rNdvsum=rNdvsum+pow((rN[cn]-rNaveN),2);
rEdvsum=rEdvsum+pow((rE[cn]-rEaveN),2);
rDdvsum=rDdvsum+pow((rD[cn]-rDaveN),2);
rLdvsum=rLdvsum+pow((rL[cn]-rLaveN),2);
rLmdvsum=rLmdvsum+pow((rLm[cn]-rLmaveN),2);//MovingBse length dev
//mHdvsum=mHdvsum+pow((mH[cn]-mHaveN),2);
}
rNdvN=rNdvsum/10;
rEdvN=rEdvsum/10;
rDdvN=rDdvsum/10;
rLdvN=rLdvsum/10;
rLmdvN=rLmdvsum/10;//MovingBase length devN
//mHdvN=mHdvsum/10;
}//if 10count averaging stdev end
//---N1 10-500count cal----
if(avecount >10 && avecount<301){
rNaveN1=((avecount-1)*rNaveN+relN)/(avecount);//avecount Nave
rEaveN1=((avecount-1)*rEaveN+relE)/(avecount);
rDaveN1=((avecount-1)*rDaveN+relD)/(avecount);
rLaveN1=((avecount-1)*rLaveN+relL)/(avecount);
rLmaveN1=((avecount-1)*rLmaveN+relLm)/(avecount);//MovingBase length aveN1
//mHaveN1=((avecount-1)*mHaveN+mHead)/(avecount);
rNdvN1=((avecount-1)*(rNdvN+pow(rNaveN,2))+pow(relN,2))/avecount-pow(rNaveN1,2);
rEdvN1=((avecount-1)*(rEdvN+pow(rEaveN,2))+pow(relE,2))/avecount-pow(rEaveN1,2);
rDdvN1=((avecount-1)*(rDdvN+pow(rDaveN,2))+pow(relD,2))/avecount-pow(rDaveN1,2);
rLdvN1=((avecount-1)*(rLdvN+pow(rLaveN,2))+pow(relL,2))/avecount-pow(rLaveN1,2);
rLmdvN1=((avecount-1)*(rLmdvN+pow(rLmaveN,2))+pow(relLm,2))/avecount-pow(rLmaveN1,2);//MovingBase length dvN1
//mHdvN1=((avecount-1)*(mHdvN+pow(mHaveN,2))+pow(mHead,2))/avecount-pow(mHaveN1,2);
//Serial.printf("AVE:flags=%d,hacc=%d,avcnt=%d,rNav=%4.2f,rNdv1=%4.2f,rEav=%4.2f,rEdv1=%4.2f,rDav=%4.2f,rDd1v=%4.2f,rLav=%4.2f,rLdv1=%4.2f,rLmav=%4.2f,rLmdv1=%4.2f\n\r",flags,hacc,avecount,rNaveN1,rNdvN1,rEaveN1,rEdvN1,rDaveN1,rDdvN1,rLaveN1,rLdvN1,rLmaveN1,rLmaveN1);
//Serial.printf("AVE=%d\n\r",avecount);
//---change next step---
rNaveN=rNaveN1;
rEaveN=rEaveN1;
rDaveN=rDaveN1;
rLaveN=rLaveN1;
rLmaveN=rLmaveN1;//MovingBase length aveN
//mHaveN=mHaveN1;
rNdvN=rNdvN1;
rEdvN=rEdvN1;
rDdvN=rDdvN1;
rLdvN=rLdvN1;
rLmdvN=rLmdvN1;//MovingBase length dvN
//mHdvN=mHdvN1;
//Serial.printf("avecount=%d:",avecount);
}//avecount 10-500
}// ave stdev end
//====SET Statistic Result rrN rrE rrD rrL =================
rrN=relN-rNaveN;
rrE=relE-rEaveN;
rrD=relD-rDaveN;
rrL=relL-rLaveN;
//standad deviation σ
rNstd=sqrt(rNdvN);
rEstd=sqrt(rEdvN);
rDstd=sqrt(rDdvN);
rLstd=sqrt(rLdvN);
rLmstd=sqrt(rLmdvN);//MovingBase length std
//mHstd=sqrt(mHdvN);
//Serial.printf("MON:relLm=%4.3f,rLmaveN1=%4.3f,rLmdvN1=%4.3f\n\r",relLm,rLmaveN1,rLmdvN1);
if(avecount>=300){//averaging result print
//Serial.printf("========avcnt=%d,rNav=%4.2f,rNstd=%4.2f,rEav=%4.2f,rEstd=%4.2f,rDav=%4.2f,rDstd=%4.2f,rLav=%4.2f,rLstd=%4.2f,mHead=%3.5f,rLmav=%4.2f,rLmdv=%4.2f\n\r",avecount,rNaveN,rNstd,rEaveN,rEstd,rDaveN,rDstd,rLaveN,rLstd,mHead,rLmaveN,rLmdvN);
Serial.printf("avcnt=%d,rLav=%4.2f,rLstd=%4.2f,mHead=%3.5f,rLmav=%4.2f,rLmdv=%4.2f\n\r",avecount,rLaveN,rLstd,mHead,rLmaveN,rLmdvN);
//--------SD WRITE Averaging Result & File parameter titles----------------------------
//SD wrinting averaging result text //rNav,rEav,rDqv,rLav,rNdv,rEdv,rDdv,rLdv,
String stc="rNav,rEav,rDqv,rLav,rNdv,rEdv,rDdv,rLdv,rLmav,rLmdv";
sprintf(monC,"%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f\n\r",rNaveN,rEaveN,rDaveN,rLaveN,rNdvN,rEdvN,rDdvN,rLdvN,rLmaveN,rLmdvN);
String stc1="epNo,itow,flags,numSV,hacc,fixtime,fixpercent,miscount0,miscount1,pDop,rrN,rrE,rrD,mHead,relN,relE,relD,relL";
myFileT = sd.open(fnameV, FILE_WRITE);
// myFileT.println(stc);
myFileT.write(monC,strlen(monC));
// myFileT.println(stc1);
myFileT.close(); //毎回クローズして万一のアクシデントに備える
delay(8);
//reset counter
avecount=0;//reset counter
califlag=1;//averaging Calculation finished
//Averaging sum RESET
rNsum=0;
rEsum=0;
rDsum=0;
rLsum=0;
rLmsum=0;//MovingBase length sum reset
rNdvsum=0;
rEdvsum=0;
rDdvsum=0;
rLdvsum=0;
rLmdvsum=0;//MovingBase length dv sum reset
}//Averaging End**************************************************************************************
//==========SD WRITING FileName set==========================================================================
//---FIlename()---
//if(Sflag==0 && millis()-starttime>20000){//startから20秒後にSDログ開始
if(califlag==1 && Sflag==0 ){//Calibration Fnished SD LOG START
Filename();
Sflag=1;//File Log OK
sc='*';
myFile = sd.open(fnameB, FILE_WRITE);
myFileR = sd.open(fnameR, FILE_WRITE);
myFileT = sd.open(fnameV, FILE_WRITE);
myFileI= sd.open(fnameI, FILE_WRITE);
epNosd=epNo;
}
//---Binary File SD WRITE---
if(Sflag==1){
tsdbin0=millis();//debug
//Serial.printf("Bin:Sflag=%d,epNo=%d,mils=%d\n\r",Sflag,epNo,millis());
//-FILE OPEN-
//tubx0=millis();
if(baseNo=0){//baseNo=0　USB0先--------------
//Open u0 as Base
digitalWrite(39, HIGH); //
//myFile = sd.open(fnameB, FILE_WRITE);
if (myFile) { //myFileチェックは、書き込み直前で行う。
for(i=0;i<100;i++) {
myFile.write(PVTdata[i]);
}
for(i=0;i<72;i++) {
myFile.write(RELPOSdata[i]);
}
}//fnameB end
//delay(8);
//if( epNo-epNosd>480 ){
// myFile.close();
// }
//Open u1 as Rover
//myFileR = sd.open(fnameR, FILE_WRITE);
if (myFileR) { //myFileチェックは、書き込み直前で行う。
for(i=0;i<100;i++) {
myFileR.write(PVT1data[i]);
}
for(i=0;i<72;i++) {
myFileR.write(RELPOS1data[i]);
}
}// if my file
}//baseNo=0 end
//-----------------------------------------
else {//baseNo=1 USB1先
//Open u0 as Base
digitalWrite(39, HIGH); //
//myFile = sd.open(fnameB, FILE_WRITE);
if (myFile) { //myFileチェックは、書き込み直前で行う。
for(i=0;i<100;i++) {
myFile.write(PVT1data[i]);
}
for(i=0;i<72;i++) {
myFile.write(RELPOS1data[i]);
}
}//if myFile end
//digitalWrite(39, LOW); //
//delay(8);
//myFile.close();
// myFileR = sd.open(fnameR, FILE_WRITE);
if (myFileR) { //myFileチェックは、書き込み直前で行う。
for(i=0;i<100;i++) {
myFileR.write(PVTdata[i]);
}
for(i=0;i<72;i++) {
myFileR.write(RELPOSdata[i]);
}
}//my file end
//delay(8);
//myFileR.close();
}//else end
tsdbin1=millis();//debug
if( epNo-epNosd>480 ){
Serial.printf("*******************************SD Close&Open epNo=%d,epNosd=%d\n\r",epNo,epNosd);
epNosd=epNo;
myFile.close();
//delay(8);
myFileR.close();
//delay(8);
myFileT.close();
//delay(8);
myFileI.close();
delay(8);
myFile = sd.open(fnameB, FILE_WRITE);
//delay(8);
myFileR = sd.open(fnameR, FILE_WRITE);
//delay(8);
myFileT = sd.open(fnameV, FILE_WRITE);
//delay(8);
myFileI = sd.open(fnameI, FILE_WRITE);
delay(8);
}
}//Sflag==1 end
epNo++;
int itow0cut=itow0%10000;
int itow1cut=itow1%10000;
String HMS0=itowToHMS(itow0);
int sa=abs(itow0-itow1);
//*********************PC MONITOR CORNER*************************************************************************************************
//フルデータプリント用
if(Sflag==1){
Serial.printf("epNo=%d,:it0=%4d,it1=%4d,flg=%d,acc=%d,pdop=%2.1f,nSV=%d,F%%=%3.1f,sec=%d,mis=%d,%d,rN=%4.2f,rE=%5.2f,rD=%5.2f,rL=%5.2f,Lav=%4.2f,Lstd=%2.2f\n\r",epNo,itow0cut,itow1cut,flags,hacc,pdop,numsv,fixpercent,fixtime,miscount0,miscount1,rrN,rrE,rrD,rrL,rLaveN,rLstd);
//Serial.printf("mHead=%3.5f,relNm=%3.3f,relEm=%3.3f,relDm=%3.3f,relLm=%3.3f,rLmav=%3.3f,rLmdv)
//Serial.printf("epNo=%d\n\r",epNo);//epNoのみ=%3.3f,%c,%d,sa=%d\n\r",mHead,relNm,relEm,relDm,relLm,rLmaveN1,rLmdvN1,sc,avecount,sa);
}
if(Sflag==0){
Serial.printf("itow=%d,epNo=%d,flags=%d,fixtime=%d,%c,%d,sa=%d\n\r",itow,epNo,flags,fixtime,sc,avecount,sa);//Timing 計測用
}
// Serial.printf("epNo=%d,:itow0=%d,itow1=%d,flags=%d,hacc=%d,Fix%%=%3.1f,fixsec=%d,mis0=%d,mi1=%d,rN=%7.2f,rE=%7.2f,rD=%7.2f,rL=%7.2f,%c\n\r",epNo,itow0cut,itow1cut,flags,hacc,fixpercent,fixtime,miscount0,miscount1,relN,relE,relD,relL,sc);
itow1_1=itow1;
//IMU 測定用　
if(Sflag==1){
Serial.printf("%d,%3.2f,%3.2f,%3.2f,%3.5f,%3.3f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f\n\r",itow,rrN,rrE,rrD,headmotd,gspeedd,relLm,mHead,relNm,relEm,relDm);
}
//*********************************************************************************
//******************MONITOR Data Sending SD print********************
//********************************************************************************
//digitalWrite(39, HIGH); //
//*********MovingBase Length ave dev　RealTime cal********************************
if(fixflag==1 && allepoch>0){
rLmaveN1=((allepoch-1)*rLmaveN+relLm)/(allepoch);//MovingBase length realTime ave
rLmdvN1=((allepoch-1)*(rLmdvN+pow(rLmaveN,2))+pow(relLm,2))/allepoch-pow(rLmaveN1,2);//MovingBase length dviatiion
//Serial.printf("*******allepoch=%d,MB length:relLm=%3.4f,rLmaveN1=%3.4f, rLmdvN1=%3.4f,ini10=[%3.4f,%4.3f]\n\r",allepoch, relLm,rLmaveN1, rLmdvN1,rLmaveN,rLmdvN);
}
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^Send Parameters^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
uint8_t llitow=PVTdata[6];//[1]itow 1byte (PVTdata[9], PVTdata[8], PVTdata[7], PVTdata[6]); //PVT iTow Calc
Serial3.write(llitow);//1
//Serial.print(llitow,HEX);
uint8_t litow=PVTdata[7];//[2]itow 2byte
Serial3.write(litow);//2
//Serial.print(litow,HEX);
uint8_t uitow=PVTdata[8];//[3]itow 3byte
Serial3.write(uitow);//3
// Serial.print(uitow,HEX);
uint8_t uuitow=PVTdata[9];//[4]itow 4byte
Serial3.write(uuitow); //4
//Serial.print(uuitow,HEX);
uint8_t uflags=flags;//PVTdata[27];//[5]1byte
Serial3.write(uflags);//5
//Serial.print(uflags,HEX);
uint8_t unumSV=PVTdata[29];//[6]2byte
Serial3.write(unumSV);//6 numSV
//Serial.print(unumSV,HEX);//numSV
//hAcc[46-49] u4 mm
uint8_t llhacc=PVTdata[46];//[7]3byte hacc　下下位バイト
Serial3.write(llhacc);//7
uint8_t lhacc=PVTdata[47];//[8]4byte hacc 下位バイト
Serial3.write(lhacc);//8
uint8_t lfixtime=fixtime%256;//[9]5byte fixtime下位バイト
Serial3.write(lfixtime);//9
uint8_t ufixtime=(uint8_t)(int(fixtime/256));//[10]6byte fixtime上位バイト
Serial3.write(ufixtime);//10
uint8_t lfixpercent=(uint8_t)(int(fixpercent*10)%256);//[11]7byte fixpercent*10 下位バイト
Serial3.write(lfixpercent);//11
uint8_t ufixpercent=(uint8_t)(int(fixpercent*10/256));//[12]８byte fixpercent*10 上位バイト
Serial3.write(ufixpercent);//12
uint8_t lepNo=epNo%256;//[13]
Serial3.write(lepNo);//13
uint8_t uepNo=(uint8_t)(int(epNo/256));//[14]
Serial3.write(uepNo);//14
uint8_t umiscount1=(uint8_t)miscount;//[15]
Serial3.write(umiscount1);//15
uint8_t lpDOP=PVTdata[82]; //[16] pDOPd[82] + d[83] * 256;
Serial3.write(lpDOP);//16
uint8_t upDOP=PVTdata[83];//[17]
Serial3.write(upDOP);//17
//現在相対座標値 long 4Byte
//rrN
uint8_t llrelN=(uint8_t)((int)(rrN*100) & 0xFF);//[18]0.00*100で整数
Serial3.write(llrelN);//18
uint8_t lrelN=(uint8_t)(((int)(rrN*100) & 0xFF00)>>8);//[19]0.00*100で整数
Serial3.write(lrelN);//19
uint8_t urelN=(uint8_t)(((int)(rrN*100) & 0xFF0000)>>16);//[20]0.00*100で整数
Serial3.write(urelN);//20
uint8_t uurelN=(uint8_t)(((int)(rrN*100) & 0xFF000000)>>24);//[21]0.00*100で整数
Serial3.write(uurelN);//21
//rrE
uint8_t llrelE=(uint8_t)((int)(rrE*100) & 0xFF);//[22]0.00*100で整数
Serial3.write(llrelE);//22
uint8_t lrelE=(uint8_t)(((int)(rrE*100) & 0xFF00)>>8);//[23]0.00*100で整数
Serial3.write(lrelE);//23
uint8_t urelE=(uint8_t)(((int)((rrE*100)) &0xFF0000)>>16);//[24]0.00*100で整数
Serial3.write(urelE);//24
uint8_t uurelE=(uint8_t)(((int)((rrE*100)) &0xFF000000)>>24);//[25]0.00*100で整数
Serial3.write(uurelE);//25
//rrD
uint8_t llrelD=(uint8_t)((int)(rrD*100)& 0xFF);//[26]0.00*100で整数
Serial3.write(llrelD);//26
uint8_t lrelD=(uint8_t)(((int)(rrD*100) & 0xFF00)>>8);//[27]0.00*100で整数
Serial3.write(lrelD);//27
uint8_t urelD=(uint8_t)(((int)(rrD*100) & 0xFF0000)>>16);//[28]0.00*100で整数
Serial3.write(urelD);//28
uint8_t uurelD=(uint8_t)(((int)(rrD*100) & 0xFF000000)>>24);//[29]0.00*100で整数
Serial3.write(uurelD);//29
//movingBase Head
uint8_t llHead=(uint8_t)(RELPOSRval[8]&0xFF);//[30] head
Serial3.write(llHead);//30
uint8_t lHead=(uint8_t)((RELPOSRval[8]&0xFF00)>>8);//[31] head
Serial3.write(lHead);//31
uint8_t uHead=(uint8_t)((RELPOSRval[8]&0xFF0000)>>16);//[32] head
Serial3.write(uHead);//32
uint8_t uuHead=(uint8_t)((RELPOSRval[8]&0xFF000000)>>24);//[33] head
Serial3.write(uuHead);//33
//-------MovingBase Len av dv-----------------------------------
//-------MB RELPOSNED monitor-----------------------
uint8_t llrNm=(uint8_t)(((int)(relNm*1000) & 0xFF));//[34]
Serial3.write(llrNm);//34
uint8_t lrNm=(uint8_t)(((int)(relNm*1000) & 0xFF00)>>8);//[35]
Serial3.write(lrNm);//35
uint8_t urNm=(uint8_t)(((int)(relNm*1000) & 0xFF0000)>>16);//[36]
Serial3.write(urNm);//36
uint8_t uurNm=(uint8_t)(((int)(relNm*1000) & 0xFF000000)>>24);//[37]
Serial3.write(uurNm);//37
//--
uint8_t llrEm=(uint8_t)(((int)(relEm*1000) & 0xFF));//[38]
Serial3.write(llrEm);//38
uint8_t lrEm=(uint8_t)(((int)(relEm*1000) & 0xFF00)>>8);//[39]
Serial3.write(lrEm);//39
uint8_t urEm=(uint8_t)(((int)(relEm*1000) & 0xFF0000)>>16);//[40]
Serial3.write(urEm);//40
uint8_t uurEm=(uint8_t)(((int)(relEm*1000) & 0xFF000000)>>24);//[41]
Serial3.write(uurEm);//41
//--
uint8_t llrDm=(uint8_t)(((int)(relDm*1000) & 0xFF));//[42]
Serial3.write(llrDm);//42
uint8_t lrDm=(uint8_t)(((int)(relDm*1000) & 0xFF00)>>8);//[43]
Serial3.write(lrDm);//43
uint8_t urDm=(uint8_t)(((int)(relDm*1000) & 0xFF0000)>>16);//[44]
Serial3.write(urDm);//44
uint8_t uurDm=(uint8_t)(((int)(relDm*1000) & 0xFF000000)>>24);//[45]
Serial3.write(uurDm);//45
//--
uint8_t llrLm=(uint8_t)(((int)(relLm*1000) & 0xFF));//[46]
Serial3.write(llrLm);//46
uint8_t lrLm=(uint8_t)(((int)(relLm*1000) & 0xFF00)>>8);//[47]
Serial3.write(lrLm);//47
uint8_t urLm=(uint8_t)(((int)(relLm*1000) & 0xFF0000)>>16);//[48]
Serial3.write(urLm);//48
uint8_t uurLm=(uint8_t)(((int)(relLm*1000) & 0xFF000000)>>24);//[49]
Serial3.write(uurLm);//49
//mon debug
// Serial.printf("*******rLm_%d_%d_%d_%d=%d,relLm=%3.3f*******\n\r",llrLm,lrLm,urLm,uurLm,(int)(relLm*1000),relLm);
//--
uint8_t llrLmav=(uint8_t)(((int)(rLmaveN1*1000) & 0xFF));//[50]
Serial3.write(llrLmav);//50
uint8_t lrLmav=(uint8_t)(((int)(rLmaveN1*1000) & 0xFF00)>>8);//[51]
Serial3.write(lrLmav);//51
uint8_t urLmav=(uint8_t)(((int)(rLmaveN1*1000) & 0xFF0000)>>16);//[52]
Serial3.write(urLmav);//52
uint8_t uurLmav=(uint8_t)(((int)(rLmaveN1*1000) & 0xFF000000)>>24);//[53]
Serial3.write(uurLmav);//53
//---
uint8_t llrLmdv=(uint8_t)(((int)(rLmdvN1*1000) & 0xFF));//[54]
Serial3.write(llrLmdv);//54
uint8_t lrLmdv=(uint8_t)(((int)(rLmdvN1*1000) & 0xFF00)>>8);//[55]
Serial3.write(lrLmdv);//55
uint8_t urLmdv=(uint8_t)(((int)(rLmdvN1*1000) & 0xFF0000)>>16);//[56]
Serial3.write(urLmdv);//56
uint8_t uurLmdv=(uint8_t)(((int)(rLmdvN1*1000) & 0xFF000000)>>24);//[57]
Serial3.write(uurLmdv);//57
//-------BaseLine Calibration AVERAGE STD------------------------------------
// Standard Deviations uint16_t/100
//rNstd
uint8_t lrNstd=(uint8_t)(((int)(rNstd*100) & 0xFF));//[58]
Serial3.write(lrNstd);//58
uint8_t urNstd=(uint8_t)(((int)(rNstd*100) & 0xFF00)>>8);//[59]
Serial3.write(urNstd);//59
//rEstd
uint8_t lrEstd=(uint8_t)(((int)(rEstd*100) & 0xFF));//[60]
Serial3.write(lrNstd);//60
uint8_t urEstd=(uint8_t)(((int)(rEstd*100) & 0xFF00)>>8);//[61]
Serial3.write(urEstd);//61
//rDstd
uint8_t lrDstd=(uint8_t)(((int)(rDstd*100) & 0xFF));//[62]
Serial3.write(lrDstd);//62
uint8_t urDstd=(uint8_t)(((int)(rDstd*100) & 0xFF00)>>8);//[63]
Serial3.write(urDstd);//63
//rLstd
uint8_t lrLstd=(uint8_t)(((int)(rLstd*100) & 0xFF));//[64]
Serial3.write(lrLstd);//64
uint8_t urLstd=(uint8_t)(((int)(rLstd*100) & 0xFF00)>>8);//[65]
Serial3.write(urLstd);//65
//avecount
uint8_t lavecount=(uint8_t)(avecount & 0xFF);//[66]
Serial3.write(lavecount);//66
uint8_t uavecount=(uint8_t)((avecount & 0xFF00)>>8);//[67]
Serial3.write(uavecount);//67
//samax
uint8_t lsa=(uint8_t)(sa & 0xFF);//[68]
Serial3.write(lsa);//68
uint8_t usa=(uint8_t)((sa & 0xFF00)>>8);//[69]
Serial3.write(usa);//69
//digitalWrite(39, LOW); //
//----------------------------------------------------------------------------
//epNo=%d,:it0=%4d,it1=%4d,flg=%d,acc=%d,pdop=%2.1f,nSV=%d,F%%=%3.1f,sec=%d,mis=%d,%d,rN=%4.2f,rE=%5.2f,rD=%5.2f,rL=%5.2f,Lav=%4.2f,Lstd=%2.2f,mHead=%3.5f,relN=%5.2f,relE=%5.2f,relD=%5.2f,relNm=%3.3f,relEm=%3.3,relDm=%3.3f,relLm=%3.3f,rLmav=%3.3f,rLmdv=%3.3f%c\n\r",epNo,itow0cut,itow1cut,flags,hacc,pdop,numsv,fixpercent,fixtime,miscount0,miscount1,rrN,rrE,rrD,rrL,rLaveN,rLstd,mHead,relN,relE,relD,relNm,relEm,relDm,relLm,rLmaveN1,rLmdvN1,sc);
sprintf(monC,"%d,%d,%d,%d,%d,%d,%d,%3.1f,%d,%d,%3.2f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f\n\r",itow,millis(),epNo,flags,unumSV,hacc,fixtime,fixpercent,miscount0,miscount1,pdop,rrN,rrE,rrD,mHead,relN,relE,relD,relL,relNm,relEm,relDm,relLm,rLmaveN1,rLmdvN1);
//******************************************************************************
//====SD WRITE TXT======
if(Sflag==1){
//myFileT = sd.open(fnameV, FILE_WRITE);
if(myFileT){
myFileT.write(monC,strlen(monC));
//myFileT.write(imuchr,strlen(imuchr));
// myFileT.println(imusum);
}
//myFileT.close();
//delay(8);
}
//ttxt1=millis();
//Serial.printf("=====SD Write TXT File ttxt0=%d,ttx1=%d TXTtime=%d\n\r",ttxt0,ttxt1,ttxt1-ttxt0);
//Serial.printf("=======lrNstd=%x,urNstd=%x,lrEstd=%x,urEstd=%x,lrDstd=%x,urDstd=%x,lrLstd=%x,urLstd=%x\n\r",lrNstd,urNstd,lrEstd,urEstd,lrDstd,urDstd,lrLstd,urLstd);
//tsdtxt1=millis();
//digitalWrite(39, LOW); //
//Serial.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n\r",llitow,litow,uitow,uuitow,uflags,unumSV,llhacc,lhacc,lfixtime,ufixtime,lfixpercent,ufixpercent);
//Serial.printf("uflags=%d,unumSV=%d,llhacc=%d,lhacc=%d,uhacc=%d,uuhacc=%d,time=%d\n\r",uflags,unumSV,llhacc,lhacc,uhacc,uuhacc,millis());
//*****************************************************************************************************************************************
//*****************************************************************************************************************************************
//*****************************************************************************************************************************************
//Serial.printf("<loop() end time=%d>\n\r",millis());
}// itow0-itow0_1==100 end
////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////IMU PRINT_SPEED=20msec period ///////////////////////////////////
if ((lastPrint + PRINT_SPEED) < millis() && Sflag==1)
{
timu0=millis();//debug
readGyroAcc();// gyro[3] acc[3] Get
applyCalibration();// offset adjust data apply
float dt = (micros() - lastMs) / 1000000.0;
lastMs = micros();
float roll = getRoll();
float pitch = getPitch();
float yaw = getYaw();
float roll_1 = getRoll_1();
float pitch_1 = getPitch_1();
float yaw_1 = getYaw_1();
//
kalAngleX = kalmanX.getAngle(roll, gyro[0], dt);
kalAngleX_1 = kalmanX_1.getAngle(roll_1, gyro_1[0], dt);
kalAngleY = kalmanY.getAngle(pitch, gyro[1], dt);
kalAngleY_1 = kalmanY_1.getAngle(pitch_1, gyro_1[1], dt);
kalAngleZ += gyro[2] * dt;
kalAngleZ_1 += gyro_1[2] * dt;
kalAngleZ = fmod(kalAngleZ, 360);
kalAngleZ_1 = fmod(kalAngleZ_1, 360);
// itow sync
int imuitow=millis()-tptime+tpitow;
//Serial.printf("[MPU_0]kx=%3.2f,ky=%3.2f,kz=%3.2f,roll=%3.2f,pitch=%3.2f,yaw=%3.2f,accX=%2.2f,accY=%2.2f,accZ=%2.2f,gx=%3.2f,gy=%3.2f,gz=%3.2f,t=%d,imitow=%d\n\r",kalAngleX,kalAngleY,kalAngleZ,roll,pitch,yaw,acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2],millis(),imuitow);
//Serial.printf("[MPU_1]kx=%3.2f,ky=%3.2f,kz=%3.2f,roll=%3.2f,pitch=%3.2f,yaw=%3.2f,accX=%2.2f,accY=%2.2f,accZ=%2.2f,gx=%3.2f,gy=%3.2f,gz=%3.2f,t=%d,imitow=%d\n\r",kalAngleX_1,kalAngleY_1,kalAngleZ_1,roll_1,pitch_1,yaw_1,acc_1[0],acc_1[1],acc_1[2],gyro_1[0],gyro_1[1],gyro_1[2],millis(),imuitow);
sprintf(imuchr,"%d,%d,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%2.3f,%2.3f,%2.3f,%3.2f,%3.2f,%3.2f\n\r",imuitow,millis(),kalAngleX,kalAngleY,kalAngleZ,roll,pitch,yaw,acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2]);
if(myFileI){
myFileI.write(imuchr,strlen(imuchr));
}
Serial.printf("[%d]kx=%3.2f,ky=%3.2f\n\r",imuitow,kalAngleX,kalAngleY);//,kalAngleZ,roll,pitch,yaw,acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2],millis());
//Serial.printf("[MPU_1]kx=%3.2f,ky=%3.2f\n\r",kalAngleX_1,kalAngleY_1);
lastPrint = millis(); // Update lastPrint time
timu1=millis();//debug
//Serial.printf("timu0=%d,timu1=%d millis=%d\n\r",timu0,timu1,millis());
}//IMU end////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
} //LOOP END
//*****************************************************************************************************************************
//*******************************loop Finished**********************************************************************
//*****************************************************************************************************************************
void Filename (){
//*********ファイル名をタイムスタンプで作ってファイルOPEN FIXしたら flags==131 *****************************************************************************
//if (PVTval[11] == 131 && Sflag == 0) { //start
Sflag = 1;
//---PVTval[1]=Year/[2]=Month/[3]=Day/[4]=Hour/[5]=Min/[6]=sec---
int JST = (PVTval[4] + 9) % 24; //UTC hourをJSTに変換
if (JST<9){//UTCが０時以前の場合日付を日本日付に修正
JST_day=1;
}
else{
JST_day=0;
}
String stime = String(PVTval[2], DEC) + "-" + String(PVTval[3]+JST_day, DEC) + "-" + String(JST, DEC) + "-" + String(PVTval[5], DEC); //MMDDHHMM
String stimeB = "/" + stime + "B.ubx"; //UBX Binary File
String stimeR ="/" +stime+"R.ubx";
String stimeV = "/" + stime + "T.txt"; //UBX Value Text File
String stimeI = "/" + stime + "I.txt"; //imu pvt Text File
int slenB = stimeB.length() + 1;
int slenR = stimeR.length() +1;
int slenV = stimeV.length() + 1;
int slenI = stimeI.length() + 1;
//ファイル名はchar配列なのでStringからchar配列変換　fname[]を得る
stimeB.toCharArray(fnameB, slenB); //stimeB to fnameB[] chara Array
stimeR.toCharArray(fnameR, slenR); //stimeB to fnameR[] chara Array
stimeV.toCharArray(fnameV, slenV); //stimeV to fnameV[] chara Array
stimeI.toCharArray(fnameI, slenV); //stimeI to fnameI[] chara Array
Serial.println();
Serial.print("fnameB=");
Serial.print(stimeB);
Serial.print("/fnameR=");
Serial.print(stimeR);
Serial.print("/fnameV=");
Serial.println(stimeV);
Serial.print("TimeStamp:Sflag=");
Serial.println(Sflag);
//myFile = sd.open(fnameB, FILE_WRITE);
//myFile.truncate(0);//Appned　
// }//timeStamp making end
}//Filename() end ******************************************************************************************************************************************
//**************************************SUB***************************************************
//NAV-SECを送って、固有IDを受信する**************************************************************
//余計なセンテンスが混じって受信された場合に備えbuf[2048]まで用意して、その中からNAV-SECの返信IDを探す
//*******************navSEC Polling read buffer and search SEC frame*************************
//********************************************************************************************
void navSEC() {
// B5 62 27 03 00 00 2A A5
uint8_t uniqID[8] = {0xB5, 0x62, 0x27, 0x03, 0x00, 0x00, 0x2A, 0xA5};
Serial.println("--navSEC---");
delay(1000);
//--------------------------b0ID get-----------------------------
if (b0IDflag == 0) {
for (i = 0; i < 8; i++) {
userialb0.write(uniqID[i]);//SEC Request
}
i = 0;
//Serial.println();
if (userialb0.available() > 15) { //SEC header search
while (userialb0.available() > 0) { //buffer read
buf[i % 2048] = userialb0.read();
//Serial.printf("buf[%d]=%x\n\r",i,buf[i]);
i++;
}
int bufsize = i - 1; //default SEC 17bytes
//Serial.printf("-------b0_bufsize=%d--------\n\r",bufsize);
for (i = 0; i < bufsize - 3; i++) {
if (buf[i] == 0xb5 && buf[i + 1] == 0x62 && buf[i + 2] == 0x27 && buf[i + 3] == 0x03) {
// b0ID[0]=buf[i+10];b0ID[1]=buf[i+11]; b0ID[2]=buf[i+12]; b0ID[3]=buf[i+13]; b0ID[4]=buf[i+14];
for (j = 0; j < 5; j++) {
b0ID[j] = buf[i + 10 + j]; //b0ID get
//Serial.print(b0ID[j],HEX);
//Serial.print(",");
}
if (b0ID[0] == 0x7F) {
b0IDname = "Rrover";
//Serial.printf("%s\n\r",b0IDname);
b0byteN = 72;
}
if (b0ID[0] == 0xEB) {
b0IDname = "Rbase";
//Serial.printf("%s\n\r",b0IDname);
b0byteN = 100;
}
b0IDflag = 1; //
Serial.printf("******Userialb0:%s:b0byteN=%d********\n\r", b0IDname, b0byteN);
break;
}
}
}
for (i = 0; i < 2048; i++) {
buf[i] = "";
}
}//b0ID flag end
//-----------------------------------------------------------------------------------------------------------
//--------------------------b1ID-----------------------------
if (b1IDflag == 0) {
for (i = 0; i < 8; i++) {
userialb1.write(uniqID[i]);
}
i = 0;
//Serial.println();
if (userialb1.available() > 15) { //SEC header search
while (userialb1.available() > 0) { //buffer read
buf[i % 2048] = userialb1.read();
//Serial.printf("buf[%d]=%x\n\r",i,buf[i]);
i++;
}
int bufsize = i - 1; //default SEC 17bytes
//Serial.printf("-------b1_bufsize=%d--------\n\r",bufsize);
for (i = 0; i < bufsize - 3; i++) {
if (buf[i] == 0xb5 && buf[i + 1] == 0x62 && buf[i + 2] == 0x27 && buf[i + 3] == 0x03) {
//Serial.printf("*******start i=%d:b1_Id=",i);
// b1ID[0]=buf[i+10];b1ID[1]=buf[i+11]; b1ID[2]=buf[i+12]; b1ID[3]=buf[i+13]; b1ID[4]=buf[i+14];
for (j = 0; j < 5; j++) {
b1ID[j] = buf[i + 10 + j];
//Serial.print(b1ID[j],HEX);
//Serial.print(",");
}
if (b0ID[0] == 0x7F) {
b1IDname = "Rrover";
//Serial.printf("%s\n\r",b1IDname);
b1byteN = 72;
}
if (b1ID[0] == 0xEB) {
b1IDname = "Rbase";
//Serial.printf("%s\n\r",b1IDname);
b1byteN = 100;
}
Serial.printf("******Userialb1:%s:b1byteN=%d********\n\r", b1IDname, b1byteN);
b1IDflag = 1;
break;
//Serial.println();
}
}
}
}//b1IDflag end
for (i = 0; i < 2048; i++) {
buf[i] = "";
}
/*
//-----------------------------------------------------------------------------------------------------------
//--------------------------b2ID-----------------------------
if(b2IDflag==0){
for (i=0;i<8;i++){
userialb2.write(uniqID[i]);
}
i=0;
//Serial.priintln();
if(userialb2.available()>15){//SEC header search
while(userialb2.available()>0){//buffer read
buf[i%2048]=userialb2.read();
//Serial.printf("buf[%d]=%x\n\r",i,buf[i]);
i++;
}
int bufsize=i-1;//default SEC 17bytes
//Serial.printf("-------b2_bufsize=%d--------\n\r",bufsize);
for(i=0;i<bufsize-3;i++){
if (buf[i]==0xb5 && buf[i+1]==0x62 && buf[i+2]==0x27 && buf[i+3]==0x03){
//Serial.printf("******Start i=%d:b2_Id=",i);
// b2ID[0]=buf[i+10];b2ID[1]=buf[i+11]; b2ID[2]=buf[i+12]; b2ID[3]=buf[i+13]; b2ID[4]=buf[i+14];
for (j=0;j<5;j++){
b2ID[j]=buf[i+10+j];
//Serial.print(b2ID[j],HEX);
//Serial.print(",");
}
b2IDflag=1;
//Serial.printf("b2IDflag=%d\n\r",b2IDflag);
break;
//Serial.println();
}
}
}
}//b2IDflag end
for(i=0;i<2048;i++){
buf[i]="";
}
//-----------------------------------------------------------------------------------------------------------
//--------------------------b3ID-----------------------------
if(b3IDflag==0){
for (i=0;i<8;i++){
userialb3.write(uniqID[i]);
}
i=0;
//Serial.println();
if(userialb3.available()>15){//SEC header search
while(userialb3.available()>0){//buffer read
buf[i%2048]=userialb3.read();
//Serial.printf("buf[%d]=%x\n\r",i,buf[i]);
i++;
}
int bufsize=i-1;//default SEC 17bytes
Serial.printf("-------b3_bufsize=%d--------\n\r",bufsize);
for(i=0;i<bufsize-3;i++){
if (buf[i]==0xb5 && buf[i+1]==0x62 && buf[i+2]==0x27 && buf[i+3]==0x03){
Serial.printf("start i=%d:b3_Id=",i);
// b3ID[0]=buf[i+10];b3ID[1]=buf[i+11]; b3ID[2]=buf[i+12]; b3ID[3]=buf[i+13]; b3ID[4]=buf[i+14];
for (j=0;j<5;j++){
b3ID[j]=buf[i+10+j];
Serial.print(b3ID[j],HEX);
Serial.print(",");
}
b3IDflag=1;
Serial.printf("b3IDflag=%d\n\r",b3IDflag);
break;
//Serial.println();
}
}
}
}//b3IDflag end
for(i=0;i<2048;i++){
buf[i]="";
}
*/
//-----------------------------------------------------------------------------------------------------------
}//navSEC end
//**********************************************************************************************
//**********************************************************************************************
void updateDeviceList() {
// Print out information about different devices.
for (uint8_t i = 0; i < CNT_DEVICES; i++) {
if (*drivers[i] != driver_active[i]) {
if (driver_active[i]) {
Serial.printf("*** Device %s - disconnected ***\n", driver_names[i]);
driver_active[i] = false;
} else {
Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
driver_active[i] = true;
const uint8_t *psz = drivers[i]->manufacturer();
if (psz && *psz) Serial.printf(" manufacturer: %s\n", psz);
psz = drivers[i]->product();
if (psz && *psz) Serial.printf(" product: %s\n", psz);
psz = drivers[i]->serialNumber();
if (psz && *psz) Serial.printf(" Serial: %s\n", psz);
// If this is a new Serial device.
if (drivers[i] == &userialb0) {
// Lets try first outputting something to our USerial to see if it will go out...
userialb0.begin(baud);
b0Open = 1;
}
}
if (drivers[i] == &userialb1) {
// Lets try first outputting something to our USerial to see if it will go out...
userialb1.begin(baud);
b1Open = 1;
}
/*
if (drivers[i] == &userialb2) {
// Lets try first outputting something to our USerial to see if it will go out...
userialb2.begin(baud);
b2Open = 1;
}
if (drivers[i] == &userialb3) {
// Lets try first outputting something to our USerial to see if it will go out...
userialb3.begin(baud);
b3Open = 1;
}
*/
}
}
}
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
int PVTcnv(uint8_t d[100], long pvt[33]) {
//PVT header[0-6]
//0:itow[6-9]
pvt[0] = B2L(d[9], d[8], d[7], d[6]);
//Serial.printf("PVTcnv:itow=%d\n\r",pvt[0]);
//1:year[10-12]
pvt[1] = (long)d[10] + d[11] * 256;
//2:month[12]
pvt[2] = (long)d[12];
//3:day[13]
pvt[3] = (long)d[13];
//4:hour[14]
pvt[4] = (long)d[14];
//5:min[15]
pvt[5] = (long)d[15];
//6:sec[16]
pvt[6] = (long)d[16];
//7:valid[17]
pvt[7] = (long)d[17];
//8:tAcc[18-21]
pvt[8] = B2L(d[21], d[20], d[19], d[18]);
//9:nano[22-25]
pvt[9] = B2L(d[25], d[24], d[23], d[22]);
//10:fixType[26]
pvt[10] =(long) d[26];
//11:flags[27] This is Fix status 131
pvt[11] = (long)d[27];
//12:flags2[28]
pvt[12] = (long)d[28];
//13:numSV[29]
pvt[13] = (long)d[29];
//14:lon[30-33]
pvt[14] = B2L(d[33], d[32], d[31], d[30]);
//15:lat[34-37]
pvt[15] = B2L(d[37], d[36], d[35], d[34]);
//16:height[38-41]
pvt[16] = B2L(d[41], d[40], d[39], d[38]);
//17:hMSL[42-45]
pvt[17] = B2L(d[45], d[44], d[43], d[42]);
//18:hAcc[46-49]
pvt[18] = B2L(d[49], d[48], d[47], d[46]);
//19:vAcc[50-53]
pvt[19] = B2L(d[53], d[52], d[51], d[50]);
//20:velN[54-57]
pvt[20] = B2L(d[57], d[56], d[55], d[54]);
//21:velE[58-61]
pvt[21] = B2L(d[61], d[60], d[59], d[58]);
//22:velD[62-65]
pvt[22] = B2L(d[65], d[64], d[63], d[62]);
//23:gSpeed[66-69]
pvt[23] = B2L(d[69], d[68], d[67], d[66]);
//24:headMot[70-73]
pvt[24] = B2L(d[73], d[72], d[71], d[70]);
//25:sAcc[74-77]
pvt[25] = B2L(d[77], d[76], d[75], d[74]);
//26:headAcc[78-81]
pvt[26] = B2L(d[81], d[80], d[79], d[78]);
//27:pDOP[82-83]
pvt[27] = (long)d[82] + d[83] * 256;
//28:flags3[84]
pvt[28] = (long)d[84];
//29:reserved1[85]
pvt[29] = (long)d[85];
//30:headVeh[86-89]
pvt[30] = B2L(d[89], d[88], d[87], d[86]);
//31:magDec[90-91]
pvt[31] = (long)d[90] + d[91] * 256;
//32:magAcc[92-93]
pvt[32] = (long)d[92] + d[93] * 256;
return (int)pvt[0];
}//PVTcnv() end
//--RELPOScnv---------------------------
int RELPOScnv(uint8_t d[72], long relpos[20]) {
//RELPOS header[0-5]
int s = 0;
//0:ver[6]
relpos[0] = (long)d[s + 6];
//1:reserved1[7]
relpos[1] =(long) d[s + 7];
//2:refStationId[8-9]
relpos[2] = (long)d[s + 8];
//3:itow[10-13]
relpos[3] = (long)B2L(d[s + 13], d[s + 12], d[s + 11], d[s + 10]);
//Serial.printf("RELPOScnv:itow=%d\n\r",relpos[3]);
//4:relposN[14-17]
relpos[4] = B2L(d[s + 17], d[s + 16], d[s + 15], d[s + 14]);
//5:relposE[18-21]
relpos[5] = B2L(d[s + 21], d[s + 20], d[s + 19], d[s + 18]);
//6:relposD[22-25]
relpos[6] = B2L(d[s + 25], d[s + 24], d[s + 23], d[s + 22]);
//7:relposLength[26-29]
relpos[7] = B2L(d[s + 29], d[s + 28], d[s + 27], d[s + 26]);
//Serial.printf("RELPOScnv:Lenghth=relpos[7]=%4.3f\n\r",relpos[7]);
//8:relposHeading[30-33]
relpos[8] = B2L(d[s + 33], d[s + 32], d[s + 31], d[s + 30]);
//Serial.printf("relposHeading[8]=%d,[33]%x,[32]%x,[31]%x,[30]%x,\n\r",relpos[8],d[33],d[32],d[31],d[30]);
//9:reserved2[34]
relpos[9] = B2L(d[s + 37], d[s + 36], d[s + 35], d[s + 34]);
//10:relposHPN[35]
relpos[10] =(long)(d[s+ 38] & 127 - (d[s+38]) & 128);
//Serial.printf("HPN=%d,d[38]=%x,d[39]=%x,d[40]=%x,d[41]=%x,&127=%dx,&128=%d\n\r",relpos[10],d[38],d[39],d[40],d[41],d[38] && 127,d[38] && 128);
//11:relposHPE[36]
relpos[11] = (long)((d[s + 39] & 127)-(d[s+39] & 128));
//12:relposHPD[37]
relpos[12] = (long)((d[s + 40] & 127)-(d[s+40] & 128));
//13:relposHPLength[38]
relpos[13] = (long)((d[s + 41] & 127)-(d[s+41] & 128));
//14:accN[38-41]
relpos[14] = B2L(d[s + 41], d[s + 40], d[s + 39], d[s + 38]);
//15:accE[42-45]
relpos[15] = B2L(d[s + 45], d[s + 44], d[s + 43], d[s + 42]);
//16:accD[46-49]
relpos[16] = B2L(d[s + 49], d[s + 48], d[s + 47], d[s + 46]);
//17:accLength[50-53]
relpos[17] = B2L(d[s + 53], d[s + 52], d[s + 51], d[s + 50]);
//18:accHeading[54-57]
relpos[18] = B2L(d[s + 57], d[s + 56], d[s + 55], d[s + 54]);
//19:reserved[57-60]
relpos[19] = B2L(d[s + 60], d[s + 59], d[s + 58], d[s + 57]);
//20:flags[60-63]
relpos[20] = B2L(d[s + 63], d[s + 62], d[s + 61], d[s + 60]);
return (int)relpos[3];
}
//+++++++++++++4byte Binary to Long ++++++++++++++++++++++++++++++++++++++++++++++
long B2L(uint8_t b4 , uint8_t b3 , uint8_t b2 , uint8_t b1 ) {
//pc.printf("B2L IN=%s,%x,%x,%x,%x,b4&0x80=%d\n\r",sen,b4,b3,b2,b1,b4 &0x80);
//pc.printf("B2L IN=b4&0x80=%d\n\r",b4 & 0x80);
long su;
if (b4 & 0x80) { //最上位ビットたっていればマイナス
//su = -(256 - (long)b1) + (255 - (long)b2) * 256 + (255 - (long)b3) * 65536 + (255 - (long)b4) * 256 * 256 * 256;
//pc.printf("B2L-:sen=%s,%d,%d,%d,%d,%d\n\r",sen,b4,b3,b2,b1,su);
uint32_t i1=b1;
uint32_t i2=b2<<8;
uint32_t i3=b3<<16;
uint32_t i4=b4<<24;
uint32_t i5=i1 | i2 | i3| i4;
su=(long)i5;
}
else {
su = (long)b1 + (long)b2 * 256 + (long)b3 * 65536 + (long)b4 * 256 * 256 * 256;
//pc.printf("B2L+:sen=%s,%d,%d,%d,%d,%d,%d\n\r",sen,b4,b3,b2,b1,su);
}
return su;
}
//=================================================================================
//+++++++++++++++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;
}
//++++++++++++++++String to CharArray Trans++++++++++++++++++++
void str2char(char c[], String dataS)
{
//String dataS;
//dataS="HELLO dataS";
int dataS_len = dataS.length() + 1;
char char_array[dataS_len];
dataS.toCharArray(c, dataS_len);
}
//itow=> HH:MM:SS ==================================================
String itowToHMS(int itow){
String HMS;
int DAY,HOUR,MIN,SEC,JHOUR;
int DAY_MOD,HOUR_MOD,MIN_MOD,SEC_MOD;
DAY=int(itow/86400000);
DAY_MOD=itow % 86400000;
HOUR=int(DAY_MOD/3600000);
HOUR_MOD=DAY_MOD % 3600000;
MIN=int(HOUR_MOD/60000);
MIN_MOD=HOUR_MOD % 60000;
SEC=int(MIN_MOD/1000);
//--UTC=>JST
if(HOUR>15){
JHOUR=HOUR+9-24;
}
else{
JHOUR=HOUR+9;
}
//=====18sec 進んでいる？補正＝＝＝＝＝＝＝
if(SEC<18){
SEC=60-(18-SEC);
}
else{
SEC=SEC-18;
}
//-------------
//Serial.printf("itowToHMS:JHOUR=%d,MIN=%d,SEC=%d\n\r",JHOUR,MIN,SEC);
HMS=String(JHOUR)+":"+String(MIN)+":"+String(SEC);
return HMS;
}//itowToHMS end=====================================================
//============MPU6500 initial setup====================
void MPU6500setup(){
/* The slope of the curve of acceleration vs measured values fits quite well to the theoretical
* values, e.g. 16384 units/g in the +/- 2g range. But the starting point, if you position the
* MPU6500 flat, is not necessarily 0g/0g/1g for x/y/z. The autoOffset function measures offset
* values. It assumes your MPU6500 is positioned flat with its x,y-plane. The more you deviate
* from this, the less accurate will be your results.
* The function also measures the offset of the gyroscope data. The gyroscope offset does not
* depend on the positioning.
* This function needs to be called at the beginning since it can overwrite your settings!
*/
Serial.println("Position you MPU6500 flat and don't move it - calibrating...");
delay(1000);
myMPU6500.autoOffsets();
myMPU6500_1.autoOffsets();
Serial.println("Done!");
/* This is a more accurate method for calibration. You have to determine the minimum and maximum
* raw acceleration values of the axes determined in the range +/- 2 g.
* You call the function as follows: setAccOffsets(xMin,xMax,yMin,yMax,zMin,zMax);
* Use either autoOffset or setAccOffsets, not both.
*/
//myMPU6500.setAccOffsets(-14240.0, 18220.0, -17280.0, 15590.0, -20930.0, 12080.0);
/* The gyroscope data is not zero, even if you don't move the MPU6500.
* To start at zero, you can apply offset values. These are the gyroscope raw values you obtain
* using the +/- 250 degrees/s range.
* Use either autoOffset or setGyrOffsets, not both.
*/
//myMPU6500.setGyrOffsets(45.0, 145.0, -105.0);
/* You can enable or disable the digital low pass filter (DLPF). If you disable the DLPF, you
* need to select the bandwdith, which can be either 8800 or 3600 Hz. 8800 Hz has a shorter delay,
* but higher noise level. If DLPF is disabled, the output rate is 32 kHz.
* MPU6500_BW_WO_DLPF_3600
* MPU6500_BW_WO_DLPF_8800
*/
myMPU6500.enableGyrDLPF();
myMPU6500_1.enableGyrDLPF();
//myMPU6500.disableGyrDLPF(MPU6500_BW_WO_DLPF_8800); // bandwdith without DLPF
/* Digital Low Pass Filter for the gyroscope must be enabled to choose the level.
* MPU6500_DPLF_0, MPU6500_DPLF_2, ...... MPU6500_DPLF_7
*
* DLPF Bandwidth [Hz] Delay [ms] Output Rate [kHz]
* 0 250 0.97 8
* 1 184 2.9 1
* 2 92 3.9 1
* 3 41 5.9 1
* 4 20 9.9 1
* 5 10 17.85 1
* 6 5 33.48 1
* 7 3600 0.17 8
*
* You achieve lowest noise using level 6
*/
myMPU6500.setGyrDLPF(MPU6500_DLPF_6);
myMPU6500_1.setGyrDLPF(MPU6500_DLPF_6);
/* Sample rate divider divides the output rate of the gyroscope and accelerometer.
* Sample rate = Internal sample rate / (1 + divider)
* It can only be applied if the corresponding DLPF is enabled and 0<DLPF<7!
* Divider is a number 0...255
*/
myMPU6500.setSampleRateDivider(5);
myMPU6500_1.setSampleRateDivider(5);
/* MPU6500_GYRO_RANGE_250 250 degrees per second (default)
* MPU6500_GYRO_RANGE_500 500 degrees per second
* MPU6500_GYRO_RANGE_1000 1000 degrees per second
* MPU6500_GYRO_RANGE_2000 2000 degrees per second
*/
myMPU6500.setGyrRange(MPU6500_GYRO_RANGE_250);
myMPU6500_1.setGyrRange(MPU6500_GYRO_RANGE_250);
/* MPU6500_ACC_RANGE_2G 2 g (default)
* MPU6500_ACC_RANGE_4G 4 g
* MPU6500_ACC_RANGE_8G 8 g
* MPU6500_ACC_RANGE_16G 16 g
*/
myMPU6500.setAccRange(MPU6500_ACC_RANGE_2G);
myMPU6500_1.setAccRange(MPU6500_ACC_RANGE_2G);
/* Enable/disable the digital low pass filter for the accelerometer
* If disabled the bandwidth is 1.13 kHz, delay is 0.75 ms, output rate is 4 kHz
*/
myMPU6500.enableAccDLPF(true);
myMPU6500_1.enableAccDLPF(true);
/* Digital low pass filter (DLPF) for the accelerometer, if enabled
* MPU6500_DPLF_0, MPU6500_DPLF_2, ...... MPU6500_DPLF_7
* DLPF Bandwidth [Hz] Delay [ms] Output rate [kHz]
* 0 460 1.94 1
* 1 184 5.80 1
* 2 92 7.80 1
* 3 41 11.80 1
* 4 20 19.80 1
* 5 10 35.70 1
* 6 5 66.96 1
* 7 460 1.94 1
*/
myMPU6500.setAccDLPF(MPU6500_DLPF_6);
myMPU6500_1.setAccDLPF(MPU6500_DLPF_6);
/* You can enable or disable the axes for gyroscope and/or accelerometer measurements.
* By default all axes are enabled. Parameters are:
* MPU6500_ENABLE_XYZ //all axes are enabled (default)
* MPU6500_ENABLE_XY0 // X, Y enabled, Z disabled
* MPU6500_ENABLE_X0Z
* MPU6500_ENABLE_X00
* MPU6500_ENABLE_0YZ
* MPU6500_ENABLE_0Y0
* MPU6500_ENABLE_00Z
* MPU6500_ENABLE_000 // all axes disabled
*/
//myMPU6500.enableAccAxes(MPU6500_ENABLE_XYZ);
//myMPU6500.enableGyrAxes(MPU6500_ENABLE_XYZ);
delay(200);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment