Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/STA25All_120msec_USBSend_HWS468_BNO_ADS_rev800_7.ino Secret

Created April 13, 2025 05:12
Download ZIP
Teensy ublox F9Px3 +BNO085x2 +ADS122U04 Logging System
Raw
STA25All_120msec_USBSend_HWS468_BNO_ADS_rev800_7.ino
//2025年３月２５日　横田様試乗が２９日迫っている
//rev600 どうしてもHardwareSerialを３個使わないと安定受信できないので、SDログを単発式にして、SETUPでOPENしたら、LOOPで無限にまわって、手動入力でCLOSEしてプログラムを終わらせる
//====>シンプルな構造にしてモニターブロックは持たない、SD書き込みも個別シリアルブロック内で別ファイルで行う。
//====>setup内でタイムスタンプファイル名を生成してOPENする
//rev601 hardwareSerial 4,6,7 buffer[192」で受信OK sd.h SD.begin(BUILTINSDCARD)宣言だけ　 OPEN 直後に172byte書いて、CLOSEを繰り返しても120msecインターバルがあるから大丈夫だった。
//＝＞LOOOP中の印字をしていてもシステムがプリントを１回で止めてしまうので、SDログ中はプリンタ無しにする必要がある
//rev601 SD.h とHWS 109byteで３個のファイル書き込み成功
//rev601LIST SD.hのLIST機能追加　chatGPTで簡単
//rev601_3　100行以降で6,7が１秒置きに間引き状態になる
//rev601_4 RX6のみで確認しても600行から間引きになってNG
//rev601_82 RX4のみで定期CLOSEをいれた。OPENは,writeの直上にないとエラーになる。OPENは、重複しても大丈夫、
//OPEN-CLOSEを毎エポックやっていると5msec消費するが、OPENだけなら2msecで済む定期CLOSEを500でいれた
//●●rev601821 SETUPでファイル名タイムスタンプにして。RX6 RX７も追加　HWSバッファサイズ　１０８にした RX4　０．１％ RX7 2%以下　RX６５％となったが全て240エラーなので補完して完全にする
//rev601>821 buf172で３ポート受信最善アルゴリズムとする。
//rev601_900_0 bnoread31 　SDログをいれてRX4,6,7の乱れモニターも作る　受信時itow SD保存時のitow抜け bnowriteがはいると５－６周期遅れて全然ダメになる、SDログがなければ正常
//rev800_0 USB転送でログする　rev601_900のfile.writeを　Serial.wtie(buf,size)で超高速転送　RX4 RX6 RX7で瞬時転送OK
//rev800_3 ADS BNOデータを１２個分bBufに収納してSerial.write転送 ubxデータ読みOK
//rev800_4 bno timepulse追加　
//rev800_6 F9P受信全部460800で欠落ゼロになった。
//rev800_7 ADSモジュール追加
#include "BNO.h"
#include <SD.h>
static int loopn,loop4n,loop4ns,loop6n,loop6ns,loop7n,loop7ns,loopbn;
//-----BNO085 parameter definitions-----------------------
//static uint8_t serialbuffer1[32];//バッファは、static変数配列で宣言しておく。32byteの倍数で確保すること
//static uint8_t serialbuffer3[32];//バッファは、static変数配列で宣言しておく。32byteの倍数で確保すること
static uint8_t bnbuf[2000];
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の合計数
//----------------------------------------------------
File file4;
File file6;
File file7;
File myFIleB;
File myFileI;//BNO ADS
//File関係パラメータ------------------------------------
static char fname4[30];//RX4
static char fname6[30];//RX6
static char fname7[30];//RX7
static char fnameI[30];//imu ADS
static char fnameT[30];//予備
static int measureflag=0;
static uint8_t serialbuffer4[108];//バッファは、64がデフォルトであるので、それを引いて設定　２の乗数ならよい
static uint8_t serialbuffer6[108];//バッファは、64がデフォルトであるので、それを引いて設定　２の乗数ならよい　
static uint8_t serialbuffer7[108];//バッファは、64がデフォルトであるので、それを引いて設定　２の乗数ならよい　
static uint8_t dBuf4[172];
static uint8_t dBuf6[172];
static uint8_t dBuf7[172];
static int i,j,k,n;
static int dcount4,dcount4_1,dcount6,dcount6_1,dcount7,dcount7_1;
static int t4,t6,t7,t4e,t6e,t7e,bnt1,bnt2,bnt3,bnt4;
//FORUM from AndyA 2025/4/2
int dBuf4Bytes = 0; // count of bytes in the serial buffer.
int dBuf6Bytes = 0; // count of bytes in the serial buffer.
int dBuf7Bytes = 0; // count of bytes in the serial buffer.
const int expectedBytes[] = {0xB5,0x62,0x01,0x07}; // expected header
const int expectedBytesLength = 4;
const int expectedPacketSize = 172;
//---------------------Base Timepulse Parameters-------------------------------
static int tIs,tIe,tw,tw_1;//bno SD write time
static int timepulse=27;//Pin27 In
static int tpt,tpt3k,tpt3k_flag,itow3k,itowtpt;//TImePulse 同期用
static String tptstr;
static int epochN=0;//受信したPVTをカウント
static int epochN_1;
static int itow4,itow4_1,itow6,itow6_1,itow7,itow7_1;
static int itowerr4,itowerr6,itowerr7,itowtime,itowtimeErr;//itowtimeはitow計算した時刻
//----------------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;
static int geta1chi=0;
static int geta2chi=0;
//------------BNO ADS parameters-----------------------------
static uint8_t bBuf[1000];//ADS+BNOの１２個分収納
static int bbn;//bBufカウンタ
//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=10;
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==================================
//---------------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------------
//---------------------------------TIME PULSE Interrupt Function--------------------------------------------
//----------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------
void tpIn(){//TimePulseが入った瞬間のマイコン時刻を返す
//int tpt;
tpt=millis();
itowtpt=itow4;
//Serial.println();
//Serial.printf("<tpIn=%d,itow4=%d,itow4 mod 3000=%d,bncntT=%d>\n\r",tpt,itow4,(itow4)%3000,bncntT);
//delay(1);
if(itow4%3000==0){//itowbが2880になっていたら
tpt3k=tpt;//タイムパルスが3000になったときのtpt
tpt3k_flag=1;//6000msecフラフ
itow3k=itow4;//3000msecでのitow値
Serial.printf("******tpt3k=%d,itow3k=%d,tpt3k_flag=%d\n\r",tpt3k,itow3k,tpt3k_flag);
}
}
//OOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO
void bnowrite(){
loopbn++;
//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に変換
uint8_t sitow[4];//itowbaseを4byteに変換
uint8_t sitowtime[4];//itowtime
uint8_t stpt[4];//tpt
uint8_t sitowtpt[4];//itowtpt
uint8_t sbncntT[4];//bncntT
if( bncnt0>bncnt0_1){//dN>500で毎回カウンタ更新された場合に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("yaw1=%4.2f,dB1[3]=%x,dB1[4]=%x\n\r",yaw1,dB1[3],dB1[4]);
//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に分解
//itow 同期データ変換
i_to_char(itow4,sitow, 0);//
i_to_char(itowtime,sitowtime, 0);//
i_to_char(tpt,stpt, 0);//
i_to_char(itowtpt,sitowtpt, 0);//
i_to_char(bncntT,sbncntT, 0);//
//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のマーク
//Serial.print(0xFF,HEX);
bBuf[bbn]=0xFF;//HEADER bbn=0
//Serial.printf("----------------bncnt0=%d-------------------\n\r",bncnt0);
//Serial.printf("bbn=%d,bncnt0=%d,time=%d\n\r",bbn,bncnt0,micros());
//dataNo---------------------------------------
for(i=0;i<4;i++){
bbn++;//bbn=1,2,3,4
bBuf[bbn]=dno[i];//dataNo 4byte
}
//ADS data1chi 4byte wtite------------------
for(i=0;i<4;i++){
bbn++;//bbn=5,6,7,8
bBuf[bbn]=adno1[i];//dataNo 4byte
}
//ADS data2chi 4byte wtite------------------
for(i=0;i<4;i++){
bbn++;//bbn=9,10,11,12
bBuf[bbn]=adno2[i];//dataNo 4byte
}
//BNO Center RX3 19byte書き込み
for(i=0;i<19;i++){
bbn++;//bbn=13-31
bBuf[bbn]=dB3[i];//dB3[0-19]
}
//BNO Top RX1 19byte書き込み
for(i=0;i<19;i++){
bbn++;//bbn=32-50
bBuf[bbn]=dB1[i];
}
//itow同期データ　書き込み
for(i=0;i<4;i++){//sitow
bbn++;//bnn=51,52,53,54
bBuf[bbn]=sitow[i];
}
for(i=0;i<4;i++){//sitowtime
bbn++;//bbn=55,56,57,58
bBuf[bbn]=sitowtime[i];
}
for(i=0;i<4;i++){//stpt
bbn++;//bbn=59,60,61,62
bBuf[bbn]=stpt[i];
}
for(i=0;i<4;i++){//sitowtpt
bbn++;//bbn=63,64,65,66
bBuf[bbn]=sitowtpt[i];
}
for(i=0;i<4;i++){//sitowtpt
bbn++;//bbn=67,68,69,70
bBuf[bbn]=sbncntT[i];
}
//Serial.printf("bbn=%d,bncnt0=%d,time=%d\n\r",bbn,bncnt0,micros());
if(bbn%840==0){
Serial.write(bBuf,840);
//Serial.println();
//Serial.printf("WRITTEN:bbn=%d,bncnt0=%d,time=%d\n\r",bbn,bncnt0,micros());
bbn=0;
//softwareReset();
}
bncnt0_1=bncnt0;
tIe=millis();
}//bncnt>bncnt_1
}//BNO 受信　ログ　END
//========================================================================================================
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@Fielname 作成　タイムスタンプ　PVTから@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
void Filename () {//dBuf4[]はグローバル
// MM=dBu4v[12],DD=dBuf4v[13],HH=dBuf4v[14],mnt=dBuf4v[15]
int nn=0;
for(nn=0;nn<20;nn++){
//Serial.printf("dBuf4[%d]=%x\n\r",nn,dBuf4[nn]);
}
//ファイル名をタイムスタンプ
int JST = (dBuf4[14] + 9) % 24; //UTC hourをJSTに変換
int JST_day;
if (JST < 9) { //UTCが０時以前の場合日付を日本日付に修正
JST_day = 1;
}
else {
JST_day = 0;
}
//MMDDHHｍｍを数値にしてから、一括itoaする　
long MMi=dBuf4[12]* 1000000;
long DDi=(dBuf4[13]+JST_day)*10000;
long HHi=(JST)*100;
long mnti=dBuf4[15];
long datei=MMi+DDi+HHi+mnti;
//Serial.printf("datei=%d\n\r",datei);
char dateic[13];
itoa(datei,dateic, 10); // 10は10進数
int dcLen=strlen(dateic);//sizeof(dateic);
for(nn=0;nn<dcLen;nn++){
// date[nn]=dateic[nn];
//Serial.printf("dateic[%d]=%c\n\r",nn,dateic[nn]);
}
//fnameB,fnameT,fnameI代入
for(nn=0;nn<dcLen;nn++){
fname4[nn]=dateic[nn];
fname6[nn]=dateic[nn];
fname7[nn]=dateic[nn];
fnameI[nn]=dateic[nn];
fnameI[nn]=dateic[nn];
//Serial.printf("dateic[%d]=%c\n\r",nn,dateic[nn]);
}
//fname4仕上げ
fname4[dcLen]='R';
fname4[dcLen+1]='4';
fname4[dcLen+2]='.';
fname4[dcLen+3]='u';
fname4[dcLen+4]='b';
fname4[dcLen+5]='x';
fname4[dcLen+6]='\0';
//fname6仕上げ
fname6[dcLen]='R';
fname6[dcLen+1]='6';
fname6[dcLen+2]='.';
fname6[dcLen+3]='u';
fname6[dcLen+4]='b';
fname6[dcLen+5]='x';
fname6[dcLen+6]='\0';
//fname7仕上げ
fname7[dcLen]='R';
fname7[dcLen+1]='7';
fname7[dcLen+2]='.';
fname7[dcLen+3]='u';
fname7[dcLen+4]='b';
fname7[dcLen+5]='x';
fname7[dcLen+6]='\0';
//fnameT仕上げ
fnameT[dcLen]='.';
fnameT[dcLen+1]='t';
fnameT[dcLen+2]='x';
fnameT[dcLen+3]='t';
fnameT[dcLen+4]='\0';
//fnameI仕上げ
fnameI[dcLen]='.';
fnameI[dcLen+1]='b';
fnameI[dcLen+2]='i';
fnameI[dcLen+3]='n';
fnameI[dcLen+4]='\0';
//---------------------------------------------
for(nn=0;nn<strlen(fname4)+1;nn++){
Serial.print(fname4[nn]);
}
Serial.println();
for(nn=0;nn<strlen(fname6)+1;nn++){
Serial.print(fname6[nn]);
}
Serial.println();
for(nn=0;nn<strlen(fname7)+1;nn++){
Serial.print(fname7[nn]);
}
Serial.println();
for(nn=0;nn<strlen(fnameI)+1;nn++){
Serial.print(fnameI[nn]);
}
//以下Stringでヒープエラー
delay(1);
delay(10);
}
//Filename() end ================================================================
void softwareReset() {
SCB_AIRCR = 0x05FA0004; // ARM Cortex-M7のソフトウェアリセット
}
//============LIST ===========================
void listFiles(File dir, int numTabs) {
while (true) {
File entry = dir.openNextFile();
if (!entry) {
// ファイルがもう無ければ終了
Serial.println("Done!");
Serial2.println("Done!");
break;
}
for (int i = 0; i < numTabs; i++) {
Serial.print("\t"); // インデントを追加
}
Serial.print(entry.name());
if (entry.isDirectory()) {
Serial.println("/");
listFiles(entry, numTabs + 1); // 再帰的にフォルダ内を探索
} else {
//Serial.print("\t");
Serial.print(",");
Serial.print("size,");
Serial.println(entry.size(), DEC);
}
entry.close();
}//while end
}
//==========================================================
void RX4rcv()
{
loop4n++;
if(Serial4.available()>4) {
loop4ns++;
t4=micros();
int temp = Serial4.read(); // read next byte
if (dBuf4Bytes < expectedBytesLength) { // if looking for a header
if (temp == expectedBytes[dBuf4Bytes]) { // if new byte matches the expected header for this position
dBuf4[dBuf4Bytes++] = temp;
} else {
dBuf4Bytes == 0; // wrong byte. Reset
}
} else {
dBuf4[dBuf4Bytes++] = temp; // outside the header, add data to buffer.
}
if (dBuf4Bytes == expectedPacketSize) { // got the whole packet
dcount4++;
i=dBuf4Bytes;
//PortNo write
dBuf4[4]=0x04;//Serial4 Marking
//Write USBSERIAL Port
Serial.write(dBuf4,i);
dBuf4Bytes = 0; // reset packet detection
t4e=micros();
Serial.printf("dcount++=%d,t4=%d,t4e=%d,time=%d,itowerr4=%d\n\r",dcount4,t4,t4e,t4e-t4,itowerr4);
itow4_1=itow4;
itow4=dBuf4[6]+dBuf4[7]*256+dBuf4[8]*256*256+dBuf4[9]*256*256*256;//BNO同期用
if(itow4-itow4_1!=120){
itow4_1=itow4;
itowerr4++;
}
//Serial.printf("dcount4=%d,i=%d\n\r",dcount,i);
}
}
}
void RX6rcv()
{
loop6n++;
if(Serial6.available()) {
loop6ns++;
t6=micros();
int temp = Serial6.read(); // read next byte
if (dBuf6Bytes < expectedBytesLength) { // if looking for a header
if (temp == expectedBytes[dBuf6Bytes]) { // if new byte matches the expected header for this position
dBuf6[dBuf6Bytes++] = temp;
} else {
dBuf6Bytes == 0; // wrong byte. Reset
}
} else {
dBuf6[dBuf6Bytes++] = temp; // outside the header, add data to buffer.
}
if (dBuf6Bytes == expectedPacketSize) { // got the whole packet
dcount6++;
j=dBuf6Bytes;
//PortNo write
dBuf6[4]=0x06;//Serial4 Marking
//write USBSErial port
Serial.write(dBuf6,j);
dBuf6Bytes = 0; // reset packet detection
t6e=micros();
}
}
}
void RX7rcv()
{
loop7n++;
if(Serial7.available()) {
loop7ns++;
t7=micros();
int temp = Serial7.read(); // read next byte
if (dBuf7Bytes < expectedBytesLength) { // if looking for a header
if (temp == expectedBytes[dBuf7Bytes]) { // if new byte matches the expected header for this position
dBuf7[dBuf7Bytes++] = temp;
} else {
dBuf7Bytes == 0; // wrong byte. Reset
}
} else {
dBuf7[dBuf7Bytes++] = temp; // outside the header, add data to buffer.
}
if (dBuf7Bytes == expectedPacketSize) { // got the whole packet
dcount7++;
k=dBuf7Bytes;
//PortNo write
dBuf7[4]=0x07;//Serial4 Marking
//write USB Serail Port
Serial.write(dBuf7,k);
//write dBuf7 to SD card
dBuf7Bytes = 0; // reset packet detection
t7e=micros();
}
}
}
//[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[
//----------SD UPDATE TEST-----------------------------------------------------------------------------------------------------
//起動後　u　キーを受信したらここでループする
void uptest(){
int i=0;
int clen=0;
char ct=' ';
char flag=' ';
int ec=0;//eror counter
while(true)
{
char c[30]={};
ct=' ';
i=0;
//Serial bufferを空にする
delay(1000);
Serial2.print("*");
Serial.print("*");
while(Serial.available()>0)
{
char d=Serial.read();
ec++;
Serial2.printf(" SerialBuffer Input Error%d:d=%x,%c\n\r",ec,d,d);
Serial.printf(" SerialBuffer Input Error%d:d=%x,%c\n\r",ec,d,d);
}
//[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[
while(ct!=0x0d )//ファイル番号入力待ち c[]にファイル名を収納
{
delay(1000);
Serial2.print("*");
Serial.print("*");
if(Serial.available()>0)
{
ct=Serial.read();
if(ct=='l'){
c[0]='l';
ct=0x0d;
}
if(ct>0x20 && ct<0xFF)
{
c[i]=ct;
}
//cs=c[i];
Serial2.printf("c[%d]=%x,%c,ct=%c\n\r",i,c[i],c[i],ct);
Serial.printf("c[%d]=%x,%c,ct=%c\n\r",i,c[i],c[i],ct);
i++;//受信文字数
}
//Serial2.println();
delay(100);
}//while 0x0d end]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]
//ｃの終端に0x20追加
c[i]=0x20;
clen=i;//ファイル番号末尾に\n
int sizec=sizeof(c);
Serial2.printf("sizeof(c)=%d,i=%d\n\r",sizec,clen);
Serial.printf("sizeof(c)=%d,i=%d\n\r",sizec,clen);
for(int n=0;n<clen;n++)
{
Serial2.printf("===>c[%d]=%x,'%c',size=%d\n\r",n,c[n],c[n],clen);
Serial.printf("===>c[%d]=%x,'%c',size=%d\n\r",n,c[n],c[n],clen);
}
delay(100);
if(c[0]=='l')
{
File root = SD.open("/");
listFiles(root, 0);
root.close();
}
//if(s.indexOf("bin")>0 ||s.indexOf("ubx") && s.length()>6)
if (clen>6)
{
for(j=0;j<clen;j++)
{
Serial2.printf("IN>6:c[%d]=%x:%c\n\r",j,c[j],c[j]);
Serial.printf("IN>6:c[%d]=%x:%c\n\r",j,c[j],c[j]);
}
//Serial2.printf("sc:%s, length=%d fn0.len=%d\n\r",ｃ,ｃ.length(),sizeof(fn0));
//myFileB=sd.open(c,FILE_READ);
File myFileB = SD.open(c, FILE_READ);
int t0=millis();
int count=0;
while(myFileB)
{
byte b=myFileB.read();
Serial.write(b);//USB 送信
count++;
}
Serial2.println("============================================================================");
Serial2.printf("s>4 %d bytes time=%d msec \n\r",count,millis()-t0);
Serial2.println("============================================================================");
Serial.println("============================================================================");
Serial.printf("s>4 %d bytes time=%d msec \n\r",count,millis()-t0);
Serial.println("============================================================================");
myFileB.close();
//delay(5000);
clen=0;
exit(0);
}
delay(500);
}
}//uptest END
//]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]
void setup() {
// put your setup code here, to run once:
Serial.begin(921600);
Serial2.begin(115200);//Bluetotth SPP ESP32E
delay(5000);
Serial.println("=============WAIT 10sec====================");
Serial2.println("=============WAIT 10sec====================");
delay(5000);
Serial1.begin(115200);//Tail BNO085 RVC MODE pin0 Tx In
//attachInterrupt(timepulse, tpIn,FALLING);//TimePulse 下がりエッジで割り込み関数tpInへ飛ぶ
Serial3.begin(115200);//Center BNO085 RVC MODE pin15 TX In
Serial5.begin(115200);//ADS IF
//*********************HARDWARE SERIAL SETUP********************************
Serial4.begin(460800);//抜け対策
Serial4.addMemoryForRead(serialbuffer4, sizeof(serialbuffer4));
Serial6.begin(460800);
Serial6.addMemoryForRead(serialbuffer6, sizeof(serialbuffer6));
Serial7.begin(460800);
Serial7.addMemoryForRead(serialbuffer7, sizeof(serialbuffer7));
//ADS init
adsinit();//
pinMode(DRDY, INPUT_PULLUP);
//attachInterrupt(digitalPinToInterrupt(2),呼び出される関数,FALLING);
attachInterrupt(DRDY, flip,FALLING);//DRDY 下がりエッジで割り込み関数flipへ飛ぶ
Serial.println("Keyin any key");
int keywait=0;
char ikey;
while(keywait==0)
{
if(Serial.available()>0)
{
ikey=Serial.read();
Serial.printf("Recieved%c\n\r",ikey);
if(ikey=='u'){
Serial.println("UPTEST IN");
uptest();//アップロードモードに入って戻ってこない
}
if(ikey=='b'){//BEND TESTへCPLT対応
keywait=1;//keywait
Serial.println("BEND TEST IN");
}
if(ikey=='a'){//計測モードへ入る
keywait=1;//keywaitぬj家出す
measureflag=1;
Serial.println("Measure START");
}
if(ikey=='m'){//Monitorモードへ入る
keywait=1;//keywait出す
measureflag=2;
Serial.println("Monitor START");
}
}
if(Serial2.available()>0)
{
ikey=Serial2.read();
Serial2.printf("Recieved%d\n\r",ikey);
if(ikey=='u'){
Serial2.println("UPTEST IN");
uptest();//アップロードモードに入って戻ってこない
}
else if(ikey=='b'){//BEND TESTへCPLT対応
keywait=1;//keywaitぬj家出す
Serial2.println("BEND TEST IN");
}
else if(ikey=='a'){//計測モードへ入る
keywait=1;//keywaitぬj家出す
measureflag=1;
Serial2.println("Measure START");
}
else if(ikey=='m'){//Monitorモードへ入る
keywait=1;//keywaitぬj家出す
measureflag=2;
Serial2.println("Monitor START");
}
}
}//key while
loop4n=0;
loop4ns=0;
bncnt0=1;
bncnt0_1=0;
}//SETUP END
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
void loop() {
loopn++;
//----------BNOREAD してSD ログ　dN>500以降常時ログ------------------------------------
bnt1=micros();
bncnt0=bnoread31(dB3,dB1,19,bncnt0);//cnt dB1[19]戻り　ｄB3 Center dB1 Top
bncntT=millis();
bnt2=micros();
bnowrite();
bnt3=micros();
//Serial.printf("bncnt0=%d TIME=%D\n\r",bncnt0,millis());
//Serial4 Recieving-----------
t4=micros();
RX4rcv();
itowtime=millis();//BNO同期用
t4e=micros();
RX6rcv();
t6e=micros();
RX7rcv();
t7e=micros();
//if( dcount4-dcount4_1>0 ){
// Serial.printf("itowerr=%d,itow4=%d\n\r",itowerr4,itow4);
// dcount4_1=dcount4;
// }
//if(dcount4-dcount4_1>0 )
// {
// dcount4_1=dcount4;
//itow4=dBuf4[6]+dBuf4[7]*256+dBuf4[8]*256*256+dBuf4[9]*256*256*256;
//itow6=dBuf6[6]+dBuf6[7]*256+dBuf6[8]*256*256+dBuf6[9]*256*256*256;
//itow7=dBuf7[6]+dBuf7[7]*256+dBuf7[8]*256*256+dBuf7[9]*256*256*256;
//Serial.printf("bncnt0=%d,bnt1=%d,bnt2%d,bnt3=%d,time=%d\n\r",bncnt0,bnt1,bnt2,bnt3,millis());
//Serial.printf("loopn=%d,loopbn=%d,loppn4n=%d,loop4ns=%d,loop6n=%d,loop6ns=%d,loop7n=%d,loop7ns=%d\n\r",loopn,loopbn,loop4n,loop4ns,loop6n,loop6ns,loop7n,loop7ns);
// Serial.printf("%d,dBuf4[0,1,2,3=%x,%x,%x,%x,t4=%d,%d,%d,%d\n\r",dcount4,dBuf4[0],dBuf4[1],dBuf4[2],dBuf4[3],t4,t4e,t4e-t4,itow4);
//Serial.printf("%d,dBuf6[0,1,2,3=%x,%x,%x,%x,t6=%d,%d,%d,%d\n\r",dcount6,dBuf6[0],dBuf6[1],dBuf6[2],dBuf6[3],t4e,t6e,t6e-t4e,itow6);
//Serial.printf("%d,dBuf7[0,1,2,3=%x,%x,%x,%x,t7=%d,%d,%d,%d\n\r",dcount7,dBuf7[0],dBuf7[1],dBuf7[2],dBuf7[3],t6e,t7e,t7e-t6e,itow7);
//softwareReset();
// }//
}//LOOP END
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment