スマホでRTK Moving Base Modeをその場でリアルタイムにモニターするために、AndroidスマホとF9PLuch-BoxをBlueTooth接続しました。事前にUBX送信シミュレータを作っていたのでBlueToothがおかしくならない条件をさがして、試行錯誤で何とか安定してスマホ受信できる条件をさがすことができました。無線は、BlueToothよりXbeeのほうがやりやすいと感じました。(写真は室内アンテナ状態ですので出鱈目測位値です)
これで、フィールドで使いながら改良評価していきます。
ucenterのRELPSONEDは小さくてみにくいのですが、スマホで大きく表示すると
見やすくてフィールドでのモニターとして便利になります。
第一段階は、UBXデータを抽出してテーブル表示する仕様です。
第二段階で、グラフィックで軌跡とheadingベクトルアニメーションを作りたいです。
必要最低限のデータをNAV-PVTとNAV-RELPOSNEDから抽出してBlueToothで送信して表示してます。このデータだけでアニメーションで軌跡とベクトルが描けます。
わざわざプログラムをつくらなくても、BlueTermで受信すればデータは確認とログができますので、時どき使います。
●作ったPgm概要
Pgm | 環境 言語 ライブラリ | 主な機能 |
F9PLuchBox内の Nucleo F446REのpgm |
mbed オンラインコンパイラ
mbed C++ ライブラリ無
|
①F9P MovingBaseモードのRoverから460800bpsで UBXデータ受信 ➁UBXデータバッファ ③UBXデータからモニター用パラメータ抽出計算 ④BlueToothでモニターパラメータ送信 ⑤UBX生データをSDカードへログ(第二段階) |
Androd スマホ Kyocera Torque G02 (ヤフオクSIMロックされた格安品) |
Processing Android モード
Processing IDE 3.5.3 Android Mode 4.0 Ketai BlueToothライブラリ 参考Pgm Prof TAKAYA様へ感謝 |
①BlueTooth受信バッファと変数代入
➁変数をテーブルへ表示更新 ③モニターデータのログ ④アニメーションで軌跡とベクトルを描く(第二段階) |
●工夫した点
①マイコン側プログラム
F9Pから460800bpsでbData[]配列へバッファします。172バイトを読み込んだら、次にF9Pから送信データが来るまでの百数十msecで、下記処理をします。
処理1:ubx_dec()関数でバイナリ-データから緯度経度、相対位置、アンテナ間距離、海抜、精度を32bit整数に変換してます。
処理2:モニターパラメータをBlueToothで115200bps送信
処理3:UBX生データをSDカードへ書き込む(未だ作ってません)
#include “mbed.h” #include “SDFileSystem.h” Serial pc(USBTX,USBRX); Serial F9P(PC_6,PC_7);;//F9P uart1 TX,RX-F446RE Serial6 TX,RX 5.62MHz Serial BT(PA_9,PA_10);//Xbee TX,RX F446RE Serail TX,RX 5.62MHz Timer t; DigitalOut myled(LED1); //==================UBX_DEC parameters========================== long relN; long relE; long relD; long Length; long Long; long Lati; long Seah; long Acc2d; long su; int sNo;//pvt start int rNo;//relposned start int i=0; int j=0; char sen[]={}; unsigned char c; unsigned char bData[]={0xB5,0x62,0x1,0x7,0x5C,0x0,0x60,0xDA,0x2D,0xB,0xE4,0x7,0x2,0x4,0x4,0x5,0x24,0x37,0x1,0x0,0x0,0x0,0x28,0xBD,0xDD,0x17,0x3,0x83,0xEA,0x15,0x35,0x7,0x30,0x52,0x6E,0x95,0xEB,0x15,0xE6,0xF2,0xD,0x0,0x0,0x65,0xD,0x0,0x88,0x0,0x0,0x0,0x86,0x0,0x0,0x0,0x9,0x0,0x0,0x0,0xFA,0xFF,0xFF,0xFF,0x8,0x0,0x0,0x0,0xB,0x0,0x0,0x0,0xF2,0x4A,0x18,0x0,0xDA,0x0,0x0,0x0,0x80,0xA8,0x12,0x1,0x7B,0x0,0x0,0x0,0xBA,0x65,0x43,0x2C,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x35,0x5D,0xB5,0x62,0x1,0x3C,0x40,0x0,0x1,0x0,0x0,0x0,0x60,0xDA,0x2D,0xB,0xE8,0xFF,0xFF,0xFF,0x3F,0x0,0x0,0x0,0xA,0x0,0x0,0x0,0x44,0x0,0x0,0x0,0x5A,0x37,0xAA,0x0,0x0,0x0,0x0,0x0,0xA8,0x0,0x24,0x34,0x64,0x0,0x0,0x0,0x64,0x0,0x0,0x0,0x64,0x0,0x0,0x0,0x64,0x0,0x0,0x0,0x6A,0x4A,0x1,0x0,0x0,0x0,0x0,0x0,0x37,0x1,0x0,0x0,0x1A,0xCD}; //================================================================================================================ long B2L(char b4 , char b3 , char b2 , char b1 ) { //pc.printf(“B2L IN=%s,%x,%x,%x,%x,b4&0x80=%d\n\r”,sen,b4,b3,b2,b1,b4 &0x80); //pc.printf(“B2L IN=b4&0x80=%d\n\r”,b4 & 0x80);if ((b4 & 0x80) && 0x80){//最上位ビットたっていればマイナス su = -(256-(int)b1)+(255-(int)b2)*256+(255-(int)b3)*65536+(255-(int)b4)*256*256*256; //pc.printf(“B2L-:sen=%s,%d,%d,%d,%d,%d\n\r”,sen,b4,b3,b2,b1,su); } else { su=(int)b1+(int)b2*256+(int)b3* 65536+(int)b4*256*256*256; //pc.printf(“B2L+:sen=%s,%d,%d,%d,%d,%d,%d\n\r”,sen,b4,b3,b2,b1,su); }return su; } //=============After bData Set Recieved Calculation ===================================================== void ubx_dec(){ //for (i=0;i<172;i++){ //if (bData[0]==0xB5){ sNo=0;//Default 0 PVT startNo rNo=100; //pc.printf(“st:%d\n\r”,t.read_ms()); //} //}//pc.printf(“Long_sNo+33,32,31,30=%d,%d,%d,%d\n\r”,bData[sNo+33],bData[sNo+32],bData[sNo+31],bData[sNo+30]); //sen[0]=’L’;sen[1]=’o’; Long=B2L(bData[sNo+33],bData[sNo+32],bData[sNo+31],bData[sNo+30]);//上位ー>下位 //pc.printf(“Lati_sNo+37,36,35,34=%d,%d,%d,%d\n\r”,bData[sNo+37],bData[sNo+36],bData[sNo+35],bData[sNo+34]); //sen[0]=’L’;sen[1]=’a’; Lati=B2L(bData[sNo+37],bData[sNo+36],bData[sNo+35],bData[sNo+34]); //pc.printf(“Seah_sNo+45,44,43,42=%d,%d,%d,%d\n\r”,bData[sNo+45],bData[sNo+44],bData[sNo+43],bData[sNo+42]); //sen[0]=’S’;sen[1]=’h’; Seah=B2L(bData[sNo+45],bData[sNo+44],bData[sNo+43],bData[sNo+42]); //pc.printf(“Acc2d_sNo+49,48,47,46=%d,%d,%d,%d\n\r”,bData[sNo+49],bData[sNo+48],bData[sNo+47],bData[sNo+46]); //sen[0]=’A’;sen[1]=’c’; Acc2d=B2L(bData[sNo+49],bData[sNo+48],bData[sNo+47],bData[sNo+46]); //pc.printf(“relN_rNo+17,16,15,14=%d,%d,%d,%d\n\r”,bData[rNo+17],bData[rNo+16],bData[rNo+15],bData[rNo+14]); //sen[0]=’r’;sen[1]=’N’; relN=B2L(bData[rNo+17],bData[rNo+16],bData[rNo+15],bData[rNo+14]); //pc.printf(“relE_rNo+21,20,19,18=%d,%d,%d,%d\n\r”,bData[rNo+21],bData[rNo+20],bData[rNo+19],bData[rNo+18]); //sen[0]=’r’;sen[1]=’E’; relE=B2L(bData[rNo+21],bData[rNo+20],bData[rNo+19],bData[rNo+18]); //pc.printf(“Length_rNo+29,28,27,26=%d,%d,%d,%d\n\r”,bData[rNo+29],bData[rNo+28],bData[rNo+27],bData[rNo+26]); //sen[0]=’L’;sen[1]=’e’; Length=B2L(bData[rNo+29],bData[rNo+28],bData[rNo+27],bData[rNo+26]);//pc.printf(“Long=%d,Lati=%d,Seah=%d,Acc2d=%d\n\r”,Long,Lati,Seah,Acc2d);//pc.printf(“et:%d,\n\r”,t.read_ms());}//======================================================================================================= int main() { pc.baud(115200); F9P.baud(460800); BT.baud(115200); t.start(); while(1){ //Get Data from F9P at 460800bps if(F9P.getc()==0xb5){ bData[0]=0xb5; for(i=1;i<172;i++){//Buffering to bData[] bData[i]=F9P.getc(); } //===============PVT,RELPOSNED CalResult BT send to Android =========== ubx_dec();// data calc BT.printf(“%d,%d,%d,%d,%d,%d,%d,%d,%d\n”,j,Long,Lati,Seah,relN,relE,Length,Acc2d,j); //j++;} } } |
➁スマホ側プログラムに工夫した点
●Processing KETAIのBlueToothは、タイミングはばらばらと来るので、キリよくデータがはいってくる保証がないので読み込んだデータ配列をさらにバッファ配列で順番を正確に整理して、バッファ配列からモニター用パラメータを抽出するという
二重に手間をかけてます。
A:BT受信 関数
void onBluetoothDataEvent(String who, byte[] data) 内で処理してます。
ProcessingのKETAIライブラリーは、スレッドで、勝手にどんどんBT受信してデータを更新しますので、メインループで、適当に表示更新していれば、データが勝手に更新されていきます。簡単といえば簡単ですが、自分でデータを管理している感じがなくて、特に時間関係は、正確にでませんので、ばらばらの周期ではいってくる場合は
個々のデータにタイムスタンプが必要になります。
■はまった点
受信データbyte data[]をバッファしてchar cData[]に格納したあとsplitでカンマ区切りで
ばらすのですが、一旦cData[]をStringにまとめてsDataにします。splitでintに変換してrData[]に各パラメータを格納しているのですが、最初のデータrData[0]の値と最後のデータの値がrData[39]がおかしく変換されてしまっているので、ダミーデータを先頭と、最後に
いれるように送信側のマイコンプログラムを変更して、欲しいデータを得らえるようにし対策しました。データはきちんときているのですがsplitのやり方がまずいみたいです。
配列の作り方がまずいのかもしれません。
B:DRAWループでテーブルのTEXTを消して書いてを繰り返してます。
C:setupは、BTの初期化をしてます。
//required for BT enabling on startup
import android.content.Intent; char[] cData=new char[1000]; void onCreate(Bundle savedInstanceState) { } void onActivityResult(int requestCode, int resultCode, Intent data) { void setup() { //start listening for BT connections // data value
} //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ //Call back method to manage data received } } String createFileName() {
|
●以後
今回は、データの加工をマイコン側でやってしまったのが正解でした。スマホ側は、BlueTooth受信と表示に集中させることで
BlueTooth受信に余計な負荷をいれないので安定した受信ができてます。これに至るまで、3本違った方法でプログラムを作りましたが、UBX生データをスマホに全部送りつける方式は、BlueToothがこけるリスクがあって安定しませんでした。
データ量を少なくして、速度も115200bpsまで遅くしてBlueToothを安定稼働してます。
以後、SDカードでマイコン側で生UBXをログするのと、スマホモニターをグラフィックアニメーションにします。