BNO085をSTA24システムに組み込んだだけでは、F9PのRTKデータと同期ができないので、
プログラムに同期部分を追加しました。ややこしいので備忘録しておきます。
※２０２３年１２月１日 １１月２８日に本記事投稿しましたが、その後、バグがあって、プログラム変更があったので
記事を変更しました。バグは、BNO０８５のデータ変換とBNOシリアル受信とデータソートの追加です。
どういうわけか、ヘッダが先頭に受信されない現象があって、SORTをいれました。
●同期に必要な機能
| 機能名 | |
| TimePulse割り込み | ピン割り込みで、パルスが１秒毎に入ったときに、同期用割り込み関数を起動 |
| 同期用割り込み関数 | F9P_RTK周期とBNO085周期とTimePuse周期が合った瞬間に同期データ作成 F9P_RTk120msec BNO 10msec TimePulse1000msecの場合は、3000msecで全部同期する瞬間のマイコンタイマー時刻 tp03kとその時の正しいitow itow03kを記録する。BNO085のカウンタとしてbn8nを３秒に一回リセットする。 bn8nは、BNO085のタイミングとTimePulse基準時間の同期を決めるカウンタ |
| BNOデータ保持配列 | data配列を受信バッファdBuf5[i]からdBufd[bn8n%300][i]に保管、bn8nは、3秒に一回リセットされる受信回数カウンタで、リアルタイムと同期するためにある。 bn8n=0は。itow%3000=0になる時刻でそこからカウントすることで、BNO085のデータがitow時刻座標に正確に乗ることができる。millis(）で管理すると、シリアルバッファがため込んでいる場合時刻が数十msecずれるので、使えない。 念のために、bnitow[bn8n%300]とBNOのitow保持配列も作成しておいてある。 |
| RTKデータとの同期 | RTKデータは、真のitow時刻より２０～７０msec遅延したばらつくため、 いつRTKデータがログルーチンに入るか不定なため、入った時点で、RTKのitowをみて、そのitowと同じitowのBNOデータを頭だししてログ出力させる。 そのため、3秒分のBNOデータを保持することにした。 |
| BNOデータのSORT | bn8n=268,FF2AFF1B0FCFFDFFFEC3000FFAAAAF6FF とヘッダのAAAAが後端にずれているので、SORTルーチンをいれた やり方は、このデータをコピーして後端に貼り付けて、倍長のデータにしてから AAAAを頭出しして、それから19byte切り取るというやり方です。このやり方だと カウンタを使わないですむので、ややこしくなりません。 |
●プログラム
https://gist.github.com/dj1711572002/f15816c4eb848efbf456e6af3c6216c8
①TimePulse割り込み関数
| /===============TimePulse Interrupt=============================== // —–BNO interrupt timepulse pin22 void myInterrupt(){ tp0=micros();//以後GPS TIMEPULSE周期でサンプリングint mod3000=itow0%3000; if(mod3000==2880) { tpflag=1; tp03k=millis(); itow03k=itow0+120; bn8n=0;//BNO085 10msecカウンタ 3000msecでリセットSerial.println(); Serial.printf(“tp03k=,%d,itow03k=,%d,itow0=,%d\n\r”,tp03k,itow03k,itow0); } //if(timersetInit==0)//初回だけタイマーセット |
②BNO０８５Serial3受信とカウンタ同期
| //======================================================================================================================================================================== //===Serial3 BNO085 UART-RCV read==============================Serial3 BNO085 UART-RCV read=================================Serial3 BNO085 UART-RCV read================== //======================================================================================================================================================================== //uint8_t dBuf5[0-287] 12*19byte //[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 //カウンタbn8nは、3000秒周期でデータ受信回数カウンタ0-299 3000msecでリセット if(Serial3.available()>19 && tpflag==1) { int bi; for(bi=0;bi<19;bi++) { dBuf5[bi]=Serial3.read();//b8n番目のdBuf5[]ヘッダアドレスから19byte書き込み dBuf5b[bn8n%300][bi]=dBuf5[bi];//dBuf5bに300個収納 } bnitow[bn8n%300]=(bn8n%300)*10+itow03k;//3000msecのitow03kからカウント開始bn8n++;//bno085サンプリング回数カウント 3000msec周期でリセット}//if serial3.avaialble end |
●結果
BlueToothモニターへ送信される内容
[m]がRTKの状態情報
[Base]itow,Baseの情報
[Rover] Roverの情報
[BNy]同期したitow,BNO085のyaw角10msec周期１２個
[BNp]BNO085のpitch角10msec周期１２個
[BNr]BNO085のrolll角10msec周期１２個
ログ結果例
| [m],67,67,163.00,178.00,98,144,0,0,450,0,65173464 [Base],423869160,-1416.7,-1851.0,-468.6,1, 0.0, 0.0,1,1,67 [Rover],423811200,0.00,0.00,0.00,54,0.00, 0.0 [BNy],423869160,11.1,11.6,12.2,10.5,14.3,13.1,17.0,15.6,18.7, 2.5,-0.3,-0.4 [BNp],-23.6,-23.7,-23.9,-24.0,-23.9,-23.5,-23.1,-22.8,-22.5,-26.2,-26.1,-25.9 [BNr],-17.3,-17.7,-18.5,-19.5,-20.5,-21.6,-22.3,-22.5,-22.9,-15.6,-15.4,-15.3 |
●以後
バー式MovingBaseの基板にBNo085を追加して、実際に振り回して100HzデータとRTKデータを比較実験する
