Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/DR_M9N_Stable3_BT_F9P_20msec_Car__rev17.ino Secret

Created July 10, 2022 05:52
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/fa30615dad79ddf06e65d1ce6f2bc150 to your computer and use it in GitHub Desktop.
Save dj1711572002/fa30615dad79ddf06e65d1ce6f2bc150 to your computer and use it in GitHub Desktop.
Download ZIP
Teensy4.1　　ArduinoIDE　　RTK DeadReckoning　F9P10Hz　by　M9N25Hz
Raw
DR_M9N_Stable3_BT_F9P_20msec_Car__rev17.ino
//DR_M9N_Stable2_BT_F9P_rev06
#include "SdFat.h"
#include "sdios.h"
#include <SPI.h>
//-----USB HOST fo Power ON switch
#include "USBHost_t36.h"
#define USBBAUD 460800//115200 //460800
uint32_t baud = USBBAUD;
uint32_t format = USBHOST_SERIAL_8N1;
USBHost myusb;
USBHub hub1(myusb);
//
static int itow1,itow1_1;
static int itow4,itow4_1;
static int mis1,mis4;
static uint32_t indexN;
static bool pflag1=false;
static bool pflag4=false;
//---M9N---------------------------
static uint8_t dBuf1[100],dBuf4[100];
static uint8_t s1buf[128],s4buf[128];
//static uint8_t dBuf11[100];
static uint8_t PVTdataM[100],PVTdataP[100];
static long PVTvalM[33],PVTvalP[33];
static uint8_t PVT1startNo,PVT4startNo;
static uint32_t rcvbyte,rcvbyte_1,rcvbyte4,rcvbyte4_1;
static int result1,result4;
static float disN,disE,disD;//100msec積分距離
//main loop ----
static uint32_t epN1,epN1_1;
static uint32_t epN4,epN4_1;
static uint8_t epoflag=0;
static uint8_t epoflag4=0;
static uint32_t sdtime,sdtime_1,starttinme,savetime;
static uint8_t s1flag,s4flag,M9Nflag;
//================SdFat-beta usage=============================
// Setup for built in SD card.
#define SPI_CLOCK SD_SCK_MHZ(50)
#define SD_CONFIG SdioConfig(FIFO_SDIO)
const int chipSelect = 10;
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
static uint8_t Sflag=0;
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];
static uint32_t epNosd,epNosd_1;
int n;//counter
static int i, j,jj, k,l,m;
static int starttime;
static uint32_t counter;
//-----M9N Value--------------------
static float headMot,gSpeed,velN,velE,velD;//main parameters
static float flags,numsv,pdop,hacc,vacc,sacc,headacc;//accuracy check parameters
//@@@@@@@@@@@@@@@@@@@STRUCT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@]
struct PVTstrct{
int spno; //Serial1:M9N Serial4:F9P
//header B5 62 01 07 00 00 0-5
long itow; //1:6-9
long year; //2:10-13
long month; //3:14
long day; //4:15
long hour;
long min;
long sec;
long valid;
long tacc;
long nano;
long fixtype;
long flags;
long flags2;
long numsv;
long lon;
long lat;
long height;
long hmsl;
long hacc;
long vacc;
long veln;
long vele;
long veld;
long gspeed;
long headmot;
long sacc;
long headacc;
long pdop;
long flags3;
long reserved1;
long headveh;
long magdec;
long magacc;
};
//--Structures--------
struct PVTstrct s1pvt,s1pvt_1,s4pvt,s4pvt_1;//現在値と一個前に値で補間する
struct PVTstrct M9Npvt[10],F9Ppvt[10];//20msecx10=200msec分
//struct PVTstrct M9Nhokan[25],F9Phokan[25];
struct PVTstrct s4pvt200,s4pvt300;
static int s4_200flag,s4_300flag;
static int hokanflag1,hokanflag2;
static int M9Ncount;
int M9NhokanN,F9PhokanN;
void setup() {
Serial.begin(460800);
Serial1.begin(115200);
Serial2.begin(115200);
Serial4.begin(115200);
Serial1.addMemoryForRead(s1buf, 128);
Serial4.addMemoryForRead(s4buf, 128);
myusb.begin();
//-----------SD CARD Initialize------------------
if (!sd.begin(SD_CONFIG)) {
Serial.println("~~~~~~~~~~~~~SD CARD initialization failed!~~~~~~~~~~");
delay(600000);
}
Serial.println("initialization done.");
Serial.println("**********Wait for M9N setup 60sec************");
int nowtime=millis();
while(counter<40)
{
counter++;
Serial.printf("%d*",counter);
delay(1000);
}
starttime=millis();
}
//////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////LOOP/////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
//----M9N Serial1 method-----
if(Serial1.available()>99){
i=0;
//Serial.println();
// Serial.printf("s1In:t=%d",millis());
while(i<100){
while(Serial1.available()){
dBuf1[i]=Serial1.read();
//Serial.print(dBuf1[i],HEX);
i++;
rcvbyte++;
}
}
epoflag=1;
//Serial.printf("[s1RCV]t=%d,i=%d",millis(),i);
//Serial.println();
}//
//
//----F9P Serial4 method-----
if(Serial4.available()>99){
j=0;
//Serial.println();
//Serial.printf("s4In:t=%d",millis());
while(j<100){
while(Serial4.available()){
dBuf4[j]=Serial4.read();
//Serial.print(dBuf4[j],HEX);
j++;
rcvbyte4++;
}
}
epoflag4=1;
//Serial.printf("[s4RCV]t=%d,j=%d",millis(),j);
//Serial.println();
}//
//==========================================================================================================
//Serial.printf("rcvbyte4=%d,epoflag4=%d,j=%d\n\r",rcvbyte4,epoflag4,j);
s1flag=rcvbyte-rcvbyte_1>99 && epoflag==1;
s4flag=rcvbyte4-rcvbyte4_1>99 && epoflag4==1;
//Serial.println();
//Serial.printf("s1flag=%d,rcvbyte=%d,epoflag1=%d,j=%d\n\r",s1flag,rcvbyte,epoflag,i);
//Serial.printf("s4flag=%d,rcvbyte4=%d,epoflag4=%d,j=%d\n\r",s4flag,rcvbyte4,epoflag4,j);
if(s4flag==1)
{
epN4++;
}
/*
//M9N itow check
if(s1flag==1 && s4flag==1)
{
itow1_1=itow1;
itow1=(int)(PVTdataM[6]+PVTdataM[7]*256+PVTdataM[8]*256*256+PVTdataM[9]*256*256*256);
Serial.printf("itow1=%d,itow1_11=%d\n\r",itow1,itow1_1);
if(itow1-itow1_1>=40)
{
M9Nflag=1;
}
else
{
M9Nflag=0;
}
}
*/
///////////////////////////////////////////////////////////////////////////////////////
/////epoch 処理//////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
if(s1flag==1 ){//|| s4flag){
//Serial.printf("In:s1flag=%d,s4flag=%d,t=%d\n\r",s1flag,s4flag,millis());
rcvbyte_1=rcvbyte;
rcvbyte4_1=rcvbyte4;
epoflag=0;//1エポック１通過用
epoflag4=0;//1エポック１通過用
epN1++;
//---40msecに1回close openしてSd SAVE---------------
if((myFile && millis()-savetime>40000) && Sflag==1 ){
savetime=millis();
myFile.close();
myFileR.close();
delay(8);
myFile = sd.open(fnameB, FILE_WRITE);//F9P
myFileR= sd.open(fnameR, FILE_WRITE);//M9N
delay(8);
Serial.println();
Serial.println("=========SD OPEN after close =========");
}
//==========================SORTING=================================================================
if(s1flag==1){
// Serial.printf("------------s1Sort,t=%d",millis());
result1=PVTsort(dBuf1,PVTdataM,1);
itow1_1=itow1;
itow1=(int)(PVTdataM[6]+PVTdataM[7]*256+PVTdataM[8]*256*256+PVTdataM[9]*256*256*256);
if(itow1-itow1_1>40){
mis1++;
}
//Serial.printf("itow1=%d,t=%d,result1=%d\n\r",itow1,millis(),result1);
}
delay(1);
if(s4flag==1){
//Serial.printf("------------s4Sort,t=%d",millis());
result4=PVTsort(dBuf4,PVTdataP,4);
itow4_1=itow4;
itow4=(int)(PVTdataP[6]+PVTdataP[7]*256+PVTdataP[8]*256*256+PVTdataP[9]*256*256*256);
if(itow4-itow4_1>102){
mis4++;
}
//Serial.printf("itow4=%d,t=%d,result4=%d\n\r",itow4,millis(),result4);
}
//===================Sorting finished====================================================
//====================PVT補間===============================================
if(Sflag==1){//SD　記録中は変換実行する
//-------------------------------------------------------
PVTcnv(PVTdataM,PVTvalM);//M9N PVTBinary 実データ変換
delay(1);
s1pvt_1=s1pvt;
strctInput(&s1pvt, PVTvalM);
delay(1);
M9N_hokan20(s1pvt_1,s1pvt);
//Serial.printf("{STRCT INPUT}s1pvt.itow=%d\n\r",s1pvt.itow);
//-------------------------------------------------------
PVTcnv(PVTdataP,PVTvalP);//F9P PVTBinary 実データ変換
delay(1);
s4pvt_1=s4pvt;
strctInput(&s4pvt, PVTvalP);
delay(1);
if(s4pvt.itow>s4pvt_1.itow)
{
F9P_hokan20(s4pvt_1,s4pvt);
// Serial.printf("-------F9PindexN=%d,{STRCT INPUT}s4pvt.itow=%d\n\r",indexN,s4pvt.itow);
}
//--------------------------------------------------------
//=========PRINT OUT HOKAN==========================================================================================
delay(1);
if(indexN==5 && pflag1==false)//F9P Timing0-5
{
for(i=0;i<6;i++)
{
//Serial.printf("F9Ppvt[%d].itow=%d,M9Npvt{%d]=%d,t=%d\n\r",i,F9Ppvt[i].itow,i,M9Npvt[i].itow,millis());
Serial.printf("%d,%3.1f,%d,%3.1f,%d,%d,%d,%d\n\r",F9Ppvt[i].gspeed,(float)F9Ppvt[i].headmot*0.00001,M9Npvt[i].gspeed,(float)M9Npvt[i].headmot*0.00001,F9Ppvt[i].lon%1000000,F9Ppvt[i].lat%1000000,s4pvt.flags,s4pvt.hacc);
Serial2.printf("%d,%3.1f,%d,%3.1f,%d,%d,%d,%d\n\r",F9Ppvt[i].gspeed,(float)F9Ppvt[i].headmot*0.00001,M9Npvt[i].gspeed,(float)M9Npvt[i].headmot*0.00001,F9Ppvt[i].lon%1000000,F9Ppvt[i].lat%1000000,s4pvt.flags,s4pvt.hacc);
}
delay(1);
pflag1=true;
pflag4=false;
}//if
else if(indexN==0 && pflag4==false)
{
for(i=6;i<10;i++)
{
//Serial.printf("F9Ppvt[%d].itow=%d,M9Npvt{%d]=%d,t=%d\n\r",i,F9Ppvt[i].itow,i,M9Npvt[i].itow,millis());
Serial.printf("%d,%3.1f,%d,%3.1f,%d,%d,%d,%d\n\r",F9Ppvt[i].gspeed,(float)F9Ppvt[i].headmot*0.00001,M9Npvt[i].gspeed,(float)M9Npvt[i].headmot*0.00001,F9Ppvt[i].lon%1000000,F9Ppvt[i].lat%1000000,s4pvt.flags,s4pvt.hacc);
Serial2.printf("%d,%3.1f,%d,%3.1f,%d,%d,%d,%d\n\r",F9Ppvt[i].gspeed,(float)F9Ppvt[i].headmot*0.00001,M9Npvt[i].gspeed,(float)M9Npvt[i].headmot*0.00001,F9Ppvt[i].lon%1000000,F9Ppvt[i].lat%1000000,s4pvt.flags,s4pvt.hacc);
}
delay(1);
pflag1=false;
pflag4=true;
}
else
{
delay(1);
}
}
//======================================================================
//Serial.printf("***********startime=%d,time=%d\n\r",starttime,millis()-starttime);
//***********************File name timestumping ****************************************
delay(1);
if(millis()-starttime>20000 && Sflag==0){//start20sec後でファイル名　タイムスタンプ
Serial.printf("[FILE NAMING]PVTvalP[1-5]=%d,%d,%d,%d,%d\n\r",PVTvalP[1],PVTvalP[2],PVTvalP[3],PVTvalP[4],PVTvalP[5]);
Filename(PVTvalP);
myFile = sd.open(fnameB, FILE_WRITE);
myFileR = sd.open(fnameR, FILE_WRITE);
delay(5);
Serial.println();
Serial.println("=========SD OPEN 1st =========");
//Sflag=1;
sdtime=millis();
}
//===F9P SD =============================================
if(Sflag==1 && myFile==1 && s4flag==1){
k=0;
//Serial.print("＋");
while(k<100) {
//----F9P ------------------------
if (myFile) { //重要！！myFileチェックは、write命令直前で行うこと
myFile.write(PVTdataP[k]);
k++;
}
else{
Serial.println("*******************F9P SD WRITE ERROR********************");
}
}
delay(1);//DSハング防止重要
//Serial.printf("SD Write Finished:k=%d,RCVBYTE=%d t=%d\m\r",k,rcvbyte,millis());
}//F9P sd write end
//===M9N SD===============================================================================
if(Sflag==1 && myFileR==1 && s1flag==1){
m=0;
//Serial.print("*");
while(m<100) {
//----F9P ------------------------
if (myFileR) { //重要！！myFileチェックは、write命令直前で行うこと
myFileR.write(PVTdataM[m]);//myFile.write(dBuf1[k]);
m++;
}
else{
Serial.println("*******************M9N SD WRITE ERROR********************");
}
}//while end
//Serial.printf("M9N SD Write Finished:k=%d,RCVBYTE=%d t=%d\m\r",k,rcvbyte,millis());
}//M9N write end
//Serial.println();
//}// epoch processing end
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++ PVTstrct Synchronizing+++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//送信周期はエポック周期と独立させて、itow同期が完了後送信する
//s1 仕分け
if(s1pvt.itow%200==0){
M9Npvt[M9Ncount%5]=s1pvt;
// Serial.printf("=====F9Ppvt[0].itow=%d,M9Npvt[0].itow=%d\n\r",F9Ppvt[0].itow,s1pvt.itow);
}
if(s1pvt.itow%200==40){
M9Npvt[1]=s1pvt;
}
if(s1pvt.itow%200==80){
M9Npvt[2]=s1pvt;
}
if(s1pvt.itow%200==120){
M9Npvt[3]=s1pvt;
}
if(s1pvt.itow%200==160){
M9Npvt[4]=s1pvt;
}
}//s1flag
}// loop end
//*************************PVTsort********************************************:
int PVTsort(uint8_t dbs[100],uint8_t PVTdata[100],int flg){
int PVTstartNo;
int j,jj;
int result;
//Serial.println("-----------SORT IN--------------");
//==========================SORTING=================================================================
for(j=0;j<100;j++) //Search PVT Header------------------------------------------------------
{
if(dbs[j]==0xb5 && dbs[j+1]==0x62 && dbs[j+2]==0x01 && dbs[j+3]==0x07) //dbs[] PVT header
{
PVTstartNo=j;
result=1;
// Serial.print("OK_result=1");
break;
}
else{
PVTstartNo=0;
result=0;
//Serial.print("Else_result=0");
}
//Serial.print(dBuf1[j],HEX);
}
//Serial.println();
//Serial.println("Search end");
//Sorting--------------------------------------------------------------------------------
for(jj=0;jj<100-PVTstartNo;jj++)//Sort dBuf1[PVT1startNo]～dBuf1[99]　To dBuf11[0]～dBuf11[99-PVT1startNo]
{
//dBuf11[jj]=dBuf1[PVT1startNo+jj];
PVTdata[jj]=dbs[PVTstartNo+jj];
}
//Serial.println();
//Serial.printf("PVTstartNo=%d",PVTstartNo);
delay(1);//これないとハングする
for(jj=0;jj<PVTstartNo;jj++)//Sort
{
//dBuf11[jj+100-PVT1startNo]= dBuf1[jj];
PVTdata[jj+100-PVTstartNo]= dbs[jj];
}
// Serial.printf("[s%dSORT]%d:",flg,millis());
for(j=0;j<5;j++)
{
//Serial.print(PVTdata[j],HEX);
}
//Serial.println();
//Serial.printf("s%dOut:t=%d\n\r",flg,millis());
return result;
//===================Sorting finished====================================================
}
//+++++++++++++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;
}
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
int PVTcnv(uint8_t d[100], long pvt[33]) {
//--PVT header[0-5]
//--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] 3=3D-fix
pvt[10] =(long) d[26];
//--11:flags[27] This is Fix status 131(F9P) M9N:1=3DFIX,3=DGNDSS FIX
pvt[11] = (long)d[27];
//--12:flags2[28]
pvt[12] = (long)d[28];
//--13:numSV[29] satellite Num
pvt[13] = (long)d[29];
//--14:lon[30-33] longtitude 136.xxxxxxx 10-7deg cm unit
pvt[14] = B2L(d[33], d[32], d[31], d[30]);
//--15:lat[34-37] latitude 27.xxxxxxx 10-7deg cm unit
pvt[15] = B2L(d[37], d[36], d[35], d[34]);
//--16:height[38-41] sea height mm
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]U4 horizontal accuracy mm
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]I4 North Velocity mm/sec
pvt[20] = B2L(d[57], d[56], d[55], d[54]);
//--21:velE[58-61]I4 East Velocity mm/sec
pvt[21] = B2L(d[61], d[60], d[59], d[58]);
//--22:velD[62-65]I4 Vertical Velocity mm/sec
pvt[22] = B2L(d[65], d[64], d[63], d[62]);
//--23:gSpeed[66-69]I4 Ground Speed mm/sec
pvt[23] = B2L(d[69], d[68], d[67], d[66]);
//--24:headMot[70-73]I4 10-5 deg Heading Motion
pvt[24] = B2L(d[73], d[72], d[71], d[70]);
//Serial.printf("===========pvt[24]:headmot=%d\n\r",pvt[24]);
//25:sAcc[74-77]U4 Speed accuracy
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
//------------------------------------------------------------------------
//@@@@@@@@@@@@@struct@@@@@@@@@@@@@@@@@@@
void strctInput(PVTstrct *test, long pvtval[33]){
(*test).itow=pvtval[0];
(*test).year=pvtval[1];
(*test).month=pvtval[2];
(*test).day=pvtval[3];
(*test).hour=pvtval[4];
(*test).min=pvtval[5];
(*test).sec=pvtval[6];
(*test).valid=pvtval[7];
(*test).tacc=pvtval[8];
(*test).nano=pvtval[9];
(*test).fixtype=pvtval[10];
(*test).flags=pvtval[11];
(*test).flags2=pvtval[12];
(*test).numsv=pvtval[13];
(*test).lon=pvtval[14];
(*test).lat=pvtval[15];
(*test).height=pvtval[16];
(*test).hmsl=pvtval[17];
(*test).hacc=pvtval[18];
(*test).vacc=pvtval[19];
(*test).veln=pvtval[20];
(*test).vele=pvtval[21];
(*test).veld=pvtval[22];
(*test).gspeed=pvtval[23];
(*test).headmot=pvtval[24];
//Serial.printf("CNV:pvtval[24]=%d\n\r",pvtval[24]);
(*test).sacc=pvtval[25];
(*test).headacc=pvtval[26];
(*test).pdop=pvtval[27];
(*test).flags3=pvtval[28];
(*test).reserved1=pvtval[29];
(*test).headveh=pvtval[30];
(*test).magdec=pvtval[31];
(*test).magacc=pvtval[32];
}
//*****************************************************************************************************************************
void Filename (long PVTval[33]){
//*********ファイル名をタイムスタンプで作ってファイルOPEN FIXしたら flags==131 *****************************************************************************
//if (PVTval[11] == 131 && Sflag == 0) { //start
Sflag = 1;
//---PVTval[1]=Year/[2]=Month/[3]=Day/[4]=Hour/[5]=Min/[6]=sec---
int JST = (PVTval[4] + 9) % 24; //UTC hourをJSTに変換
if (JST<9){//UTCが０時以前の場合日付を日本日付に修正
JST_day=1;
}
else{
JST_day=0;
}
String stime = String(PVTval[2], DEC) + "-" + String(PVTval[3]+JST_day, DEC) + "-" + String(JST, DEC) + "-" + String(PVTval[5], DEC); //MMDDHHMM
String stimeB = "/" + stime + "P.ubx"; //fnameB=F9P PVT,UBX Binary File
String stimeR ="/" +stime+"M.ubx"; //fnameR=M9N PVT,UBX Binary File
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 ******************************************************************************************************************************************
///////////////////F9P補間20msec////////////////////////////////////////////
//２エポックに１回補間擦るINDEX=(itowend-itowstart)/20
void M9N_hokan20(PVTstrct start,PVTstrct end)
{
int itow0,itow1,itow2;
int indexs,index1,indexe;
if(end.itow-start.itow==40)
{
indexs=(int)(start.itow%200)/20;
index1=(indexs+1)%10;
indexe=(index1+1)%10;
//Serial.printf("------M9N_start.itow=%d[%d],end.itow=%d[%d]------\n\r",start.itow,indexs,end.itow,indexe);
M9Npvt[indexs]=start;
M9Npvt[indexe]=end;
M9Npvt[index1].itow=start.itow+20;
//Serial.printf("%d,M9Npvt[,%d,],itow=,%d,%d,%d,%d,%d,%d,%d,\n\r", indexs,indexs,M9Npvt[indexs].itow,millis(),M9Npvt[indexs].veln,M9Npvt[indexs].vele,M9Npvt[indexs].veld,M9Npvt[indexs].gspeed,M9Npvt[indexs].headmot);
//velN
M9Npvt[index1].veln=(long)(end.veln-start.veln)/2+start.veln;
//velE
M9Npvt[index1].vele=(long)(end.vele-start.vele)/2+start.vele;
//veld
M9Npvt[index1].veld=(long)(end.veld-start.veld)/2+start.veld;
//gspeed
M9Npvt[index1].gspeed=(long)(end.gspeed-start.gspeed)/2+start.gspeed;
//headmot
M9Npvt[index1].headmot=(long)(end.headmot-start.headmot)/2+start.headmot;
//Serial.printf("%d,M9Npvt[,%d,],itow=,%d,%d,%d,%d,%d,%d,%d\n\r", M9NhokanN,index1,M9Npvt[index1].itow,millis(),M9Npvt[index1].veln,M9Npvt[index1].vele,M9Npvt[index1].veld,M9Npvt[index1].gspeed,M9Npvt[index1].headmot);
//Serial.printf("%d,M9Npvt[,%d,],:itow=,%d,%d,%d,%d,%d,%d,%d\n\r", M9NhokanN,indexe,M9Npvt[indexe].itow,millis(),M9Npvt[indexe].veln,M9Npvt[indexe].vele,M9Npvt[indexe].veld,M9Npvt[indexe].gspeed,M9Npvt[indexe].headmot);
M9NhokanN++;//hokan count　５回でアップ
}
else
{
Serial.printf("Error M9Nhokan20 ,mis1=,%d\n\r",mis1);
}
delay(1);
}
//２エポックに１回補間擦るINDEX=(itowend-itowstart)/20
void F9P_hokan20(PVTstrct start,PVTstrct end)
{
int itow0,itow1,itow2;
int indexs,index1,indexe;
if(end.itow-start.itow==100)//F9P 100msec
{
indexs=(int)(start.itow%200)/20;//0,5
indexe=(indexs+5)%10;
//Serial.printf("--indexs indexe chech----F9P_start.itow=%d[%d],end.itow=%d[%d]------\n\r",start.itow,indexs,end.itow,indexe);
F9Ppvt[indexs]=start;
F9Ppvt[indexe]=end;
// Serial.printf("%d,F9Ppvt[,%d,],itow=,%d,%d,%d,%d,%d,%d,%d\n\r",F9PhokanN,indexs,F9Ppvt[indexs].itow,millis(),F9Ppvt[indexs].veln,F9Ppvt[indexs].vele,F9Ppvt[indexs].veld,F9Ppvt[indexs].gspeed,F9Ppvt[indexs].headmot);
//F9Ppvt[index1].itow=start.itow+20;
for(i=1;i<5;i++)//[indexs+1][indexs+2][indexs+3][indexs+4]
{
//itow
F9Ppvt[indexs+i].itow=(long)F9Ppvt[indexs].itow+i*20;
//lon
F9Ppvt[indexs+i].lon=(long)((float)(end.lon-start.lon)*i*0.2+start.lon);
//lat
F9Ppvt[indexs+i].lat=(long)((float)(end.lat-start.lat)*i*0.2+start.lat);
//velN
F9Ppvt[indexs+i].veln=(long)((float)(end.veln-start.veln)*i*0.2+start.veln);
//velE
F9Ppvt[indexs+i].vele=(long)((float)(end.vele-start.vele)*i*0.2+start.vele);
//veld
F9Ppvt[indexs+i].veld=(long)((float)(end.veld-start.veld)*i*0.2+start.veld);
//gspeed
F9Ppvt[indexs+i].gspeed=(long)((float)(end.gspeed-start.gspeed)*i*0.2+start.gspeed);
//headmot
F9Ppvt[indexs+i].headmot=(long)((float)(end.headmot-start.headmot)*i*0.2+start.headmot);
//Serial.printf("[%d]F9Ppvt[%d].lon=%d,F9Ppvt.lat=%d\n\r",F9Ppvt[indexs+i].itow,indexs+i,F9Ppvt[indexs+i].lon,F9Ppvt[indexs+i].lat);
//Serial.printf("F9Ppvt.headmot{%d]=%d\n\r",indexs+i,F9Ppvt[indexs+i].headmot);
// Serial.printf("%d,F9Ppvt[,%d,],itow=,%d,%d,%d,%d,%d,%d\n\r",F9PhokanN,indexs+i,F9Ppvt[indexs+i].itow,millis(),F9Ppvt[indexs+i].veln,F9Ppvt[indexs+i].vele,F9Ppvt[indexs+i].veld,F9Ppvt[indexs+i].gspeed,F9Ppvt[indexs+i].headmot);
F9PhokanN++;//hokan count　５回でアップ
}
// Serial.printf("%d,F9Ppvt[,%d,],itow=,%d,%d,%d,%d,%d,%d,%d\n\r",F9PhokanN,indexe,F9Ppvt[indexe].itow,millis(),F9Ppvt[indexe].veln,F9Ppvt[indexe].vele,F9Ppvt[indexe].veld,F9Ppvt[indexe].gspeed,F9Ppvt[indexe].headmot);
}
else
{
Serial.printf("Error F9Phokan20 mis4=,%d\n\r",mis4);
}
delay(1);
indexN=indexe;
}
////////////////////////////////////////////////////////////////////////
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment