プログラムを作りながら測定データの精査と検証をしてます。
IMUの40msec周期に対して、F9Pの120msec周期の同期が狂ってくる現象について、対策をうちました。
itow基準でF9P出力基準だったのが間違いで、真の時刻であるTimePulseを基準にしていきます。
ズレたままマイコン時間で、発生した順番で出力すると、時間軸がばらばらでログされるので後処理で大変に
なってしまうので、マイコン出力前に、整理して出力したほうがリアルタイム処理になるので、手間をかけることにしました。
※2023年３月２３日追記 結構大変な修正でした。拡張性を一切考えてないプログラムなので、修正しにくくて
時間がかかってしまいました。今度作るときは、ライブラリーにできるようなモジュール管理でやっていかないとシステムの発展性が見込めないと反省しております。
●同期がとりにくい原因
１；F9Pの出力タイミングが10-80msec範囲でばらついている。
２：リアルタイムは、timePulse信号でピン割り込みで基準としているが、1秒周期だが、F9Pの周期設定が120msecなので、１秒では同期できなくて、３秒で同期できる変則的な条件になってしまっている。
■RTKシステムが120msec周期になった経緯
STA23システムの周期が 120msecである点がややこしくしなった原因ですが、IMUが40msec周期で、RTKは、最速100mseなので、周期が合う最小が120msecだったというだけです。更にF9Hを使う場合の100msecは無理なので結局120msecで落ち着いたということです。
●IMUとRTKの同期の方法
GPSのタイムパルスは、１秒間隔で出てくるので、120msecと1000msecのタイミングが一致するのは 25回周期の
3000msec 3秒に一回のタイミングしかありません。その瞬間にIMUの測定周期を開始させて、次の3秒まで
40msec間隔でサンプリングします。
下図を見ながら、アルゴリズムを考えていきます。
●実測してみたらタイミング合わせ結構大変
40msec周期とitowを合わせるのですが、timepulse３秒おきの周期に合わせて、ログするのが結構大変です。
timepulseの割り込みタイミングとitow受信タイミングが40msec遅れるのが多いのですがたまに80msec遅れることがあります。製品規格上は10Hz保障なので、最悪100msec未満で遅延する可能性がありますので、プログラムも100msec遅延に耐えるように作っておかないといけません。
ログ出力の実時間と遅れたRTKデータを合わせるのが面倒なので、そのまま、ログしてデータ処理時につじつま合わるやり方だと、リアルタイムでの同期ができてないことになりますので、マイコン内で、合わせるほうがいいのですが、合わせるプログラム作り面倒なことになります。
IMUの測定タイミングは、timepulse受信時刻基準に40msec周期で、サンプリングしているので正確な時刻で記録されてますので、RTKデータの受信時刻だけ合わせるアルゴリズムが問題です。
●itowとtimePulseとBNOサンプリングデータ同期アルゴリズム備忘録
１日半考えて何とかまとめました。作りながらメモしていきます。
①ＢＮＯ配列データの一時保存
３秒間分のBNOデータ配列で保持しておく。
②BNOサンプリング開始タイミングをitow=2880msecに設定する
itowとtimePulseが一致するitow%3000==0時点では、すでに、timePulseのピン割り込み処理が
入ってしまっているので、itow%3000=2880時点で、配列のカウンタをゼロリセットして、timePulseとBNOのサンプリング開始を一致させる。こうすることで、BNOのデータがitowの３秒おきの周期に合致するので、itow120msec周期内にBNOの40msecデータが３個収納されて同期できる。
③ログ出力のタイミングばらつき対策として周期制限条件を追加
F9Pの出力がばらつくので、毎回ログタイミングが狂うため、早い場合は、BNOサンプリングが２回しか入らない現象も発生してしまうので、ログタイミングに前回のログ時刻から周期120msecに対して、115msec以上経過しないと次のログタイミングに入らないように条件を追加した。
④BNOのサンプリング周期39msecにした
BNOのサンプリング時間40msecだと、全体の周期120msecと重なってしまうので、39msecで
サンプリングするようにして、誤差が累積しないように、3秒に一回BNOのサンプリングタイミングをリセットします。
●プログラム
構造体を使うと、今までの関数群が使えなくなるので、配列を１個追加しただけで逃げました。
F9Pの遅延時間が電波受信状態によって、40-100msecまでずれるので、それに合わせてBNOのサンプリングを配列で保持して、全データがつじつま合った段階で１エポック出力するという方法です。本プログラムぐちょぐちょなので、参考にはならないと思います。
STA23_F9P-BNO_TMP_itow_230423_rev970Kaiten.ino
https://gist.github.com/dj1711572002/2124ed5f8e978b8450ab7fd59d91af95
■時間精度
本プログラムの時間精度は、TimePulseからの精度は8msec以内で収まってます。
元々、BNOのライブラリーが6-10msec食っているので、BNO055の計測の時間精度は
10msec程度で、一般の加速度センサよりは、悪いので高速な用途では使えないです。
Quarternion EULER処理だと40msecかかる仕様なので、そんなものです。
■割り込み関数 Timepulseピン割り込みが入ったら、tpflagをたてる。tp０時刻も記録
| //===============TimePulse Interrupt=============================== // —–BNO interrupt timepulse pin22 void myInterrupt(){ tp0=millis();//以後40msec周期でサンプリング tpflag=1;//tpflagがたったら、BNOの配列のゼロスタート } |
■BNOサンプリング開始タイミングがitow3000msec一個手前2880msecで設定する関数
| //=================BNO Timepulse Itow 同期==================== //Timepulseが入った瞬間にこの関数を実行して、itow0が3000msecに近ければ、bn番号ゼロを返す。 int bn_itow3() { int mod3000=itow0%3000; int rn; if(mod3000==2880) { rn=0; } else { rn=1; } return rn; } |
■BNOサンプリング部
| //============bnoread()配列に25エポック周期で収納 25エポックx3個=75個配列==================================================== //タイムパルスに同期させる tp0が3秒に一回itowと周期が合う //=============================================================== int sabn=(millis()-tp0)%40; if(tpflag==1 && bn_itow3()==0)// timepulseが入って、itowが2880だったらbn=0 { bn=0; bnoread(bn%75); //初回 Serial2.printf(“First-BNread:itow0=%d,bn_itow3=%d,tp0=%d:bn=%d,tpflag=%d,tp=%d,tp_1=%d\n\r”,itow0,bn_itow3(),tp0,bn,tpflag,tp,tp_1);tpflag=0; bn=1; bninitflag=1; } else if(bninitflag==1) { tp=millis(); //Serial2.printf(“else:tpflag=%d,tp,tp_1=%d,%d,%d\n\r”,tpflag,tp,tp_1,tp-tp_1); if(tp-tp_1>39 ) { int BNOinn=millis(); bnoread(bn%75); tp_1=tp; Serial2.printf(“BNread:bNo=%d,bn=%d,BNOInn,%d->Out,%d,tp=%d,tp0=%d,itow0=%d,tp3flag=%d,tpflag=%d\n\r”,bNo(),bn,BNOinn,millis(),tp,tp0,itow0,tp3flag,tpflag); tpflag=0; bn++; } } |
■F9Pログ出力タイミングばらつき対策
| //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ //@@@@@@@@@@@@@@@@@@@@エポック処理へ入る@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@epochinT=millis(); if(umuB*flagB+umuR*flagR+umuM*flagM>0 && count_upB>count_upB_1 && (epochinT-epochinT_1) >115 ){//１デバイスでもイン可能 2023/4/21 epochinTで時間規制 115msec以上 epochinT_1=epochinT; count_upB_1=count_upB; / |
