Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/STA25_BASE_MB_SD_NoSAT_rev10_2.ino Secret

Created March 6, 2025 13:29
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save dj1711572002/f9c1f6936ecaa7252bd59d9cb0545ab1 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save dj1711572002/f9c1f6936ecaa7252bd59d9cb0545ab1 to your computer and use it in GitHub Desktop.
Download ZIP
STA25 _7Sensor Concted SD LOG OK
Raw
STA25_BASE_MB_SD_NoSAT_rev10_2.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分だけ保存してつなぎ目でおちをなくす。
//0853rev00 BNO追加　ファイルタブ分割Ok
//0853 rev01 BNO SDがうまくいかないASCIIだと変だ
//0853 rev02 BNO binaryでカウント４バイト＋１９バイト＝２３バイト/10msecで HWBを960バイトまで拡大OKログOK
//0853 rev03 BNOトップ Serial1とSerial3別関数だと同期とれてないから１関数内で２つ受信にする
//0853 rev04 １関数内２つ受信して同期を確保OK
//0853 rev05 BNO SD write OK
//0853 rev06 基板ADS線延長。Roverコネクタ追加 ads bno同期ＳＤログｏｋ　BAXXX.bin ファイル保存　header 0xFF bnocnt0(4byte)data1chi(4byte)data2chi(4byte)0xaa0xaa....0xaa0xaa..
//0853 rev07 TimePulseの処理方法を決めてTIMEPULSE.txtで保存　itow更新がうまくできないのは、basercvがエポックになってないから
//0853 rev08 basercvでPVTヘッドから受信開始することに大変更するSORTは、basercv内でPVT先頭で行うOK
//0853 rev08_1 メイン基板テスト用１　ADSオフにして、RTkのみ
//rev09_0 Rover RX6 TRX8追加 2025/3/1 OPENエラー発生　Seril6,7追加でOPEエラーになった
//rev09_0_1 OPENエラー対策　
//rev10_0 OPENエラーはinitializeエラーとNAV-SATのバッファが大きすぎた原因らしい、Serial4,6,7のバッファを192ならOPENエラーでない if(serial4.avaialble()>171)は必須。
//rev10_1 Roverをsdwriteに追加してbaseファイルにいれる。 Printをコンパクト化OK
//rev10_2 全センサを接続してLOGファイル確認
#include "BNO.h"
#include "Base.h"
#include "SdFat.h"
#define SD_CONFIG SdioConfig(FIFO_SDIO)
// SdFat-beta usage
SdFs sd;
FsFile myFile;
FsFile myFileB;//Base FILE bin
FsFile myFileI;//IMU FILE bin
FsFile myFileT;//TimePulse File txt
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,dcount_1;
//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 fnameT[30];//averaging File
char monC[200];//Monitor Text data Save
int chk;
//--------Rover Parameters------------------------------------
static uint8_t serialbuffer6[192];
static uint8_t serialbuffer7[192];
static uint8_t dBuf6[172],dBuf7[172],dBuf6c[172],dBuf7c[172],dBuf7P[172],dBuf8P[172];
static uint8_t dBuf6v[100],dBuf6r[72],dBuf7v[100],dBuf7r[72];//Rover PVT RELPOS配列定義
static int s6time,s7time,flags6,flags7,stt;
static int itow6,itow6_1,itow6b,itow7,itow7b,itow7_1;
//--------Base Parameters-------------------------------------
static int timepulse=27;//Pin27 In
static int tpt,tpt3k,tpt3k_flag,itow3k;//TImePulse 同期用
static int epochN=0;//受信したPVTをカウント
static int itow_base;
static int pvt_topN;
static uint8_t serialbuffer4[192];//staticにしないとsdFatとぶつかる
static uint8_t dBuf0[172],dBuf4[172],dBuf4c[172],dBuf4P[172],dBuf4v[100],dBuf4r[72],dBuf4s[172];
static int i,n,j,k;
static int dN,dN_1,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,tw0,tw1,tw1_1,tw2,tp0,tp1,tp2;
static int pvtpos,relpos,satpos;
//ubx
static uint32_t itowb[25],itowT[25];
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;
//-----BNO085 parameter definitions-----------------------
static uint8_t serialbuffer1[32];//バッファは、static変数配列で宣言しておく。32byteの倍数で確保すること
static uint8_t serialbuffer3[32];//バッファは、static変数配列で宣言しておく。32byteの倍数で確保すること
static uint8_t dBuf1[19];
static uint8_t dBuf3[19];
static uint8_t dB1[19];
static uint8_t dB3[19];
static uint8_t dB1_1,dB3_1;//１個前のIndex
volatile static int bn85=0;
int iij;
float yawf[12],pitchf[12],rollf[12];
float axf[12],ayf[12],azf[12];
//uint8_t c[300];
static int p1,p1_1;
static int p3,p3_1;
static int period[12];
static String bnostr3,bnostr1;//bnoreadで
static String bnostr312;//12個まとめた文字列 ヘッダにシリアル番号３
static String bnostr112;//12個まとめた文字列ヘッダにシリアル番号１
static uint32_t bncnt0,bncnt0_1,bncnt01,bncntT;//serial3　Serial1の合計数
//----------------Texsus ADS122U04--------------------------
static int DRDY = 23; //DataRdy Pin 23
static int DRDY_1;
//NGstatic long ft1, ft2,ft3,ts,ts_1,ts_10;//,period;
int RREGwait;
static int pt0,pt0_1;
static int flipN,endflip;
static uint8_t RR0, RR1, RR2, RR3, RR4;
static uint8_t ad0, ad1, ad2, ad3, ad4, ad5, ad6;
static long data1chi, data2chi;
static double data1ch, data2ch;
static int ats,ats_1,ats_10;
int DRDYflag = 0;
int res;
int cd=0;
static int flipt0,flipt1,ft1,ft2,periodt;
//ADS122 InterruptIn DRDY====================
void flip()
{
flipN++;
//Serial.printf("flipN=%d\n\r",flipN);
flipt0=micros();
ats=micros();
//delayMicroseconds(100000);
if (flipN % 2 == 0 )//偶数回は１CH,奇数回は2CH　Ch間ずれ1.4msec 周期2.65msec 370Hz
{
//1CH-----Register00[7:4(MUX)3:1(GAIN)0(PGA_BYPASS)]--------------------------------------------------------------
Serial5.write(0x55);//Synchronization word 0x55
Serial5.write(0x40);//WriteREGister 0x04 0x00(0000register00 Selected)
Serial5.write(0x0E);//7:0(0000 1110=0x0E)[7:4(0000)MUX AINp=AIN0,AINn=AIN1 3:1(111)GAIN128, 0:0(0)PGA enabled]
//Serial5.write(0x0C);//7:0(0000 1100=0x0c)[7:4(0000)MUX AINp=AIN0,AINn=AIN1 3:1(110)GAIN64, 0:0(0)PGA enabled]
delayMicroseconds(50);
Serial5.write(0x55);
delayMicroseconds(cd);
Serial5.write(0x10);
delayMicroseconds(10);
ad0 = Serial5.read();
delayMicroseconds(10);
ad1 = Serial5.read();
delayMicroseconds(10);
ad2 = Serial5.read();
delayMicroseconds(10);
data1chi=Toint24(ad0,ad1,ad2);
//Serial.printf("[CH1]%d:d0=%x,d1=%x,d2=%x,data=%d\n\r",millis(),d0,d1,d2,data1chi);
ft1 = (int)(micros());
endflip=0;
//Serial.printf("%d,%6.2f\n\r",data1chi,ft1);
}
else
{
//2CH-----Register00[7:4(MUX)3:1(GAIN)0(PGA_BYPASS)]--------------------------------------------------------------
delayMicroseconds(50);
Serial5.write(0x55);//Synchronization word 0x55
delayMicroseconds(cd);
Serial5.write(0x40);//WriteREGister 0x04 0x00(0000register00 Selected)
delayMicroseconds(cd);
Serial5.write(0x6E);//7:0(0110 1110=0x6E)[7:4(0110)MUX AINp=AIN2,AINn=AIN3 3:1(111)GAIN128, 0:0(0)PGA enabled]
//Serial5.write(0x60);//7:0(0110 0000=0x60)[7:4(0110)MUX AINp=AIN2,AINn=AIN3 3:1(000)GAIN1, 0:0(0)PGA enabled]
delayMicroseconds(100);
Serial5.write(0x55);
delayMicroseconds(cd);
Serial5.write(0x10);
delayMicroseconds(10);
ad3 = Serial5.read();
delayMicroseconds(10);
ad4 = Serial5.read();
delayMicroseconds(10);
ad5 = Serial5.read();
delayMicroseconds(10);
data2chi=Toint24(ad3,ad4,ad5);
//Serial.printf("[CH2]%d:d3=%x,d4=%x,d5=%x,data=%d\n\r",millis(),d3,d4,d5,data2chi);
ft2 = (int)(micros());
int ft3=int(micros())-ats;
periodt=ats-ats_1;
ats_1=ats;
endflip=1;
//Serial.printf("%d,%d,%d,%d,%d,%d\n\r",flipN,data1chi,data2chi,ft1,ft2,ft3);
//if(ts-ts_10>10000)
//{
//Serial.printf("%d,%d,%d,%d,%d,%d\n\r",flipN,data1chi,data2chi,ats,period,ats-ats_10);
//Serial.printf("%d,%d,%d,%d\n\r",flipN,data1chi,data2chi,periodt);
ats_10=ats;
//}
}
flipt1=micros();
}
//ADS122 flip END===================================
//-----------ADS122U04 Initialize-------------
void adsinit()
{
int cd=0;
Serial.println("ADS INIT IN");
//=============ADS122U04レジスタ初期設定==========================================
//ADS122U04 RESET
Serial5.write(0x55);
delayMicroseconds(cd);
Serial5.write(0x06);
delayMicroseconds(1000);
int res= readReg(0);
//================Register Setting=============================================================================
RREGwait = 1000; //RREG wait time usec 2*Tbaud(10usec)
//-----Register00[7:4(MUX)3:1(GAIN)0(PGA_BYPASS)]--------------------------------------------------------------
Serial.println("Register0 writing");
Serial5.write(0x55);//Synchronization word 0x55
delayMicroseconds(cd);
Serial5.write(0x40);//WriteREGister 0x04 0x00(0000register00 Selected)
delayMicroseconds(cd);
//Serial5.write(0x0E);//7:0(0000 1110=0x0E)[7:4(0000)MUX AINp=AIN0,AINn=AIN1 3:1(111)GAIN128, 0:0(0)PGA enabled]
Serial5.write(0x0C);//7:0(0000 1100=0x0C)[7:4(0000)MUX AINp=AIN0,AINn=AIN1 3:1(110)GAIN64, 0:0(0)PGA enabled]
delayMicroseconds(1000);
res=readReg(0);
//-----Register01[7:5(DataRate)4:(OperationMode)3:(ConversionMode)2:1(VREF)0:(TemperatureSensor mode)]---------
Serial.println("Register1 writing");
Serial5.write(0x55);//Synchronization word 0x55
delayMicroseconds(cd);
Serial5.write(0x42);// WriteREGister 0x04 0x02(0010register01 Selected)
delayMicroseconds(cd);
Serial5.write(0xCE);//7:0(1100 1110=0xCE)[7:5(110)DataRate1000sps,4:(0)Normal Mode, 3:(1)Continuous conversion mode,2:1(11)VDD-VSS VRef,0:(0)Temp disabled]
//Serial5.write(0xAE);//7:0(1010 1110=0xAE)[7:5(101)DataRate600sps,4:(0)Normal Mode, 3:(1)Continuous conversion mode,2:1(11)VDD-VSS VRef,0:(0)Temp disabled]
//Serial5.write(0x8E);//7:0(1000 1110=0x8E)[7:5(100)DataRate330sps,4:(0)Normal Mode, 3:(1)Continuous conversion mode,2:1(11)VDD-VSS VRef,0:(0)Temp disabled]
//Serial5.write(0x4E);//7:0(0100 1110=0x0E)[7:5(010)DataRate90sps,4:(0)Normal Mode, 3:(1)Continuous conversion mode,2:1(11)VDD-VSS VRef,0:(0)Temp disabled]
delayMicroseconds(1000);
res=readReg(1);
//-----Register02[7:DRDY)6:(DCNT)5:4(CRC)3:(BCS)2:0(IDAC)]-----------------------------------------------------
Serial.println("Register2 writing");
Serial5.write(0x55);//Synchronization word 0x55
delayMicroseconds(cd);
Serial5.write(0x44);// WriteREGister 0x04 0x04(0100register02 Selected)
delayMicroseconds(cd);
Serial5.write(0x00);//7:0(0000 1000=0x08)[7:(0)DRDY,6:(0)DCNT disable, 5:4(00)) inverte,3:(1)BCS Off（0）,2:0(0)IDAC off]
delayMicroseconds(1000);
res=readReg(2);
//----Register03[7:5(I1MUX)4:2(I2MUX)1(RSERVED)0(AUTO)]----------------------------------------------------
Serial.println("Register3 writing");
Serial5.write(0x55);//Synchronization word 0x55
delayMicroseconds(cd);
Serial5.write(0x46);// WriteREGister 0x04 0x06(0110register03 Selected)
delayMicroseconds(cd);
Serial5.write(0x00);//7:0(00000000=0x00)[7:5(000)default,4:2(000)default, 1:(0)Reserved,0:(0)Manual Read
delayMicroseconds(1000);
res=readReg(3);
delayMicroseconds(1000);
//----Register04[7:(Reserved)6:(GPIO2DIR)5:(GPIO1DIR)4:(GPIO0DIR)3:(GPIO2SEL)2:(GPIO2DAT)1:(GPIO1DAT)0:(GPIODAT0)]----------------------------------------------------
Serial.println("Register4 writing");
Serial5.write(0x55);//Synchronization word 0x55
delayMicroseconds(cd);
Serial5.write(0x48);// WriteREGister 0x04 0x08(1000register04 Selected)
delayMicroseconds(cd);
Serial5.write(0x48);//7:0(0100 1000=0x08)[6:(1)GPIO2output,5:(0)GPIO1Input, 4:(0)GPIO0Input,3:(1)GPIO2SEL DRDY,2:(0)GPIO2DAT Low,1:(0)GPIO1DAT Low,0:(0)GPIO0DAT Low
delayMicroseconds(1000);
res=readReg(4);
Serial.println("CHECK Written Registers");
delay(5000);
//Start/Sync
Serial5.write(0x55);
delayMicroseconds(cd);
Serial5.write(0x08);
delayMicroseconds(100);//************************このWAIT重要************************************
}
//--------------toint32-----------------------------
int Toint24(byte d0, byte d1, byte d2)
{
long val;
if ((d2 & B10000000) == B10000000)
{ //最上位ビットたっていればマイナス
val=(d0+d1*256+d2*256*256)-0xFFFFFF+1;
}
else
{
val = d0+d1*256+d2*256*256;
}
return val;
}
//関数--------------------------------------------------
int readReg(int n)
{
//Serial.println("In readReg");
int RREGwait=600;
int regN=0;
int RR;
if(n==0){regN=0x20;}
if(n==1){regN=0x22;}
if(n==2){regN=0x24;}
if(n==3){regN=0x26;}
if(n==4){regN=0x28;}
Serial5.write(0x55);//Synchronization word 0x55
delayMicroseconds(cd);
Serial5.write(regN);//Synchronization word 0x55 Read REGister00=0010 0000=0x20
delayMicroseconds(RREGwait);
int flag=0;
while (flag==0)
{
if(Serial5.available())
{
RR = Serial5.read();
delayMicroseconds(RREGwait);
//Serial.printf("readRegister%x=>[0x%x]",n,RR);
//Serial.println();
flag=1;
}
}
//Serial5.clear();
return RR;
}
//ADS122 functions END==================================
//+++++++++++++++++++++=================================================================================+
//+++++++++++++++++++++Base Recieve++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++=================================================================================+
int basercv()
{
int bufcnt=0;
if(Serial4.available()>300)//Serial4から一挙に受信
{
i=0;
j=0;
rcvtime_1=rcvtime;
rcvtime=millis();
while(Serial4.available()){
if(Serial4.available()>0){
dBuf4[j]=Serial4.read();
j++;
}
}//All data read
dN=j;
Serial.printf("Bas4 RX4:%x,%x,%x,%x,",dBuf4c[0],dBuf6[1],dBuf6[2],dBuf6[3]);
for(k=0;k<dN;k++){//b5 62 01 07検索dBuf4
if(dBuf0[k]==0xB5 && dBuf0[k+1]==0x62 && dBuf0[k+2]==0x01 && dBuf0[k+3]==0x07){//PVT Header
for(i=0;i<dN-k;i++){
dBuf4[i]=dBuf0[i+k];
}
for(i=dN-k;i<k;i++){
dBuf4[i]=dBuf0[i];
}
itow_base=dBuf4[6]+dBuf4[7]*256+dBuf4[8]*256*256+dBuf4[9]*256*256*256;
//Serial.printf(">>itowbase=%d,dN=%d,epochN=%d\n\r",itow_base,dN,epochN);
dcount++;
epochN++;
}//if 0xB5 end
//if(dBuf0[k]==0xB5 && dBuf0[k+1]==0x62 && dBuf0[k+2]==0x01 && dBuf0[k+3]==0x3C){//RELPOSNED存在
// Serial.printf("============RELPOSNED hit epochN=%d,dN=%d\n\r",epochN,dN);
//}
}//for k=0
}//if Serial.available
else{
//Serial.printf("NOT Serial3.available：Else time=%d\n\r",millis());
//dN=0;
}
//Rover 受信もBaseと同時におこなう
if(Serial6.available()>171){ //バッファ1が172バイトになるまで処理しない
s6time=millis();
//Serial.println();
i=0;//Base Serial1 counter
while(i<172){
stt=millis();
while(Serial6.available()){
dBuf6[i]=Serial6.read();
//Serial.print(dBuf6[i],HEX);
i++;
}
//Serial.printf("Rover RX6:%x,%x,%x,%x,",dBuf6[0],dBuf6[1],dBuf6[2],dBuf6[3]);
}
}//if serial6 end
//=============Rover RX8===============================================================
if(Serial7.available()>171){ //バッファ1が172バイトになるまで処理しない
s7time=millis();
//Serial.println();
i=0;//Rover Serial7 counter
while(i<172){
stt=millis();
while(Serial7.available()){
dBuf7[i]=dBuf7c[i];
dBuf7c[i]=Serial7.read();
//Serial.print(dBuf7[i],HEX);
i++;
}
//Serial.printf("Rover RX8):%x,%x,%x,%x,",dBuf7[0],dBuf7[1],dBuf7[2],dBuf7[3]);
}
}//Serial7 end
return dN;
}
//Base rcv end===================================================================================================
//============SD関数=========================================================================
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=========");
}
*/
}
int sdwrite(int dcount)//Base dBuf４を保存+Rover dBuf6 dBuf8
{
int c;
//==========================================================================================================
//=================OPEN CLOSE R=========================================================================
//=========================================================================================================
//if(millis()%20000<=>0 )//30secに一回close open
//dcount++;
if(dcount%500==0)//60sec
{
//dcount=0;
if(myFile){
myFile.close();
myFileI.close();
delay(15);//このWAIT重要close open間
Serial.println("-----Closed------");
}
//delay(15);
myFile = sd.open(fnameB, FILE_WRITE);
//delay(5);
Serial.println("-------OPENED-------");
if(!myFile){
Serial.println("************myFIleB timer OPEN ERROR*******************");
c=1;
}
else{
Serial.printf("myFileB Close&Open t=%d*******************\n\r",millis());
}
//myFileI OPEN
myFileI =sd.open(fnameI, FILE_WRITE);
if(!myFileI){
Serial.println("************myFIleI timer OPEN ERROR*******************");
//c=1;
}
else{
Serial.printf("****myFileI Close&Open t=%d*****************\n\r",millis());
}
//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=========");
ts=micros();
for(i=0;i<172;i++)
{
if(myFile){
myFile.write(dBuf4[i]);//Base RX4
}
}
for(i=0;i<172;i++)
{
if(myFile){
myFile.write(dBuf6[i]);//Rover RX6
}
}
for(i=0;i<172;i++)
{
if(myFile){
myFile.write(dBuf7[i]);//Rover RX7
}
}
te=micros();
c=0;
}
//SD LOG END
return c;//c=0 OK c=1 OPEN error c=2 write error
}
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 = "base_"+stime + ".ubx"; //UBX Binary File
String stimeT="TP_"+ stime + ".txt"; //TimePulse txt File
String stimeI="BA_"+stime+".bin";//BNO data binary File
int slenB = stimeB.length() + 1;
int slenT=stimeT.length()+1;
int slenI=stimeI.length()+1;
//ファイル名はchar配列なのでStringからchar配列変換　fname[]を得る
stimeB.toCharArray(fnameB, slenB); //stimeB to fnameB[] chara Array
stimeT.toCharArray(fnameT, slenT); //stimeAv to fnameAv[] chara Array
stimeI.toCharArray(fnameI,slenI);//stimeI to fnameI[] char Array
Serial.print("fnameB=");
Serial.println(stimeB);
Serial.print("fnameT=");
Serial.println(stimeT);
Serial.print("fanemI=");
Serial.println(stimeI);
delay(10);
}//Filename() end
//-----------------------------------------------------------
//---------------------------------TIME PULSE Interrupt Function--------------------------------------------
void tpIn(){//TimePulseが入った瞬間のマイコン時刻を返す
int tpt;
tpt=millis();
//Serial.println();
//Serial.printf("<tpIn=%d,itow_base=%d,itow_base-3000=%d>\n\r",tpt,itow_base,itow_base%3000);
delay(1);
if((itow_base+120)%3000==0){//itowbが2880になっていたら
tpt3k=tpt;//タイムパルスが3000になったときのtpt
tpt3k_flag=1;//3000msecフラフ
itow3k=itow_base+120;//3000msecでのitow値
//Serial.printf("tpt3k_flag=%d\n\r",tpt3k_flag);
}
}
//---itow get------------------------------------
int itowget(int dn){//dBuf4からitow検索
int i;
for(i=0;i<dN;i++){
if(i>=3 && dBuf4[i]==0xB5 && dBuf4[i+1]==0x62 && dBuf4[i+2]==0x01 && dBuf4[i+3]==0x07){
itow_base=dBuf4[i+6]+dBuf4[i+7]*256+dBuf4[i+8]*256*256+dBuf4[i+9]*256*256*256;
epochN++;
//Serial.printf("epochN=%d,itow_base=%d\n\r",epochN,itow_base);
break;
}
}
return itow_base;
}
//●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●
//SETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUPSETUP
void setup()
{
Serial.begin(460800);
Serial1.begin(115200);//Tail BNO085 RVC MODE pin0 Tx In
//Serial1.addMemoryForRead(serialbuffer1, sizeof(serialbuffer1));//serialbuffer1のメモリーを 256byteバッファ指定
Serial3.begin(115200);//Center BNO085 RVC MODE pin15 TX In
//Serial3.addMemoryForRead(serialbuffer1, sizeof(serialbuffer1));//serialbuffer1のメモリーを 256byteバッファ指定
Serial5.begin(115200); //Teensy4.1 RX5-ADS122 RX5(21)-TX,TX5(20)-RX
Serial4.begin(115200);//Base
Serial4.addMemoryForRead(serialbuffer4, sizeof(serialbuffer4));
//---------------------ROVER------------------------------
//Serial4 def ROVER SimpelRTK2B
Serial6.begin(115200);//Rover Rx6 pin25 Green
Serial6.addMemoryForRead(serialbuffer6, sizeof(serialbuffer6));
Serial7.begin(115200);//Rover RX8 pin28 Brown
//Serial7.addMemoryForRead(serialbuffer7, sizeof(serialbuffer7));
delay(10000);
adsinit();
pinMode(DRDY, INPUT_PULLUP);
//attachInterrupt(digitalPinToInterrupt(2),呼び出される関数,FALLING);
attachInterrupt(DRDY, flip,FALLING);//DRDY 下がりエッジで割り込み関数flipへ飛ぶ
attachInterrupt(timepulse, tpIn,FALLING);//TimePulse 下がりエッジで割り込み関数tpInへ飛ぶ
Serial.println("==============STA25_BASE_MB_SD_NoSAT_rev10_1.ino==========================");
if (!sd.begin(SD_CONFIG)) {
Serial.println("initialization failed!");
exit(0);
}
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
Serial.println("------------SET UP Finshed-----------");
}
//●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●
//+++++++++++++++++++LOOP LOOP LOOP LOOP LOOP++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void loop()
{
int t4,t4_1,t6,t6_1,t7,t7_1;
//p1=micros();
//----------BNOREAD してSD ログ　dN>500以降常時ログ------------------------------------
bncnt0=bnoread31(dB3,dB1,19,bncnt0);//cnt dB1[19]戻り　ｄB3 Center dB1 Top
bncntT=millis();
//===========itow 同期TimePulse BNO 3秒に一回ファイル書き込み========================
//String化 "itow3k+tpt3k+bncnt0+bncntT"
if(tpt3k_flag==1){
tpt3k_flag=0;
String tptstr=String(itow3k)+","+String(tpt3k)+","+String(bncnt0)+","+String(bncntT)+","+String(epochN);
Serial.println();
Serial.println(tptstr);
if(fnflag==1){
//File write
myFileT = sd.open(fnameT, FILE_WRITE);//タイマーでOPENするので初め５００カウントまでエラー
if(!myFileT){//エラーなら記録しない
Serial.println("=====myFileT Open Error=====");
}
else{//write string
myFileT.println(tptstr);
}
myFileT.close();
}
}
//p1_1=micros();
//Serial.printf("bnoread time=%d\n\r",p1_1-p1); //bnoread31は1microsecしかかからない
/*//------DEBUG BNO OUTPUT-------------------
//Serial.println(bncnt1); 　
if(dB3[2]!=dB3_1){
dB3_1=dB3[2];//１個前indexを記憶
Serial.printf("bnoread31,cnt=%d,",bncnt1);
for(i=0;i<16;i++){
Serial.printf("%x,",dB3[i]);
}
if(dB1[2]!=dB1_1){
dB1_1=dB1[2];//1個前indexを記憶
for(i=0;i<16;i++){
Serial.printf("%x,",dB1[i]);
}
Serial.printf(",%d msec",millis());
Serial.println();
}
}
*///DEBUG END-----------------------------------------------
// dB1_1=dB1[2];
//Serial.printf("bncnt=%d,bnstr3=%s\n\r",bncnt,bnostr3);
uint8_t dno[4];//bnocntを4byteに変換
uint8_t adno1[4];//data1chiを4byteに変換
uint8_t adno2[4];//data2chiを4byteに変換
if(fnflag==1 && bncnt0>bncnt0_1){//dN>500で毎回カウンタ更新された場合にSD　Write
//myFileI = sd.open(fnameI, FILE_WRITE);//タイマーでOPENするので初め５００カウントまでエラー
if(!myFileI){//エラーなら記録しない
//Serial.printf("BNOmyFileI open Error======fnameI=%c,bncnt0=%d\n\r",fnameI,bncnt0);
//exit(0);
}
else//エラーでないならSD write
{
//Serial.println("BNO==============myFileI Write Successed=========");
//if(endflip==1){//2chが終わったらデータ取得
//DEBUG用に角度変換　aa[0],aa[1],index[2],yawL[3],yawH[4],pitchL[5],pitchH[6],rollL[7],rollH[8]
//float yaw1=(float)(bin2int(dB1[3],dB1[4])*0.01);//Top yaw
//float pitch1=(float)(bin2int(dB1[5],dB1[6])*0.01);//Top pitch
//float roll1=(float)(bin2int(dB1[7],dB1[8])*0.01);//Top roll
//Serial.printf("ADS:1ch=%d,2ch=%d,ft2=%d,bncnt0=%d\n\r",data1chi,data2chi,ft2,bncnt0);
//Serial.printf("ADS:1ch=%d,2ch=%d,ft2=%d,bncnt0=%d,yaw=%3.2f,pitch=%3.2f,roll=%3.2f\n\r",data1chi,data2chi,ft2,bncnt0,yaw1,pitch1,roll1);
//CPLT
//Serial.printf("%d,%d,%3.2f,%3.2f,%3.2f,%d\n\r",data1chi,data2chi,yaw1,pitch1,roll1,bncnt0);
//endflip=0;
//}
i_to_char(bncnt0,dno, 0);//bncntを４バイトのdnoに分解
i_to_char(data1chi,adno1, 0);//bncntを４バイトのdnoに分解
i_to_char(data2chi,adno2, 0);//bncntを４バイトのdnoに分解
//Serial.printf("BNO:bncnt0=%d,dno[]=%x,%x,%x,%x\n\r",bncnt0,dno[0],dno[1],dno[2],dno[3]);
//Serial.printf("ADS1ch:data1chi=%D,dno[]=%x,%x,%x,%x\n\r",data1chi,adno1[0],adno1[1],adno1[2],adno1[3]);
//Serial.printf("ADS2ch:data2chi=%d,dno[]=%x,%x,%x,%x\n\r",data2chi,adno2[0],adno2[1],adno2[2],adno2[3]);
//BNO bnocnt0 4byte write
myFileI.write(0xFF);//Serial3のマーク
myFileI.write(dno[0]);//bnocntをヘッドに格納
myFileI.write(dno[1]);
myFileI.write(dno[2]);
myFileI.write(dno[3]);
//ADS data1chi 4byte wtite
myFileI.write(adno1[0]);//data1chiを次に格納
myFileI.write(adno1[1]);
myFileI.write(adno1[2]);
myFileI.write(adno1[3]);
//ADS data2chi 4byte wtite
myFileI.write(adno2[0]);//data2chiを次に格納
myFileI.write(adno2[1]);
myFileI.write(adno2[2]);
myFileI.write(adno2[3]);
//Serial.print("3:");
for(i=0;i<19;i++){
myFileI.write(dB3[i]);//dB3 Center IMU
//Serial.print(dB3[i],HEX);
}
//Serial.println();
//Serial.print("1:");
for(i=0;i<19;i++){
myFileI.write(dB1[i]);//dB1 Top IMU
//Serial.print(dB1[i],HEX);
}
bncnt0_1=bncnt0;
}// myFIleI ELSE END
}//BNO 受信　ログ　END
//********************************************************
//***************Base ROVER 受信******************************
//********************************************************
int bufcnt=0;
// if(millis()-tw1_1>119){//119msecで一挙に読む場合
//===Serial1Base(F9P) read=========================================================
if(Serial4.available()>171){ //バッファ1が172バイトになるまで処理しない
i=0;//Base Serial1 counter
while(i<172){
while(Serial4.available()){
dBuf4[i]=Serial4.read();
//Serial.print(dBuf4[i],HEX);
i++;
}
dN=i;
//Serial.printf("Base RX4:%x,%x,%x,%x,%d\n\r",dBuf4[0],dBuf4[1],dBuf4[2],dBuf4[3],i);
}
tw1_1=tw1;
tw1=millis();
itow_base=dBuf4[6]+dBuf4[7]*256+dBuf4[8]*256*256+dBuf4[9]*256*256*256;
dcount++;
epochN++;
//Serial.printf("dcount=%d,fnflag=%d,itow_base=%d\n\r",dcount,fnflag,itow_base);
}//Base RX4 end
if(fnflag==0 && dcount>200){//タイムススタンプ作成
//=====PVT,relpos分離==============
for(i=0;i<100;i++)//PVT1配列作成
{
dBuf4v[i]=dBuf4P[i];
}
for(i=relpos;i<dN;i++)//relpos配列作成
{
dBuf4r[i-relpos]=dBuf4P[i];
}
//=======NAV-PVTからFname ================
int pvti=PVTcnv(dBuf4v,pvt);
Filename (pvt); //pvt送ってfnameBいれる
fnflag=1;
}
//======================================
//ROver受信なければOPENエラーでない
if(Serial6.available()>171){ //バッファ1が172バイトになるまで処理しない
s6time=millis();
//Serial.println();
i=0;//Base Serial1 counter
while(i<172){
stt=millis();
while(Serial6.available()){
dBuf6[i]=Serial6.read();
//Serial.print(dBuf6[i],HEX);
i++;
}
//
}
//Serial.printf("Rover RX6:%x,%x,%x,%x,%d\n\r",dBuf6[0],dBuf6[1],dBuf6[2],dBuf6[3],i);
}//if serial6 >171
//=============Rover RX8===============================================================
if(Serial7.available()>171){ //バッファ1が172バイトになるまで処理しない
s7time=millis();
//Serial.println();
i=0;//Rover Serial7 counter
while(i<172){
stt=millis();
while(Serial7.available()){
dBuf7[i]=Serial7.read();
//Serial.print(dBuf7[i],HEX);
i++;
}
}
//Serial.printf("Rover RX7):%x,%x,%x,%x,%d\n\r",dBuf7[0],dBuf7[1],dBuf7[2],dBuf7[3],i);
}//
//Base受信データSDログ起動dcountがインクリメントしたら
if(dcount>300 && dcount>dcount_1 && fnflag==1){//fnameBがdcount=200で着くまで待つ
//ts=micros();
chk=sdwrite(dcount);//chk=0ならOK chk=1,2,3OPEN WRITEエラー
//te=micros();
dcount_1=dcount;
}
// } //191msec
//***************************************************************
//********************Rover END *****************************
//***************************************************************
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//SD LOG+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
//+++------------Base 受信　ログ-----------------------------------------------------++++
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//delay(50);
tw0=millis();
int sa=tw0-tw2;//前回の受信から経過時間
if(sa>119 ){//119msec経過していたら受信それまでは受信しない受信は1msec以内で終わる
dN=basercv();//dBuf3に収納される960byteまで収納
tw2=millis();//受信時刻記録
//itow_base=itowget(dN);
//Serial.printf("basercv: itow_base=%d, epochN=%dtime=%d,\n\r",itow_base,epochN,millis()-tw2);
//dcount_1=dcount;
}
tw1=millis();
//Base受信データSDログ起動dcountがインクリメントしたら
if(dcount>300 && dcount>dcount_1){//fnameBがdcount=200で着くまで待つ
//ts=micros();
chk=sdwrite(dcount);//chk=0ならOK chk=1,2,3OPEN WRITEエラー
//te=micros();
dcount_1=dcount;
}
*/
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//-----------------受信結果1sec毎にPRINT-----------------------------------------------------------------------------
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//Serial.println();
//Serial.printf("RECIEVED,dN=,%d ,toptime=%d,recvtime-rcvtime_1=%d\n\r",dN,toptime,rcvtime-rcvtime_1);
if( tw1-tp0>1000)
{
tp0=millis();
//##############################Base PVT RELPOS抽出######################################################
//------------------BaseR PVT RELP抽出----------------------------------------------------------------------
if(dBuf4[0]==0xB5 && dBuf4[1]==0x62 && dBuf4[2]==0x01 && dBuf4[3]==0x07)
{
for(i=0;i<100;i++){//PVT 抽出
dBuf4v[i]=dBuf4[i];//PVT dBuf4v[100]代入
}
for(i=0;i<72;i++){
dBuf4r[i]=dBuf4[i+72];//RELPOS dBuf4r[100]代入
}
}
//RX4 PVT
long pvt4[100];
int pvt4i=PVTcnv(dBuf4v,pvt4); //pvt4変換
//---RX4 RELPCNV-----------------------------------------------------------
//RX4 RELPOS
long relp4[100];
int relp4i=RELPOScnv(dBuf4r,relp4); //prelp6変換
//RX4
long itow_base=pvt4[0];
long flags4=pvt4[11];
double hacc4=(double)pvt4[18]/100;//cm
long len4=relp4[7];
long relhd4=relp4[8];
//------------------ROVER PVT RELP１抽出----------------------------------------------------------------------
//hacc6 hacc8,len6,len8,hdM6,hdM8
//RX6
if(dBuf6[0]==0xB5 && dBuf6[1]==0x62 && dBuf6[2]==0x01 && dBuf6[3]==0x07)
{
for(i=0;i<100;i++){//PVT 抽出
dBuf6v[i]=dBuf6[i];//PVT dBuf6v[100]代入
}
for(i=0;i<72;i++){
dBuf6r[i]=dBuf6[i+72];//reLPOS dBuf6r[100]代入
}
}
//RX8
if(dBuf7[0]==0xB5 && dBuf7[1]==0x62 && dBuf7[2]==0x01 && dBuf7[3]==0x07)
{
for(i=0;i<100;i++){//PVT 抽出
dBuf7v[i]=dBuf7[i];//PVT dBuf8v[100]代入
}
for(i=0;i<72;i++){
dBuf7r[i]=dBuf7[i+72];//RELPOS dBuf7r[100]代入
}
}
//-------------------------------------------------------------------
//PVT RELPOS 変換--------------------------------------------------------------------------
//---PVTCNV-----------------------------------------------------------
//RX6 PVT
long pvt6[100];
int pvt6i=PVTcnv(dBuf6v,pvt6); //pvt6変換
//RX8 PVT
long pvt7[100];
int pvt7i=PVTcnv(dBuf7v,pvt7); //pvt8変換
//---RELPCNV-----------------------------------------------------------
//RX6 RELPOS
long relp6[100];
int relp6i=RELPOScnv(dBuf6r,relp6); //prelp6変換
//R7 relp7
long relp7[100];
int relp7i=RELPOScnv(dBuf7r,relp7); //prelp8変換
//RX6
long flags6=pvt6[11];
double hacc6=(double)pvt6[18]/100;//cm
long len6=relp6[7];
long relhd6=relp6[8];
//RX7
long flags7=pvt7[11];
double hacc7=(double)pvt7[18]/1000;//cm
long len7=relp7[7];
long relhd7=relp7[8];
//BNO MONITOR
//DEBUG用に角度変換　aa[0],aa[1],index[2],yawL[3],yawH[4],pitchL[5],pitchH[6],rollL[7],rollH[8]
float yaw1=(float)(bin2int(dB1[3],dB1[4])*0.01);//Top yaw
float pitch1=(float)(bin2int(dB1[5],dB1[6])*0.01);//Top pitch
float roll1=(float)(bin2int(dB1[7],dB1[8])*0.01);//Top roll
float yaw3=(float)(bin2int(dB3[3],dB3[4])*0.01);//Center yaw
float pitch3=(float)(bin2int(dB3[5],dB3[6])*0.01);//Center pitch
float roll3=(float)(bin2int(dB3[7],dB3[8])*0.01);//Center roll
//Serial.printf("ADS:1ch=%d,2ch=%d,ft2=%d,bncnt0=%d\n\r",data1chi,data2chi,ft2,bncnt0);
//Serial.printf("ADS:1ch=%d,2ch=%d,ft2=%d,bncnt0=%d,yaw=%3.2f,pitch=%3.2f,roll=%3.2f\n\r",data1chi,data2chi,ft2,bncnt0,yaw1,pitch1,roll1);
//=======PRINT======================================================================================================
Serial.printf("chk=%d,epochN=%d,numSVr=%d,flags=%d,%d,%d,hacc=%4.2f,%4.2f,%4.2f,bncnt0=%d,itowb=%d,len=%d,%d,Head=%d,%d\n\r",chk,epochN,numSVr,flags4,flags6,flags7,hacc4,hacc6,hacc7,bncnt0,itow_base,len6,len7,relhd6,relhd7);
//Serial.printf("chk=%d,epochN=%d,dc=%d,dN=%d,dL=%d,numSV=%d,Upper40=%d.Upper50=%d,numSVr=%d,flags=%d,hacc=%4.2f,bncnt0=%d\n\r",chk,epochN,dcount,dN,L,numSV,n40,n50,numSVr,flags,hacc,bncnt0);
Serial.printf("BNO:yaw31=%3.1f,%3.1f,pitch31=%3.1f,%3.1f,roll31=%3.1f,%3.1f,ADS:1ch=%d,2ch=%d,bncnt0=%d\n\r",yaw3,yaw1,pitch3,pitch1,roll3,roll1,data1chi,data2chi,bncnt0);
delay(1);
//tp1=millis();
//Serial.printf("tp0=%d,tp1=%d,tp1-tp0=%d,dN=%d,tw0=%d,tw1=%d,tw2=%d\n\r",tp0,tp1,tp1-tp0,dN,tw0,tw1,tw2);
//}
n40=0;
n50=0;
pvtpos=1;
satpos=0;
relpos=0;
//delay(20);
}//if dN>172
}// loop end
//++++++++++++++++++++++++++++++LOOP END+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment