Created
October 29, 2022 04:48
-
-
Save dj1711572002/ce48884a9dc91ad94a3171830893c6bd to your computer and use it in GitHub Desktop.
Ublox F9P FW1.32 UART2 OUT Data Check Pgm
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_USBHOSt_Dual_Buf172_sdFAT_rev098 | |
//RTKMovingBaseモードでのデータログシステムPgm | |
// Base:F9P NAV-PVT+RELPOSNED出力=>USB0へ接続 | |
//Rover:F9P NAV-PVT+RELPOSNED出力=>Serial4へ接続 | |
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 calcount=1500;//averaging count | |
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> | |
#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. | |
// Why does this #ifdef have to be here? I do not understand. If it is defined | |
// then why does the compiler give this error without the #ifdef/#endif: | |
// "ReadWrite:39: error: 'SDCARD_SS_PIN' was not declared in this scope" | |
// Either it is defined previously or it is not right??? | |
// This one has me stumped. Uncomment the #ifdef and #endif and you will see what | |
// happens. I am probably missing something again! | |
#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 | |
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; | |
//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; | |
//----Hardware Serial Set--------------- | |
int dcount; | |
static uint8_t serialbuffer1[172]; | |
static uint8_t serialbuffer4[172]; | |
//================================================================= | |
//****************************************************************** | |
void setup() | |
{ | |
Serial.begin(115200); | |
if (!sd.begin(SD_CONFIG)) { | |
Serial.println("SD CARD initialization failed!"); | |
return; | |
} | |
Serial.println("SD initialization done."); | |
int stopf=0; | |
while(stopf==0) | |
{ | |
myFile = sd.open("test.txt", FILE_WRITE); | |
if (myFile) { //myFileチェックは、書き込み直前で行う。 | |
Serial.printf("---myFile In---time=%d",millis()); | |
for(i=0;i<100;i++) | |
{ | |
myFile.write(i); | |
} | |
stopf=1; | |
} | |
} | |
delay(8); | |
myFile.close(); | |
//Serial.begin(460800); | |
Serial1.begin(115200);//RX1(p0)-TX1(p1) from Base UART2 rcv=ubx ,tx=RTCM3 | |
Serial1.addMemoryForRead(serialbuffer1, 172); | |
Serial3.begin(115200);//Rx3(p15)-Tx3(p14) from M5StickC IMU & BlueTooth | |
Serial4.begin(115200);//Rx4(p16)-Tx4(p17) from RoverF9P uart1 ubx in high speed460800NG | |
Serial4.addMemoryForRead(serialbuffer4, 172); | |
while (!Serial && (millis() < 5000)) ; // wait for Arduino Serial Monitor | |
Serial1.begin(115200); // We will echo stuff Through Serial1... | |
//-----------SD CARD Initialize------------------ | |
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 | |
Serial.println("\n\nUSB Host Testing - Serial"); | |
//=================time pule interrupt pin===================== | |
//F9Pタイムパルスを受けて、 | |
pinMode(38,INPUT_PULLUP);// | |
attachInterrupt(digitalPinToInterrupt(38),timepulse,FALLING); | |
//============================================================== | |
/* | |
myFile=sd.open("test02.txt", FILE_WRITE); | |
if (myFile) { | |
//for(n=0;n<1000;n++){ | |
String timestr=String(micros()); | |
myFile.println(timestr); | |
//} | |
} | |
delay(5); | |
myFile.close(); // close the file: | |
*/ | |
}//*****set up end************************************************* | |
//======================================================================================= | |
//***************************************************************** | |
//**************TIME PULSE INTERRUPT******************************* | |
void timepulse(){ | |
if(millis()-tptime>900){// TimePulse Noise よけ900msec周期より早ければノイズ | |
tptime=millis(); | |
tpflag=1; | |
tpcount++; | |
// Serial.println("*"); | |
} | |
} | |
//******************************************************************** | |
//******************************************************************** | |
//======================================================================================================================================= | |
//==========LOOP ======================================================================================================================== | |
//======================================================================================================================================= | |
void loop() { | |
//-----------------------------------TimePulse処理 タイミングずれの初期計測------------------------------------------------------------- | |
if(tpflag==1 ){//itow millis()初期値決定 TimePulse HIGHに立ち上がった時 | |
if(itowinitflag==0){//itow initできるまで | |
tptime0=tptime; | |
if(itow0%1000==875 ){//itow0がxxxxxx875msec | |
tpitow0=int((itow0+120)/1000)*1000; | |
itowinitflag=1; | |
Serial.printf(">>> >>>>>>>>>>>>>>>>>>>>INITIAL TimePulse Interrupted tptime0=%d, itow0=%d\n\r",tptime0,tpitow0); | |
} | |
else if(itow0%1000==120){ | |
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){//itow0が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 HIGH | |
tpflag=0; //timepulse LOWセット | |
//-----------------------------------------TimePulse----------------------------------------------------------------------------------- | |
//} | |
//===Serial1-4 Base 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 ){ //バッファが満杯になるまで読まないで、一挙によむ | |
if(Serial1.available()>171){ //UART1 from Base UART2 RX | |
trcv0=millis();//debug | |
digitalWrite(41, HIGH); // | |
i=0; | |
while(i<172){ | |
while(Serial1.available()){ | |
dBuf0[i]=Serial1.read(); | |
Serial.print(dBuf0[i],HEX); | |
i++; | |
} | |
} | |
} | |
// delay(3); | |
//===Serial4 Rover read========================================================= | |
//Serial.printf("Serial4.available=%d\n\r",Serial4.available()); | |
if(Serial4.available()>171){ | |
//Serial.println(); | |
j=0;//Rover Serial4 counter | |
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 1'st | |
flags0=dBuf0[27];//Base PVT flags0 | |
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("Base_U_0_PVT_i=,%d,[,%d,],mis0=,%d,(,%x,%x,%x,%x),%d,flags0=,%d\n\r",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){ //Serial4 RELPOSNED 1'st | |
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("Base_U_0_RELPOS_i=,%d,[,%d,],mis0=,%d,(%x,%x,%x,%x),%d\n\r",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];//Rover PVT flags1 | |
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("U_1_PVT_j=,%d,[,%d,],mis1=,%d,(,%x,%x,%x,%x),%d,flags1=,%d\n\r",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("U_1_RELPOS_j=,%d,[,%d,],mis1=,%d,(%x,%x,%x,%x),%d\n\r",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>=117 ){//&& itow0==itow1){ //8.3Hz=120msecのばらつき117msec以上itow差で周期完了------------------------ | |
digitalWrite(40, HIGH); // Epoch process IN | |
//if(itow0==10){ //stop | |
itow0_1=itow0;//重複プリント防止 | |
itow1_1=itow1; | |
//Serial.printf("<Base_userialb0.read Finished time= %d>\n\r",millis()); | |
//================================================================= | |
// if(itow0>itow0_1){ | |
//===============dBuf=>PVTdata,RELPOSdata 代入========================== | |
//Serial.println("USB0data:"); | |
for(i=0;i<100;i++){//USB0 PVT | |
PVTdata[i]=dBuf0[i]; | |
//Serial.print(PVTdata[i],HEX); | |
} | |
// Serial.println("-"); | |
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 | |
//================================================================ | |
//================================================================ | |
//*********************************************************************************** | |
//**********************120msec Process********************************************** | |
//*********************************************************************************** | |
sa0=itow0-itow0_1; | |
sa1=itow1-itow1_1; | |
// ======================バイナリーから実数データへ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=RELPOSBval[7]=%4.3f,RELPOSRval[7]=%4.3f\n\r",(float)RELPOSBval[7]/100,(float)RELPOSRval[7]/100); | |
//////////////////////////////////////////////////////////////////////////////// | |
//--------- 基本パラメータ産出-------------------------------------- | |
itow=PVTBval[0]; | |
flags=PVTBval[11]; | |
if(fixflag==0) | |
{ | |
Serial.printf("flags=%d\n\r",flags); | |
} | |
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 && 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("BL_Averaging_fixflag=%d,califlag=%d\n\r",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 | |
//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; | |
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); | |
}//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("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>=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); | |
//--------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************************************************************************************** | |
/*sdFat動作チェック | |
myFile=sd.open("test02.txt", FILE_WRITE); | |
if (myFile) { | |
n++; | |
String timestr=String(micros()-starttime); | |
myFile.println(timestr); | |
} | |
delay(5); | |
myFile.close(); // close the file: | |
*/ | |
//==========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; | |
Serial.println("====Files Opened==="); | |
} | |
//---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(); | |
//Open u0 as Base | |
digitalWrite(39, HIGH); // | |
myFile = sd.open(fnameB, FILE_WRITE); | |
// Serial.printf("myFile=%d\n\r",myFile); | |
if (myFile) { //myFileチェックは、書き込み直前で行う。 | |
Serial.println("---myFile In---"); | |
for(i=0;i<100;i++) { | |
myFile.write(PVTdata[i]); | |
} | |
for(i=0;i<72;i++) { | |
myFile.write(RELPOSdata[i]); | |
} | |
}//fnameB end | |
delay(8); | |
myFile.close(); | |
//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 | |
tsdbin1=millis();//debug | |
if( epNo-epNosd>100000480 ){ | |
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]8byte 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 | |
} //LOOP END | |
//***************************************************************************************************************************** | |
//*******************************loop Finished********************************************************************** | |
//***************************************************************************************************************************** | |
void Filename (){ | |
//*********ファイル名をタイムスタンプで作ってファイルOPEN FIXしたら flags==131 ***************************************************************************** | |
//if (PVTBval[11] == 131 && Sflag == 0) { //start | |
Sflag = 1; | |
//---PVTBval[1]=Year/[2]=Month/[3]=Day/[4]=Hour/[5]=Min/[6]=sec--- | |
int JST = (PVTBval[4] + 9) % 24; //UTC hourをJSTに変換 | |
if (JST<9){//UTCが0時以前の場合日付を日本日付に修正 | |
JST_day=1; | |
} | |
else{ | |
JST_day=0; | |
} | |
String stime = String(PVTBval[2], DEC) + "-" + String(PVTBval[3]+JST_day, DEC) + "-" + String(JST, DEC) + "-" + String(PVTBval[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 ****************************************************************************************************************************************** | |
//********************************************************************************************** | |
//********************************************************************************************** | |
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ | |
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ | |
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ | |
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===================================================== |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment