２０２２年最新システム動作確認完了しました。アイデア出しながらで２か月半くらいかかりました。
目的は、STA22の新機能の開発用です（高精度、高信頼性、IMU補間、板のたわみセンシング）
※２０２３年３月追記 信州ＭＡＫＥＲＳでは、開発を日々進めてますので、記事は過去のものです。
最新の情報は、カテゴリー RTKとSTAで最新記事をご覧ください。
F9Pを使ったモーション測定は、２０２３年からIMUとの相互補完システムがメインになってます。
各デバイスの得意、不得意を組み合わせて、目的に合わせて使いこなすコトづくりとモノづくり思想が基本です。
※２０２３年３月末追記 ２０２３年シーズンで測定した大量のデータ処理プログラムのコツ
【STA23】RTK-MovingBase４CH分のデータ処理してグラフ化＜コツ備忘録＞
●2022年５月末バージョンと今までのシステムからの改良点
メインＣＰＵ：teensy4.1でUSBHost接続と高速ＳＤログ、多センサ同時ログ
＝＞ArduinoそのままでSD書き込み、計算速度、コンパイルが超速で、ESP32,M5Stackへは、戻れなくなりました。
WiFi,BlueToothモジュール：M5StickCとRN42モジュールをuartでつけてます
RTKモニター：腕時計型M5StickCブザー付きとAndroidスマホ
SkiTurnアナライザー：Win10タブレットで、SDカードを外さないでMTPモードで読み込んでGraphicデータ処理
基板セット：４月まではF9P-F9H基板セットでしたが、５月からF9P-F9Pに変更してF9H不要になりました
＝＞板のたわみをMovingBaseのLengthで測定する方針変更で、F9HだとLengthがで得られないため。
＝＞i2cにIMU２個ぶら下がてRTKデータとクロックと同期させてあります。
※6月４日追記 「IMUは、RTK補間用には使えそうもないので、足と上体の角度測定用に使います。」
IMUをいじってみて定番のKALMANフィルター、IIRフィルタをいじってみて、さらに、DeadReckoning技術の論文を調査した結果、IMUだけでは、RTKの補間できるほど精度がでないと判断しました。そこで、IMUに代わる技術として、GPSドップラ速度ベクトルを利用することにしました。高速測位できるGPSチップM9Nを試してみます。詳細記事はこちらです。
※２０２２年１１月９日追記 シリアル受信をSDカードログする場合についてのTIPS記事
SDカードログが曲者で、４M変動に弱いため、毎回プログラムを作る度にハマります。
そこで、TIPSを備忘録して、毎回ハマらないようにしたいです。下記記事も参考にお使いください。
■2021システムからの改良点違い
違い０：BaseとRoverを分離させて、Baseの基線長を常時管理できるようにしました。
＝＞SimpleRTK2B HeadingKitでは、MovingBaseをしながら基線長管理ができないです。
違い１：位置分解能が0.1mm単位で全て管理してます。（２０２１年はｃｍ単位）
違い２：緯度経度を一切使わないで、基準局からの基線長座標で現在位置を表現してます。
違い３：測定開始前に、統計モードで３シグマばらつき測定してから、計測開始するようになってます。
違い４：ＩＭＵを２個つけて、ＲＴＫ時刻と同期させて、運動軌跡の補間機能を開発します。
＝＞RTKが８－１０HzでIMUが４０－５０Hzサンプリングして同期ログします。
違い４：MovingBaseのLengthを管理して、スキー板のたわみ変形との相関を計測する機能をつけました。
違い５：SDログファイルは４個同時ログ（Base用UBX、Rover用UBX,モニターデータTXT、IMUデータTXT）
■ノウハウの備忘録(細かいのでわすれてしまいそう）
昨年までのＭ５Ａｔｏｍの受信プログラムでは、数％データ落ちがあったので、２０２２年では３シグマ以下のデータ落ちまで、管理してます。３シグマを達成するのは、すごく手間と時間がかかります。覚えているうちに備忘録しておきます。
F9Pは、シリアルデータ送信タイミングが衛星電波受信後５０～70msecの範囲で遅延してばらつきます。ですので受信プログラム次第で、データ落ちしやすくなります。特に、衛星電波状態が悪いとき、メッセージ数が多いときなど計算能力を目いっぱい使っているときは、タイミングの乱れが多く発生して、データ落ちしやすくなります。良好で安定した状態なら、あまり気にしなくてもいいですが、数多くの測定をしていくと、プログラムの出来不出来でデータ落ちエラー率が違ってきます。
２０２２・５・３１のプログラムGISTに置いておきます。
https://gist.github.com/dj1711572002/5bf724f4570cec5a9c84ced0be4991c0
STA22_8Hz_USBHOST_Uart4_MPU6500x2_Kalman_Stable2_rev10391_6.ino
IMUは、MPU65002個使って、RTKデータと同期させてあります。BaseアンテナとIMUを自在に運動させる治具を作って、６月からRTKのIMU補間機能を開発していきます。
※F9Pのような出力タイミングがばらつく変なチップを扱うときは、シリアル能力に余裕があるＣＰＵのほうが、プログラム作りが楽ですので、できるならTeensy4.1をお勧めします。ESP32系、M5系はシリアルが貧弱なので、
一旦Ｆ９Ｐシステムを作ってもそれ以上の発展性は、望めません。私は、２０２１年に苦労してM5StackとM5Atomでシステム作りましたが、２０２２年の新システムで性能を上げるには、無理があったので、Teensyに乗り換えた経緯があります。最初からTeensyを使っていれば、１－２年くらい得できたはずです。
下記にTeensyのシリアルの凄さがわかる記事を書いてあります。
【RTK22】超高速マイコンCortex-M7搭載Teensy4.1いじる その5＜HardwareSerial 超便利＞
①シリアル受信部のF9P対応コツ
最新仕様は、baseは、USB HOSTシリアル、roverは、普通のシリアル接続です。USB HOSTの場合データ落ちは非常に少ないのでSDログする分には問題ないのですが、２ポートだとbase,rover間の同期がずれることが多く、同期がずれてしまうとリアルタイムとの同期がとりにくくなるので、ずれが多いroverをシリアル通信に変更して、シリアル専用バッファを172バイトで作って、一括受信できるようにシリアル受信を実現してあります。
F9Pで、高信頼性のデータログをしながら他の仕事をする場合は、ESP32,M5Stack等速度が遅く、シリアル能力が低いので無理なことがわかってきました。teensy4.1一旦使ってしまうとM5Stack系は、使えません。
■シリアル受信の場合（ESP32系でシリアルバッファ固定でマルチタスク受信している場合）
細切れで読むと収拾がつかなくなります。F9Pは、電波環境によって送信時間が刻々変わりますので、一定ペースでデータを送信してくれんません、調子の良い時なら、どんな受信プログラムでもＯＫですが調子が悪いときは、へたなプログラムだとこけます。ですので、一気に１７２バイトよむまでずっと待つことが重要です。
更に、いけないのは、受信タイミングによってヘッダーから入ってこない場合があります。その場合、１７２バイト受信してからＢ５，６２，０１，０７の４バイトのヘッダを探して、ソートしないと、ＰＶＴメッセージが解読できません。アルゴリズムが結構大変になります。
１)loopでのシリアル受信部
バッファに１個以上データ入ったら、すかさず、while(j<172)に入って、１７２バイト受信できるまで待ち続けます。USB HOSTの場合は、バッファが１７２バイトになるまで待ってから読みにいくのですが、普通のマイコンのシリアルだとバッファが小さくて、１７２バイトためこめないので、１バイトでもはいってきたら、すかさず受信whileに入ってしまわないと、データ欠落してしまいます。バッファが小さい分プログラムに負担が大きく他の処理が難しくなります。ESP32系の場合は、受信してバイナリデータをSDに書き込んで、無線でデータ送信するのがギリギリの処理です。他の機能として、精度向上、統計計算などいれる余裕がありません。
| //UART4で受信
if(Serial4.available()){ //１個以上データが入ったら受信開始 ｝ |
●Teensyの場合は、ハードウェアシリアル機能は、凄いです
Teensyの場合は、UARTが８個あって、すべてハードウェアシリアルで、ハードで自動的にバッファにため込んでくれますので、CPUの負担ゼロでシリアル受信できます。さらに、ハードウェアのバッファサイズが自由に指定できるので、シリアル通信の受信ループは、待ち無しで読み込めますので、プログラムの浪費時間は1msec以下（172byteの場合)一気に読み込みます。マルチタスク受信だとCPUが結局遅くなりますが、ハードウェアシリアルだと一切CPUが遅くなりませんので、シリアル８個全部同時に使ってもプログラムの遅延がでません。
Teensyでハードウェアシリアルのバッファサイズを指定するのは、setup()内で行います。32byteの倍数で指定します。
１００バイトて指定したら、とんでもないことになりましたので、バッファサイズは３２の倍数にすることが安全です。
解説ページは、https://www.pjrc.com/teensy/td_uart.html
命令文は、これだけです。bufferは、クラス直下でグローバル変数として static uint8_t buffer[size]；であらかじめ宣言しておきます。
//===============Teensyの場合のシリアル受信Pgmサンプル==============
void setup(){
—————–
Serial1.begin(115200);
int size=192;
Serial1.addMemoryForRead(buffer, size)；//これが凄く便利です。
—————–
}// setup end
void loop(){
—————
if(Serial1.available（）＞171){ //size-1で172byte以上になったら読み込む
i=0;
while(i<172){
buffer[i]=Serial1.read();
i++;
}
}//このif文は、1msec以内で済んでしまいます。
}// loop end
//======================================================
■USB HOSTシリアル２CHの場合
シリアル通信ほど苦しくないですが、速度とバッファに余裕がありすぎて、Ｆ９Ｐの微妙なタイミングばらつきを拾って、ＣＨ間の同期がずれます。データ落ちは３シグマ以下ですが、ＣＨ間同期しないので別々のファイルで保存してます。F9P自体が５０－７０msec遅延したり、周期ばらばらでデータ送信してくるので受信データ落ちが発生することがあります。データ落ちを全エポック数の０．３％以下に抑えるために、受信部のアルゴリズムの最適化をはかるのにずいぶん試行錯誤しました。
コツは、「規定バイト数がバッファにたまるまで読み始めないことです。」
＝＞USBHOSTの場合は、バッファで大きいのでたまるまで待つことができますが、シリアルの場合は、上記の読みながら待つ方式が良いです。USBHOSTとシリアルの組み合わせの場合、USBHOSTはバッファ余裕があるので、172Byteたまるまで読み始めないif文にして、シリアルは、余裕がないので、1Byteでもバッファにたまったら即読み込むif文にしてwhileループで172Byteたまるまで回します。これで走らせると、常にシリアル受信が最初に終わって、次にUＳＢＨＯＳＴ受信が終わるという順になりました。ＵＳＢＨＯＳＴ２ポートの場合は、useriab0,userialb111の順になるので、if文の設定の仕方で読み込み方は大きく変化します。
1MBPSくらいで読むので、数μ秒で読み終わってしまうので、遅くてバラツクＦ９Ｐの出力をじっと待つほうがいいだろうということです。待たずにコマギレで読むと、後のプログラム処理が大変になります。
受信部のif文例です。
| //==========バッファが規定バイト数（１７２Byte）になるまで待ってから、一挙によむ========
if(userialb0.available()>171 && userialb1.available()>171 ) { }// USB ２Ports Recieve end |
②BaseがＵＳＢ０になるとは限らない。
当初は、データ受信前にボードＩＤをリクエストして、ボードＩＤからUSB0とUSB1のbaseかroverかの区別をしていたのですが、高速受信でパラメータ分別をしないといけないので、１回受信してから、baseとroverをデータ内容で区別してＲＴＫデータに代入する方法にしました。
区別の方法は、BaseのRELPOSNEDのLengthは、数ｋｍですが、RoverのRELPOSNEDのLengthは、数十ｃｍから数ｍなので、区別がつきます。
＝＞USB HOSTをbase、roverをシリアル通信に変更したので、USB０がbaseに常になるので、本機能は不要となりました。
③iTow基準でエポック判別
受信が完了したのを認識して次の処理に回す判断基準は、現在のitowと１個前のitow_1との差が
itow-itow_1>123 としました。１２３とは、規定周期125msec(8Hz)に対してばらつきを含めてます。
さらに、１２３より大きければいくつでもいいいので、Ｆ９Ｐがつまずいて、データが数エポック飛んでしまってもデータ処理してログできるようにしてあります。
④データ抜けのチェック
受信部の直後にPVTのitow計算して、一個前のitow_1と引き算 itow-itow_1>127msec以上離れていたら抜け発生としてカウントアップしていきます。このミスカウント数が全ログエポック数の０．３％以内におさまっていれば、計測データは有効と判断します。
⑤BaseとRoverで別ファイルにしてログ
BaseとRoverでそれぞれ独立して抜けが発生すると、itowが狂ってくるので、同じ行にログすると、ｐｃで処理するときに面倒になるので、別々のファイルにすることで、ｐｃで読み込むときにbaeeとroverのitowを同期しやすくしました。最悪１－２秒ずれますが、それぞれの抜けが０．３％程度なので、ほとんどのエポックで同期できます。
⑥IMU取得時刻とのＧＰＳ時刻の同期
Ｆ９Ｐのデータ送信タイミングが実時間より50-70msec遅れるので、ＲＴＫデータを受信したタイミングは、役に立ちませんので、別途baseのtimePulseピンから信号をTeensy4.1のdigitalピン３８にＩＮＰＵＴしました。
測定開始時にＲＴＫデータのitow値tpitow0とTimePulse立ち上がりエッジの時刻tptime0=millis()を決める処理を
します。ＣＰＵは、ＲＴＫデータでなく、自分のクロックで衛星受信タイミングをカウントしていきます。＝＞Teensyのクロックが14ppmくらいなので、計測時間３０分で25msecずれる程度なのでIMUの１周期程度で結果に影響はでないと判断しました。
そのカウント値でＩＭＵの計測時刻を決めます。IMUデータには、ヘッダーでimuitowといitow時刻をつけてログすることで、ＲＴＫ位置は125msec毎に正確に得られてますので、IＭＵデータ群を積分処理しながら位置補間に使えるということになります。125msecごとにIMU積分リセットして、バイアスノイズの影響を小さく抑える効果が期待できます。この実験をするためのシステムです。うまくいけば、遅いＦ９Ｐでも４０－５０Hz相当の位置補間で高速測位で使えることの証明できるかもしれません、そうすれば、歩行、スキー競技、高速な運動分野でもＦ９Ｐが使えるようになる可能性があります。
⑦ＩＭＵ ＬＳＭ９ＤＳ１の使い方＝＞LSM9DS1からMPU6500に変更しました。２個使いし易さと供給性理由です。
９軸ですが、磁気は使ってないので、６軸です。Arduinoi用にSpaarkfunのライブラリを使てます。
https://learn.sparkfun.com/tutorials/lsm9ds1-breakout-hookup-guide?_ga=1.167528382.1408024151.1440392747
これに、KalmanFilterを加えて処理してます。動いている場合の角度変化は、生AccデータのATAN計算した角度より角速度GyroデータとAccデータを使っているKalmanFilterのほうが、よい値がでます。しかし、回転がない併進運動では、KalmanFilterも役に立ちませんので、ＲＴＫと比較してＡｃｃの積分補正をする以外にないと考えてます。
KalmaFilterは、Arduinoの標準的なもので、KJ Electronicsさんのカルマンフィルタライブリーを使ってます。サンプルプログラムをコピペでできます。
https://www.arduinolibraries.info/libraries/kalman-filter-library
LSM9DS1はSwitch Scienceさんのボードなのですが、いかんせん、I2C Idが１個しか使えない仕様になってました。
https://www.switch-science.com/catalog/2405/
そこで、wire2を使ってもう１個のLSM9DS1を駆動しようとしてますが、Sparkfunのライブラリの使い方がわかりません、どうしてもだめなら、自分用の専用のＬＳＭ９ＤＳ１ドライバー作成します。
⑧ＩＭＵ周期確保とＳＤ保存高速化
今までは、１データごとにOpen－Closeしていたのですが、OpenとCloseの時間間隔を8msec遅延させないと暴走してしまうので、結局１行のデータセットをＳＤ書き込みするのに、１０数msecかかってました。なぜ毎回open-closeするかというと、スキー滑走中に万一電源が落ちたら、全データがパーになってしまうリスクがあるからです、しかし、ＩＭＵで20msecサンプリングするためには、ＳＤで１０数msec無駄してられませんのんで、１分間に一回open-Closeをいれることにしました。こうすると、BaseのＵＢＸ,,RovrのＵＢＸの２ファイルで2msec、統計データのＴＸＴファイルで2msec,IMUデータのＴＸＴファイルで3msecで、ＳＤログで7msec程度、ＵＳＢ受信で4ー１０msec,無線送信で1msec程度でＩＭＵ周期20-25msecは確保できまそしたがばらつくので、ヘッダにimuitowのタイムスタンプをつけてあります。
●プログラムコード https://gist.github.com/dj1711572002/dbd4b903f55ad8fe32f0b2eacf1498ab
●以後
夏場で気温があがるとteensyが熱暴走するので、夏場の実験ではヒートシンクを載せます。
屋外での実測でいろいろ不具合がでると予想されますので、対策をうちながらすすめていきます。
