BNO085 STAシステムに組み込みました。100Hzで姿勢角が出力できるので、そのままログできるようにプログラム改造しました。５か月ぶりにシステムプログラムいじるので、思い出すのに３日かかりました。
●苦労した点
BNO085の出力100Hzとなると、10msecなので、ＳＴＡ全システムのループを10msec周期に余裕を持たせて回さないといけません。実際は、4-6msecで回ってます。ログしてない時は、1msec以内でSerial受信検知ループしてます。
ＳＤカード書き込み、BlueTooth送信、ＰＣ出力などやりながら、Base ,Rover,IMUのデータを受信する仕事があるので、Teensyの能力フルで使わないとできません。ESP32系では絶対できないシステムです。
ESP32系のシリアルは、別コアのバックグランドで動いてますが、WiFiとか無線もバックグランドで動いているので、プログラムが忙しくなるとシリアルがデータ落ちします。そもそもUARTが2本しかありません。
●BNO085 の100Hzデータ受信 UART-RCVモード
①周期が10msecでシステムループで間に合うか？
普通のI2CのＩＭＵと違って、UART-RCVモードは、垂れ流しで送ってきます。ばらつくのでいつ入ってくるのか判りません。システムのメインループで、面倒をみているとデータ落ちしてしまうので、Teensy Hardwareシリアルで、解決できました。ハードバッファが自動的に受信してくれるので、ＲＴＫの120msec周期と10msecを独立して受信できます。ＢＮＯ０８５の受信部分を備忘録しておきます。
①全部で19バイトを漏れなく読み込まないとバッファが空にならないから、要らないデータでも読み込む。
②ハードバッファサイズは２５６バイト確保してあるが、１９バイトになったら、読み込むので、１９バイトしか
使ってない。setup でhard serialのバッファを宣言しておきます。
| static uint8_t serialbuffer5[256];//バッファは、static変数配列で宣言しておく。32byteの倍数で確保すること
//Serial3 def BNO085 |
③ＲＴＫシステムの１周期120msecあるので、IMU周期10msecで１２回分ため込むためにバッファ配列dBuf5[228]
を用意して１２回分のデータを保持する。
| //==================================================================================================== //===Serial3 BNO085 UART-RCV read===================================================================== //==================================================================================================== //[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]Chcksumif(Serial3.available()>19){ //10msecのデータセット19byte になったら読み込むuint8_t b0=Serial3.read(); uint8_t b1=Serial3.read(); uint8_t b2=Serial3.read();if(b0==0xaa && b1==0xaa) { int n=(bn85%12)*19;//0-228 dBuf5[0+n]=b0; dBuf5[1+n]=b1; dBuf5[2+n]=b2; //Serial.printf(“%d,%x,%x,”,bn85%144,b0,b1); for(i=0;i<16;i++)//12byteずつ読み込み格納12×1 { dBuf5[i+n+3]=Serial3.read();} bn85++;; }//0xaa && 0xaa end}//if serial3.avaialble end |
|
|
④10msecシリアル受信結果
●BlueToothログ結果
|
