衛星からのメッセージ
ZED-F9Pから出力される衛星からの情報を確認します。
衛星からの情報はNMEAプロトコルを使って送られてきます。
色々なメッセージが送られてきますが、緯度・経度・FIXEDの情報を含むメッセージは
??RMCになります。??には衛星の種類の識別(GP,GL,GA,GB,GQ,GN)が入ります。
例
$??RMC,052400.00,A,3539.3146239,N,13945.6411751,E,0.467,346.16,271121,,,R,V*0E
[0]識別 $??RMC
[1]時刻 052400.00
[2]データ有効性 A
[3]緯度 3539.3146239
[4]南北 N
[5]経度 13945.6411751
[6]東西 E
[7]移動速度 0.467
[8]真方位 346.16
[9]日付 27112 ddmmyy
[10]磁気偏角 _
[11]磁気偏角の方向 _
[12]モード R (N=測位不能、A=単独測位、D=DGPS、F=RTK float、R=RTK fix)
[13]ナビゲーション*チェックサム V*0E
$から始まり、識別～*チェックサム、crlf[改行]で終わる文字列です。
特に大事な情報は[3]緯度、[5]経度、[12]モード
この文字列を処理する事になります。
この緯度・経度情報を直交座標に変換します。
直交座標にできれば、高校程度の数学で現在の位置と目標の位置との関係を計算する事ができます。
Raspberry Pi Picoで処理する
PicoはMicropythonを使って文字列を処理する事ができます。
まず、ZED-F9PのUART1のTXからPicoのUART0のRXを接続してデータを受け取ります。
受け取ったデータをそのままPicoのモニターに表示します。
１．ZED-F9PのUART1の出力内容をNMEAのみに制限します。
２．PicoのUART0にNMEAのデータを取り込みます。配線します。
３．Picoのmicropythonでデータを処理します。
４．結果が母艦のPCのモニターに表示されます。
Picoのmicropythonプログラム
とりあえず、直角座標座標に変換までのコードです。pythonのプログラムの体裁になっていませんが一応動きます。
5系基準点となっているのは筆者の住所が兵庫県だからです。
緯度・経度を直交座標に変換するプログラムはネット上にあったものですが、Numpyライブラリがmicropythonには無いので 今流行りのAIにNumpyを使わない方法で変換を依頼しました。でも少し間違っていたのでそれを修正しました。
Picoのmicropythonでの浮動小数演算は単精度なのですが、倍精度版がネット上にあったのでそれを使っています。
なお、このプログラムを使用して発生した問題には筆者は責任を負いません。自己責任でお願いします。
from machine import Pin, UART
import time
import math
# 5系基準点
Orlat_deg = 36.0 # 北緯36度0分
Orlon_deg = 134.0 + 20./60 # 東経134度20分 -> 度に変換
# ============================ 緯度・経度 直角座標 変換 =================
def conv_gpsval(gpsstr):
gpsval = float(gpsstr)
dosu = int(gpsval/100)
shou = (gpsval - dosu * 100)/60
return dosu + shou
#--------
def calc_xy2(phi_deg, lambda_deg, phi0_deg, lambda0_deg):
# 緯度経度・平面直角座標系原点をラジアンに直す
phi_rad = math.radians(phi_deg)
lambda_rad = math.radians(lambda_deg)
phi0_rad = math.radians(phi0_deg)
lambda0_rad = math.radians(lambda0_deg)
# 補助関数
def A_array(n):
A0 = 1 + (n**2)/4. + (n**4)/64.
A1 = - (3./2)*( n - (n**3)/8. - (n**5)/64. )
A2 = (15./16)*( n**2 - (n**4)/4. )
A3 = - (35./48)*( n**3 - (5./16)*(n**5) )
A4 = (315./512)*( n**4 )
A5 = -(693./1280)*( n**5 )
return [A0, A1, A2, A3, A4, A5]
def alpha_array(n):
a0 = float('nan') # dummy
a1 = (1./2)*n - (2./3)*(n**2) + (5./16)*(n**3) + (41./180)*(n**4) - (127./288)*(n**5)
a2 = (13./48)*(n**2) - (3./5)*(n**3) + (557./1440)*(n**4) + (281./630)*(n**5)
a3 = (61./240)*(n**3) - (103./140)*(n**4) + (15061./26880)*(n**5)
a4 = (49561./161280)*(n**4) - (179./168)*(n**5)
a5 = (34729./80640)*(n**5)
return [a0, a1, a2, a3, a4, a5]
# 定数 (a, F: 世界測地系-測地基準系1980（GRS80）楕円体)
m0 = 0.9999
a = 6378137.
F = 298.257222101
# (1) n, A_i, alpha_iの計算
n = 1. / (2*F - 1)
A_array = A_array(n)
alpha_array = alpha_array(n)
# (2), S, Aの計算
A_ = ( (m0*a)/(1.+n) )*A_array[0] # [m]
S_ = ( (m0*a)/(1.+n) )*( A_array[0]*phi0_rad + sum([A_array[i]*math.sin(2*i*phi0_rad) for i in range(1,6)])) # [m]
# (3) lambda_c, lambda_sの計算
lambda_c = math.cos(lambda_rad - lambda0_rad)
lambda_s = math.sin(lambda_rad - lambda0_rad)
# (4) t, t_の計算
t = math.sinh( math.atanh(math.sin(phi_rad)) - ((2*math.sqrt(n)) / (1+n))*math.atanh(((2*math.sqrt(n)) / (1+n)) * math.sin(phi_rad)) )
t_ = math.sqrt(1 + t*t)
# (5) xi', eta'の計算
xi2 = math.atan(t / lambda_c) # [rad]
eta2 = math.atanh(lambda_s / t_)
# (6) x, yの計算
x = A_ * (xi2 + sum([alpha_array[i]*math.sin(2*xi2*i)*math.cosh(2*eta2*i) for i in range(1, 6)])) - S_ # [m]
y = A_ * (eta2 + sum([alpha_array[i]*math.cos(2*xi2*i)*math.sinh(2*eta2*i) for i in range(1, 6)])) # [m]
# return
return x, y
# ============================ $XXXXXX*XX形式をチェック =================
def IsCompleteline(s_data):
try:
id_str = s_data.index('$')
kugiri = s_data.index('*')
datalen = len(s_data)
if id_str == 0 and kugiri + 3 == datalen:
return True
else:
return False
except ValueError:
return False
# ============================ NMEAのチェックサムをチェック =================
def IsCheckSum(s_data):
try:
id_str = s_data.index('$')
kugiri = s_data.index('*')
# $,*が存在すれば値を返すが存在しなければexcept ValueError:へ移動する
# 中途半端なデーターを排除して完全なデータのみを処理する
datalen = len(s_data) # 文字列の長さ
# 区切り位置で2つに分ける
headdata = s_data[1:kugiri]
# b'GPRMC,085120.307,A,3541.1493,N,13945.3994,E,000.0,240.3,181211,,,A'
taildata = s_data[kugiri+1:datalen]
# b'6A'
# チェックサム計算
b_headdata = bytes(headdata,'utf-8') # bytesに変換
checksum = 0
for c in b_headdata:
checksum ^= c
# 6Aを数値化
b_taildata = bytes(taildata,'utf-8') # bytesに変換
checksum2 = int(b_taildata,16)
# 比較
return checksum == checksum2
except ValueError:
return False
# ============================ $??RMVメッセージから緯度・経度を取り出して直角座標に変換 =================
def idokeido1(strdata):
if len(strdata) > 0:
elements = strdata.split(',')
# print(elements)
if '$' in elements[0]:
if 'RMC' in elements[0]:
print(elements[3],elements[5],elements[12])
if elements[12] != 'N':
lati = float(elements[3])
longi = float(elements[5])
xx, yy = calc_xy2(conv_gpsval(lati), conv_gpsval(longi), Orlat_deg, Orlon_deg)
print('X:', xx, 'Y:',yy)
# ==========================================================
# ============= メインプログラム ===========================
# ==========================================================
uart = UART(0,baudrate=38400, tx=Pin(0), rx=Pin(1))
# ----
sss = ''
#while True: とりあえず 1000ループで終了する
for j in range(0,1000):
if uart.any():
data = uart.read(80) # 80バイトのデータを読み取る
if data:
# print("Received data:", data)
# ここにデータを処理するコードを追加
try:
ss = data.decode('utf-8')
sss += ss
except UnicodeError:
pass # 変換失敗は何もしない
time.sleep(0.01) # 小休止を入れて無駄な処理を避ける
# 38400/8=4800byte/s 0.01→48byte 長く待ちすぎるとデータが消える
#
liststr = sss.splitlines() # listを作成
k = len(liststr) # listの行数が返る
if k > 0:
# 最後のリストk-1のデータが不完全な場合がある。その時はsssに代入して次の処理に回す
saigo = liststr[k-1]
if IsCompleteline(saigo): # 最後のリストデータは完全か？
sss = '' # 次の処理に渡すデータはない
else:
# k-1の位置のリストを削除してそれをsssに代入
sss = liststr.pop(k-1) # 次の処理に回す
# リストを全て処理
for sdata in liststr:
# print(sdata)
if 'RMC' in sdata:
if IsCheckSum(sdata):
idokeido1(sdata)
