Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/STA22_IPsystem_AllIn_221109_105PreOK.ino Secret

Created November 12, 2022 09:09
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save dj1711572002/2d26c4ef88b18017a841719b450a8d46 to your computer and use it in GitHub Desktop.
Save dj1711572002/2d26c4ef88b18017a841719b450a8d46 to your computer and use it in GitHub Desktop.
Download ZIP
SkiTurnAnalyzer2022_InteporationSystem_F9P-F9H-M9N_SD LOG
Raw
STA22_IPsystem_AllIn_221109_105PreOK.ino
//STA22 のIP用システムの初期動作チェック
//2022-11-06 Serial1,4,5 同時受信OK
//2022-11-07 SDカードログ　３個のGNSSデータを３個のファイル
//data def
static uint8_t dBuf1[172], dBuf4[172],dBuf5[300];//バッファなので32の倍数でないといけない
//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 velN,velE,velD;
//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
//imu
String imus[100];
String imusum="";
//デバイス有無機能
int goflag=0;//デバイスが１個でも受信していればログをgoしていいフラグ
int rcvflagB=0;//１回でも受信にはいれば、１に立つ
int rcvflagR=0;//１回でも受信にはいれば、１に立つ
int rcvflagM=0;//１回でも受信にはいれば、１に立つ
int umuB=1;//３０秒後umuB=rcvflagBで無しならゼロで、umuB==0ならflagB=1に常時しておく
int umuR=1;//
int umuM=1;//
int sd1stflag=1;//1回目だけOPENするためのフラグ
//time
int stt,ett;
static int BRflg,RRflg,MRflg;//Serial Read finish flag
int sdstart,sdend;//time measure
static int flagB,flagR,flagM;
static long count_upB,count_upR,count_upM;
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;
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
#include "SdFat.h"
// it commented to use the built in sd card.
// 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;
//============初期　関数================================-
//#######################################################################################
//***************************************************************************************
//***************************************************************************************
//***************************************************************************************
void setup() {
Serial.begin(460800);
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();
//preがあるので、initは不要
// myFileA = sd.open("Init_1_Min.ubx", FILE_WRITE);
// myFileA.truncate(1);//initファイルは重ね書きで容量増やさない
}
//*********************************************************************************************************************
//*********************************************************************************************************************
//*********************************************************************************************************************
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++;
}
}
// Serial.println();
//delay(3);
//Serial.println(millis());
ett=millis();
if(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++;
}
}
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++;
}
}
//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("*********************Reciving OK**********************************");
}
else{
Serial.println("**********************Reciving 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 ){//１デバイスでもイン可能
count_upB_1=count_upB;
Serial.printf("%d,",count_upB);
//###########起動から２８秒後にファイル名タイムスタンプから作る##################################f(millis()-startT>28000 && initflag==0 && preflag==1){//1回だけ２８秒後のタイムスタンプでファイル名作る
flags=dBuf1[27];
//Serial.printf("-----flags=%d\n\r",flags);
if(flags==131 && initflag==0 ){//FILE NAME 作成
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にファイル名を格納する
// int lenB=sizeof(fnameB);
//for(i=0;i<lenB;i++){
// Serial.print(fnameB[i]);
// }
// Serial.println();
sd1stflag=0;
namedflag=1;
}//if flags==131
//初回だけここでOPENする
if(namedflag==1){//FIlenameが既存なら
delay(10);
myFileA = sd.open(fnameB, FILE_WRITE);//timestampファイル名fnameB
if(!myFileA){
Serial.println("=============1st File Open Error============");
}
else{
Serial.println("==============1st File Open Seccessed=========");
}
namedflag=0;
logcount=0;
Sflag=1;
delay(10);
}
if(myFileA && Sflag==1){
for(i=0;i<172;i++){
myFileA.write(dBuf1[i]);
}
for(i=0;i<172;i++){
myFileA.write(dBuf4[i]);
}
for(i=0;i<300;i++){
myFileA.write(dBuf5[i]);
}
//logcount++;
//Serial.printf("logcount=%d\n\r",logcount);
}else {
// if the file didn't open, print an error:
Serial.println(">>>>>>>>>>>>>>>>error opening myFile");
}//myFileM
//if(millis()-startT>30000 && Sflag==1){//30secに一回CLOSE OPEN する
delay(1);
if(count_upB%250==0 && Sflag==1){
logcount++;
startT=millis();
myFileA.close();
delay(10);
//myFileA = sd.open(AllFname, FILE_WRITE);
myFileA = sd.open(fnameB, FILE_WRITE);//timestampファイル名fnameB
//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*30);
}
flagB=0;
flagR=0;
flagM=0;
}//if flagB,flagR,flagM
//Serial.println();
}
//*********************************SUBs************************************************************
//---------------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が０時以前の場合日付を日本日付に修正
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==131 && 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 = sd.open(fnameAv,FILE_WRITE);
if(myFileAv){
myFileAv.write(monC,strlen(monC));
}
delay(8);
// myFileT.println(stc1);
myFileAv.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