【Mosaic-HAT】Raspberry Pi+RTKLIBでRTK移動局を作ってみよう２

19032 ワード
RaspberryPi RTKLIB GNS localization RTK RaspberryPi テキストリンク

前回、移動局（Rover）を作りましたが、補正処理をRTKLIBに任せました。
Mosaic-HAT自身も補正処理機能を備えているので、そちらで動かしてみましょう。
※移動局その１はこちら。https://qiita.com/KIT-tokunaga/items/f9a7249bdba8b1aceb3b

1. 構成

構成は下図の通り。基準局の測位情報を、RTK2goなどのNtripサーバを経由し、さらにラズパイを経由し、mosaicHATに入れてあげるだけ。基準局はRTCMv3などのフォーマットで情報を配信しているので、このフォーマットのままシリアルポートに流し込んであげればMosaicHATがフォーマットを理解し、誤差補正してくれるのです。近くに基準局があれば、わざわざRTK2goを通さずとも、シリアルからシリアルへと伝達してあげれば、同じことができます。

2. RTKLIB/str2strの設定

RTK2goから基準局の情報を読み出し、Mosaic-HATに流し込むツールは、RTKLIB/str2strを使います。これは基準局の作り方でも出てきました。
※基準局はこちら。https://qiita.com/KIT-tokunaga/items/5df5854f1471528f31ee

基準局では、Mosaic-HATの情報をRTK2goに配信するために使いましたが、移動局はその逆になります。つまり、inとoutを変えます。

rover.sh
cd /home/pi/myRTKLIB/RTKLIB/app/str2str/gcc
./str2str \
        -in ntrip://rtk2go.com:2101/MOUNT_POINT#rtcm3 \
        -out serial://ttyS0:9600:8:n:1:off#rtcm3

cd先はstr2strの実態のあるパスにしてください。
また、-inにあるMOUNT_POINTには、使用する基準局のマウントポイントにしてください。-outは、シリアルポートを示します。Mosaic-HATと繋がるコネクタにアサインされているポートはttyS0になります。スピードを9,600bpsにしていますが、インターネット側が遅れた場合を想定しています。なお、注意点として、このシリアルポートのMosaic-HAT側（COM1）は、デフォルト119,200bpsとなっていますので、上記を実行する前に、速度を変更しないといけません。次節にMosaic-HATの設定コマンドをまとめて示します。

3. Mosaic-HATの設定

Mosaic-HATに設定すべき内容は、以下の3点です。
1) 出力データのフォーマット指定
2) 送るメッセージ種別の設定
3) 速度を9600に変更

1)は、setDataInOutコマンドでCOM1の入力をコマンド（CMD)、出力をNMEAメッセージに指定します。2)は、NMEAメッセージのうち、どれを出力させるかの設定となり、移動局は補正済の位置情報が最低限必要となるので、setNMEAOutputコマンドを使ってGGAメッセージを1秒間隔で出力する設定をします。3)は、setCOMSettingsコマンドで設定します。以下にスクリプト例を示します。この例では、最初にリセット（GPIO5をLOW）して、コマンドモードのおまじない（S連打）の後に、3つの設定を行っています。なお、コマンドを確実に認識したかを見るために、Mosaic-HATからの反復メッセージ（$Rで始まるコメンドのオウム返し）を見るようにしました。
なお、２のstr2strよりも先にこのスクリプトを実行してください。２のシェルの最初に、"python3 ./setNMEA.py"を入れておけば、まとめて実行できます。

setNMEA.py
import serial
import RPi.GPIO as GPIO
from time import sleep

#Module Reset
print("Module Reset")
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)

GPIO.setup(5, GPIO.OUT, initial=GPIO.LOW)
sleep(1)
GPIO.output(5, GPIO.HIGH)
sleep(10)

#COM Initialize
print("COM Initialize")
ser = serial.Serial('/dev/ttyS0', 115200)
sleep(1)        # wait for connection

#Module Initialize
def PutCmd(command) :
        ser.write(command)
        while 1 :
                msg = ser.readline().decode()
                print(msg)
                if '$R' in msg :
                        print(msg)
                        break

print("Module Initialize")
ser.write(b'SSSSSSSSSSSSS\n')   # push MOSAIC to run in command mode
sleep(0.1)
PutCmd(b'sdio ,COM1,CMD,NMEA\n')  # SetDataInOut command
sleep(0.1)
PutCmd(b'sno ,Stream1,COM1,GGA,sec1\n')  # SetNMEAOutput command
sleep(0.1)
ser.write(b'scs ,COM1,baud9600\n')  # SetCOMSettings command

print("Finish")
ser.close()

3. 測位情報の読み出し

最後に、測位情報を取り出します。Mosaic-HATは、NMEAフォーマットでGGAメッセージを出力しつづけているので、これをラズパイで採取します。以下のスクリプト例は、取得した位置情報を表示するとともに、sol.posというファイル名でログを出力します。RTKLIBのrtkplotで読み込むことができます。
注意点ですが、GGAメッセージの緯度経度は、dddmm.mmmmというフォーマットです。例えば、3612.3456という値は、36°12.3456分を指します。分秒ではありません。なので、10進に変換するには、ddd+mm.mmmm/60という計算をします。min2decでその計算をしています。

getNMEA.py
import serial
import time
from datetime import datetime, timezone, timedelta
import chardet


def min2dec(value) :
        val_deg = int(value)
        val_min = value * 100 - val_deg * 100
        val_dec = float(val_deg + val_min / 60)
        return val_dec


#Initialize log file
with open('sol.pos', mode='w') as f :
        f.write("%  JST                   latitude(deg) longitude(deg)  height(m)   Q \n")

while True:
        with serial.Serial('/dev/ttyS0', 9600) as ser:  # Mosaic's COM1
                msg_bytes = ser.readline()
        if chardet.detect(msg_bytes)['encoding'] != "ascii" :
                continue
        msg_string = str(msg_bytes.decode())
        msg_string = msg_string.rstrip()
        if (msg_string.startswith('$GPGGA')):
                nmea_array = [element.strip() for element in msg_string.split(',')]
                Quality_Indicator = int(nmea_array[6])
                if Quality_Indicator == 0:
                        print("No GPS Fix Available!")
                else:
                        # parse NMEA GGA message
                        UTC_Time = datetime.fromtimestamp(float(nmea_array[1]), timezone.utc)
                        Latitude = float(nmea_array[2])*0.01
                        Latitude_direction = nmea_array[3]
                        Longitude = float(nmea_array[4])*0.01
                        Longitude_direction = nmea_array[5]
                        Height = float(nmea_array[9])
                        Height_unit = nmea_array[10]
                        Lat_deg = min2dec(Latitude)
                        Long_deg = min2dec(Longitude)

                        # print coordicates
                        print('Quality: ' + str(Quality_Indicator))
                        print('UTC Time: ' + UTC_Time.strftime("%H:%M:%S.%f"))
                        print(' Latitude: ' + str(Latitude) + Latitude_direction)
                        print(' Longitude: ' + str(Longitude) + Longitude_direction)
                        print(' Height: ' + str(Height) + Height_unit)
                        print(' Lat_deg: ' + str(Lat_deg))
                        print(' Long_deg: ' + str(Long_deg))

                        # pos.sol output
                        with open('sol.pos', mode='a') as f :
                                f.write("0 {0} {1} {2} {3} {4}\n".format(UTC_Time.strftime("%H:%M:%S.%f"), Lat_deg, Long_deg, Height, Quality_Indicator))

                time.sleep(0.1)
        else:
                continue