Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/STA25_BASE_NAV_SAT_SD_TEST_0853.ino Secret

Created January 12, 2025 08:51
Download ZIP
Teemsy41_F9P SAT data Log Pgm
Raw
STA25_BASE_NAV_SAT_SD_TEST_0853.ino
//STA25 Base SAT Rcieve Test & SDIO LOG 2025/1/11
//08はloop直下で他の処理殺して動作OK
//081は、ソート処理を後処理へ移動、受信のみloop直下
//084　バッファ768で動作OK　しかしバグ
//0841 バッファ１０２４に戻すやはり　OPENERRORがでた
//0842 768に戻すしかしOpen errorでた084でOKしてから再度0842OKだったN＝３回起動テスト全OKだった
//0843 dBuf4[]を変換後にゼロリセット追加 N1OK
//085 Filenameで日付ファイル名に変更OK
//0851 実データwrite
//0852 シリアルバッファ動作解析=>満杯の場合は置いたまま後処理に行くのでrelposを置いてしまうからダメ
//0853 sdwrite４で受信直後にdBuf4をdN分だけ保存してつなぎ目でおちをなくす。
#include "SdFat.h"
#define SD_CONFIG SdioConfig(FIFO_SDIO)
// SdFat-beta usage
SdFs sd;
FsFile myFile;
FsFile myFileB;
FsFile dir;
FsFile file;
#define error(s) sd.errorHalt(&Serial, F(s))
//---------SD parameters---------------
//String fnameI="test.csv";
static int startime,sdflag;
static int dcount;
//FileNames
static int fnflag;//1回だけ使う0=>1
int JST_day;
//timestamp file name
volatile static uint8_t Sflag = 0;
uint8_t logflag=0;
static 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
int chk;
//--------Base Parameters-------------------------------------
static uint8_t serialbuffer4[768];//staticにしないとsdFatとぶつかる
static uint8_t dBuf4[1024],dBuf4P[1024],dBuf4V[100],dBuf4R[72],dBuf4S[1024];
static int i,n,j;
static int dN,stPVTN,stRELN,stSATN;
uint8_t d0,d1,d2,d3,d4;//B5,62,01,07,5c
static int toptime,rcvtime,rcvtime_1;
static int ts,te;
static int pvtpos,relpos,satpos;
//ubx
static long itowpvt,itowsat,itowrelp;
static long pvt[33],relp[20];
void basesort(int dn,int stPVTN);
//--SATTELLITE Parameters
static int numSV,messize,paykoadsize;
static uint8_t gnssid[100];
static uint8_t satid[100];
static int cno[100];
static int elevation[100];
static int azimath[100];
static int upper40[100],n40;//40dB以上の衛星配列番号記録
static int upper50[100],n50;//50dB以上の衛星配列番号記録
static int flags,numSVr;
static double hacc;
//============SD関数=========================================================================
void sdlist()
{
int ci=0;//file couter
// Open root directory
if (!dir.open("/")){
error("dir.open failed");
}
// Open next file in root.
// Warning, openNext starts at the current position of dir so a
// rewind may be necessary in your application.
while (file.openNext(&dir, O_RDONLY)) {
ci++;
file.printFileSize(&Serial);
delay(1);
//file.printFileSize(&Serial2);
Serial.write(' ');
//Serial2.write(' ');
file.printModifyDateTime(&Serial);
delay(1);
//file.printModifyDateTime(&Serial2);
Serial.write(' ');
delay(1);
//Serial2.write(' ');
file.printName(&Serial);
delay(1);
//file.printName(&Serial2);
if (file.isDir()) {
// Indicate a directory.
Serial.write('/');
//Serial2.write('/');
}
Serial.println();
//Serial2.println();
file.close();
}
if (dir.getError()) {
Serial.println("openNext failed");
} else {
Serial.println("Done!");
}
}
//+++++++++++++++++++++++++++++++++
void doReboot()
{
Serial.println("#####################DO REBOOT##############");
SCB_AIRCR = 0x05FA0004;
}
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
String stimeI="bno_"+stime+".bin";//BNO data binary File
int slenB = stimeB.length() + 1;
int slenAv=stimeAv.length()+1;
int slenI=stimeI.length()+1;
//ファイル名はchar配列なのでStringからchar配列変換　fname[]を得る
stimeB.toCharArray(fnameB, slenB); //stimeB to fnameB[] chara Array
stimeAv.toCharArray(fnameAv, slenAv); //stimeAv to fnameAv[] chara Array
stimeI.toCharArray(fnameI,slenI);//stimeI to fnameI[] char Array
Serial.print("fnameB=");
Serial.println(stimeB);
Serial.print("fnameAv=");
Serial.println(stimeAv);
Serial.print("fanemI=");
Serial.println(stimeI);
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 + 61], d[s + 60], d[s + 59], d[s + 58]);
//20:flags[60-63]
relpos[20] = B2L(d[s + 65], d[s + 64], d[s + 63], d[s + 62]);
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;
// Serial.printf("itochar:i=%d,d[0]=%x,d[1]=%x,d[2]=%x,d[3]=%x\n\r",i,d[0],d[1],d[2],d[3]);
}
//++++++++++++++++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=
//===========================================================================================
void basesort(int dn,int stPVTN)
{
//******************生データをソートする場所で２分割表示***************************::
//Serial.println();
// Serial.printf("********************SourceData dBuf4[] CHECK dN=%d,stPVTN=%d*******************************:\n\r",dN,stPVTN);
int x=0;//普段は使わない
if(x==1)
{
for(i=0;i<dN;i++)
{
if(i<stPVTN)
{
//Serial.print(dBuf4[i],HEX);
//Serial.print(",");
}
else if(i==stPVTN)
{
//Serial.println();
//Serial.print("HEADER=>,");
//Serial.print(dBuf4[i],HEX);
//Serial.print(",");
}
else if(i>stPVTN)
{
//Serial.print(dBuf4[i],HEX);
//Serial.print(",");
}
}
}
//===Base SORT処理開始========================================================
//Serial.println();
//Serial.printf("----------------------BaseSORTing＝＞SORT dBuf4=>dBuf4P dN=%d stPVTN=%d-------------------------------------\n\r",dN,stPVTN);
//Serial.print(" dBuf4:");
j=stPVTN;
int PVT1startNo = j;
//Serial.printf("PVT1startNo=%d\n\r",PVT1startNo);
//Sorting--------------------------------------------------------------------------------
int jj;
for (jj = 0; jj <dN - PVT1startNo; jj++) //後半部Sort dBuf4[PVT1startNo]～dBuf4[dN-1]　To dBuf14[0]～dBuf4[dN-1-PVT1startNo]
{
dBuf4P[jj] = dBuf4[PVT1startNo + jj];//ヘッダーのある位置からよみとって、新しい配列dBuf4Pに代入して、データの最後までdN-PVT1startNo　個コピー
if(jj<6){
//Serial.printf("dBuf4P[%d]=%x\n\r", jj,dBuf4P[jj]);
}
if(jj>dN- PVT1startNo-6){
//Serial.println();
//Serial.printf("dBuf4P[%d]=%x\n\r",jj,dBuf4P[jj]);
}
}
for (jj = 0; jj < PVT1startNo; jj++) //先頭部ソート
{
dBuf4P[jj + dN- PVT1startNo] = dBuf4[jj];
//Serial.printf("dBuf4P[%d]=%x,",jj+dN-PVT1startNo,dBuf4[jj]);
if(jj<6){
//Serial.printf("dBuf4P[%d]=%x\n\r",jj + dN- PVT1startNo,dBuf4[jj]);
}
if(jj>PVT1startNo-6){
//Serial.println();
//Serial.printf("dBuf4P[%d]=%x\n\r",jj + dN- PVT1startNo,dBuf4[jj]);
}
}
//Serial.println("===================SORTED:dBuf4P:======================================");
for(j=0;j<dN;j++)
{
//Serial.print(dBuf4P[j],HEX);
//Serial.print(",");
dBuf4[i]=0;//dBuf4クリア
}
}
void sdinit()
{
if (!sd.begin(SD_CONFIG)) {
Serial.println("SD CARD initialization failed!");
return;
}
Serial.println("SD initialization done.");
/*
myFileI = sd.open("test01.txt", FILE_WRITE);
if(!myFileI)
{
Serial.println("=============myFileI Open Error============");
exit(0);
}
else
{
Serial.println("==============myFileI Open Successed=========");
}
*/
}
//Base Recieve
int basercv()
{
int bufcnt=0;
if(Serial4.available()>172)//Serial4から一挙に受信
{
i=0;
rcvtime_1=rcvtime;
rcvtime=millis();
while(Serial4.available()>0)
{
if(Serial4.available())//readの直前にSerial.available()を入れないとバグる
{
dBuf4[i+4]=Serial4.read();
i++;
}
bufcnt++;
}
dN=i;//受信データ数
dcount++;
//Serial.printf("RECIEVED,dN=,%d ,toptime=%d,recvtime-rcvtime_1=%d\n\r",dN,toptime,rcvtime-rcvtime_1);
//Serial.printf("==================recieved dN=%d,bufcnt=%d\n\r",dN,bufcnt);
}//if serial
return dN;
}
//-----------------------------------------------------------
int sdwrite(int dcount)//dBuf４を保存
{
int c;
//==========================================================================================================
//=================OPEN CLOSE TIMER=========================================================================
//=========================================================================================================
//if(millis()%20000<=>0 )//30secに一回close open
//dcount++;
if(dcount%500==0)//60sec
{
//dcount=0;
myFile.close();
delay(15);//このWAIT重要close open間
Serial.printf("***********************************myFileB Close&Open t=%d***********************************************************\n\r",millis());
myFile = sd.open(fnameB, FILE_WRITE);
if(!myFile){
Serial.println("************myFIle timer OPEN ERROR*******************");
c=1;
}
//for(i=0;i<dN;i++)
//{
//if(myFile)
//{
//myFile.write(dBuf4P[i]);
//Serial.print("*");
//}
//else
//{
//Serial.print("x");
//}
//}
}
//1回OPEN
//=================SD LOG===========================
delay(5);
if(!myFile)
{
Serial.printf("=============myFile open Error============%d\n\r",dcount);
c=2;
//exit(0);
}
else
{
//Serial.println("==============myFile Write Successed=========");
for(i=0;i<dN;i++)
{
if(myFile){
myFile.write(dBuf4[i]);
}
}
c=0;
}
//SD LOG END
return c;//c=0 OK c=1 OPEN error c=2 write error
}
void setup()
{
Serial.begin(460800);
Serial4.begin(115200);
Serial4.addMemoryForRead(serialbuffer4, sizeof(serialbuffer4));
delay(10000);
if (!sd.begin(SD_CONFIG)) {
Serial.println("initialization failed!");
return;
}
Serial.println("initialization done.");
delay(100);
// 1st test OPEN
myFileB = sd.open("fname00.ubx", FILE_WRITE);
if(!myFileB)
{
Serial.println("=============myFileB 1st Open Error============");
exit(0);
}
else
{
Serial.println("==============myFileB 1st Open Successed=========");
for(i=0;i<100;i++)
{
if(myFileB){
myFileB.write(0x0A);
}
}
}
myFileB.close();
//test open end
Serial.println("sdlist GO");
sdlist();
Serial.println("---------Wait For AnyKeyIn");
while(Serial.available()==0)
{
if(Serial.available()>0)
{
Serial.println("GO START");
}
}
startime=millis();
sdflag=0;
chk=3;//3:LOG無し 2:Writeエラー　１:OPENエラー　０：SD書き込みOK
}
void loop()
{
delay(50);//１周期120msecなので十分データをためてから全部まとめて読むためのDELAY
dN=basercv();
if(dcount>300){//fnameBがdcount=200で着くまで待つ
chk=sdwrite(dcount);
}
//-----------------受信結果-----------------------------------------------------
//Serial.println();
//Serial.printf("RECIEVED,dN=,%d ,toptime=%d,recvtime-rcvtime_1=%d\n\r",dN,toptime,rcvtime-rcvtime_1);
if( dN>172)
{
//###############################ソースデータSORTしてdBUf4P[]に収納######################################################
for(n=0;n<dN;n++)
{
if(dBuf4[n]==0xB5 && dBuf4[n+1]==0x62 && dBuf4[n+2]==0x01 &&dBuf4[n+3]==0x07)
{
stPVTN=n;//PVTスタート番号
if(n==0)
{
stPVTN=0;
Serial.println();
Serial.printf("-----------NoSort HeaderOK dN=%d stPVTN=%d-------------------\n\r",dN,stPVTN);
for(i=0;i<dN;i++)
{
dBuf4P[i]=dBuf4[i];//後処理用配列dBuf4Pに渡す
//Serial.print(dBuf4P[i],HEX);
// Serial.print(",");
}
//Serial.printf("dN=%d,stPVTN=%d,%x,%x,%x,%x",dN,stPVTN,dBuf4[n],dBuf4[+1],dBuf4[n+2],dBuf4[n+3]);
}
else
{
basesort(dN,stPVTN);
//Serial.println();
//Serial.printf("dN=%d,stPVTN=%d,%x,%x,%x,%x",dN,stPVTN,dBuf4[n],dBuf4[+1],dBuf4[n+2],dBuf4[n+3]);
}
}
}
//Serial.printf("dcount=%d,dN=%d,te-ts=%d-------------\n\r",dN,te-ts);
ts=millis();
//###############################メッセージ分類######################################################
if(dBuf4P[0]!=0xb5)
{
Serial.println("SORT ERROT");
}
//メッセージ開始位置検索
//Serial.printf("<<<<<<<<<<dn=%d>>>>>>>>>>>>>>>\n\r",dN);
for(i=0;i<dN;i++)
{
dBuf4[i]=0;//使わなくなった受信バッファ配列をゼロリセット
if(dBuf4P[i]==0xB5 && dBuf4P[i+1]==0x62 && dBuf4P[i+2]==0x01 &&dBuf4P[i+3]==0x07)
{
pvtpos=i;
//Serial.printf("pvtpos=%d,dBuf4P[%d-%d]=%x,%x,%x,%x\n\r",pvtpos,i,i+3,dBuf4P[i],dBuf4P[i+1],dBuf4P[i+2],dBuf4P[i+3]);
}
//RELPOSNED抽出
if(dBuf4P[i]==0xB5 && dBuf4P[i+1]==0x62 && dBuf4P[i+2]==0x01 &&dBuf4P[i+3]==0x3c)
{
relpos=i;
//Serial.printf("relpos=%d,dBuf4P[%d-%d]=%x,%x,%x,%x\n\r",relpos,i,i+3,dBuf4P[i],dBuf4P[i+1],dBuf4P[i+2],dBuf4P[i+3]);
}
if(dBuf4P[i]==0xB5 && dBuf4P[i+1]==0x62 && dBuf4P[i+2]==0x01 &&dBuf4P[i+3]==0x35)
{
satpos=i;
//Serial.printf("satpos=%d,dBuf4P[%d-%d]=%x,%x,%x,%x\n\r",satpos,i,i+3,dBuf4P[i],dBuf4P[i+1],dBuf4P[i+2],dBuf4P[i+3]);
}
}//for i
//Serial.printf("HeaderPos:dN=%d,pvtpos=%d,satpos=%d,relpos=%d\n\r",dN,pvtpos,satpos,relpos);
//^^^^^^^^^^^^^^^^^^^^^^NAV-SAT GET^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
int pvtnum,relpnum,satnum;
for(i=0;i<100;i++)//PVT1配列作成
{
dBuf4V[i]=dBuf4P[i];
pvtnum++;
}
for(i=relpos;i<dN;i++)//relpos配列作成
{
dBuf4R[i-relpos]=dBuf4P[i];
relpnum++;
}
for(i=satpos;i<relpos;i++)//satの最後がRELPOSの開始
{
dBuf4S[i-satpos]=dBuf4P[i];
satnum++;
}
//=======NAV-PVTからFname ================
int pvti=PVTcnv(dBuf4V,pvt);
if(fnflag==0 && dcount>200){
Filename (pvt); //pvt送ってfnameBいれる
fnflag=1;
}
//======================================
//dBuf4V[27]=flags
//dBuf4V[29]=numSVRTK
//dBuf4V[46]=hAcc LL/dBuf4V[47]=hAcc LH/dBuf4V[48]=hAcc HL/dBuf4V[49]=hAcc HH
flags=dBuf4V[27];
numSVr=dBuf4V[29];//RTK計算で使っている衛星数SATのnumSVより少ない
hacc=double(dBuf4V[46]+dBuf4V[47]*256+dBuf4V[48]*256*256+dBuf4V[49]*256*256*256)/100; //m単位
///pvtcnv
/*========NAV-SAT 内訳===================
dBuf4S[0]=0xB5;
dBuf4S[1]=0x62;
dBuf4S[2]=0x01;
dBuf4S[3]=0x35;
dBuf4S[4]=0x35;//Message Length L
dBuf4S[5]=0x35;//Message Length H
dBuf4S[6]=Serial4.read();//itow0
dBuf4S[7]=Serial4.read();//itow1
dBuf4S[8]=Serial4.read();//itow2
dBuf4S[9]=Serial4.read();//itow3
dBuf4S[10]=Serial4.read();//ver
dBuf4S[11]=Serial4.read();//numSV
dBuf4S[12]=Serial4.read();//reserved1
dBuf4S[13]=Serial4.read();//reserved2
//SAT Payloads N x 12 --------------------
dBuf4S[14]=//GNSS ID
dBuf4S[15]=//SATELLITE ID　衛星番号　１バイト
dBuf4S[16]=//CNO Carrier tonoise ratio dBHz　強度１バイト
dBuf4S[17]=//Elevation 90deg 　仰角１バイト
dBuf4S[18]=//Azimas 360deg L
dBuf4S[19]=//Azimzas 360deg H　方位角２バイト
dBuf4S[20]=//Pseuidorange residuals L
dBuf4S[21]=//Pseuidorange residuals L
dBuf4S[22]=//Flags_1Bit /SignalQuality id_3Bit/svUsed_Bit1/Health_2Bit/diffCur_1Bit/Smoothed_1Bit
dBuf4S[23]=//OrbitSoiurce_3Bit/Ephmeris_1Bit/
dBuf4S[24]=//
dBuf4S[25]=//
//Next Satellite
dBuf4S[26]=//GNSS ID
*/
//itowpvt=dBuf4V[6]+dBuf4V[7]*256+dBuf4V[8]*256*256+dBuf4V[9]*256*256*256;
//itowsat=dBuf4S[6]+dBuf4S[7]*256+dBuf4S[8]*256*256+dBuf4S[9]*256*256*256;
//itowrelp=dBuf4R[10]+dBuf4R[11]*256+dBuf4R[12]*256*256+dBuf4R[13]*256*256*256;
//Serial.printf("itowsat=%d,itowrelp=%d,itowpvt=%d\n\r",itowsat,itowrelp,itowpvt);;
//NAV-SAT解読 [14]から開始[26] [38].....
int mesLen=dBuf4S[4]+dBuf4S[5]*256;
int numSV=dBuf4S[11];
int GNSSID=dBuf4S[14];
int SATID=dBuf4S[15];
int CNO=dBuf4S[16];
//Serial.printf("===================mesLen=%d,numSV==%d===================\n\r",mesLen,numSV);
for(n=0;n<numSV;n++)
{
int stN=14+n*12;
gnssid[n]=dBuf4S[stN];
satid[n]=dBuf4S[stN+1];
cno[n]=dBuf4S[stN+2];
elevation[n]=dBuf4S[stN+3];
azimath[n]=dBuf4S[stN+4];dBuf4S[stN+5];
if(cno[n]>=40)
{
upper40[n40]=n;
n40++;
}
else if(cno[n]>=50)
{
upper50[n50]=n;
n50++;
}
//Serial.printf("%d:gnsid=%d,satid=%d,cno=%d,elev=%d,azimath=%d\n\r",n,gnssid[n],satid[n],cno[n],elevation[n],azimath[n]);
}
//SATが全部含まれているセットだけ採用
int L=numSV*12+14+100;
if(dN>L && numSV>10){
Serial.printf("%d,%d,%d,numSV=%d,Upper40=%d.Upper50=%d,numSVr=%d,flags=%d,hacc=%4.3f\n\r",chk,dcount,dN,numSV,n40,n50,numSVr,flags,hacc);
}
n40=0;
n50=0;
pvtpos=1;
satpos=0;
relpos=0;
te=millis();
delay(20);
}//if dN>172
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment