【RTK22】RTKモニターPgm完成<Teensy-M5Atom無線システム>

2022年仕様のRTKモニタープログラム完成しました。4月7日に信州SKYパークでフィールド測定1回目をやって、発生した不具合の対策で、3週間まるまるプログラムに時間を取られました。
Teensy rev10279,M5Core2 rev04 M5Atom rev02の3本作りました。
5月連休中に、2回目のフィールド試験を実施します。
2021年版は、M5AtomとM5Core2でしたが、スキー場で時々暴走したりデータ抜け頻度が高くて
測定データの歩留まりが悪かった課題があったので、2022年は、プログラムの信頼性を向上させてます。
2022年5月末の最新システムでIMU2個追加しました、こちらです。ノウハウの塊となってます

※2022年5月27日追記「M5AtomからM5StickCへ変更」
本記事ではESP-Now送信モジュールとしてM5Atomを使ってましたが、電源オンからESP-NOW自動接続ができない課題が浮上してきて、M5AtomをやめてM5StickCに変更しました。電池をもっているとESP=NOW自動接続がスムーズで破綻がありませんが、M5AtomだとRESETボタンを毎回押さないとESP-NOW接続できませんでした。

※2022年5月10日追記 「M5Core2からM5StickCへ変更しました。」
ブザーぶつけてで衝撃壊れてしまったので、Core2はやめてM5StickCのブザーHAT付に変更しました。
メリット1:ブザー面積が大きくて聞きやすい
メリット2:M5StickCのLCDは、直射日光でもよく見える。(段差が効果大)
メリット3:腕時計バンド付きなので、取扱が便利
デメリット1:液晶小さい180×80(Core2 360×240)しかし、Core2で12行一括表示しても見にくかったので
M5StickCで3ページx4行と分割表示してボタンでトグルさせる方式にしたので、便利に使える。
デメリット2:電池がもたないので、モバイルバッテリーから充電しながら使う。

 


●特徴
1)キャリブレーションモード
基線長解析を取り入れたことで、昨年までcm単位だったが、2022年からは0.1mm単位に分解能向上した。
測定開始位置を基準点として静止する。電源してFIXした瞬間から約1分間静止状態で、基線長解析を行って、平均値と標準偏差を得る。標準偏差の3倍である3シグマ値を測定値の精度ばらつき範囲として扱う。キャリブレーションで5cmを超えるばらつきが発生した場合は、測定を開始しないで、原因追及と対策を行う。

2)TeeensyUSBHOSTによるデバイス接続の汎用性、フレキシビリティ
USB HOSTとしてUSBデバイスからのデータ受信、デバイスコントロール、電源供給を1本のUSBケーブルで済ませられる

 

3)Teensy4.1 Cortex-M7による高速処理(複数ファイルログ、複数センサ、WiFi SPPへの出力)
5MbpsでSDカードに書き込めるため1msec以内でRTKデータを保存、余り時間で監視データも別ファイルとして保存、
来年以降のGNSSボードの高速化(20-50Hz化)に対応できる高速性確保。

●モニターしている内容(本体からESP-NOW無線方式でM5Core2送信して液晶表示)
手のひらに載る小型サイズなのでスマホより扱いが便利ですが、屋外視認性が悪いので、カメラ用の液晶シェードを取り付けます、さらに測定中は、ブザー音で状態を知らせる機能も追加します。

 


全データをモニターのM5Core2に流すと、計算と表示処理重くて暴走するので、必要なデータを厳選しました。

監視機能 Pgm変数名 内容
データの時刻 itow Time Of Week 1週間をmsec単位で表現
補足衛星数 numSV baseがRTKで使っている衛星数
位置精度低下率 pDop 衛星の仰角精度をいれた精度指標 2以下ならOK
RTK状態フラグ flags 1で受信開始、67でfloat 131でFIXです。
水平精度 hacc 2次元(NとE)RTK計算最小二乗法での誤差、1エポック(1周期のデータ)内の誤差であって、測定データ全体の誤差ではない、大体はhaccの3倍くらいがデータ全体の統計ばらつきになる傾向がある。(cm)
Fix時間 fixtime RTK開始からFIXするまでの時間(秒)
Fix率 fixpercent Fix後の全データでFIXの比率100%ー99%が普通(%)
データ欠落数 miscount usbhost受信ルーチンでタイミングが狂ってデータが抜けてしまう個数(個)0.3%以下を許容
エポック数 epNo 全測定データ数(データ1回分をEpochと呼ぶ)
基準位置の基線長平均値 rLaveN キャリブレーションモードで測定した
NTRIP基準局との直線距離の平均値(Km)
0.1mm単位で測定してあるがKmで表示
基線長3シグマ rLstd*3 上記基線長の平均を測定したときの標準偏差ぼ3倍
(cm)  以下全て0.01cm単位で測定
MovingBase Head角 mHead base基準でroverの方角角度右回り(deg)
相対位置NED座標 rrH,rrE,rrD 基準位置をゼロとした相対座標の現在位置(cm)
NED座標基線3シグマ tNstd*3,rEstd*3,rDstd*3 キャリブレーションモードで基線解析で得た標準偏差の3倍(cm)
基準位置から現在距離 rrL rrN,rrE,rrDから距離計算(cm)

今回は、1CHですが、スキーの場合左右両足なので2CHで表示も2CH分増やします。

●システムの概要備忘録
Teensyには専用WiFiモジュールがないのでM5Atomを汎用SPPモジュールとしてシリアル接続しました。
Teensyのぷろぐらむからバイト配列のデータをSerial3.write()すれば送ったデータをそのままESP-NOWでCORE2へ無線転送
してくれます。

 

