Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/Teensy_STA25_ADS122U04_1_2ch_BNO085x2_rev02.ino Secret

Created January 6, 2025 15:24
Show Gist options
  • Save dj1711572002/0164a2993157aa0d089a4822604a17e6 to your computer and use it in GitHub Desktop.
Save dj1711572002/0164a2993157aa0d089a4822604a17e6 to your computer and use it in GitHub Desktop.
Download ZIP
Teensy41 STA25 ADS122U04 and BNo085x2 Sychronizing Pgm
Raw
Teensy_STA25_ADS122U04_1_2ch_BNO085x2_rev02.ino
//2025/1/4/15:36 flipのDRDYピン割り込みで2.3msec周期なので、4周に一回ログ
//20251/612:07 BNO085と同期合体はじめ
//For Teensy4.1
//----------------Texsus ADS122U04--------------------------
static int DRDY = 23; //DataRdy Pin 23
static int DRDY_1;
static 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 d0, d1, d2, d3, d4, d5, d6;
static long data1chi, data2chi;
static double data1ch, data2ch;
int DRDYflag = 0;
int res;
int cd=0;
//-----BNO085 parameter definitions-----------------------
static uint8_t serialbuffer1[256];//バッファは、static変数配列で宣言しておく。32byteの倍数で確保すること
static uint8_t serialbuffer3[256];//バッファは、static変数配列で宣言しておく。32byteの倍数で確保すること
static uint8_t dBuf1[32];
static uint8_t dBuf3[32];
static float dB1[16];
static float dB3[16];
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];
//==================InterruptIn DRDY====================
void flip()
{
flipN++;
//Serial.printf("flipN=%d\n\r",flipN);
ts=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);
d0 = Serial5.read();
delayMicroseconds(10);
d1 = Serial5.read();
delayMicroseconds(10);
d2 = Serial5.read();
delayMicroseconds(10);
data1chi=Toint24(d0,d1,d2);
//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);
d3 = Serial5.read();
delayMicroseconds(10);
d4 = Serial5.read();
delayMicroseconds(10);
d5 = Serial5.read();
delayMicroseconds(10);
data2chi=Toint24(d3,d4,d5);
//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())-ts;
//period=ts-ts_1;
ts_1=ts;
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,ts,period,ts-ts_10);
//Serial.printf("%d,%d\n\r",data1chi,data2chi);
ts_10=ts;
}
}
}
//-----------ADS122U04 Initialize-------------
void adsinit()
{
//=============ADS122U04レジスタ初期設定==========================================
//ADS122U04 RESET
Serial5.write(0x55);
delayMicroseconds(cd);
Serial5.write(0x06);
delayMicroseconds(1000);
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;
}
//----------------------------------------------------------------------
//----------------------------------------------------------------------
//BNO085 read SUB--------------------------------------------------------
//-----------------------------------------------------------------------
//-----------------------------------------------------------------------
//----BNO085バイナリー整数変換---------------------------------------------
int bin2int(uint8_t c0,uint8_t c1){
int yawi=c0+c1*256;
if(c1>=128){
yawi=yawi-65535;
}
return yawi;
}
//-----------------------------------------------------------
int bnoread1(float *data,int n)//1shotを受信して変換してdataで返す
//returnは、時刻 配列 d[]のポインタを渡されるので、*dがd番地の実数　nは、バイト数
{
int bn85=0;//
int t,t_1;
if(Serial1.available()>19){ //19byte x12回でdBuf5満杯
//[0]0xaa,[1]0xaa,[2]index,[3]yawL,[4]yawM,[5]pitchL,[6]pitchM,[7]rollL,[8]roolM,[9]axL,[10]axM,[11]ayL,[12]ayM,[13]azL,[14]azM,[15]MI,[16]MR,[17]Rsv,[18]Chcksum
uint8_t b0=Serial1.read();
uint8_t b1=Serial1.read();
uint8_t b2=Serial1.read();
t=micros();
if(b0==0xaa && b1==0xaa)
{
period[bn85%12]=t-t_1;
t_1=t;
int n=(bn85%12)*19;//0-228
dBuf1[0+n]=b0;
dBuf1[1+n]=b1;
dBuf1[2+n]=b2;
//Serial.printf("%db0=%x,b1=%x,",bn85%144,b0,b1);
int i;
for(i=0;i<16;i++)//12byteずつ読み込み格納12x1
{
dBuf1[i+n+3]=Serial1.read();
dB1[i]=dBuf1[i+n+3];
//Serial.print( dBuf1[i+n+3],HEX);
//Serial.print(",");
//---------------------------
}
if(bn85%12==0)
{
data[0]=(float)(bin2int(dB1[0],dB1[1])*0.01);//yaw
data[1]=(float)(bin2int(dB1[2],dB1[3])*0.01);//pitch
data[2]=(float)(bin2int(dB1[4],dB1[5])*0.01);//roll
data[3]=(float)(bin2int(dB1[6],dB1[7])*0.01);//ax
data[4]=(float)(bin2int(dB1[8],dBuf1[9])*0.01);//ay
data[5]=(float)(bin2int(dBuf1[10],dBuf1[11])*0.01);//az
//Serial.printf("1,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%d\n\r",data[0],data[1],data[2],data[3],data[4],data[5],millis());
}
//Serial.printf(":[%d]\n\r",millis());
bn85++;;
}//0xaa && 0xaa end
}//if serial available
t=micros();
return t;
}//bnoread1 END
//-----------------------------------------------------------
int bnoread3(float *data,int n)//1shotを受信して変換してdataで返す
//returnは、時刻 配列 d[]のポインタを渡されるので、*dがd番地の実数　nは、バイト数
{
int bn85=0;//
int t,t_1;
if(Serial3.available()>19){ //19byte x12回でdBuf5満杯
//[0]0xaa,[1]0xaa,[2]index,[3]yawL,[4]yawM,[5]pitchL,[6]pitchM,[7]rollL,[8]roolM,[9]axL,[10]axM,[11]ayL,[12]ayM,[13]azL,[14]azM,[15]MI,[16]MR,[17]Rsv,[18]Chcksum
uint8_t b0=Serial3.read();
uint8_t b1=Serial3.read();
uint8_t b2=Serial3.read();
t=micros();
if(b0==0xaa && b1==0xaa)
{
period[bn85%12]=t-t_1;
t_1=t;
int n=(bn85%12)*19;//0-228
dBuf1[0+n]=b0;
dBuf1[1+n]=b1;
dBuf1[2+n]=b2;
//Serial.printf("%db0=%x,b1=%x,",bn85%144,b0,b1);
int i;
for(i=0;i<16;i++)//12byteずつ読み込み格納12x1
{
dBuf1[i+n+3]=Serial3.read();
dB1[i]=dBuf1[i+n+3];
//Serial.print( dBuf1[i+n+3],HEX);
//Serial.print(",");
//---------------------------
}
if(bn85%12==0)
{
data[0]=(float)(bin2int(dB1[0],dB1[1])*0.01);//yaw
data[1]=(float)(bin2int(dB1[2],dB1[3])*0.01);//pitch
data[2]=(float)(bin2int(dB1[4],dB1[5])*0.01);//roll
data[3]=(float)(bin2int(dB1[6],dB1[7])*0.01);//ax
data[4]=(float)(bin2int(dB1[8],dBuf1[9])*0.01);//ay
data[5]=(float)(bin2int(dBuf1[10],dBuf1[11])*0.01);//az
//Serial.printf("3,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%d\n\r",data[0],data[1],data[2],data[3],data[4],data[5],millis());
}
//Serial.printf(":[%d]\n\r",millis());
bn85++;;
}//0xaa && 0xaa end
}//if serial available
t=micros();
return t;
}//bnoread1 END
//-----------------------------------------------------------------------
void setup() {
// シリアル設定
Serial.begin(115200);//USB
Serial1.begin(115200);//Tail BNO085 RVC MODE pin0 Tx In
Serial3.begin(115200);//Center BNO085 RVC MODE pin15 TX In
Serial1.addMemoryForRead(serialbuffer1, sizeof(serialbuffer1));//serialbuffer1のメモリーを 256byteバッファ指定
Serial3.addMemoryForRead(serialbuffer1, sizeof(serialbuffer1));//serialbuffer1のメモリーを 256byteバッファ指定
Serial5.begin(115200); //Teensy4.1 RX5-ADS122 RX5(21)-TX,TX5(20)-RX
delayMicroseconds(1000);
adsinit();
pinMode(DRDY, INPUT_PULLUP);
//attachInterrupt(digitalPinToInterrupt(2),呼び出される関数,FALLING);
attachInterrupt(DRDY, flip,FALLING);//DRDY 下がりエッジで割り込み関数flipへ飛ぶ
}
void loop() {
// put your main code here, to run repeatedly:
pt0_1=micros();
p1=bnoread1(dB1,16);
p3=bnoread3(dB3,16);
if(endflip==1 && p1-p1_1>9500)
{
Serial.printf("[%d,%d],%d,%d,%d,%d\n\r",data1chi,data2chi,ft1,ft2,pt0-pt0_1,micros());
Serial.printf("%d,%d,[%4.3f,%4.3f,%4.3f],[%4.3f,%4.3f,%4.3f],%d\n\r",p1,p3,dB1[0],dB1[1],dB1[2],dB3[0],dB3[1],dB3[2],p1-p1_1);
p1_1=p1;
pt0=micros();
//Serial.println("*");
endflip=0;
//delayMicroseconds(100000);//5msec Period
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment