【STA25】リアルタイムでRTK捕捉衛星管理　その１＜NAV-SAT受信Pgm＞

STA25では、ここ４年間手つかずで合った、衛星捕捉データを取得して、RTKの信頼性管理機能を
ログプログラムに追加しました。
衛星捕捉情報は、データ量が大きいのと、リアルタイムでころころデータ数が変わるので、
受信プログラムとデータ処理にテクニックが必要なので、この４年間できなかったということです。
２０２５年になってようやく、F9P受信プログラムのレベルが向上したので、正月からプログラムを一から
作りはじめてます。

●やったこと
①F9P Baseからの送信メッセージにNAV-SATを追加しました。
今までは、NAV-PVT(100byte)とNAV-RELPOSNED(72byte)の172バイトしかなかったのですが、
NAV-SATは、400～1000バイトくらいに膨れ上がるし、毎回データ長が変わるので、扱いが難しいです。
Ucenterでじっくり観察しながら受信処理の方法を考えました。

②NAV-SATの内部
　F9P　InterfaceDescription 　HPG1.32の１９ページに表があります。
https://content.u-blox.com/sites/default/files/documents/u-blox-F9-HPG-1.32_InterfaceDescription_UBX-22008968.pdf
表をみながら、Ucenteの実際のデータと対比して、データの順番を確認しました。
NAV-SATの場合RTK　Fixが必要ないので、室内窓際に置いたアンテナで十分デバッグできて便利です。
ucenterでデータをログして、再生しながら衛星のグラフの値とバイナリ値が合っているか確認します。
NAV-SATは、ヘッダ部が１４バイトあります。次にペイロード部がありますが、衛星数によって
増減します。　ここで、注意することは、B5,62,01,35の次に２バイトのメッセージ長があるのですが
このメッセージ長の定義が8+n*12となってますが、実際のバイト数だと14+n*12なので、プログラム時は
14+n*12でデータ位置を計算してパラメータを取得します。
ペイロードの先頭は、GNSSIDで衛星番号がついてます。

これとUcenterのバイナリコンソールを見ながらHEX値を解読します。１２バイト分できればOKです。

●NAV-SATモニター結果
左がプログラムしたモニター結果で、右のUcenterの値を比較してます。
一番強度CNOが強いのが７番目のGPS28で、仰角(Elevation)２０度方位(azimath)143度となってます。
窓が南東向きなので、一番視界がとおる衛星であることがわかります。

このデータを元に、RTK成立条件　40dB以上１０個　50dB以上が数個と全体の平均　標準偏差など
統計値をリアルタイムモニターをしてRTKの信頼性を管理できるようにします。

●プログラムの備忘録
NAV-SAT受信テスト用プログラムをGISTに置いておきます。
https://gist.github.com/dj1711572002/e80c1046bc5f6f09fd1ea92ab713e726

①シリアル受信のテクニック
NAV-SATくらい大きなデータを受信処理するには、Teensy4.1が必須です。ESP32系では無理です。
CortexM7程度の処理能力とハードウェアシリアルがついていることが必須です。
A:Serial.available()を多用する。
注１：超高速なので、一行の間1μsecで通過するので、データ受信行はスキがあるとエラーになります。
「Serial.read()の直前に if(Serial.available())を必ず置く必要があります」無くても動作しますが、
時々おかしくなります。
注２：F9Pは120msecが最高速なので、のんびりデータが入ってきますので、全部バッファにたまるまで
delayをいれて待ちます。50msec程度待てば全データ通信してくるので、それから一挙にバッファから
プログラム配列に取り込みます。ハードウェアシリアルのバッファサイズは１０２４バイトに設定してます

loop(){

delay(50);//１周期120msecなので十分データをためてから全部まとめて読むためのDELAY

if(Serial4.available()>172)//Serial4から一挙に受信
{
i=0;
rcvtime_1=rcvtime;
rcvtime=millis();
while(Serial4.available()>0)
{
if(Serial4.available())//readの直前にSerial.available()を入れないとバグる
{
dBuf4[i+4]=Serial4.read();
i++;
}
}
dN=i;//受信データ数
//Serial.printf(“RECIEVED,dN=,%d ,toptime=%d,recvtime-rcvtime_1=%d\n\r”,dN,toptime,rcvtime-rcvtime_1);
Serial.printf(“==================recieved dN=%d\n\r”,dN);
}//if serial

//続く

②データの頭出しソート処理
実際に受信したデータをみると、先頭が科ならずヘッダでははじまってません。ランダムで受信されてます。これはバッファの受信アルゴリズムによるものですが、決まったサイズの場合
ランダムに変化せずに、ほとんど、ヘッダが先頭になるので、今回は、NAV-SATで不定サイズのバラバラ受信なので、ほとんどのデータの先頭にヘッダがきてませんでした。
ソートのやり方：PVTが先頭と決めてあるので、0xB5,0x62,0x01,0x07を検索して
そこの順番を基準にデータ前半と後半にわけて、後半の後ろに前半を合体させてヘッダが
先頭にくる新たなデータ配列を作成します。
　関数void basesort(int dn,int stPVTN)の分割部分は下記です。

//Sorting——————————————————————————–
int jj;
for (jj = 0; jj <dN – PVT1startNo; jj++) //後半部Sort dBuf4[PVT1startNo]～dBuf4[dN-1]　To dBuf14[0]～dBuf4[dN-1-PVT1startNo]
{
dBuf4P[jj] = dBuf4[PVT1startNo + jj];//ヘッダーのある位置からよみとって、新しい配列dBuf4Pに代入して、データの最後までdN-PVT1startNo　個コピー
if(jj<6){
//Serial.printf(“dBuf4P[%d]=%x\n\r”, jj,dBuf4P[jj]);
}
if(jj>dN- PVT1startNo-6){
//Serial.println();
//Serial.printf(“dBuf4P[%d]=%x\n\r”,jj,dBuf4P[jj]);
}
}
for (jj = 0; jj < PVT1startNo; jj++) //先頭部ソート
{
dBuf4P[jj + dN- PVT1startNo] = dBuf4[jj];
//Serial.printf(“dBuf4P[%d]=%x,”,jj+dN-PVT1startNo,dBuf4[jj]);
if(jj<6){
//Serial.printf(“dBuf4P[%d]=%x\n\r”,jj + dN- PVT1startNo,dBuf4[jj]);
}
if(jj>PVT1startNo-6){
//Serial.println();
//Serial.printf(“dBuf4P[%d]=%x\n\r”,jj + dN- PVT1startNo,dBuf4[jj]);
}
}

このプログラムややこしいので、デバッグに手間をかけて２日間かかりました。
ソート前の生データをヘッダで２分して、ソート後と比較して切取りを確認しました。