Created
December 1, 2022 06:55
-
-
Save dj1711572002/39dbf6dd2c986a45bcab27feae4cb407 to your computer and use it in GitHub Desktop.
STA22 SD LOG Stable version OPEN-CLOSE 1pair
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
//STA22 のIP用システムの初期動作チェック | |
//2022-11-06 Serial1,4,5 同時受信OK | |
//2022-11-07 SDカードログ 3個のGNSSデータを3個のファイル | |
//22-11-28 sdFat1BasicExampleをコピペ | |
#include "SdFat.h" | |
// Uncomment 'USE_EXTERNAL_SD' define to use an external SD card adapter or leave | |
// it commented to use the built in sd card. | |
//#define USE_EXTERNAL_SD | |
#ifdef USE_EXTERNAL_SD | |
const uint8_t SD_CS_PIN = SS; | |
#define SPI_CLOCK SD_SCK_MHZ(10) | |
#define SD_CONFIG SdSpiConfig(SD_CS_PIN, SHARED_SPI, SPI_CLOCK) | |
#else // Use built in SD card. | |
#ifdef SDCARD_SS_PIN | |
const uint8_t SD_CS_PIN = SDCARD_SS_PIN; | |
#endif // SDCARD_SS_PIN | |
// Setup for built in SD card. | |
#define SPI_CLOCK SD_SCK_MHZ(50) | |
#define SD_CONFIG SdioConfig(FIFO_SDIO) | |
#endif // USE_EXTERNAL_SD | |
// SdFat-beta usage | |
SdFs sd; | |
//FsFile myFile; | |
//FsFile myFileB; | |
//FsFile myFileR; | |
//FsFile myFileM; | |
FsFile myFileA; | |
//FsFile myFileAv; | |
// | |
//data def | |
static uint8_t dBuf1[172], dBuf4[172],dBuf5[300];//バッファなので32の倍数でないといけない | |
static uint8_t dBuf11[172], dBuf44[172];//,dBuf55[300];//SORT用ダミーバッファ | |
//32の倍数でバッファ定義 | |
int dcount; | |
int ep0No=0; | |
static uint8_t serialbuffer1[192];//Base PVT+RELPOS172bytes | |
//static uint8_t serialbuffer2[256];//BlueTooth | |
static uint8_t serialbuffer4[192];//Rover PVT+RELPOS172bytes | |
static uint8_t serialbuffer5[320];//M9N PVT100bytes*3 | |
const int ledPin = 13; | |
int i,ii,j,jj,k,kk; | |
static int startT;//,endT; | |
int initflag=0; | |
int flag10sec=0; | |
// preprocess 追加GLOBAL変数 | |
static uint8_t preflag=0; | |
static uint8_t stflag,fixflag,fixed; | |
static uint8_t namedflag; | |
int RTKstart,RTKfix,fixtime,fixitow;//flgs=1からflags=131までの時間 | |
static int avecount; | |
int mbcount; | |
float ellapsedtime, ellapsedtime_1; | |
int calcount=500;//averaging count | |
uint8_t califlag=0; | |
//------------------- Monitoring Prameters------------------------ | |
long itow0, itow0_1,itow0_2; | |
long itow1, itow1_1,itow1_2; | |
long length0,length1; | |
int itow, itow_1, itowR; | |
int hacc, accL,numsv; | |
uint8_t flags,mflags; | |
uint8_t flags0,flags0_1,flags1,flags1_1; | |
uint8_t fix0,fix1; | |
double pdop; | |
int headmoti,gspeedi; | |
double headmotd,gspeedd; | |
//int RTKstart,RTKfix,fixtime,fixitow;//flgs=1からflags=131までの時間 | |
//uint8_t stflag,fixflag,fixed; | |
uint8_t PVT0flag,RELPOS0flag,PVT1flag,RELPOS1flag; | |
int epNo,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;//初回静止時の基準位置 | |
double relN,relE,relD,relL;//基準局からの現在位置 | |
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 | |
//モニター 補間用 | |
//F9P補間用データ追加 | |
long gSpeed,headMot; | |
long velN,velE,velD; | |
//M9N 補間用データ | |
uint8_t PVTm9n_1[100],PVTm9n_2[100],PVTm9n_3[100]; | |
long PVTvalm9n_1[33],PVTvalm9n_2[33],PVTvalm9n_3[33]; | |
long itowm9n_1,itowm9n_2,itowm9n_3; | |
long velNm9n_1,velNm9n_2,velNm9n_3; | |
long velEm9n_1,velEm9n_2,velEm9n_3; | |
long velDm9n_1,velDm9n_2,velDm9n_3; | |
long gSpeedm9n_1, gSpeedm9n_2, gSpeedm9n_3; | |
long headMotm9n_1,headMotm9n_2,headMotm9n_3; | |
int shokai0=0; | |
//imu | |
String imus[100]; | |
String imusum=""; | |
//デバイス有無機能 | |
int goflag=0;//デバイスが1個でも受信していればログをgoしていいフラグ | |
int rcvflagB=0;//1回でも受信にはいれば、1に立つ | |
int rcvflagR=0;//1回でも受信にはいれば、1に立つ | |
int rcvflagM=0;//1回でも受信にはいれば、1に立つ | |
int umuB=1;//30秒後umuB=rcvflagBで無しならゼロで、umuB==0ならflagB=1に常時しておく | |
int umuR=1;// | |
int umuM=1;// | |
int sd1stflag=1;//1回目だけOPENするためのフラグ | |
//time | |
int stt,ett; | |
//Sorintg ------------------------------------------- | |
static int PVT1startNo; | |
static int sortflagB=0;//Base data order flag=0 OK flag=1 NG | |
static int sortflagR=0;//Rover data order flag=0 OK flag=1 NG | |
static int sortflagM=0;//M9N data order flag=0 OK flag=1 NG | |
//------------------------------------------------------ | |
//static int BRflg,RRflg,MRflg;//Serial Read finish flag | |
char ckey;//File Name Make='s' Open='o' Close='c' | |
int sdstart,sdend;//time measure | |
static int flagB,flagR,flagM; | |
static int openflag; | |
static long count_upB,count_upR,count_upM; | |
static long count_upB0; | |
static long count_upB_1;//,count_upR_1,count_upM_1; | |
static long logcount=0; | |
//static long logcount_1=0; | |
//FileNames | |
char BaseFname[]="B110711.ubx"; | |
char RoverFname[]="R110711.ubx"; | |
char M9NFname[]="M110711.ubx"; | |
char AllFname[]="InitAllin.ubx"; | |
int JST_day; | |
//timestamp file name | |
uint8_t Sflag = 0; | |
uint8_t logflag=0; | |
char fnameB[30];//UBX Binary File name | |
char fnameR[30];//Rover Binary | |
char fnameM[30];//UBX Value File name NAV-PVT+NAV-RELPOSNED | |
char fnameI[30];//imu TXT | |
char fnameAv[30];//averaging File | |
char monC[200];//Monitor Text data Save | |
//---ubx 変換データ--- | |
uint8_t PVTdata[100]; | |
uint8_t RELPdata[72]; | |
long PVTval[33]; | |
long PVT1val[33]; | |
long RELPOSval[21]; | |
long RELPOS1val[21]; | |
long PVTBval[33];//Base PVT | |
long PVTRval[33]; | |
long RELPOSBval[21]; | |
long RELPOSRval[21]; | |
//---------------------- | |
uint8_t RELPOSBdata[172];//base line RELPOS | |
volatile uint8_t RELPOS1Bdata[172];//base line RELPOS | |
// it commented to use the built in sd card. | |
/* | |
// SdFat-beta usage | |
SdFs sd; | |
FsFile myFile; | |
FsFile myFileB; | |
FsFile myFileR; | |
FsFile myFileM; | |
FsFile myFileA; | |
FsFile myFileAv; | |
*/ | |
//============初期 関数================================- | |
//####################################################################################### | |
//*************************************************************************************** | |
//*************************************************************************************** | |
//*************************************************************************************** | |
void setup() { | |
Serial.begin(460800); | |
Serial2.begin(115200); | |
while (!Serial) { | |
; // wait for serial port to connect. Needed for native USB port only | |
} | |
//---------------SD Card Write SetUp--------------------------------- | |
if (!sd.begin(SD_CONFIG)) { | |
Serial.println("SD CARD initialization failed!"); | |
return; | |
} | |
Serial.println("SD initialization done."); | |
//Serial1 def Base | |
Serial1.begin(115200);//RX1(p0)-TX1(p1) PVT+RELPOS from Base UART2 rcv=ubx ,tx=RTCM3 | |
Serial1.addMemoryForRead(serialbuffer1, sizeof(serialbuffer1)); | |
//Serial4 def F9H | |
Serial4.begin(115200);//Rx2(p16)-Tx2(p17) from BlueTooth | |
Serial4.addMemoryForRead(serialbuffer4, sizeof(serialbuffer4)); | |
//Serial5 def m9N | |
Serial5.begin(115200);//Rx5(p21)-Tx5(p20) PVT from M9N uart1 | |
Serial5.addMemoryForRead(serialbuffer5, sizeof(serialbuffer5)); | |
while (!Serial && (millis() < 5000)) ; // wait for Arduino Serial Monitor | |
//付加機能用 | |
count_upB=0; | |
count_upR=0; | |
count_upM=0; | |
//F9Pの起動時間待ち | |
// Serial.println("****************Waiting 10sec for GNSS Signal*************"); | |
// delay(10000); | |
pinMode(ledPin, OUTPUT); | |
while (!Serial && (millis() < 8000)) ; // wait for Arduino Serial Monitor | |
startT=millis(); | |
Serial.println("----------PRE PROCESSING START BaseLine Check-------------------"); | |
} | |
//********************************************************************************************************************* | |
//********************************************************************************************************************* | |
//********************************************************************************************************************* | |
void loop() { | |
//===Serial1 F9P read========================================================= | |
if(Serial1.available()>171){ //バッファ1が172バイトになるまで処理しない | |
//Serial.println(); | |
i=0;//Base Serial1 counter | |
while(i<172){ | |
digitalWrite(ledPin, HIGH); // set the LED on | |
//Serial.printf("F9P:%d,",millis()); | |
stt=millis(); | |
while(Serial1.available()){ | |
dBuf1[i]=Serial1.read(); | |
//Serial.print(dBuf1[i],HEX); | |
i++; | |
} | |
} | |
//---Receive ORDER CHECK------------------------------------------------------------------------------------------------------------ | |
if(dBuf1[0]==0xB5 && dBuf1[1]==0x62 && dBuf1[2]==0x01 && dBuf1[3]==0x07){ | |
Serial.print("*"); | |
} | |
else{ | |
sortflagB=1; | |
Serial.printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>Base(F9P):Recieve ORDER ERROR dBuf1[0]:%x,%x,%x,%x,%x\n\r",dBuf1[0],dBuf1[1],dBuf1[2],dBuf1[3]); | |
} | |
//----------------------------------------------------------------------------------------------------------------------------------- | |
ett=millis(); | |
if(initflag==0 && count_upB%50==0){ | |
flags=dBuf1[27]; | |
Serial.printf("[%d][%d][%d]F9P:%d,%d\n\r",count_upB,flags,avecount,stt,ett); | |
//Serial.print("B"); | |
} | |
digitalWrite(ledPin, LOW); // set the LED on | |
flagB=1; | |
rcvflagB=1; | |
count_upB++; | |
}// | |
//***************************************************************************** | |
//プリプロセス void preprocess(uint8_t *db1){//dBuf1[]を配列ポインタで渡される | |
if(flagB==1 && preflag==0){//preflag==0の場合 | |
preprocess(dBuf1); | |
//Serial.println();//RCVマークの改行 | |
flagB=0; | |
} | |
//===Serial1 F9H read========================================================= | |
if(Serial4.available()>171){ //バッファ4が172バイトになるまで処理しない | |
j=0;//Rover Serial1 counter | |
while(j<172){ | |
digitalWrite(ledPin, HIGH); // set the LED on | |
// Serial.printf("F9H:%d,",millis()); | |
stt=millis(); | |
while(Serial4.available()){ | |
dBuf4[j]=Serial4.read(); | |
j++; | |
} | |
} | |
//---Receive ORDER CHECK------------------------------------------------------------------------------------------------------------ | |
if(dBuf4[0]==0xB5 && dBuf4[1]==0x62 && dBuf4[2]==0x01 && dBuf4[3]==0x07){ | |
} | |
else{ | |
sortflagR=1; | |
Serial.printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>Rover(F9H):Recieve ORDER ERROR dBuf4[0]:%x,%x,%x,%x,%x\n\r",dBuf4[0],dBuf4[1],dBuf4[2],dBuf4[3]); | |
} | |
//----------------------------------------------------------------------------------------------------------------------------------- | |
ett=millis(); | |
if(initflag==0 && count_upR%50==0){ | |
Serial.printf("F9H:%d,%d\n\r",stt,ett); | |
//Serial.print("R"); | |
} | |
digitalWrite(ledPin, LOW); // set the LED on | |
flagR=1; | |
rcvflagR=1; | |
count_upR++; | |
}// | |
//===Serial5 M9N read========================================================= | |
//int flagM; | |
if(Serial5.available()>299){ //バッファ5が300バイトになるまで処理しない | |
//Serial.println(); | |
k=0;//Rover Serial5 counter | |
while(k<300){ | |
// digitalWrite(ledPin, HIGH); // set the LED on | |
//Serial.printf("M9N:%d,",millis()); | |
stt=millis(); | |
while(Serial5.available()){ | |
dBuf5[k]=Serial5.read(); | |
k++; | |
} | |
} | |
//---Receive ORDER CHECK------------------------------------------------------------------------------------------------------------ | |
if(dBuf5[0]==0xB5 && dBuf5[1]==0x62 && dBuf5[2]==0x01 && dBuf5[3]==0x07){ | |
} | |
else{ | |
sortflagM=1; | |
//Serial.printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>M9N:Recieve ORDER ERROR dBuf4[0]:%x,%x,%x,%x,%x\n\r",dBuf5[0],dBuf5[1],dBuf5[2],dBuf5[3]); | |
} | |
//----------------------------------------------------------------------------------------------------------------------------------- | |
//delay(3); | |
ett=millis(); | |
if(initflag==0 && count_upM%50==0){ | |
//Serial.printf("M9N:%d,%d\n\r",stt,ett); | |
//Serial.print("M"); | |
} | |
digitalWrite(ledPin, LOW); // set the LED on | |
flagM=1; | |
rcvflagM=1; | |
count_upM++; | |
}// | |
//####################################################################################### | |
//####################################################################################### | |
//####################################################################################### | |
//---デバイス有無判定-- | |
if((millis()-startT > 10000) && flag10sec==0 && preflag==0){//10秒後にデバイス有無判定 | |
flag10sec=1; | |
//デバイス判定 | |
umuB=rcvflagB; | |
umuR=rcvflagR; | |
umuM=rcvflagM; | |
if(umuB==0 && umuR==0 && umuM==0){//デバイス接続ゼロ警告だして停止 | |
Serial.println("<<<<<<<<<<<<<<<<<No Device Error!!>>>>>>>>>>>>>>>>>>>>>"); | |
while(umuB==0 && umuR==0 && umuM==0){} | |
} | |
Serial.printf("rcvflagB=%d,umuB=%d,rcvflagR=%d,umuR=%d,rcvflagM=%d,umuM-%d\n\r",rcvflagB,umuB,rcvflagR,umuR,rcvflagM,umuM); | |
//順序判定 | |
Serial.printf("dBuf1[0]=%x,dBuf1[1]=%x,dBuf1[2]=%x,dBuf1[3]=%x\n\r",dBuf1[0],dBuf1[1],dBuf1[2],dBuf1[3]); | |
if(dBuf1[0]==0xB5 && dBuf1[1]==0x62 && dBuf1[2]==0x01 && dBuf1[3]==0x07){ | |
Serial.println("*********************Recieving OK**********************************"); | |
} | |
else{ | |
Serial.println("**********************Recieving NG**********************************"); | |
_reboot_Teensyduino_(); | |
} | |
} | |
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
//@@@@@@@@@@@@@@@@@@@@エポック処理へ入る@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
// if(count_upB % 50==0 && preflag==1){ | |
// int cp=umuB*flagB+umuR*flagR+umuM*flagM; | |
// Serial.printf("cp=%d,countup_B=%d,countup_B_1=%d,preflag=%d\n\r",cp,count_upB,count_upB_1,preflag); | |
// } | |
if(umuB*flagB+umuR*flagR+umuM*flagM>0 && count_upB>count_upB_1 && preflag==1 ){//1デバイスでもイン可能 | |
count_upB_1=count_upB; | |
Serial.printf("%d,",count_upB); | |
//SORTING=================================================================================================================================== | |
//===Base SORT======================================================== | |
if(sortflagB==1){ | |
Serial.println("----------------------Base Recieve ORDER ERROR=>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) //Serial1 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("SORTED:dBuf11:"); | |
for(j=0;j<172;j++) | |
{ | |
Serial.print(dBuf11[j],HEX); | |
dBuf1[j]=dBuf11[j]; | |
} | |
Serial.println(); | |
sortflagB=0;//SORT終わったので戻す | |
}//sortflagB==1 END | |
//===Rover SORT======================================================== | |
if(sortflagR==1){ | |
Serial.println("----------------------Rover Recieve ORDER ERROR=>SORT dBuf4=>dBuf44-------------------------------------"); | |
Serial.print(" dBuf4:"); | |
for (j = 0; j < 172; j++) //Search PVT Header------------------------------------------------------ | |
{ | |
if (dBuf4[j] == 0xb5 && dBuf4[j + 1] == 0x62 && dBuf4[j + 2] == 0x01 && dBuf4[j + 3] == 0x07) //Serial4 PVT header | |
{ | |
PVT1startNo = j; | |
} | |
Serial.print(dBuf4[j],HEX); | |
} | |
Serial.println(); | |
//Sorting-------------------------------------------------------------------------------- | |
for (jj = 0; jj < 172 - PVT1startNo; jj++) //Sort dBuf1[PVT1startNo]~dBuf1[171] To dBuf11[0]~dBuf11[171-PVT1startNo] | |
{ | |
dBuf44[jj] = dBuf4[PVT1startNo + jj]; | |
} | |
for (jj = 0; jj < PVT1startNo; jj++) //Sort | |
{ | |
dBuf44[jj + 172 - PVT1startNo] = dBuf4[jj]; | |
} | |
Serial.print("SORTED:dBuf44:"); | |
for(j=0;j<172;j++) | |
{ | |
Serial.print(dBuf44[j],HEX); | |
dBuf4[j]=dBuf44[j]; | |
} | |
Serial.println(); | |
sortflagR=0;//SORT終わったので戻す | |
}//sortflagR==1 END | |
//===M9N SORT 300byteを100ずつばらしてソート=================================================== | |
if(sortflagM==1){ | |
Serial.println("----------------------M9N Recieve ORDER ERROR=>SORT dBuf4=>dBuf44-------------------------------------"); | |
Serial.print(" dBuf4:"); | |
for (j = 0; j < 172; j++) //Search PVT Header------------------------------------------------------ | |
{ | |
if (dBuf4[j] == 0xb5 && dBuf4[j + 1] == 0x62 && dBuf4[j + 2] == 0x01 && dBuf4[j + 3] == 0x07) //Serial4 PVT header | |
{ | |
PVT1startNo = j; | |
} | |
Serial.print(dBuf4[j],HEX); | |
} | |
Serial.println(); | |
//Sorting-------------------------------------------------------------------------------- | |
for (jj = 0; jj < 172 - PVT1startNo; jj++) //Sort dBuf1[PVT1startNo]~dBuf1[171] To dBuf11[0]~dBuf11[171-PVT1startNo] | |
{ | |
dBuf44[jj] = dBuf4[PVT1startNo + jj]; | |
} | |
for (jj = 0; jj < PVT1startNo; jj++) //Sort | |
{ | |
dBuf44[jj + 172 - PVT1startNo] = dBuf4[jj]; | |
} | |
Serial.print("SORTED:dBuf44:"); | |
for(j=0;j<172;j++) | |
{ | |
Serial.print(dBuf44[j],HEX); | |
dBuf4[j]=dBuf44[j]; | |
} | |
Serial.println(); | |
sortflagR=0;//SORT終わったので戻す | |
}//sortflagR==1 END | |
//SORTING FINISHED============================================================================================================================================================== | |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
//+++++++++++++++++++++++++++++++LOG Control+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
flags=dBuf1[27]; | |
//Serial.printf("-----flags=%d\n\r",flags); | |
if(flags>67 && initflag==0 ){//FILE NAME 作成 's' | |
ckey='s'; | |
initflag=1; | |
count_upB0=count_upB; | |
} | |
if(initflag==1 && logflag==0){ | |
ckey='o'; | |
logflag=1; | |
} | |
if(logflag==1 && (count_upB-count_upB0)%1000==0 && openflag==1){//1000epoch 毎にCLOSE 1EPOCH抜けるが不良率0.1%3.29シグマ | |
ckey='c'; | |
logflag=0; | |
} | |
//if(Serial.available()){ | |
//char ckey=Serial.read(); | |
//Serial.printf("ckey=%c\n\r",ckey); | |
if(ckey=='s'){ | |
//initflag=1;//初回のみ実行 | |
//Serial.printf("<<<<<<<Device:umuB=%d,umuR=%d,umuM=%d>>>>>>>>\n\r",umuB,umuR,umuM); | |
//------------初回にタイムスタンプ用ファイル名作成------------------------------- | |
//BaseからPVTを読み取る | |
//oneBaseRcv(dBuf1);//Serial1受信してdBuf1[]に代入 | |
dBuf_To_PVT_RELP(dBuf1,PVTval,RELPOSval);//dBufからPVT値 RELPOS値を得る | |
Filename (PVTval);//グローバルchar[]のfnameBにファイル名を格納する | |
ckey=""; | |
}//ckey='s' File name end | |
if(ckey=='o'){ | |
delay(10); | |
myFileA = sd.open(fnameB, FILE_WRITE);//timestampファイル名fnameB | |
// myFileA.open(fnameB, FILE_WRITE); | |
if(!myFileA){ | |
Serial.println("=============File Open Error============"); | |
openflag=0; | |
} | |
else{ | |
openflag=1; | |
Serial.println("==============File Open Successed========="); | |
} | |
//namedflag=0; | |
//logcount=0; | |
Sflag=1; | |
//count_upB0=count_upB;//start counter cout_upB0 | |
Serial.printf("===========OpenOK:count_upB0=%d,Sflag=%d=========\n\r",count_upB0,Sflag); | |
ckey=""; | |
}//ckey="o" end | |
if(ckey=='c'){ | |
myFileA.close(); | |
Serial.println("==============File Closed=================="); | |
ckey=""; | |
delay(10); | |
} | |
//} | |
if(openflag==1 && Sflag==1 && myFileA ){ | |
Serial.print("+"); | |
for(i=0;i<172;i++){ | |
if(myFileA){ | |
myFileA.write(dBuf1[i]); | |
} | |
else { | |
Serial.println("dBuf1 Write Error>>>>>>>>>>>>>>>>error opening myFile"); | |
} | |
} | |
for(i=0;i<172;i++){ | |
if(myFileA){ | |
myFileA.write(dBuf4[i]); | |
} | |
else { | |
Serial.println("dBuf4 Write Error>>>>>>>>>>>>>>>>error opening myFile"); | |
} | |
} | |
for(i=0;i<300;i++){ | |
if(myFileA){ | |
myFileA.write(dBuf5[i]); | |
} | |
else { | |
Serial.println("dBuf5 Write Error>>>>>>>>>>>>>>>>error opening myFile"); | |
} | |
} | |
//logcount++; | |
//Serial.printf("logcount=%d\n\r",logcount); | |
}//myFileM | |
/* | |
delay(1); | |
//Serial.printf("count_upB-count_upB0 %500=%d,Sflag=%d\n\r",(count_upB-count_upB0)%500,Sflag); | |
//if((count_upB-count_upB0)%500==0 && Sflag==1 && (count_upB-count_upB0)>499){//500count =1min | |
if(Serial.available()){ | |
logcount++; | |
startT=millis(); | |
myFileA.close(); | |
delay(120); | |
return; | |
//myFileA = sd.open(AllFname, FILE_WRITE); | |
//myFileA = sd.open(fnameB, FILE_WRITE);//timestampファイル名fnameB | |
dBuf_To_PVT_RELP(dBuf1,PVTval,RELPOSval);//dBufからPVT値 RELPOS値を得る | |
Filename (PVTval);//グローバルchar[]のfnameBにファイル名を格納する | |
//myFileA=sd.open(fnameB, FILE_WRITE); | |
//if(!myFileA){ | |
// Serial.println(">>>>>>>>>>>>>>>>>>>>>>>>>>>>myFileA ReOpen ERROR"); | |
// } | |
// myFileA.truncate(0);//重ね書きしない追記APPEND設定 | |
//Serial.println(); | |
Serial.printf("*********************Close&Open,count_upB=%d,t=%d sec**************\n\r",count_upB,logcount*300); | |
} | |
*/ | |
flagB=0; | |
flagR=0; | |
flagM=0; | |
//Serial.println(); | |
// int btmonstart; | |
// btmonstart=millis(); | |
// BTMon(); | |
//Serial.printf("BTMon:%d,%d\n\r",btmonstart,millis()); | |
}//if flagB,flagR,flagM | |
//********************************** Monitor SELECTION ********************************************************************************************** | |
/* | |
if(Serial2.available()>0){ | |
char cin=Serial2.read(); | |
Serial.printf("===========================================KEYIN=%x===============================================\n\r",cin); | |
if(cin=='0'){ //reboot teensy | |
_reboot_Teensyduino_(); | |
} | |
}// Monitor SELECTION END | |
*/ | |
//Serial.println("LOOP Bottom"); | |
} | |
//*********************************SUBs************************************************************ | |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
//++++++++++++++++++++++++MONITOR BLUETOOTH SPP++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
//測定中変化するパラメータ1:itow,flags,pdop,hacc,numSV,miscount | |
//測定中変化するパラメータ2:double rrN,rrE,rrD,rrL;//基準位置平均値からの現在位置relN-rNaveN | |
//測定中変化するパラメータ3:gSpeed,headMot,velN,velE,velD,relN,relE,relD | |
//初期確認パラメータ3:int fixtime,double rNaveN,rEaveN,rDaveN,rLaveN,mHaveN;//FIX時間と基準位置平均値5個 | |
//初期確認パラメータ4:double rNsgmN,rEsgmN,rDsgmN,rLsgmN,mHsfmN;//基準位置のσ値 5個 | |
// | |
void BTMon(){ | |
dBuf_To_PVT_RELP(dBuf1,PVTBval,RELPOSBval);//dBufからPVT値 RELPOS値を得る | |
long monflags=PVTBval[11]; | |
long monpdop=PVTBval[27]; | |
double pdop=(double)monpdop*0.01;// | |
long monhacc=PVTBval[18]; | |
long monnumsv=PVTBval[13]; | |
//基準位置ゼロにした現在位置 | |
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; | |
//Serial.printf("flags=%d,pdop=%3.2f,hacc=%d,numSV=%d,miscount=%d,relN=%4.2f,relE=%4.2f,relD=%4.2f,relL=%4.2f\n\r",monflags,pdop,monhacc,monnumsv,miscount,relN,relE,relD,relL); | |
//M9N 補間評価用モニター | |
//Base: relN,relE,relD,relL,velN,velE,velD,gSpeed,headMot M9N: velNm9n,velEm9n,velDm9n,headMotm9n | |
itow=PVTBval[0]; | |
velN=PVTBval[20]; | |
velE=PVTBval[21]; | |
velD=PVTBval[22]; | |
gSpeed=PVTBval[23]; | |
headMot=PVTBval[24]; | |
//m9n | |
m9nToPVT(dBuf5);//M9N3個分を変換 | |
//結果 | |
if(shokai0==0){ | |
shokai0=1; | |
Serial.printf("itow,velN,velE,velD,gSpeed,headMot,"); | |
Serial.printf("itowm9n_1,velNm9n_1,velEm9n_1,velDm9n_1,gSpeedm9n_1,headMotm9n_1,"); | |
Serial.printf("itowm9n_2,velNm9n_2,velEm9n_2,velDm9n_2,gSpeedm9n_2,headMotm9n_2,"); | |
Serial.printf("itowm9n_3,velNm9n_3,velEm9n_3,velDm9n_3,gSpeedm9n_3,headMotm9n_3\n\r"); | |
} | |
Serial.printf("Pos,%d,%4.2f,%d,%d,%d,%4.2f,%4.2f,%4.2f,%4.2f\n\r",monflags,pdop,monhacc,monnumsv,miscount,relN,relE,relD,relL); | |
Serial.printf("Base,%d,%d,%d,%d,%d,%d\n\r",itow,velN,velE,velD,gSpeed,headMot); | |
Serial.printf("M9N1,%d,%d,%d,%d,%d,%d\n\r",itowm9n_1,velNm9n_1,velEm9n_1,velDm9n_1,gSpeedm9n_1,headMotm9n_1); | |
Serial.printf("M9N2,%d,%d,%d,%d,%d,%d\n\r",itowm9n_2,velNm9n_2,velEm9n_2,velDm9n_2,gSpeedm9n_2,headMotm9n_2); | |
Serial.printf("M9N3,%d,%d,%d,%d,%d,%d\n\r",itowm9n_3,velNm9n_3,velEm9n_3,velDm9n_3,gSpeedm9n_3,headMotm9n_3); | |
//Serial2 BlueTooth | |
Serial2.printf("Pos,%d,%4.2f,%d,%d,%d,%4.2f,%4.2f,%4.2f,%4.2f\n\r",monflags,pdop,monhacc,monnumsv,miscount,relN,relE,relD,relL); | |
Serial2.printf("Base,%d,%d,%d,%d,%d,%d\n\r",itow,velN,velE,velD,gSpeed,headMot); | |
Serial2.printf("M9N1,%d,%d,%d,%d,%d,%d\n\r",itowm9n_1,velNm9n_1,velEm9n_1,velDm9n_1,gSpeedm9n_1,headMotm9n_1); | |
Serial2.printf("M9N2,%d,%d,%d,%d,%d,%d\n\r",itowm9n_2,velNm9n_2,velEm9n_2,velDm9n_2,gSpeedm9n_2,headMotm9n_2); | |
Serial2.printf("M9N3,%d,%d,%d,%d,%d,%d\n\r",itowm9n_3,velNm9n_3,velEm9n_3,velDm9n_3,gSpeedm9n_3,headMotm9n_3); | |
} | |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
//M9N dBuf5[] to PVT 3Blocks | |
void m9nToPVT(uint8_t *dB){//300byte データ配列のポインタを渡す | |
uint8_t db1[100],db2[100],db3[100]; | |
for(i=0;i<300;i++){//300byteを3分割 | |
if(i<100){ | |
db1[i]=dBuf5[i]; | |
} | |
if(i>=100 && i<200){ | |
db2[i-100]=dBuf5[i]; | |
} | |
if(i>=200 && i<300){ | |
db3[i-200]=dBuf5[i]; | |
} | |
} | |
//PVT変換 | |
PVTcnv(db1,PVTvalm9n_1); | |
PVTcnv(db2,PVTvalm9n_2); | |
PVTcnv(db3,PVTvalm9n_3); | |
//itowm9n[3],velNm9n[3],velEm9n[3],velDm9n[3],gSpeedm9n[3],headMotm9n[3]; | |
itowm9n_1=PVTvalm9n_1[0]; | |
itowm9n_2=PVTvalm9n_2[0]; | |
itowm9n_3=PVTvalm9n_3[0]; | |
// | |
velNm9n_1=PVTvalm9n_1[20]; | |
velNm9n_2=PVTvalm9n_2[20]; | |
velNm9n_3=PVTvalm9n_3[20]; | |
// | |
velEm9n_1=PVTvalm9n_1[21]; | |
velEm9n_2=PVTvalm9n_2[21]; | |
velEm9n_3=PVTvalm9n_3[21]; | |
// | |
velDm9n_1=PVTvalm9n_1[22]; | |
velDm9n_2=PVTvalm9n_2[22]; | |
velDm9n_3=PVTvalm9n_3[22]; | |
// | |
gSpeedm9n_1=PVTvalm9n_1[23]; | |
gSpeedm9n_2=PVTvalm9n_2[23]; | |
gSpeedm9n_3=PVTvalm9n_3[23]; | |
// | |
headMotm9n_1=PVTvalm9n_1[24]; | |
headMotm9n_2=PVTvalm9n_2[24]; | |
headMotm9n_3=PVTvalm9n_3[24]; | |
// | |
} | |
//---------------Base(Serial1)から1エポックデータ取得---------- | |
void oneBaseRcv(uint8_t *d){ | |
int i; | |
int counterB=0; | |
while(counterB<172){ | |
//--Serial1 Base F9P read-- | |
if(Serial1.available()>171){ //バッファ1が172バイトになるまで処理しない | |
//Serial.println(); | |
i=0;//Base Serial1 counter | |
while(i<172){ | |
digitalWrite(ledPin, HIGH); // set the LED on | |
while(Serial1.available()){ | |
d[i]=Serial1.read(); | |
i++; | |
} | |
} | |
digitalWrite(ledPin, LOW); // set the LED on | |
counterB++; | |
}// | |
}//while couterB>172 | |
}//oneBaseRcv end | |
//---------------dBufからPVTとRELPOSへ変換------------------- | |
//--------------pointer渡し--------------------------------- | |
//---uint_8t *dは、d[172]F9Pの生バイナリーPVT+RELPOS 172byte--- | |
//---uint8_t *pvtは、pvtdata[100] | |
//---uint8_t *relpは、relpdata[72] | |
//----------------------------------------------------------- | |
void dBuf_To_PVT_RELP(uint8_t *d,long *pvtval,long *relpval){ | |
//--dataから分離-- | |
uint8_t pvt[100]; | |
uint8_t relp[72]; | |
int i=0; | |
int k=0; | |
int j=0; | |
for(i=0;i<172;i++){ | |
if(i<100){//PVT抽出pvt[0]-pvt[99] | |
pvt[k]=d[i]; | |
k++; | |
} | |
else if(i>99){//relp抽出relp[0]-relp[71] | |
relp[j]=d[i]; | |
j++; | |
} | |
} | |
//--PVT変換-- | |
PVTcnv(pvt,pvtval); | |
//--relp変換-- | |
RELPOScnv(relp,relpval); | |
}//dBuf_To_pvt_relp() end | |
//-------------------Filename()------------------------------- | |
//--------Baseデータからタイムスタンプのファイル名をつける-------- | |
//--------------pointer渡し---------------------------------- | |
//---uint_8t *dは、d[172]F9Pの生バイナリーPVT+RELPOS 172byte--- | |
//---long *PVT,*RELPは、変換後のlong配列 PVT[33],RELP[20]------ | |
//---char *fnmは、ファイル名char配列 fnm[30]------------------- | |
//----------------------------------------------------------- | |
void Filename (long *PVT) {//ファイル名をタイムスタンプ | |
//if (PVTBval[11] == 131 && Sflag == 0) { //start | |
//---PVTBval[1]=Year/[2]=Month/[3]=Day/[4]=Hour/[5]=Min/[6]=sec--- | |
int JST = (PVT[4] + 9) % 24; //UTC hourをJSTに変換 | |
if (JST < 9) { //UTCが0時以前の場合日付を日本日付に修正 | |
JST_day = 1; | |
} | |
else { | |
JST_day = 0; | |
} | |
String stime = String(PVT[2], DEC) + "-" + String(PVT[3] + JST_day, DEC) + "-" + String(JST, DEC) + "-" + String(PVT[5], DEC); //MMDDHHMM | |
String stimeB = stime + ".ubx"; //UBX Binary File | |
String stimeAv="Ave"+ stime + ".txt"; //Averaging txt File | |
int slenB = stimeB.length() + 1; | |
int slenAv=stimeAv.length()+1; | |
//ファイル名はchar配列なのでStringからchar配列変換 fname[]を得る | |
stimeB.toCharArray(fnameB, slenB); //stimeB to fnameB[] chara Array | |
stimeAv.toCharArray(fnameAv, slenAv); //stimeAv to fnameAv[] chara Array | |
Serial.println(); | |
Serial.print("fnameB="); | |
Serial.println(stimeB); | |
Serial.print("fnameAv="); | |
Serial.println(stimeAv); | |
delay(10); | |
}//Filename() end ****************************************************************************************************************************************** | |
//+++PVTcnv++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
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]; | |
//Serial.printf("PVTcnv:hour=%d\n\r",pvt[4]); | |
//5:min[15] | |
pvt[5] = (long)d[15]; | |
//Serial.printf("PVTcnv:min=%d\n\r",pvt[5]); | |
//6:sec[16] | |
pvt[6] = (long)d[16]; | |
//Serial.printf("PVTcnv:sec=%d\n\r",pvt[6]); | |
//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]=%d\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===================================================== | |
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX | |
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX | |
//XXXXXXXXXXXXXXXXXXXXXXXXXXXプリプロセスで全部関数内で処理して、測定メインを使わないXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX | |
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX | |
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX | |
void preprocess(uint8_t *db1){//dBuf1[]を配列ポインタで渡される | |
//Serial.println("<<<<<<<<<<<<<<<<<preprocess IN>>>>>>>>>>>>>>>>>>"); | |
// ============GNSS RTK top terms======================== | |
/*GLOBAL 変数の参照用コピー | |
static uint8_t dBuf1[172], dBuf4[172],dBuf5[300];//バッファなので32の倍数でないといけない | |
//---ubx 変換データ--- | |
uint8_t PVTdata[100]; | |
uint8_t RELPdata[72]; | |
long PVTval[33]; | |
long PVT1val[33]; | |
long RELPOSval[21]; | |
long RELPOS1val[21]; | |
long PVTBval[33];//Base PVT | |
long PVTRval[33]; | |
long RELPOSBval[21]; | |
long RELPOSRval[21]; | |
*/ | |
//データ配列 | |
//============================================================================================= | |
dBuf_To_PVT_RELP(dBuf1,PVTBval,RELPOSBval); | |
//============================================================================================= | |
//変換結果確認 | |
// int nn; | |
// for(nn=0;nn<21;nn++){ | |
// Serial.printf("RELPOSBval[%d]=%d\n\r",nn,RELPOSBval[nn]); | |
// } | |
//============================================================================================= | |
//Fix判定 | |
//============================================================================================= | |
//受信状態確認 flags,itowでミスカウント | |
flags0=PVTBval[11];//flags //dBuf0[27];//Base PVT flags0 | |
if(flags0==131 && fix0==0){ | |
miscount0=0; | |
fix0=1; | |
} | |
itow0_1=itow0; | |
itow0 =PVTBval[0];// B2L(dBuf0[9], dBuf0[8], dBuf0[7], dBuf0[6]); //dBuf0 iTow Calc | |
if(itow0-itow0_1>122){//120msec | |
miscount0++; | |
} | |
else{ | |
ep0No++; | |
} | |
if(itow0>itow0_1 && flags0>0 && Sflag==0){ //<131 | |
// Serial.printf("Base_PVT_i=,%d,[,%d,],mis0=,%d,(,%x,%x,%x,%x),%d,flags0=,%d\n\r",i,itow0,miscount0,dBuf1[0],dBuf1[1],dBuf1[2],dBuf1[3],millis(),flags0); | |
} | |
//============================================================================================================ | |
//Base 受信して、基線長解析をおこなう | |
//------------------------------------------------------------------ | |
//---------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; | |
// Serial.printf("relN=%4.2f,relE=%4.2f,relD=%4.2f,relL=%4.2f\n\r",relN,relE,relD,relL); | |
//--------------------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のばらつきを統計処理 | |
//-------------------------------------------------------------------------------------------------------- | |
//*****************FIX 判定********************************************************* | |
if(flags0==1 && stflag==0){ | |
RTKstart=millis(); | |
stflag=1; | |
//Serial.printf("@@@@@@@@RTKstart=%d\n\r",RTKstart); | |
} | |
else if(flags0>=67 && fixflag==0){//fixed | |
RTKfix=millis(); | |
fixflag=1; | |
//Serial.printf("@@@@@@@@RTKfix=%d\n\r",RTKfix); | |
//Fixしたらファイル名を作る | |
Filename (PVTBval);//グローバルchar[]のfnameAvにファイル名を格納する | |
} | |
if(stflag==1 && fixflag==1 && fixed==0){ | |
fixtime=int(((double)RTKfix-RTKstart)/1000);//sec | |
//Serial.printf(" <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<fixtime=%d,RTKfix=%d,RTKstart=%d>>>>\n\r",fixtime,RTKfix,RTKstart); | |
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("avecount=%d,BL_Averaging_fixflag=%d,califlag=%d\n\r",avecount,fixflag,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 | |
// Serial.printf("10==:rLaveN=%4.2f,rLsum=%4.2f\n\r",rLaveN,rLsum); | |
//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 | |
// Serial.printf("10==:rLdvN=%4.2f,rLdvsum=%4.2f\n\r",rLdvN,rLdvsum); | |
//mHdvN=mHdvsum/10; | |
//Serial.printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>10>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,rNaveN,rNdvN,rEaveN,rEdvN,rDaveN,rDdvN,rLaveN,rLdvN,rLmaveN,rLmaveN); | |
}//if 10count averaging stdev end | |
//---N1 10-500count cal---- | |
if(avecount >10 && avecount<calcount){ | |
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("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); | |
//Serial.printf("10<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); | |
//Serial.printf("CaliLAST:avecount=%d,calcount=%d\n\r",avecount,calcount); | |
//Serial.print(avecount); | |
//Serial.print(","); | |
if(avecount>=calcount){//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); | |
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); | |
//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"; | |
//---Averaging Result File Write | |
//myFileAv.open(fnameAv,FILE_WRITE); | |
if(myFileA.open(fnameAv,FILE_WRITE)){ | |
myFileA.write(monC,strlen(monC)); | |
} | |
delay(8); | |
// myFileT.println(stc1); | |
myFileA.close(); //毎回クローズして万一のアクシデントに備える | |
//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 | |
preflag=1; //preprocess STOP | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment