【L-RTK】スマホRTKモニターTable表示まで出来た<BlueTooth受信に負荷をかけない>

スマホで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様へ感謝
http://prof-takaya.blogspot.com/2013/02/connecting-android-and-arduino-by.html

①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;
import android.os.Bundle;
import ketai.sensors.*;
import android.location.Location.*;
import ketai.net.bluetooth.*;
import ketai.ui.*;
import ketai.net.*;
PrintWriter output;
//***************************BT定義***********************************
int i=0;
int j=0;
int k=0;
int n=0;
int n_1=1;
int m=0;
int startNo=0;
int endNo=0;
int datasu=100;
byte b;
int np=0;//受信data連番 リングバッファ絶対値カウンタsampN=np%1500となる
int sampN=0;//累計サンプリング連番0-1499
//===================================PVT RELPOSNED data=========================
int kosu;
float Long;
float Lati;
float Seah;
float relN;
float relE;
float Length;
float Acc2d;

char[] cData=new char[1000];
int[] startN=new int[1500];//
int[] endN=new int[1500];//
PFont fontMy;
boolean bReleased = true; //no permament sending when finger is tap
KetaiBluetooth bt;
boolean isConfiguring = true;
String info = “”;
char[] pinfo=new char[100];
String p1=””;
//String ndata=””;
KetaiList klist;
ArrayList devicesDiscovered = new ArrayList();
//********************************************************************
//********************************************************************
// The following code is required to enable bluetooth at startup.
//********************************************************************

void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
bt = new KetaiBluetooth(this);

}

void onActivityResult(int requestCode, int resultCode, Intent data) {
bt.onActivityResult(requestCode, resultCode, data);
}
//**********************************************************************
//**********************************************************************

void setup() {
String fileName = createFileName();
output=createWriter(fileName);
size(1340, 700);
frameRate(10);
// orientation(PORTRAIT);
orientation(LANDSCAPE);
background(0);

//start listening for BT connections
bt.start();
//at app start select device…
isConfiguring = true;
//font size
fontMy = createFont(“SansSerif”, 40);
textFont(fontMy);
}
//@@@@@@@@@@@@@@@@@@@@@@ DRAW @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
void draw() {
//at app start select device
if (isConfiguring)
{
ArrayList names;
background(78, 93, 75);
klist = new KetaiList(this, bt.getPairedDeviceNames());
isConfiguring = false;
}
background(0,50,0);
//Table item
textSize(36);
text(“Longtitude(deg)”,200,50,300,40);
text(“Latitude(deg)”,750,50,300,40);
text(“relN(cm)”,230,250,300,40);
text(“relE(cm)”,480,250,300,40);
text(“Length(cm)”,780,250,300,40);
text(“SeaHeight(m)”,210,470,300,40);
text(“Acc2D(mm)”,780,470,450,40);

// data value
textSize(64);
text(str(Long),150,120,550,200);
text(str(Lati),700,120,550,200);
textSize(96);
text(str(relN),150,320,300,200);
text(str(relE),450,320,300,200);
text(str(Length),750,320,300,200);
text(str(Seah),150,520,300,200);
text(str(Acc2d),780,520,300,200);

 

}

//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
void onKetaiListSelection(KetaiList klist) {
String selection = klist.getSelection();
bt.connectToDeviceByName(selection);
//dispose of list for now
klist = null;
}

//Call back method to manage data received
void onBluetoothDataEvent(String who, byte[] data) {
//println(isConfiguring);
if (isConfiguring )// 1st rimte isConfig==True after 2nd isCofig=False
return;
else
for (int i = 0; i <data.length; i++) {
j++;
cData[j]=char(data[i]);
println(“data[“+i+”]=”+char(data[i])+”,”+hex(data[i]));
if ((char)data[i]==0x0A && j>36 ){
String sData = new String(cData);
println(“sData=”+sData);
int [] rData= int(split(sData,’,’));
kosu=rData[0];
Long=rData[1];
Lati=rData[2];
Seah=rData[3]/1000;
relN=rData[4];
relE=rData[5];
Length=rData[6];
Acc2d=rData[7];
println(“Acc2d=”+rData[7]);
j=0;
}

}

}

String createFileName() {
String fileName=”//sdcard/DCIM/”+ nf(year(), 2) + nf(month(), 2) + nf(day(), 2) +”-“+ nf(hour(), 2) + nf(minute(), 2) + nf(second(), 2);
fileName += “.ubx”;
return fileName;
}

 

●以後
 今回は、データの加工をマイコン側でやってしまったのが正解でした。スマホ側は、BlueTooth受信と表示に集中させることで
 BlueTooth受信に余計な負荷をいれないので安定した受信ができてます。これに至るまで、3本違った方法でプログラムを作りましたが、UBX生データをスマホに全部送りつける方式は、BlueToothがこけるリスクがあって安定しませんでした。
 データ量を少なくして、速度も115200bpsまで遅くしてBlueToothを安定稼働してます。
 以後、SDカードでマイコン側で生UBXをログするのと、スマホモニターをグラフィックアニメーションにします。

コメントを残す

メールアドレスが公開されることはありません。 が付いている欄は必須項目です