LoginSignup
0
0

More than 1 year has passed since last update.

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

Posted at

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

1. 構成

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

diagram1.png

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で始まるコメンドのオウム返し)を見るようにしました。
なお、2のstr2strよりも先にこのスクリプトを実行してください。2のシェルの最初に、"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
0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
0
0