●解決した課題
①roverデータが欠落する
現象:PgmでItowを見ながら抜けをチェックしているのですが、時々抜けが発生して、100msec周期が200msec周期になってしまう場合が頻度として数%レベルで常時発生していた。
原因:ZED-F9Hチップの仕様上、10Hzは、無理があったようです。8Hzにするとデータ歯抜けがなくなりました。仕様書では、10Hz上限と書いてあるのですが、チャンピオン仕様だったみたいで、安定仕様は5Hzくらいが実力みたいです。

②衛星電波状態が悪い時にデータ送信順が狂う。
現象:通常調子が良い時(FIXが1分以内)の場合は、発生しないのですが、FIXが5分とか電波状態が悪い場合
baseのF9Pで、通常は、PVT->RELPOSNEDの順番でメッセージが入ってくるのですが、順番が逆になる場合がある
原因:F9Pの計算処理が忙しい時に、データ送信時間をくって、タイミングが狂うみたいです。一般的なチップなら破綻するはずの場合でもデータを守るために、タイミングを遅らせてでも送信するアルゴリズムになっているようです。受信プログラムを作る側だと、ルールが違ってしまうので、大変面倒なことになります。
対策:データ順が狂ったエポックをカウントして、監視することにしました。全体のエポック数の0.3%以下に収まっている場合は
その測定データはOKと判断することにしました。電波状況が良い場合は発生しないので、実際のフィールド測定時は、電波状況が悪い時は測定しないので、発生頻度は、非常に少ないという判断で、受信アルゴリズムは変更しないことにしました。

③USB HOST接続時に、ポート割付順が狂う
現象:ポート0にbase、ポート1にroverだったのが、ある日突然ポート0がrover、ポート1がbaseに変わってしまう。
原因:USBポートは接続順にポート番号が付与されるので、違ってしまう可能性がある
対策:ポート0とポート1でどちらに接続されてもいいような受信プログラムにしておく。
=>メッセージの最初のヘッダ4文字で、baseかroverかを判別する
=>データ通信前に、F9PのユニークIDを読みに行って、USBポート番号とユニークIDを一致させてからデータ
受信ブロックに入る(面倒なので、やめました)
=>データ数で判別しようとしましたが、データ数を確定するタイミング次第でデータ数が変わるので、危ないのでやめました。

④Teensyは、if文で判別変数が化ける場合がある。
現象:int flagと整数型定義でif (flag) { }とiF文の判別に使った場合、flagがとんでもない大きな数値になってしまうことが何回か発生したため、
原因:多分、ライブラリーにマルチタスクを使っているものを使用していると変数が化けることがある。
USBHOSTライブラリーが怪しい。
対策:uint8_tf flagと符号なしバイト型に定義しなおしたら安定して、if文がうごくようになった。

⑤桁数の大きい符号付倍精度データをバイト配列変換で一苦労
今までデータは符号なし整数しか扱ってこなかったのですが、RTKの高精度データだと、桁数が上が12桁、小数点以下5桁までの巨大な数値となります。
ESP-NOWでlongをバイトにばらして、ESP-NOWで無線送信して、受信後にバイトをlongに戻す、M5Core2に戻す関数を作りました。
byTodbl(uint8_t b0,uint8_t b1,uint8_t b2,uint8_t b3)
uint32_tをバイトごとにシフトしてORをとおて合算してからlongに変換

●プログラムコード備忘録まだバグがあるので、備忘録だけです。
コードの解説は、別記事で解説します。
①Teensy 4.1用Pgm  STA22_8Hz_USBHOST_STABLE_M5cali_rev10279.ino
https://gist.github.com/dj1711572002/1287d1bf8204e1ec8ee914f1824bc7c3

電源オンでUSBHOST接続してからbase(F9P9)とrover(F9H)に電源オンしてRTK開始します。
開始から20秒後にSDカードログ開始されてubxファイルでSDカード保存されます。
FIXしたらキャリブレーションモードに入って500データサンプリングして平均と標準偏差を統計計算して
基線長をえて、その測定で発生するばらつき精度を3シグマで確定します。3シグマの目安は、スキーの場合はN,E,Lが5cm以内であることDは10cm以内であること。
主なSUB
USBHOST準備
NAV-PVT変換
NAV-RELPOSNED変換

②M5Atom用 SPPプログラム    M5Atom-STA22_Teensy-Serial_ESP-NOW_rev02.ino

TeensyぼUART3のTXピンからシリアル受信してバッファのデータをそのまESP-NOW送信します。
ESP-NOW設定で送信相手のMACアドレスを書き込んでおくとその相手だけに送信します。MACアドレスをFFでうめるとESP-NOW端末すべてに一斉送信します。データバイト数は250以下なら自由です。
https://gist.github.com/dj1711572002/d5cc9b72833ac7480116a6d94a35a4d9

 

③M5Core2用 モニタープログラム     Core2_STA22_ESP-NOW_for-rev10279_rev04.ino

Teensy-M5Atomから送られてくるバイナリーデータセットをLCDに表示する。
LCDの反応速度が遅いため1秒に一回に間引きしてある。
さらに、ちらつきをおさえるために、spriteを使って描画している。
受信データ内にカウンタ(epNo)を仕込んでそれで間引きしている。
後日、ブザー機能を追加する。
https://gist.github.com/dj1711572002/2495bd34bdf67a0510a11a9b38d30127

 

●これからの想定課題

1:USBHUBを超小型にして、ケーブルも細くなるので、電流供給能力とノイズ耐性が課題になるかもしれない
2:フィールドでのコネクタ類の接触不良が発生しないか。
3:アンテナ線揺動による受信不良が発生しないか。

連休中にフィールドで実験をして課題を抽出します。

 

コメントを残す

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