Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/BasicDataTrans_STA24_ver0.42.py Secret

Created June 22, 2024 21:17
Show Gist options
  • Save dj1711572002/82330327afeb927ae94742fbabf4bd25 to your computer and use it in GitHub Desktop.
Save dj1711572002/82330327afeb927ae94742fbabf4bd25 to your computer and use it in GitHub Desktop.
Download ZIP
Python STA24 data transform pgm
Raw
BasicDataTrans_STA24_ver0.42.py
# This is a sample Python script.
# 機能：STA24 SystemのログデータをCSV形式で読み込んで、データの検査と修復をおこなって各デバイス間の同期を整頓する
# 　iTOW抜け：ループで回して、発見したら補間した行を作って、DFの再下端に追加していく、全部ループが終了したらiTOWをSORTして整頓
# graph00では、1個のグラフだったので、01では、全体を網羅するグラフ散布図ベクトル矢印を作る　
# BootsOnのデータも読み込む
#py_CSVread_PandasDF04_TurnRL07
import math
import statistics
import numpy
import pandas as pd
import matplotlib.pyplot as plt
import matplotlib.cm as cm
import tkinter
import tkinter.filedialog as FileDialog
import os
import sys
import numpy as np
import pickle
import keyboard
from sklearn import linear_model
from scipy.signal import find_peaks # type: ignore
from sklearn.linear_model import LinearRegression
import statsmodels.api as sm
def is_num(x): # 数値か文字かを判別する関数
try:
float(x)
except ValueError:
return False
else:
return True
def additions(df):
# ---headMot Heading 角度補正
print(df)
print(df.dtypes)
df["headMot360"] = df.headMot_B.multiply(0.00001)
df["headMot360"] = np.where(df["headMot360"] > 360, df["headMot360"] - 360, df["headMot360"])
print(df.headMot360)
# ---Heading角　補正
df["Heading180"] = df.relPosHeading_R.multiply(0.00001) + 180
df["Heading360"] = np.where(df["Heading180"] > 360, df["Heading180"] - 360, df["Heading180"])
print(df.Heading360)
# ---headMot移動平均３個MA3
df["headMotMA3"] = df["headMot360"].rolling(3).mean()
print(df.headMot360, df.headMotMA3)
# ---差分　diff()
df["headMotMA3diff"] = df["headMotMA3"].diff()
# ---差分ゼロ点を１してマーク
zerocnt = 0
zeroflag = 0
zeroflag_1 = 0
for i in range(2, len(df.index)):
if df.loc[i, "headMotMA3diff"] * df.loc[i - 1, "headMotMA3diff"] < 0 and df.loc[
i - 1, "diffzero"] == 0 and i - zeroflag > 4:
zeroflag = i
df.loc[i, "diffzero"] = zerocnt
zerocnt += 1
else:
df.loc[i, "diffzero"] = 0
return df
def A2H(x0, y0, x1, y1):
dx = x1 - x0
dy = y1 - y0
a2 = math.degrees(math.atan2(dy, dx))
if dx < 0 and dy > 0:
an = 180 + a2
else:
an = 90 - a2
return an
def additions2(df):
df["H-Ysa"] = df["Heading360"] - df["yaw"]
df["H-Ysa360"] = np.where(df["H-Ysa"] > 360, df["H-Ysa"] - 360, df["H-Ysa"])
df["skid"] = df["headMotMA3"] - df["Heading360"]
# Heading from Position
for i in range(1, len(df) - 1):
x0 = df.loc[i - 1, "relPosE_B"]
y0 = df.loc[i - 1, "relPosN_B"]
x1 = df.loc[i, "relPosE_B"]
y1 = df.loc[i, "relPosN_B"]
an = A2H(x0, y0, x1, y1)
df.loc[i, "Hfp"] = an
# print("Hfp[",i,"]=",an)
return df
# ||||||||||||||||||||||||||||def readMBFile||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
def readMBFile(filename, basename, head0):
# print(result.gSpeed_B)
df = pd.read_csv(filename, names=head0, low_memory=True) # CSV　Read dfに読み込まれる head0の列名がつく
print("PATH:", filename, "READ:", basename) # basenameは、パスをのぞいたファイル名　ファイル名ubx とbnoで処理分ける
pd.set_option('display.max_columns', 120) # 120列まで表示
df_Base = df[df.iloc[:, 0] == 'B'] # id列[:,0]が'B'ならばdf_Baseとして抽出
df_Base = df_Base.fillna(0) # NANをゼロに変換
df_Base = df_Base.reset_index() # index番号リセット
df_Base = df_Base.add_suffix('_B', axis=1) # head0の名称末尾に'_B'をつけてBaseのデータ列命名
df_Base = df_Base.drop(columns='index_B') # index_Bは不要なので消去
df_Base = df_Base.drop(columns='id_B') # id_Bも不要なので消去　数値以外のデータ除去
headB = df_Base.columns # 完成したヘッダ文字LISTを保持
print("headB=", headB)
k = [] # itow エラー　list k[]宣言
# ------------df_Baseをlistへ読み出し-----------
list_data = [] # df_Baseの値をlistに読み込む　DFでの処理よりlistで処理したほうが速い
list_data = df_Base.to_numpy().tolist() # list_dataに値を代入
# print("list_data=",list_data)
# print("list_data size=", [len(v) for v in list_data]) #
# -----------------------------------------
lbase = len(df_Base) # 全行数
# lastrow = len(df_Base)
for i in range(1, lbase): # 行数ループ　整数iでカウント
# print("range i=",i)
if i > 1: # i-1対策
itow0 = (int)(df_Base.iloc[i, 0]) # 整数で番地指定しているのでiloc使用
itow_1 = (int)(df_Base.iloc[i - 1, 0]) # 1個手前のitow_1
# print(itow0-itow_1)
c = [] # 補間計算データ行リスト
k = 0
if itow0 - itow_1 > 120: # itow差が120以上あったらエラー
itowadd = itow_1 + 120
# print("itowadd=", itowadd, "itow0=", itow0, "itow_1=", itow_1)
for j in range(0, 55): # 列をループして平均値計算
ss = df_Base.iloc[i, j]
# if is_num(ss):
a = float(df_Base.iloc[i, j])
b = float(df_Base.iloc[i - 1, j])
# print(j,a,b)
c.append(int((a + b) / 2)) # 補間行リストに平均値代入
# add c to list_data
list_data.append(c) # 行代入が完了したcをlist_dataに追加　元のlistデータの最高単に追加される
# print("c len=",len(c))
# print("Err made c=",c)
# print("list_data size=", [len(v) for v in list_data])
# ================All list_data をdf_Base2にしてitow SORTして修正完成=====================================
df_Base2 = pd.DataFrame(list_data, columns=headB) # i,jでループ完了したら、list_dataから新たなDF df_Base2を作成
# 数値以外のデータが含まれている行の抽出
non_numeric_rows = df_Base2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all(), axis=1)
# print("nonnumericRow=",non_numeric_rows)
# 数値以外のデータが含まれている列の抽出
non_numeric_cols = df_Base2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all())
# print("nonnumericCol=",non_numeric_cols)
# df_Rover2 = df_Base2.fillna(0)
# df_Base2['iTOWB']=df_Base2['iTOW_B'].astype('int')
# -------------------iTOW_BでSORTする前にiTOW_Bのゴミを完全に排除する--------------------------------------
df_Base2['iTOW_B'] = df_Base2['iTOW_B'].astype('str')
df_Base2['iTOW_B'] = df_Base2['iTOW_B'].str.replace('-', '').str.replace(':', '').str.replace(' ', '')
df_Base2['iTOW_B'] = df_Base2['iTOW_B'].astype("float64")
# ----------------------------------------------------------------------------------------------------
print(df_Base2)
df_Base2.info()
# pd.set_option('display.max_rows', None)
df_Base2 = df_Base2.sort_values("iTOW_B", ascending=True) # SORT iTOWBでSORT
print("Sorted df_Base2=", df_Base2)
# ----------CHECK SORT--------------
itowerr = 0
for i in range(1, len(df_Base2) - 1):
itow0 = df_Base2.iloc[i + 1, 0]
itow_1 = df_Base2.iloc[i, 0]
if ((itow0 - itow_1) != 120):
itowerr += 1
# print("BASE***********i=", i, itow0, "itowErr=", itowerr)
# ============================BASE ITOW 補修終了===============================
# ========================ROVER iTOW処理======================================
# df_Base2.info()
# df_Rover = df[df.iloc[:, 0] == 'R']
# df_Rover.reset_index(drop=True, inplace=True)
# df_Rover = df_Rover.add_suffix('_R', axis=1)
df_Rover = df[df.iloc[:, 0] == 'R'] # id列[:,0]が'B'ならばdf_Baseとして抽出
df_Rover = df_Rover.fillna(0) # NANをゼロに変換
df_Rover = df_Rover.reset_index() # index番号リセット
df_Rover = df_Rover.add_suffix('_R', axis=1) # head0の名称末尾に'_B'をつけてBaseのデータ列命名
df_Rover = df_Rover.drop(columns='index_R') # index_Bは不要なので消去
df_Rover = df_Rover.drop(columns='id_R') # id_Bも不要なので消去　数値以外のデータ除去
headR = df_Rover.columns # 完成したヘッダ文字LISTを保持
print("headR=", headR)
# print(df_Rover)
print("============================df_Rover describe===============================")
print(df_Rover.describe())
kr = [] # itow エラー　list kr[]宣言
# ------------df_Roverをlistへ読み出し-----------
list_datar = [] # df_Baseの値をlistに読み込む　DFでの処理よりlistで処理したほうが速い
list_datar = df_Rover.to_numpy().tolist() # list_datarに値を代入
# print("list_datar=",list_datar)
print("list_datar size=", [len(v) for v in list_datar]) #
# ------Rover itow 抜け------------------------------
lbaser = len(df_Rover) # 全行
for i in range(1, lbaser): # 行数ループ　整数iでカウント
# print("range i=",i)
if i > 1: # i-1対策
itow0 = (int)(df_Rover.iloc[i, 0]) # 整数で番地指定しているのでiloc使用
itow_1 = (int)(df_Rover.iloc[i - 1, 0]) # 1個手前のitow_1
# print(itow0-itow_1)
cr = [] # 補間計算データ行リスト
kr = 0
if itow0 - itow_1 == 240: # itow差が120以上あったらエラー
itowadd = itow_1 + 120
print("df_Rover:itowadd=", itowadd, "itow0=", itow0, "itow_1=", itow_1)
for j in range(0, 55): # 列をループして平均値計算
ss = df_Rover.iloc[i, j]
# if is_num(ss):
a = float(df_Rover.iloc[i, j])
b = float(df_Rover.iloc[i - 1, j])
c = int((a + b) / 2)
print("Rover Error Row itow0", itow0, ":i,j,a,b=", i, j, a, b)
cr.append(c) # 補間行リストに平均値代入
# add c to list_data
list_datar.append(cr) # 行代入が完了したcをlist_dataに追加　元のlistデータの最高単に追加される
print("cr len=", len(cr))
elif itow0 - itow_1 > 240:
print("Err made cr=", cr)
#else:
#print("itow Check OK i=", i, itow0, itow_1)
# print("list_datar size=", [len(v) for v in list_datar])
# ================All list_data をdf_Base2にしてitow SORTして修正完成=====================================
df_Rover2 = pd.DataFrame(list_datar, columns=headR) # i,jでループ完了したら、list_dataから新たなDF df_Base2を作成
# df_Rover2.to_csv(r'C:/RTK_Log/df_Rover2_' + basename)
# 数値以外のデータが含まれている行の抽出
non_numeric_rows = df_Rover2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all(), axis=1)
# print("nonnumericRow=", non_numeric_rows)
# 数値以外のデータが含まれている列の抽出
non_numeric_cols = df_Rover2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all())
# print("nonnumericCol=", non_numeric_cols)
df_Rover2 = df_Rover2.fillna(0)
# df_Rover2['iTOW_R']=df_Rover2['iTOW_R'].astype('int')
# -------------------iTOW_BでSORTする前にiTOW_Bのゴミを完全に排除する--------------------------------------
df_Rover2['iTOW_R'] = df_Rover2['iTOW_R'].astype('str')
df_Rover2['iTOW_R'] = df_Rover2['iTOW_R'].str.replace('-', '').str.replace(':', '').str.replace(' ', '')
df_Rover2['iTOW_R'] = df_Rover2['iTOW_R'].astype("float64")
# ----------------------------------------------------------------------------------------------------
# print(df_Rover2)
df_Rover2.info()
# pd.set_option('display.max_rows', None)
df_Rover2 = df_Rover2.sort_values("iTOW_R", ascending=True) # SORT iTOW_RでSORT
# print("Sorted df_Rover2=", df_Rover2)
# ----------CHECK SORT--------------
itowerr = 0
for i in range(1, len(df_Rover2) - 1):
itow0 = df_Rover2.iloc[i + 1, 0]
itow_1 = df_Rover2.iloc[i, 0]
if ((itow0 - itow_1) != 120):
itowerr += 1
print("***********i=", i, itow0, "itowErr=", itowerr, "sa=", itow0 - itow_1)
# ============================Rover ITOW 補修終了===============================
# df_Base2とdf_Rover2の長さを短いほうにあわせる
# drop(range(削除したい最初の行, 削除したい最後の行+1), axis=”rows“)
# print("df_Base2 tail=", df_Base2.tail())
# print("df_Rover2 tail=", df_Rover2.tail())
Base2len = len(df_Base2)
Rover2len = len(df_Rover2)
if Base2len > Rover2len:
last_row = df_Base2.shape[0] - 1
upper_row = last_row - (len(df_Base2) - len(df_Rover2))
df_Base2 = df_Base2.drop(range(upper_row, last_row), axis="rows")
elif len(df_Base2) < len(df_Rover2):
last_row = df_Rover2.shape[0] - 1
upper_row = last_row - (len(df_Rover2) - len(df_Base2))
df_Rover2 = df_Rover2.drop(range(upper_row, last_row), axis="rows")
df_Base2.reset_index()
df_Rover2.reset_index()
# print("df_Base2 len=", len(df_Base2), "df_Rover2 len=", len(df_Rover2))
print("===================df_Rover2 describe===========================")
print(df_Rover2.describe())
print("--------------------CONCATでfd_Base2とfd_Rover2を横に合体BR_ファイルCSV保存---------------------")
result = pd.concat([df_Base2, df_Rover2], axis=1)
result = result.fillna(0) # NANをゼロに変換
result = result.reset_index(drop=True)
result = result.drop(result.index[-1])
# print("relposHeading Max=", result["relPosHeading_B"].max(), "min=", result["relPosHeading_B"].min())
cols=result.columns
result[cols]=result[cols].astype("float64")
print(result.dtypes)
additions(result)
result.to_csv(r'C:/RTK_Log/BR_' + basename)#BR_ファイル保存で完成
# print(result)
print("============================result describe=========================================")
print(result.describe())
return result
def plotheadmot(result, basename):
# ===========================================================================================
# ===================PLOT headMot Heading diff NutralPoint===================================
# =========================================================================================
# １つのfigにたくさんグラフを作る
# fig, ax = plt.subplots(2, 2, figsize=(12, 10))
fig, ax = plt.subplots(figsize=(12, 10))
fig.suptitle(basename)
# graph1
# ax[0,0].plot(result.index,result.gSpeed_B,"green","gSpeed_B")# Graph1 gSpeed
# graph2
# ax2= ax.twinx()
# ax[0, 0].plot(result.index, result.headMot360, "red", "headMot360")#Graph2 headMot
# ax[0, 0].plot(result.index, result.Heading360, "blue", "Heading360")#Graph2 Heading
ax.plot(result.index, result.headMot360, "red", label="headMot360") # Graph2 headMot
ax.plot(result.index, result.Heading360, "blue", label="Heading360") # Graph2 Heading
# ax[0,1].plot(result.index,result.headMotMA3,"black","headMotMA3")#Graph3 headMotMA3
# ax2 = ax[0,0].twinx()#2軸作成
ax2 = ax.twinx() # 2軸作成
ax2.plot(result.index, result.headMotMA3diff, "black", label="headMotMA3diff") # Graph3 headMotMA3diff
ax2.grid()
# 縦線
for i in range(2, len(result.index)):
if result.loc[i, "headMotMA3diff"] * result.loc[i - 1, "headMotMA3diff"] < 0:
ax2.axvline(i, color="aqua")
result.diffzero = 1
else:
result.diffzero = 0
# graph3
result['relPosN_B'] = result['relPosN_B'].astype('int')
result['relPosE_B'] = result['relPosE_B'].astype('int')
# print(result.relPosE_B)
# print(result.relPosN_B)
# ax[1, 0].scatter(result.relPosE_B, result.relPosN_B)#Graph4 BaseXY
ax.legend(loc="upper left")
ax2.legend(loc="center left")
# ax2.legend()
plt.show()
# ----------------PLOT VECTOR-----------------------------------------------------
def resultrange(result):
# headmotMA3のMaxMin
hmax = result["headMotMA3"].max()
hmin = result["headMotMA3"].min()
return hmax, hmin
def arrowmake(df, index):
x0 = df.loc[index, "relPosE_B"]
y0 = df.loc[index, "relPosN_B"]
x1 = df.loc[index + 1, "relPosE_B"]
y1 = df.loc[index + 1, "relPosN_B"]
dist = numpy.sqrt((x1 - x0) ** 2 + (y1 - y0) ** 2) * 0.8
# headMot
hmotdeg = df.loc[index, "headMot360"]
hmotrad = math.radians(df.loc[index, "headMot360"])
vxm = dist * math.sin(hmotrad)
vym = dist * math.cos(hmotrad)
# print("vxm=", vxm, "vym=", vym)
# Heading
headdeg = df.loc[index, "Heading360"]
headrad = math.radians(df.loc[index, "Heading360"])
hxm = dist * math.sin(headrad)
hym = dist * math.cos(headrad)
gsp = df.loc[index, "gSpeed_B"] * 0.001 * 3600 * 0.001
skid = hmotdeg - headdeg
high = df.loc[index, "height_B"] * 0.001
edge = df.loc[index, "roll"]
return vxm, vym, hxm, hym, gsp, skid, high, edge
# Nutral Point検索　diffとRollとheadMot から
def getnutral(df):
npt = [] # Nutral Point index No
npx = [] # Nutral Point relPosE
npy = [] # Nutral Point relPosN
npt_1 = 1
nptc = 0
for i in range(1, len(df.index) - 1):
ar = df.loc[i, "roll"]
ar_1 = df.loc[i - 1, "roll"]
if ar * ar_1 < 0 and i - npt_1 > 3:
npt_1 = i
npt.append(i)
npx.append(df.loc[i, "relPosE_B"])
npy.append(df.loc[i, "relPosN_B"])
nptc += 1
# ("Nutral Point No=",nptc,"index=",i,"npx=",df.loc[i,"relPosE_B"],"npy=",df.loc[i, "relPosN_B"])
return npt, npx, npy
def yawH2XY(df):#ver0.4用に変更
#----------yaw角からBase　Rover座標を計算--------------------------------
lenBR:float64=100#仮想アンテナ間距離100cm
lenCB:float64=50#BootsアンテナからBaseアンテナまでの距離
lenCR:float64=lenBR-lenCB#BootsアンテナからRoverアンテナまでの距離
df["rpNR_BT"]=0#Rover(後アンテナ)N座標
df["rpER_BT"]=0#Rover(後アンテナ)E座標
df["rpNB_BT"]=0#Base(前アンテナ)N座標
df["rpEB_BT"]=0#Base(前アンテナ)E座標
df["rpNR_BT"]=df["rpNR_BT"].astype("float64")
df["rpER_BT"]=df["rpER_BT"].astype("float64")
df["rpNB_BT"]=df["rpNB_BT"].astype("float64")
df["rpEB_BT"]=df["rpEB_BT"].astype("float64")
#df=df.astype("float64")
for i in range(0,len(df)):
xbt0=df.loc[i,"relPosE_T"]
ybt0=df.loc[i,"relPosN_T"]
yawt=df.loc[i,"yawH_T"]
yawtrad=math.radians(yawt)
yawtrad180=math.radians(yawt-180)
df.loc[i,"rpEB_BT"]=xbt0+lenCB*math.sin(yawtrad)
df.loc[i,"rpNB_BT"]=ybt0+lenCB*math.cos(yawtrad)
df.loc[i,"rpER_BT"]=xbt0+lenCR*math.sin(yawtrad180)
df.loc[i,"rpNR_BT"]=ybt0+lenCR*math.cos(yawtrad180)
#df.loc[i,"rpNR_BT"]=yr
#df.loc[i,"rpER_BT"]=xr
#df.loc[i,"rpNB_BT"]=ybr
#df.loc[i,"rpEB_BT"]=xb
df_CPLT = pd.DataFrame()
df_CPLT = df[["headMotMA3_T", "yawH_T","rpEB_BT","rpNB_BT","rpER_BT","rpNR_BT","relPosE_T","relPosN_T"]]
#print("csv add4")
df_CPLT.to_csv(r'C:/RTK_Log/CPLT_BTUBNO_' + basename, index=False) # CSV save
print(df)
return df
#******************************df_BTplot用追加処理*******************************************************
#NutralPointは、BRBNに合わせる　グラフ原点もBRBNに合わせる
def additions4(df):#df_BTplotの追加列
df["headMotT360"] = df.headMot_T.multiply(0.00001)
df["headMotT360"] = np.where(df["headMotT360"] > 360, df["headMotT360"] - 360, df["headMotT360"])
df["headMot_TMA3"] = df["headMotT360"].rolling(3).mean()
# ---headMot_T差分　diff()
df["headMotTMA3diff"] = df["headMot_TMA3"].diff()
# -----------headMot_TMA3とyaw_Tのpeaks 検出して補正値計算-------------------------------
peaks, _ = find_peaks(df["headMot_TMA3"], distance=8)
ypeaks, _ = find_peaks(df["yaw_T"], distance=12)
print(peaks)
print(ypeaks)
df["headMot_peaks"]=0
df["yawT_peaks"]=0
df["yawT_peakPos"]=0
df["yawHosei"]=0
df["headMot_peaks"]=df["headMot_peaks"].astype("float64")
df["yawT_peaks"]=df["yawT_peaks"].astype("float64")
df["yawT_peakPos"]=df["yawT_peakPos"].astype("float64")
df["yawHosei"]=df["yawHosei"].astype("float64")
headMotmax=[]
headMotindex=[]
yawmax=[]
yawindex=[]
#df = df.astype('float64')
#peaksに近いypeaksを探す
if len(peaks)>len(ypeaks):
ln=len(ypeaks)
else:
ln=len(peaks)
for i in range(0,ln-1):
print("i=",i)
headMotindex.append(peaks[i])
crow=peaks[i]
headMotmax.append(df.loc[crow,"headMot_TMA3"])
df.loc[crow,"headMot_peaks"]=headMotmax[i]
for j in range(0,len(ypeaks)-1):
if peaks[i]>ypeaks[j] and peaks[i]<ypeaks[j+1] :
maesa=peaks[i]-ypeaks[j+1]
atosa=ypeaks[j+1]-peaks[i]
if maesa<=atosa:
df.loc[ypeaks[j],"yawT_peakPos"]=100#yawpeak位置マーク
yawmax.append(df.loc[ypeaks[j],"yaw_T"])
yawindex.append(ypeaks[j])
df.loc[crow,"yawT_peaks"]=yawmax[i]#yawpeask値をheadMotpeak値に並べる
else:
df.loc[ypeaks[j+1],"yawT_peakPos"]=100#yawpeak位置マーク
yawmax.append(df.loc[ypeaks[j+1],"yaw_T"])
yawindex.append(ypeaks[j+1])
df.loc[crow,"yawT_peaks"]=yawmax[i]#yawpeask値をheadMotpeak値に並べる
elif peaks[i]==ypeaks[j] or peaks[i]==ypeaks[j+1]:#headMot peakとyaw peakが等しい時
df.loc[ypeaks[j],"yawT_peakPos"]=100
df.loc[crow,"yawT_peaks"]=df.loc[ypeaks[j],"yaw_T"]
yawmax.append(df.loc[ypeaks[j],"yaw_T"])
yawindex.append(ypeaks[j])
print("headMotmax:",headMotmax)
print("headMotindex:",headMotindex)
print("yawmax:",yawmax)
print("yawindex:",yawindex)
#補正値で埋める
df["yawHosei"]=0
for i in range(0,len(headMotindex)-1):
for j in range(yawindex[i],yawindex[i+1]):
df.loc[j,"yawHosei"]=headMotmax[i]-yawmax[i]
df.loc[j,"yawH"]=df.loc[j,"yawHosei"]+df.loc[j,"yaw_T"]
#----------yaw角からBase　Rover座標を計算--------------------------------
lenBR:float64=100#仮想アンテナ間距離100cm
lenCB:float64=50#BootsアンテナからBaseアンテナまでの距離
lenCR:float64=lenBR-lenCB#BootsアンテナからRoverアンテナまでの距離
df["rpNR_BT"]=0#Rover(後アンテナ)N座標
df["rpER_BT"]=0#Rover(後アンテナ)E座標
df["rpNB_BT"]=0#Base(前アンテナ)N座標
df["rpEB_BT"]=0#Base(前アンテナ)E座標
df["rpNR_BT"]=df["rpNR_BT"].astype("float64")
df["rpER_BT"]=df["rpER_BT"].astype("float64")
df["rpNB_BT"]=df["rpNB_BT"].astype("float64")
df["rpEB_BT"]=df["rpEB_BT"].astype("float64")
#df=df.astype("float64")
for i in range(0,len(df)):
xbt0=df.loc[i,"relPosE_T"]
ybt0=df.loc[i,"relPosN_T"]
yawt=df.loc[i,"yawH"]
yawtrad=math.radians(yawt)
yawtrad180=math.radians(yawt+180)
df.loc[i,"rpEB_BT"]=xbt0+lenCB*math.sin(yawtrad)
df.loc[i,"rpNB_BT"]=ybt0+lenCB*math.cos(yawtrad)
df.loc[i,"rpER_BT"]=xbt0+lenCR*math.sin(yawtrad180)
df.loc[i,"rpNR_BT"]=ybt0+lenCR*math.cos(yawtrad180)
#df.loc[i,"rpNR_BT"]=yr
#df.loc[i,"rpER_BT"]=xr
#df.loc[i,"rpNB_BT"]=ybr
#df.loc[i,"rpEB_BT"]=xb
df_CPLT = pd.DataFrame()
df_CPLT = df[["headMotMA3_T", "yawH_T","rpEB_BT","rpNB_BT","rpER_BT","rpNR_BT","relPosE_T","relPosN_T"]]
print("csv add4")
df_CPLT.to_csv(r'C:/RTK_Log/add4_BTplot_' + basename, index=False) # CSV save
print(df)
return df
def additions5(df):#resultBRBN用のyaw補正処理
#df["headMotT360"] = df.headMot_T.multiply(0.00001)
#df["headMotT360"] = np.where(df["headMotT360"] > 360, df["headMotT360"] - 360, df["headMotT360"])
#df["headMot_TMA3"] = df["headMotT360"].rolling(3).mean()
# ---headMot_T差分　diff()
#df["headMotTMA3diff"] = df["headMot_TMA3"].diff()
# -----------headMot_TMA3とyaw_Tのpeaks 検出して補正値計算-------------------------------
peaks, _ = find_peaks(df["headMot_TMA3"], distance=8)
ypeaks, _ = find_peaks(df["yaw_T"], distance=12)
print(peaks)
df["headMot_peaks"]=0
df["yaw_peaks"]=0
df["yaw_peakPos"]=0
df["yawHosei"]=0
headMotmax=[]
headMotindex=[]
yawmax=[]
yawindex=[]
#peaksに近いypeaksを探す
for i in range(0,len(peaks)-1):
print("i=",i)
headMotindex.append(peaks[i])
crow=peaks[i]
headMotmax.append(df.loc[crow,"headMot_TMA3"])
df.loc[crow,"headMot_peaks"]=headMotmax[i]
for j in range(0,len(ypeaks)-1):
print("j=",j)
if peaks[i]>ypeaks[j] and peaks[i]<ypeaks[j+1] :
maesa=peaks[i]-ypeaks[j+1]
atosa=ypeaks[j+1]-peaks[i]
if maesa<=atosa:
df.loc[ypeaks[j],"yaw_peakPos"]=100#yawpeak位置マーク
yawmax.append(df.loc[ypeaks[j],"yaw_T"])
yawindex.append(ypeaks[j])
df.loc[crow,"yaw_peaks"]=yawmax[i]#yawpeask値をheadMotpeak値に並べる
else:
df.loc[ypeaks[j+1],"yaw_peakPos"]=100#yawpeak位置マーク
yawmax.append(df.loc[ypeaks[j+1],"yaw_T"])
yawindex.append(ypeaks[j+1])
df.loc[crow,"yaw_peaks"]=yawmax[i]#yawpeask値をheadMotpeak値に並べる
elif peaks[i]==ypeaks[j] or peaks[i]==ypeaks[j+1]:#headMot peakとyaw peakが等しい時
df.loc[ypeaks[j],"yaw_peakPos"]=100
df.loc[crow,"yaw_peaks"]=df.loc[ypeaks[j],"yaw_T"]
yawmax.append(df.loc[ypeaks[j],"yaw_T"])
yawindex.append(ypeaks[j])
print("headMotmax:",headMotmax)
print("headMotindex:",headMotindex)
print("yawmax:",yawmax)
print("yawindex:",yawindex)
#補正値で埋める
df["yawHosei"]=0
df["yasH"]=0
for i in range(0,len(headMotindex)-1):
for j in range(yawindex[i],yawindex[i+1]):
df.loc[j,"yawHosei"]=headMotmax[i]-yawmax[i]
df.loc[j,"yawH"]= df.loc[j,"yawHosei"]+df.loc[j,"yaw_T"]
#print(df)
return df
def getnutral4(df):
npt = [] # Nutral Point index No
npx = [] # Nutral Point relPosE
npy = [] # Nutral Point relPosN
npt_1 = 1
nptc = 0
df=additions4(df)
print
for i in range(1, len(df.index) - 1):
ar = df.loc[i, "roll"]
ar_1 = df.loc[i - 1, "roll"]
if ar * ar_1 < 0 and i - npt_1 > 3:
npt_1 = i
npt.append(i)
npx.append(df.loc[i, "relPosE_B"])
npy.append(df.loc[i, "relPosN_B"])
nptc += 1
print("GetNutral4 Point No=",nptc,"index=",i,"npx=",df.loc[i,"relPosE_B"],"npy=",df.loc[i, "relPosN_B"])
return npt, npx, npy
def Yaw2Rover(dfrtk, dfbno):
# yaw角から1mMovingBase Rover座標に変換
print()
# df_Turn
def turncut(dfdata):
thead = ["height_B", "gSpeed_B", "headMot_B", "pDOP_B", "relPosN_B", "relPosE_B", "relPosD_B", "relPosLength_B",
"Heading360", "headMotMA3", "yaw", "pitch", "roll", "ax", "ay", "az", "H-Ysa360", "skid", "Hfp"]
descr = ["count", "mean", "std", "min", "25%", "50%", "75%", "max"]
r0 = getnutral(dfdata)
npt = r0[0]
npx = r0[1]
npy = r0[2]
desc = []
dft = pd.DataFrame()
dftd = pd.DataFrame()
dftcon = pd.DataFrame()
dftdcon = pd.DataFrame()
for i in range(1, len(npt) - 1):
# print("i=",i,"start=",npt[i],"stop=",npt[i+1])
dft = dfdata.loc[npt[i]:npt[i + 1], thead]
dftd = dft.describe()
dft.insert(0, 'turnNo', i)
dftd.insert(0, 'turnNo', i)
dftd.insert(0, "describe", descr)
# print(dftd)
dftcon = pd.concat([dftcon, dft])
dftdcon = pd.concat([dftdcon, dftd])
# print(dftcon)
# print(dftdcon)
dftcon.to_csv(r'C:/RTK_Log/Turn_' + basename, index=False) # CSV save
dftdcon.to_csv(r'C:/RTK_Log/TurnDesc_' + basename, index=False) # CSV save
def plotvector3(df):#v キーで起動　RBRNファイル読み込み済み
print("-----------------plotvector3----------------------------")
cntj = 1
# dfzero = df.query('diffzero >0')
# zeropoint = dfzero.index
gn = getnutral(df) # Nutral Point 計算
zeropoint = gn[0] # NutralPoint index LIST
npx = gn[1] # NutaralPoint X座標LIST
npy = gn[2] # NutralPoint Y座標LIST
# print("zeropoint=",zeropoint)
# print("npx=",npx)
# print("npy=",npy)
# =============================================
st = ""
while st != 'e':
tn = []
x = []
y = []
xp = []
yp = []
xpr = []
ypr = []
xprd = []
yprd = []
hmt=[]
hdR=[]
lcorr=[]
vmx = []
vmy = []
hmx = []
hmy = []
gsp = []
skid = []
high = []
edge = []
# 描画範囲　NutralPoint 番号指定　開始ー終了
# for i in range(0,len(zeropoint)):
# print("i=",i,"index=",zeropoint[i]
print("範囲1-",len(zeropoint))
st = input("範囲 indexNoを入力してください: Exit=>'e'")
tn = st.split('-')
# print("tn=",tn)
# for i in range(1, len(zeropoint) - 1):
stindex = zeropoint[int(tn[0])] # ターン開始result.index番号
stpindex = zeropoint[int(tn[1])] # ターン終了result.index番号
# print("plot start=",stindex,"stop=",stpindex)
# Turn Slope計算
xd = df.loc[stpindex, "relPosE_B"] - df.loc[stindex, "relPosE_B"]
yd = df.loc[stpindex, "relPosN_B"] - df.loc[stindex, "relPosN_B"]
hd = df.loc[stpindex, "height_B"] - df.loc[stindex, "height_B"]
hd = hd * 0.1
# figタイトル用　平均　標準偏差
# skid 平均　正負にわける
sum0 = 0
sum1 = 0
skid0 = []
skid1 = []
spds = []
spdsa = []
spdsv = []
itowB=[]
for n in range(stindex, stpindex): # 指定範囲の統計値
spds.append(df.loc[n, "gSpeed_B"])
itowB.append(df.loc[n,"iTOW_B"])
c = df.loc[n, "skid"]
if c < 0:
# sum0=sum0+df.loc[n,"skid"]
skid0.append(c)
else:
skid1.append(c)
"""
spdsa = statistics.mean(spds)
spdsa = spdsa * 0.000001 * 3600
spdsastr = "{:.1f}".format(spdsa)
spdsv = statistics.pstdev(spds) * 2
spdsv = spdsv * 0.000001 * 3600
spdsvstr = "{:.1f}".format(spdsv)
skida0 = statistics.mean(skid0)
skida1 = statistics.mean(skid1)
skidv0 = statistics.pstdev(skid0) * 2
skidv1 = statistics.pstdev(skid1) * 2
skidastr0 = "{:.1f}".format(skida0)
skidastr1 = "{:.1f}".format(skida1)
skidvstr0 = "{:.1f}".format(skidv0)
skidvstr1 = "{:.1f}".format(skidv1)
"""
turndist = math.sqrt(xd ** 2 + yd ** 2)
turnslope = math.degrees(math.atan(hd / turndist))
turnslopestr = "{:.1f}".format(turnslope)
rng = stpindex - stindex
# print(stindex, "-", stpindex)
x0 = df.loc[stindex, "relPosE_B"] # Baseスタート位置X
y0 = df.loc[stindex, "relPosN_B"] # Baseスタート位置Y
xr0 = df.loc[stindex, "relPosE_R"] + x0 # Roverスタート位置X
yr0 = df.loc[stindex, "relPosN_R"] + y0 # Roverスタート位置Y
# print("x0=",x0,"y0=",y0)
for j in range(stindex, stpindex):
# print(cntj, j, ":", df.loc[j, "relPosN_B"], df.loc[j, "relPosE_B"])
rPNB = df.loc[j, "relPosE_B"] # Base relE
rPEB = df.loc[j, "relPosN_B"] # Base relN
dpxB = int(rPNB - x0) # Baseターン座標X
dpyB = int(rPEB - y0) # Baseターン座標y
xp.append(dpxB) # Baseプロット座標LIST xp
yp.append(dpyB) # Baseプロット座標LIST yp
# if j%5==0:
rPER = df.loc[j, "relPosE_R"] # Rover relE
rPNR = df.loc[j, "relPosN_R"] # Rover relN
dpxR = rPER + dpxB # Roverターン座標X
dpyR = rPNR + dpyB # Roverターン座標Y
xpr.append(dpxR) # Roverプロット座標　xpr
ypr.append(dpyR) # Roverプロット座標　ypr
hmt.append(df.loc[j,"headMot_B"]*0.00001)
hdR.append(df.loc[j,"Heading360"])
if 'linecorr' in df.columns :
lcorr.append(df.loc[j,"linecorr"])
# print("j=", j, "rPNB,rPEB=", rPNB, rPEB,"rPER,rPNR=",rPER,rPNR, "dpxB,dpyB=", dpxB, dpyB,"dpxR,dpyR=",dpxR,dpyR)
# print("j=",j,"xr,yr",dxr,dyr)
# print("x[",cntj,"]=",x[cntj],"y[",cntj,"]=",y[cntj])
# print(x)
# print(y)
# headMot
vm = arrowmake(df, j)
vmx.append(vm[0])
vmy.append(vm[1])
hmx.append(vm[2])
hmy.append(vm[3])
gsp.append(vm[4])
skid.append(vm[5])
high.append(vm[6])
edge.append(vm[7])
# print("vmx=", vmx, "vmy=", vmy)
# print("hmx=", hmx, "hmy=", hmy)
# print("gsp=", gsp)
# print("skid=", skid)
# print("high=", high)
# print("edge=", edge)
cntj += 1
# Fig plot
fig, ax = plt.subplots(figsize=(12, 10))
tstr = (
"\nTurnSlope=" + turnslopestr + "deg" )
ttl = "TurnNo" + tn[0] + "-" + tn[1] + "indexNo" + str(stindex) + "-" + str(stpindex) + "<" + basename + tstr
fig.suptitle(ttl)
ax.set_aspect('equal')
# dataの最大最小を得る
print("[xp]=", xp)
print("[yp]=", yp)
xpmax = max(xp)
xpmin = min(xp)
ypmax = max(yp)
ypmin = min(yp)
print(xpmax, xpmin, ypmax, ypmin)
xprmax = max(xpr)
xprmin = min(xpr)
yprmax = max(ypr)
yprmin = min(ypr)
print(xprmax, xprmin, yprmax, yprmin)
# max
if xpmax > xprmax:
xmax = xpmax
else:
xmax = xprmax
if ypmax > yprmax:
ymax = ypmax
else:
ymax = yprmax
# min
if xpmin < xprmin:
xmin = xpmin
else:
xmin = xprmin
if ypmin < yprmin:
ymin = ypmin
else:
ymin = yprmin
# ax.set_xlim(xpmin, xpmax)
# ax.set_ylim(ypmin, ypmax)
xmin=int(xmin*1.2)
xmax=int(xmax*1.2)
ymin=int(ymin*1.2)
ymax=int(ymax*1.2)
print(xmin, xmax, ymin, ymax)
v = [xmin, xmax, ymin, ymax]
ax.axis(v)
# ax.scatter(xp, yp, s=40, c="black")
ax.plot(xp, yp,color='red')
ax.plot(xpr, ypr,color='green')
# 描画範囲を取得する。
# xmin, xmax = ax.get_xlim()
# ymin, ymax = ax.get_ylim()
# print(f"x: [{xmin}, {max}], y: [{ymin}, {ymax}]")
ax.plot([xp, xpr], [yp, ypr], color="black")
# ax.quiver(x, y, vmx, vmy, color="red", angles='xy', scale_units='xy', scale=1, headwidth=2, headlength=1.3,
# headaxislength=1.3)
# ax.quiver(x, y, hmx, hmy, color="blue", angles='xy', scale_units='xy', scale=1, headwidth=2, headlength=1.3,
# headaxislength=1.3)
# 指定した位置に注釈入れる
if stpindex - stindex < 500:
for k in range(1, rng):
if skid[k] > 0:
xt = xp[k]
yt = yp[k]
elif skid[k] <= 0:
xt = xp[k]
yt = yp[k]
# gspstr=str(gsp[k])
gspstr = "{:.1f}".format(gsp[k])
# gspstr=gspstr+"kmh"
skidstr = "{:.1f}".format(skid[k])
highstr = "{:.2f}".format(high[k] - high[0])
edgestr = "{:.1f}".format(edge[k])
itowBstr="{:.0f}".format(itowB[k])
hmtstr="{:.0f}".format(hmt[k])
hdRstr="{:.0f}".format(hdR[k])
# linecorrがある場合
if 'linecorr' in df.columns :
lcorrstr="{:.4f}".format(lcorr[k])
else:
lcorrstr=""
txtstr = "[itow"+itowBstr+"]\nspd" + gspstr + "kmh" + "\nskid" + skidstr + "deg"+"hmt"+hmtstr+"-hdR"+hdRstr+"deg" + "\nDrop" + highstr + "m" + "\nEdge" + edgestr+"deg,lcorr"+lcorrstr
#ax.text(xt, yt, txtstr, fontsize="x-small")
ax.annotate(txtstr, xy=(xt, yt),xytext=(xt+30,yt), size=6,color="black", arrowprops=dict())
# matplotlib.pyplot.text(x, y, s, fontdict=None, withdash= < deprecatedparameter >, ** kwargs)
plt.show()
input("ANY key return")
#-BootsOn RTKプロットDFをresultBRBNへ追加
def RTK2BRBN(df0,df1):
print(df0)
print(df1)
#df0 resultBRBNとdf1 df_BTplotの先頭itowと長さを獲る
startitowBR=int(df0.loc[0,"iTOW_B"])
startitowBT=int(df1.loc[0,"iTOW_T"])
btmod=startitowBT % 120
startitowBT=startitowBT+btmod
BRlen=len(df0)
BTlen=len(df1)
isa=startitowBR-startitowBT
list_BTplot=[]
if isa<=0:#BRBNが先にログ開始した場合右RTKが遅れる
brindxl= list(df0.reset_index().query('iTOW_B == @startitowBT').index)
brindx=brindxl[0]
print(brindx)
btcoln=len(df1.columns)
list_BTplot = [['0']*btcoln] * brindx # 上のデータゼロ埋めてlist_BTplot作成
print(list_BTplot)
btlen = len(df1)
for i in range(0, btlen - 1): # 中間にdf_BTplotをlistに追加
r = df1.iloc[i, :]
list_BTplot.append(r)#df1をlistにコピー
sn = brindx + len(df1)
rn = len(df0)
print("Upper:list_BTplot=", len(list_BTplot))
print("for sn=", sn, "rn=", rn, "result len=", len(df0))
for i in range(sn, rn + 1): # 下のデータ埋め
l0 = [['0']*btcoln]
list_BTplot.append(l0)
print("list_BTplot=", len(list_BTplot))
df2 = pd.DataFrame(list_BTplot, columns=df1.columns)
print("df2=",df2)
else:#右RTKが先にログ開始した場合右BRBNが遅れる
btindx = df1[df1['iTOW_T'] == startitowBR].index
print("=============Repair UDP File Shorten:startitowBR=",startitowBR,"startitowBT=",startitowBT)
print(startitowBR,startitowBT,"BR-BT=",startitowBR-startitowBT,BRlen,BTlen)
#print("Okure df2=",df2)
# resultBRBNとdf2をconcate
df3= pd.concat([df0, df2], axis=1)
print(df3)
return df3
# |||||||||||||||||||||||||||||||||readMBFIle END|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
# [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[def main]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]
def itow120df(df): # dfは、先頭がiTOW
itow120list = []
for i in range(1, len(df) - 1):
mod = int(df.iloc[i, 0]) % 120
if mod < 10:
itow120list.append(df.iloc[i, :])
print("itow120df_Check:",i,">bnitow=",df.iloc[i,0])
#print(itow120list)
df_itow120 = pd.DataFrame(itow120list)
#print("df_itwo120=",df_itow120)
return df_itow120
def bnitow2100(df):
itow100list = []
for i in range(1, len(df) - 1):
mod = int(df.iloc[i, 0]) % 100
if mod < 10 or mod>90:
itow100list.append(df.iloc[i, :])
#print("itow100df_Check:",i,">bnitow=",df.iloc[i,0])
#print(itow100list)
df_itow100 = pd.DataFrame(itow100list)
#print("df_itwo120=",df_itow120)
return df_itow100
def bnitowsort(df):#bno dataの時間抜けを無視してデータ順を尊重して最初基準で10msecインクリメントで統一しなおす
firstbnitow = 0
i = 0
dflen=len(df)
df=df.reset_index(drop=True)#indexをリセットして欠番をなくしてからソート
while i<dflen-1 and firstbnitow<10000:#カウント開始itow行の探索
if int(df.loc[i, "bnitow"]) < 100000 :
print("DROP i=",i,"bnioitow=",df.loc[i, "bnitow"])
df=df.drop(index=i)
else:
firstbnitow = int(df.loc[i, "bnitow"])
i += 1
#dflen=len(df)
#if i%1000==0:
print("bnoirowsort_i=",i)
#firstbnitowから10stepで埋める
#print(df.head(2))
df=df.reset_index(drop=True)
df.to_csv(r'C:/RTK_Log/bnitow10_dropped_' + basename) # CSV save itow OK
#df["bnitow"].apply(lambda x:x+10)
#df['bnitow'] = range(1, len(df.index) + 1)
for i in range(1, len(df)):#頭出しitowから10msecインクリメント
df.loc[i, "bnitow"] = firstbnitow + 10 * i
#print("bnosorted bnitow[",i,"]=",df.loc[i,"bnitow"])
#print(df)
print("--------bnitow File sorted---------dflen=",len(df))
df.to_csv(r'C:/RTK_Log/bnitow10_sorted' + basename) # CSV save
return df
def bnitowRe(df): # bnitowの異常値を修正
# 0,1,2行ゼロ処理
czeroflag = 0
for i in range(0, len(df) - 1):
if int(df.loc[i, "bnitow"]) == 0:
j = i
while int(df.loc[j, "bnitow"]) == 0:
print("==0:j=", j, int(df.loc[j, "bnitow"]))
j += 1
for k in range(j, i, -1): # j+1のbnitow値から-10decriment
df.loc[k - 1, "bnitow"] = int(df.loc[k, "bnitow"]) - 10
print("==0", df.loc[k - 1, "bnitow"])
elif int(df.loc[i, "bnitow"]) > 10:
j = i
while int(df.loc[j, "bnitow"]) > 10:
print(">10:j=", j, int(df.loc[j, "bnitow"]))
j += 1
for k in range(j, i, -1): # j+1のbnitow値から-10decriment
df.loc[k - 1, "bnitow"] = int(df.loc[k, "bnitow"]) - 10
print(">10:", df.loc[k - 1, "bnitow"])
print("def Repaired=", df)
return df
def gSpeedStart(df):
print("---Search gSpeed Start point index")
gSpeed100=[]
gSpeed100= df["gSpeed_B"].rolling(100).mean()
print(gSpeed100)
return gSpeed100
def readBRBNTnutral(df):#headMot_peaksでNutralPointを読んでindex をlistで返す
npt = [] # Nutral Point index No
#gSpeedStart(df)
for i in range(0, len(df.index) - 1):
if df.loc[i,"headMot_peaks"]!=0:
print(i,"=================:headMot_peaks=",df.loc[i,"headMot_peaks"])
npt.append(i)
#else:
#print(i,":headMot_peaks=",df.loc[i,"headMot_peaks"])
npt[0]=npt[1]-500
return npt
def comnt(df,sti,stpi):
print()
BRstrs=[]#BR戻しLIST
BTstrs=[]#BT戻しLIST
for j in range(sti, stpi):
hmotdeg = df.loc[j, "headMot360"]
hmotdegT= df.loc[j, "headMotT360"]
headdeg = df.loc[j, "Heading360"]
headdegT = df.loc[j, "yaw_T"]
gsp = df.loc[j, "gSpeed_B"] * 0.001 * 3600 * 0.001
gspT = df.loc[j, "gSpeed_T"] * 0.001 * 3600 * 0.001
skid = hmotdeg - headdeg
skidT = hmotdegT - headdegT
high = df.loc[j, "height_B"] * 0.001
high0= df.loc[0, "height_B"] * 0.001
highT = df.loc[j, "height_T"] * 0.001
high0T = df.loc[0, "height_T"] * 0.001
edge = df.loc[j, "roll"]
edgeT = df.loc[j, "roll_T"]
itowB=df.loc[j,"iTOW_B"]
itowT=df.loc[j,"bnitow_T"]#実時間はbnitow　iTOW_Tは間引きされてズレてる
#print("itowB=",itowB,"itowT=",itowT)
gspstr = "{:.1f}".format(gsp)
skidstr = "{:.1f}".format(skid)
highstr = "{:.2f}".format(high - high0)
edgestr = "{:.1f}".format(edge)
itowBstr=str(itowB)#"{:.0f}".format(itowB)
gspstrT = "{:.1f}".format(gsp)
skidstrT = "{:.1f}".format(skidT)
highstrT = "{:.2f}".format(highT - high0T)
edgestrT = "{:.1f}".format(edgeT)
itowBstrT=str(itowT)#"{:.0f}".format(itowT)
timestr=str(math.floor(df.loc[j,"min_B"]))+":"+str(math.floor(df.loc[j,"sec_B"]))+"."+str(math.floor(df.loc[j,"iTOW_B"]%1000))
BRstr = timestr+"["+itowBstr+"]\nspd" + gspstr + "kmh" + "\nskid" + skidstr +"deg" + "\nDrop" + highstr + "m" + "\nEdge" + edgestr
BTstr = timestr+"["+itowBstrT+"]\nspdT" + gspstrT + "kmh" + "\nskidT" + skidstrT +"deg" + "\nDropT" + highstrT + "m" + "\nEdgeT" + edgestrT
BRstrs.append(BRstr)
BTstrs.append(BTstr)
return BRstrs,BTstrs
# [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[def main END ]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]
# ********************************************RL PLOT plotvector4(df_BRBNT)**********************************************************
# ************************************* df_BRBNTをわたされてRL描画*************************************************
def plotvector4(df):
print("-------------------plotvector4-------------------------------")
#df to float64
#print(df.dtypes)
#cols=df.columns
#print("cols=",cols)
#df[cols]=df[cols].astype("float64")
pd.set_option('display.max_rows', 200)
print(df.dtypes)
cntj = 1
# dfzero = df.query('diffzero >0')
# zeropoint = dfzero.index
#if bnOK==0:
#gn = getnutral4(df) # Nutral Point 計算
zeropoint=readBRBNTnutral(df)#BRBNTの"headMotpeaks_zero"がゼロになるポイントのINDEXをzeropointに収納
#zeropoint = gn[0] # NutralPoint index ゼロ番
#npx = gn[1] # NutaralPoint X座標LIST
#npy = gn[2] # NutralPoint Y座標LIST
print("zeropoint=",zeropoint)
# print("npx=",npx)
# print("npy=",npy)
# =============================================
st = ""
while st != 'e':
tn = []
x = []
y = []
xp = []
xpbt=[]
yp = []
ypbt=[]
xpr = []
xprbt=[]
ypr = []
yprbt=[]
xprd = []
xprdbt=[]
yprd = []
yprdbt=[]
vmx = []
vmy = []
hmx = []
hmy = []
gsp = []
gspbt=[]
skid = []
skidbt=[]
high = []
edge = []
edgebt=[]
lcorr=[]
# 描画範囲　NutralPoint 番号指定　開始ー終了
# for i in range(0,len(zeropoint)):
# print("i=",i,"index=",zeropoint[i])
print("範囲1-",len(zeropoint))
st = input(" indexNoを入力してください: Exit=>'e'")
tn = st.split('-')
# print("tn=",tn)
# for i in range(1, len(zeropoint) - 1):
stindex = zeropoint[int(tn[0])] # ターン開始result.index番号
stpindex = zeropoint[int(tn[1])] # ターン終了result.index番号
print("plot start=",stindex,"stop=",stpindex)
# Turn Slope計算
xd = df.loc[stpindex, "relPosE_B"] - df.loc[stindex, "relPosE_B"]
yd = df.loc[stpindex, "relPosN_B"] - df.loc[stindex, "relPosN_B"]
hd = df.loc[stpindex, "height_B"] - df.loc[stindex, "height_B"]
hd = hd * 0.1
# figタイトル用　平均　標準偏差
# skid 平均　正負にわける
sum0 = 0
sum1 = 0
skid0 = []
skid1 = []
spds = []
spdsa = []
spdsv = []
itowB=[]
for n in range(stindex, stpindex): # 指定範囲の統計値
spds.append(df.loc[n, "gSpeed_B"])
c = df.loc[n, "skid"]
if c < 0:
# sum0=sum0+df.loc[n,"skid"]
skid0.append(c)
else:
skid1.append(c)
spdsa = statistics.mean(spds)
spdsa = spdsa * 0.000001 * 3600
spdsastr = "{:.1f}".format(spdsa)
spdsv = statistics.pstdev(spds) * 2
spdsv = spdsv * 0.000001 * 3600
spdsvstr = "{:.1f}".format(spdsv)
#skida0 = statistics.mean(skid0)
#skida1 = statistics.mean(skid1)
#skidv0 = statistics.pstdev(skid0) * 2
#skidv1 = statistics.pstdev(skid1) * 2
#skidastr0 = "{:.1f}".format(skida0)
#skidastr1 = "{:.1f}".format(skida1)
#skidvstr0 = "{:.1f}".format(skidv0)
#skidvstr1 = "{:.1f}".format(skidv1)
turndist = math.sqrt(xd ** 2 + yd ** 2)
turnslope = math.degrees(math.atan(hd / turndist))
turnslopestr = "{:.1f}".format(turnslope)
rng = stpindex - stindex
# print(stindex, "-", stpindex)
x0 = df.loc[stindex, "relPosE_B"] # Baseスタート位置X
y0 = df.loc[stindex, "relPosN_B"] # Baseスタート位置Y
xr0 = df.loc[stindex, "relPosE_R"] + x0 # Roverスタート位置X
yr0 = df.loc[stindex, "relPosN_R"] + y0 # Roverスタート位置Y
#BT座標 BASE:xbtb,ybtb ROVER:xbtr ybtr
print("x0=",x0,"y0=",y0)
for j in range(stindex, stpindex):
itowB.append(df.loc[j,"iTOW_B"])
#----------SkiMB座標-------------------
# print(cntj, j, ":", df.loc[j, "relPosN_B"], df.loc[j, "relPosE_B"])
rPEB = df.loc[j, "relPosE_B"] # Base relE　基線座標
rPNB = df.loc[j, "relPosN_B"] # Base relN　基線座標
dpxB = int(rPEB - x0) # Baseターンプロット座標X
dpyB = int(rPNB - y0) # Baseターンプロット座標y
xp.append(dpxB) # Baseプロット座標LIST xp
yp.append(dpyB) # Baseプロット座標LIST yp
# if j%5==0:
rPER = df.loc[j, "relPosE_R"] # Rover relE　MB座標
rPNR = df.loc[j, "relPosN_R"] # Rover relN　MB座標
dpxR = rPER + dpxB # Roverターンプロット座標X
dpyR = rPNR + dpyB # Roverターンプロット座標Y
xpr.append(dpxR) # Roverプロット座標LIST　xpr
ypr.append(dpyR) # Roverプロット座標LIST　ypr
#-----------BT座標------------------------
rPEB_BT=df.loc[j,"rpEB_BT"] #BT Base Y　基線座標
rPNB_BT=df.loc[j,"rpNB_BT"] #BT Base X　基線座標
dpxB_BT=int(rPEB_BT-x0)#Base　プロット座標X　原点x0はSki　Base（前）
dpyB_BT=int(rPNB_BT-y0)#Base プロット座標Y　原点y0はSki　Base（前）
xpbt.append(dpxB_BT) # Baseプロット座標LIST xpbt
ypbt.append(dpyB_BT) # Baseプロット座標LIST ypbtr
#rover BT
rPER_BT=df.loc[j,"rpER_BT"] #BT Rover X　基線座標
rPNR_BT=df.loc[j,"rpNR_BT"] #BT Rover Y　基線座標
dpxR_BT=int(rPER_BT-x0)#Rover　プロット座標X　原点x0はSki　Base（前）
dpyR_BT=int(rPNR_BT-y0)#Rover　プロット座標X　原点x0はSki　Base（前）
xprbt.append(dpxR_BT)# Roverプロット座標LIST xprbt
yprbt.append(dpyR_BT) # Roverプロット座標LIST ypbt
#print(df.loc[j,"iTOW_B"],"j=", j, "rPNB,rPEB=", rPNB, rPEB,"rPER,rPNR=",rPER,rPNR, "dpxB,dpyB=", dpxB, dpyB,"dpxR,dpyR=",dpxR,dpyR)
#print(df.loc[j,"iTOW_B"],"j=", j, "rPNB_BT,rPEB_BT=", rPNB_BT, rPEB_BT,"rPER_BT,rPNR_BT=",rPER_BT,rPNR_BT, "dpxB_BT,dpyB_BT=", dpxB_BT, dpyB_BT,"dpxR_BT,dpyR_BT=",dpxR_BT,dpyR_BT)
# print("j=",j,"xr,yr",dxr,dyr)
# print("x[",cntj,"]=",x[cntj],"y[",cntj,"]=",y[cntj])
# print(x)
# print(y)
# headMot
vm = arrowmake(df, j)
vmx.append(vm[0])
vmy.append(vm[1])
hmx.append(vm[2])
hmy.append(vm[3])
gsp.append(vm[4])
skid.append(vm[5])
high.append(vm[6])
edge.append(vm[7])
# print("vmx=", vmx, "vmy=", vmy)
# print("hmx=", hmx, "hmy=", hmy)
# print("gsp=", gsp)
# print("skid=", skid)
# print("high=", high)
# print("edge=", edge)
cntj += 1
# Fig plot
fig, ax = plt.subplots(figsize=(12, 10))
tstr = (
"\nTurnSlope=" + turnslopestr + "deg" + ",AvSpd=" + spdsastr + "±" + spdsvstr + "kmh" )
ttl = "TurnNo" + tn[0] + "-" + tn[1] + "indexNo" + str(stindex) + "-" + str(stpindex) + "<" + basename + tstr
fig.suptitle(ttl)
ax.set_aspect('equal')
# dataの最大最小を得る
#print("[xp]=", xp)
#print("[yp]=", yp)
xpmax = max(xp)
xpmin = min(xp)
ypmax = max(yp)
ypmin = min(yp)
#print(xpmax, xpmin, ypmax, ypmin)
xprmax = max(xpr)
xprmin = min(xpr)
yprmax = max(ypr)
yprmin = min(ypr)
#print(xprmax, xprmin, yprmax, yprmin)
# max
if xpmax > xprmax:
xmax = xpmax
else:
xmax = xprmax
if ypmax > yprmax:
ymax = ypmax
else:
ymax = yprmax
# min
if xpmin < xprmin:
xmin = xpmin
else:
xmin = xprmin
if ypmin < yprmin:
ymin = ypmin
else:
ymin = yprmin
# ax.set_xlim(xpmin, xpmax)
# ax.set_ylim(ypmin, ypmax)
print("xmin,xmax,ymin,ymax=",xmin, xmax, ymin, ymax)
m=100
if tn[0]==0:
m=500
v = [xmin-m, xmax+m, ymin-m, ymax+m]
ax.axis(v)
# ax.scatter(xp, yp, s=40, c="black")
ax.plot(xp, yp,color='red')
ax.plot(xpr, ypr,color='green')
ax.plot(xpbt, ypbt,color='magenta')
ax.plot(xprbt, yprbt,color='aqua')
# 描画範囲を取得する。
# xmin, xmax = ax.get_xlim()
# ymin, ymax = ax.get_ylim()
# print(f"x: [{xmin}, {max}], y: [{ymin}, {ymax}]")
c=0
for j in range(stindex, stpindex-1):
if df.loc[j,"iTOW_B"]%120==0 and df.loc[j,"iTOW_B"]%600>0:
ax.plot([xp[c], xpr[c]], [yp[c], ypr[c]], color="black", linewidth=1)
ax.plot([xpbt[c], xprbt[c]], [ypbt[c], yprbt[c]], color="blue",linewidth=1)
if df.loc[j,"iTOW_B"]%600==0:
#ax.plot([xpbt[c-1], xprbt[c-1]], [ypbt[c-1], yprbt[c-1]], color="blue"r,linewidth=3)#1個前にずれてるので１個減らした
ax.plot([xpbt[c], xprbt[c]], [ypbt[c], yprbt[c]], color="orangered",linewidth=3)
ax.plot([xp[c], xpr[c]], [yp[c], ypr[c]], color="orangered", linewidth=3)
#print("itowB[",c,"]=",itowB[c],"xpbt[",c,"]=",xpbt[c],"ypbt=",ypbt[c],"xprbt[",c,"]=",xprbt[c],"yprbt=",yprbt[c])
#print("itowB[",c,"]=",itowB[c],"]xp[",c,"]=",xp[c],"yp=",yp[c],"xpr[",c,"]=",xpr[c],"ypr=",ypr[c])
c+=1
# 指定した位置に注釈入れる
if stpindex - stindex < 2000:
for k in range(0, rng):
print(k)
if skid[k] > 0:
xt = xp[k]
yt = yp[k]
elif skid[k] <= 0:
xt = xp[k]
yt = yp[k]
xtT=xpbt[k]
ytT=ypbt[k]
# gspstr=str(gsp[k])
gspstr = "{:.1f}".format(gsp[k])
# gspstr=gspstr+"kmh"
skidstr = "{:.1f}".format(skid[k])
highstr = "{:.2f}".format(high[k] - high[0])
edgestr = "{:.1f}".format(edge[k])
itowBstr="{:.0f}".format(itowB[k])
hmtstr="0"#"{:.0f}".format(hmt[k])
hdRstr="0"#"{:.0f}".format(hdR[k])
#timestr=str(math.floor(df.loc[k,"min_B"]))+":"+str(math.floor(df.loc[k,"sec_B"]))+"."+str(math.floor(df.loc[k,"iTOW_B"]%1000))
#print("timestr=",timestr,"iTOW_B=",df.loc[k,"iTOW_B"])
# linecorrがある場合
if 'linecorr' not in df.columns :
lcorrstr="0"#"{:.4f}".format(lcorr[k])
rtn= comnt(df,stindex,stpindex)
#df_rtn=pd.DataFrame(rtn)
#df_rtn.to_csv(r'C:/RTK_Log/comntdf_' + basename) # CSV save
BRstr=[]
BRstr=rtn[0]
#print("BRstr[",k,"]=",BRstr[k])
BTstr=[]
BTstr=rtn[1]
#print("BTstr[",k,"]=",BTstr[k])
if(itowB[k]%600==0):
ax.annotate(BRstr[k], xy=(xt, yt),xytext=(xt+50,yt), size=6,color="black", arrowprops=dict(width=1,headwidth=1))#MB
ax.annotate(BTstr[k], xy=(xtT, ytT),xytext=(xtT-140,ytT), size=6,color="blue", arrowprops=dict(width=1,headwidth=1))#Boots 右
#print("itowB[",k,"]=",itowB[k],"xtT[",k,"]=",xtT,",ytT=",ytT)
plt.show()
input("ANY key return")
def sqrtv(x,y):
v=math.sqrt(x**2+y**2)
return v
#*******************BNO10msec加速度ベクトルを２回積分で距離をだす初期値はgSpeedを与える************
def BTlinereg(df,m):#mは奇数
print("---pandas Line Regression")
print("-----BTBNOlincorr計算---")
df["linecorr_T"]=0
#直線の判定　M点の直線回帰で最大振れ値で規定
#回帰奇数個mをセンター振り分け
n=int(m/2)#５個なら２個振り分け
count=0
for i in range(m,len(df)-m):
x=[]
y=[]
for j in range(0,m-1):
x.append(df.loc[j+i,"relPosE_T"])
y.append(df.loc[j+i,"relPosN_T"])
# データを用意
# リストをps.Seriesに変換
X=pd.Series(x)
Y=pd.Series(y)
# pandasを使用してPearson's rを計算
res=X.corr(Y) # numpy.float64 に格納される
df.loc[i+n,"linecorr_T"]=res#i+n番目に代入５個なら i,i+1,[i+2],i+3,i+4
count+=1
return count
#BRBNファイルとBNOファイルを読み込む
def bnointegral():
print("**************************BNO Integral Choose BNOFile & BRBN FILE*************")
fok=False
bnof=False
brf=False
while fok==False:
root = tkinter.Tk()
root.withdraw()
iDir = r'C:/RTK_Log/'
fTyp = [("データファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
# filenameからbasenameをとりだしてファイル種類区別
basename = os.path.basename(filename)
if "BRBN" not in basename and "BNO" in basename:#bno単独データファイル読み込み
print("************************bno File read****************************")
bnof=True
basename_BNO=basename
list_BNO=[]
head1 = list_BNO
BNOflag = True
#df_BNO = pd.read_csv(filename, names=head1, low_memory=True)
df_BNO = pd.read_csv(filename, low_memory=True)
print("BNO File read=",filename)
BNOitow = int(df_BNO.loc[0, "bnitow"])
print("BNOitow=",BNOitow)
elif "BRBN" in basename:
brf=True
basename_BRBN=basename
brbnOK=1#BRBNファイルでSKI MBプロットvectorplot3
bnOK=0
df_BRBN = pd.read_csv(filename, low_memory=True)
print("df_BRBN read",filename)
BRBNitow = int(df_BRBN.iloc[0, 1])
print("BRBNitow=", BRBNitow)
BRBNflag=True
fok=bnof and brf
print("READ:BNO File=",bnof,"BRBN FIle=",brf)
dflen=len(df_BNO)
print("df_BNO=",dflen,":[",df_BNO.loc[0,"bnitow"],"-",df_BNO.loc[dflen-1,"bnitow"],"]",basename_BNO)
dflen=len(df_BRBN)
print("df_BRBN=",dflen,":[",df_BRBN.loc[0,"iTOW_B"],"-",df_BRBN.loc[dflen-1,"iTOW_B"],"]",basename_BRBN)
pd.set_option('display.max_columns', 40)
#積分準備　df_BNOに積分に必要なデータをdf_BRBNから追加する
#[1]itow_Bの積分範囲 [2]
#積分用データ列追加
df_BNO["vx"]=0#x速度成分　加速度1回積分
df_BNO["vy"]=0#y速度成分　加速度1回積分
df_BNO["dx"]=0#x距離成分　加速度2回積分
df_BNO["dy"]=0#y距離成分　加速度2回積分
df_BNO["Va"]=0#xy加速度ベクトル
df_BNO["Vv"]=0#xy速度ベクトル 1回積分
df_BNO["Vd"]=0#xy距離ベクトル 2回積分
#Vaをax ayから計算代入df['D'] = df.apply(lambda row: row['A'] + row['B']
df_BNO['Va'] = df_BNO.apply(lambda row:math.sqrt(row['ax']**2+row['ay']**2), axis=1)
#print(df_BNO['Va'])
#BRBNデータ列追加
df_BNO["RTKdist"]=0#１周期での移動距離
df_BNO["HeadingSa"]=0
df_BNO["gSpeed_B"]=0
df_BNO["relPosN_B"]=0
df_BNO["relPosE_B"]=0
df_BNO["headMot_B"]=0
df_BNO["relPosHeading_R"]=0
df_BNO["flags_B"]=0
df_BNO["diffzero"]=0
df_BNO["skid"]=0
df_BNO["linecorr"]=0
cols=["vx","vy","dx","dy","Va","Vv","Vd","RTKdist","HeadingSa","gSpeed_B","relPosN_B","relPosE_B","headMot_B","relPosHeading_R","flags_B","diffzero","skid","linecorr"]
df_BNO[cols]=df_BNO[cols].astype("float64")
#print(df_BNO.dtypes)
#BNO%120==0とBRGNOのスタート地点探す
firstflag=False
bnindex120=[]#bnitow==120のindex list
bnindex=[]
df_BNO = df_BNO.round({'bnitow': -1})
#for i in range(0,len(df_BNO)):
startflag=False
i=0#bnoindex
while startflag==False:
for j in range(0,len(df_BRBN)):
BRBNitow=int(df_BRBN.loc[j,"iTOW_B"])
if BRBNitow==df_BNO.loc[i,"bnitow"]:
startBNOindex=i
startBNOitow=df_BNO.loc[i,"bnitow"]
startBRBNindex=j
startBRBNitow=BRBNitow
startflag=True
i+=1
print("Start BNO:",startBNOindex,startBNOitow,"start BRBN:",startBRBNindex,startBRBNitow)
k=0
for i in range(startBRBNindex,len(df_BRBN)-1):
brbnitow=int(df_BRBN.loc[i,"iTOW_B"])
j=startBNOindex+12*k
bnitow=df_BNO.loc[j,"bnitow"]
df_BNO.loc[j,"iTOW_B"]=df_BRBN.loc[i,"iTOW_B"]
df_BNO.loc[j,"gSpeed_B"]=df_BRBN.loc[i,"gSpeed_B"]
df_BNO.loc[j,"relPosN_B"]=df_BRBN.loc[i,"relPosN_B"]
df_BNO.loc[j,"relPosE_B"]=df_BRBN.loc[i,"relPosE_B"]
df_BNO.loc[j,"headMot_B"]=df_BRBN.loc[i,"headMot_B"]*0.00001
#df_BNO.loc[j,"headMot_B"]=df_BRBN.loc[i,"headMot_B"]
df_BNO.loc[j,"relPosHeading_R"]=df_BRBN.loc[i,"relPosHeading_R"]*0.00001
df_BNO.loc[j,"flags_B"]=df_BRBN.loc[i,"flags_B"]
df_BNO.loc[j,"flags_R"]=df_BRBN.loc[i,"flags_R"]
df_BNO.loc[j,"diffzero"]=df_BRBN.loc[i,"diffzero"]
df_BNO.loc[j,"skid"]=df_BRBN.loc[i,"skid"]
df_BNO.loc[j,"linecorr"]=df_BRBN.loc[i,"linecorr"]
#relPosN-E変位 RTKdist
ndist=df_BRBN.loc[i,"relPosN_B"]-df_BRBN.loc[i-1,"relPosN_B"]
edist=df_BRBN.loc[i,"relPosE_B"]-df_BRBN.loc[i-1,"relPosE_B"]
df_BNO.loc[j-1,"RTKdist"]=math.sqrt(ndist**2+edist**2)*0.01
#relPosHeading差
head=df_BRBN.loc[i,"relPosHeading_R"]
head_1=df_BRBN.loc[i-1,"relPosHeading_R"]
df_BNO.loc[j-1,"HeadingSa"]=(head-head_1)*0.00001
#Vv初期値
df_BNO.loc[j,"Vv"]=df_BRBN.loc[i,"gSpeed_B"] *0.001
#Vv初期値
k+=1
if k>=2:
#----積分用RTK初期値　終端値
vinit=df_BNO.loc[j-12,"gSpeed_B"]*0.001
vend=df_BNO.loc[j,"gSpeed_B"]*0.001
hminit=df_BNO.loc[j-12,"headMot_B"]
hmend=df_BNO.loc[j,"headMot_B"]
rphinit=df_BNO.loc[j-12,"relPosHeading_R"]
rphend=df_BNO.loc[j,"relPosHeading_R"]
#積分範囲
si=j-12
ei=j
for n in range(1,12):#積分ループ
ax=df_BNO.loc[si+n,"ax"]
ax_1=df_BNO.loc[si+n-1,"ax"]
ay=df_BNO.loc[si+n,"ay"]
ay_1=df_BNO.loc[si+n-1,"ay"]
df_BNO.loc[si+n,"vx"]=(ax+ax_1)/2*0.01+df_BNO.loc[si+n-1,'vx']
df_BNO.loc[si+n,"vy"]=(ay+ay_1)/2*0.01+df_BNO.loc[si+n-1,'vy']
vx=df_BNO.loc[si+n,"vx"]
vx_1=df_BNO.loc[si+n-1,"vx"]
vy=df_BNO.loc[si+n,"vy"]
vy_1=df_BNO.loc[si+n-1,"vy"]
df_BNO.loc[si+n,"dx"]=(vx+vx_1)/2*0.01+df_BNO.loc[si+n-1,'vx']
df_BNO.loc[si+n,"dy"]=(vy+vy_1)/2*0.01+df_BNO.loc[si+n-1,'vy']
#Vector積分
va=df_BNO.loc[si+n,"Va"]
va_1=df_BNO.loc[si+n-1,"Va"]
df_BNO.loc[si+n,"Vv"]=(va+va_1)/2*0.01+df_BNO.loc[si+n-1,'Vv']
vv=df_BNO.loc[si+n,"Vv"]
vv_1=df_BNO.loc[si+n-1,"Vv"]
df_BNO.loc[si+n,"Vd"]=(vv+vv_1)/2*0.01+df_BNO.loc[si+n-1,'Vd']
df_BNO.to_csv(r'C:/RTK_Log/IntBNO_' + basename) # CSV save
#直線部検出して"linecorrに各行相関係数記録　BR用
def linereg(df,m):#mは奇数
print("---pandas Line Regression")
df["linecorr"]=0
#直線の判定　M点の直線回帰で最大振れ値で規定
#回帰奇数個mをセンター振り分け
n=int(m/2)#５個なら２個振り分け
count=0
for i in range(m,len(df)-m):
x=[]
y=[]
for j in range(0,m-1):
x.append(df.loc[j+i,"relPosE_B"])
y.append(df.loc[j+i,"relPosN_B"])
# データを用意
# リストをps.Seriesに変換
X=pd.Series(x)
Y=pd.Series(y)
# pandasを使用してPearson's rを計算
res=X.corr(Y) # numpy.float64 に格納される
df.loc[i+n,"linecorr"]=res#i+n番目に代入５個なら i,i+1,[i+2],i+3,i+4
count+=1
return count
#直線部検出して"linecorrに各行相関係数記録 BT用
def lineregBT(df,m):#mは奇数 dfは
print("---pandas Line Regression")
df["slope_T"]=0
df["intercept_T"]=0
#直線の判定　M点の直線回帰で最大振れ値で規定
#回帰奇数個mをセンター振り分け
n=int(m/2)#５個なら２個振り分け
count=0
for i in range(m,len(df)-m):
x=[]
y=[]
for j in range(0,m-1):
x.append(df.loc[j+i,"relPosE_T"])
y.append(df.loc[j+i,"relPosN_T"])
# データを用意
#Xn=np.array(x)
#Yn=np.array(y)
#Xr=Xn.reshape(-1, 1)
#Yr=Yn.reshape(-1, 1)
#reg = linear_model.LinearRegression()
# sklearnを使う時は、numpy形式に変換する
# 回帰分析を実行
#reg.fit(Xr, Yr)
# 結果の出力
#print('回帰係数：', reg.coef_)
#print('切片：', reg.intercept_)
# print('決定係数：', reg.score(x, y))
# リストをps.Seriesに変換
X=pd.Series(x)
Y=pd.Series(y)
#a = np.dot(Xn, Yn)/ (Xn ** 2).sum()
#df.loc[i,"slope_T"]=reg.coef_
#df.loc[i,"intercept_T"]=reg[1]
# pandasを使用してPearson's rを計算
res=X.corr(Y) # numpy.float64 に格納される
df.loc[i+n,"linecorr_T"]=res#i+n番目に代入５個なら i,i+1,[i+2],i+3,i+4
count+=1
return df
def BTyawhosei(df):#ver0.41で0.999以上の範囲を平均して補正値とする機能に変更
print("BT yaw hosei作成")
yhv=0
df["yawHosei_T"]=0
df["yawHosei_T"]=df["yawHosei_T"].astype("float64")
df["yawH_T"]=0
df["yawH_T"]=df["yaw_T"].astype("float64")
df["LHoseindx_T"]=0
df["LHoseiindx_T"]=df["yaw_T"].astype("float64")
#>0.999範囲取得
df_LHosei=pd.DataFrame()
df_LHosei["linecorr_T"]=0
df_LHosei["linindex_T"]=0
df_LHosei["headMot_T"]=0
df_LHosei["yaw_T"]=0
df_LHosei["renban_T"]=0
df_LHosei["hmotave_T"]=0
df_LHosei["yawave_T"]=0
df_LHosei["hoseiti_T"]=0
df_LHosei["rencount_T"]=0
df_LHosei["hoseiIndex_T"]=0
df_LHosei["hoseistarti_T"]=0
df_LHosei["hoseistopi_T"]=0
cols=df_LHosei.columns
df_LHosei[cols]=df_LHosei[cols].astype("float64")
j=0
rn=0
for i in range(0,df.shape[0]-1):
if abs(df.loc[i,"linecorr_T"])>0.999:
df_LHosei["linecorr_T"]=df.loc[i,"linecorr_T"]
df_LHosei.loc[j,"linindex_T"]=i
df_LHosei.loc[j,"headMot_T"]=df.loc[i,"headMot_T"]
df_LHosei.loc[j,"yaw_T"]=df.loc[i,"yaw_T"]
if j>0:
if(i-df_LHosei.loc[j-1,"linindex_T"])==1:
rn+=1
df_LHosei.loc[j,"renban_T"]=rn
df.loc[i,"LHoseindx_T"]=rn
else:
rn=0
j+=1
#renban＿Tをみならが、平均値をだして、補正値算出
for i in range(0,df_LHosei.shape[0]-20):
hmotsum=0
yawsum=0
rencount=0
hoseitopindex=[]
if df_LHosei.loc[i,"renban_T"]==1 and df_LHosei.loc[i+1,"renban_T"]==2:
while df_LHosei.loc[i+rencount,"renban_T"]>0:
hmotsum=hmotsum+df_LHosei.loc[i+rencount,"headMot_T"]
yawsum=yawsum+df_LHosei.loc[i+rencount,"yaw_T"]
rencount+=1
df_LHosei.loc[i,"hmotave_T"]=hmotsum/(rencount)
df_LHosei.loc[i,"yawave_T"]=yawsum/(rencount)
df_LHosei.loc[i,"hoseiti_T"]= df_LHosei.loc[i,"hmotave_T"]-df_LHosei.loc[i,"yawave_T"]
df_LHosei.loc[i,"rencount_T"]=rencount
#df_LHosei.loc[i,"hoseistarti_T"]= df_LHosei.loc[i,"linindex"]
#df_LHosei.loc[i,"hoseistopi_T"]=df_LHosei.loc[i,"linindex"]+df_LHosei.loc[i,"rencount_T"]-1
df_LHosei.to_csv(r'C:/RTK_Log/BTLHosei_' + basename) # CSV save
# 補正値リストとdf範囲リストを作る
renleverl=12# 12個連続で直線と認定しその直線でのhmotave_Tとyawave_Tをhoseistart-hoseristo範囲内で採用
startlist=[]
hoseitilist=[]
startlist.append(0)#初期値位置ゼロで確保
hoseitilist.append(0)#初期値位置ゼロで確保
for i in range(0,df_LHosei.shape[0]-1):
if df_LHosei.loc[i,"rencount_T"]>4:
startlist.append(df_LHosei.loc[i,"linindex_T"])
hoseitilist.append(df_LHosei.loc[i,"hoseiti_T"])
else:
print("-----------yawhosei Not enable-------- change threshhold rlevel 11->4 ")
hoseitilist[0]=hoseitilist[1]
#dfへ補正値書き込み
sc=0#start counter
for i in range(0,df.shape[0]-1):
if sc<len(startlist):
if i==startlist[sc]:
hval=hoseitilist[sc]
sc+=1
df.loc[i,"yawHosei_T"]=hval
df.loc[i,"yawH_T"]=df.loc[i,"yaw_T"]+ df.loc[i,"yawHosei_T"]
df.to_csv(r'C:/RTK_Log/BTLHoseiZUmi_' + basename) # CSV save
return df
#***************BaseRoverで補正してheading角と比較するver0.41で0.999以上の範囲を平均して補正値算出************
def BRyawhosei(df):
print("BT yaw hosei作成")
yhv=0
df["yawHosei"]=0
df["yawHosei"]=df["yawHosei"].astype("float64")
df["yawH"]=0
df["yawH"]=df["yaw"].astype("float64")
df["LHoseindx"]=0
df["LHoseiindx"]=df["yaw"].astype("float64")
#>0.999範囲取得
df_LHosei=pd.DataFrame()
df_LHosei["linindex"]=0
df_LHosei["headMot360"]=0
df_LHosei["yaw"]=0
df_LHosei["renban"]=0
df_LHosei["hmotave"]=0
df_LHosei["yawave"]=0
df_LHosei["hoseiti"]=0
df_LHosei["rencount"]=0
df_LHosei["hoseiIndex"]=0
df_LHosei["hoseistarti"]=0
df_LHosei["hoseistopi"]=0
cols=df_LHosei.columns
df_LHosei[cols]=df_LHosei[cols].astype("float64")
j=0
rn=0
for i in range(0,df.shape[0]-1):
if abs(df.loc[i,"linecorr"])>0.99:
df_LHosei["linecorr"]=df.loc[i,"linecorr"]
df_LHosei.loc[j,"linindex"]=i
df_LHosei.loc[j,"headMot360"]=df.loc[i,"headMot360"]
df_LHosei.loc[j,"yaw"]=df.loc[i,"yaw"]
if j>0:
if(i-df_LHosei.loc[j-1,"linindex"])==1:
rn+=1
df_LHosei.loc[j,"renban"]=rn
df.loc[i,"LHoseindx"]=rn
else:
rn=0
j+=1
#renban＿Tをみならが、平均値をだして、補正値算出
for i in range(0,df_LHosei.shape[0]-20):
hmotsum=0
yawsum=0
rencount=0
hoseitopindex=[]
if df_LHosei.loc[i,"renban"]==1 and df_LHosei.loc[i+1,"renban"]==2:
while df_LHosei.loc[i+rencount,"renban"]>0:
hmotsum=hmotsum+df_LHosei.loc[i+rencount,"headMot360"]
yawsum=yawsum+df_LHosei.loc[i+rencount,"yaw"]
rencount+=1
df_LHosei.loc[i,"hmotave"]=hmotsum/(rencount)
df_LHosei.loc[i,"yawave"]=yawsum/(rencount)
df_LHosei.loc[i,"hoseiti"]= df_LHosei.loc[i,"hmotave"]-df_LHosei.loc[i,"yawave"]
df_LHosei.loc[i,"rencount"]=rencount
#df_LHosei.loc[i,"hoseistarti_T"]= df_LHosei.loc[i,"linindex"]
#df_LHosei.loc[i,"hoseistopi_T"]=df_LHosei.loc[i,"linindex"]+df_LHosei.loc[i,"rencount_T"]-1
df_LHosei.to_csv(r'C:/RTK_Log/BRLHosei_' + basename) # CSV save
# 補正値リストとdf範囲リストを作る
renleverl=12# 12個連続で直線と認定しその直線でのhmotave_Tとyawave_Tをhoseistart-hoseristo範囲内で採用
startlist=[]
hoseitilist=[]
startlist.append(0)#初期値位置ゼロで確保
hoseitilist.append(0)#初期値位置ゼロで確保
for i in range(0,df_LHosei.shape[0]-1):
if df_LHosei.loc[i,"rencount"]>11:
startlist.append(df_LHosei.loc[i,"linindex"])
hoseitilist.append(df_LHosei.loc[i,"hoseiti"])
hoseitilist[0]=hoseitilist[1]
#dfへ補正値書き込み
sc=0#start counter
for i in range(0,df.shape[0]-1):
if sc<len(startlist):
if i==startlist[sc]:
hval=hoseitilist[sc]
sc+=1
df.loc[i,"yawHosei"]=hval
df.loc[i,"yawH"]=df.loc[i,"yaw"]+ df.loc[i,"yawHosei"]
df.to_csv(r'C:/RTK_Log/BRLHoseiZUmi_' + basename) # CSV save
return df
#120msecでheadMotとHeading360が粗いので、BNO10msec座標でNutralPointを細分化して精度向上させる
#def BNHM(dfbr,dfbn):
#df_BTUBXのitow抜けを補修
def base_hoshu(df):
df_Base = df#[df.iloc[:, 0] == 'B'] # id列[:,0]が'B'ならばdf_Baseとして抽出
df_Base = df_Base.fillna(0) # NANをゼロに変換
df_Base = df_Base.reset_index() # index番号リセット
#df_Base = df_Base.add_suffix('_B', axis=1) # head0の名称末尾に'_B'をつけてBaseのデータ列命名
#df_Base = df_Base.drop(columns='index_B') # index_Bは不要なので消去
#df_Base = df_Base.drop(columns='id_T') # id_Bも不要なので消去　数値以外のデータ除去
headB = df_Base.columns # 完成したヘッダ文字LISTを保持
print("headB=", headB)
df_Base[headB] = df_Base[headB].astype("float64")
print(df_Base.dtypes)
k = [] # itow エラー　list k[]宣言
# ------------df_Baseをlistへ読み出し-----------
list_data = [] # df_Baseの値をlistに読み込む　DFでの処理よりlistで処理したほうが速い
list_data = df_Base.to_numpy().tolist() # list_dataに値を代入
# print("list_data=",list_data)
# print("list_data size=", [len(v) for v in list_data]) #
# -----------------------------------------
lbase = len(df_Base) # 全行数
# lastrow = len(df_Base)
for i in range(1, lbase): # 行数ループ　整数iでカウント
# print("range i=",i)
if i > 1: # i-1対策
itow0 = (int)(df_Base.iloc[i,1]) # 整数で番地指定しているのでiloc使用
itow_1 = (int)(df_Base.iloc[i - 1, 1]) # 1個手前のitow_1
# print(itow0-itow_1)
c = [] # 補間計算データ行リスト
k = 0
if itow0 - itow_1 > 100 and itow0 - itow_1<250: # itow差が120以上あったらエラー
itowadd = itow_1 + 100
# print("itowadd=", itowadd, "itow0=", itow0, "itow_1=", itow_1)
for j in range(0, 55): # 列をループして平均値計算
ss = df_Base.iloc[i, j]
# if is_num(ss):
a = float(df_Base.iloc[i, j])
b = float(df_Base.iloc[i - 1, j])
# print(j,a,b)
c.append(int((a + b) / 2)) # 補間行リストに平均値代入
# add c to list_data
list_data.append(c) # 行代入が完了したcをlist_dataに追加　元のlistデータの最高単に追加される
# print("c len=",len(c))
# print("Err made c=",c)
# print("list_data size=", [len(v) for v in list_data])
elif itow0 - itow_1>250:#500など大きく抜けてる場合は同じデータでフィル
sa=int((itow0 - itow_1)/100)
for k in range(1,sa):
d=[]
df_Base.iloc[i-1,1]=df_Base.iloc[i-1,1]+100
d=df_Base.iloc[i-1,:]
#print(k,";",d)
list_data.append(d)
print("Error sa=",sa,"i=",i)
# ================All list_data をdf_Base2にしてitow SORTして修正完成=====================================
df_Base2 = pd.DataFrame(list_data, columns=headB) # i,jでループ完了したら、list_dataから新たなDF df_Base2を作成
# 数値以外のデータが含まれている行の抽出
non_numeric_rows = df_Base2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all(), axis=1)
# print("nonnumericRow=",non_numeric_rows)
# 数値以外のデータが含まれている列の抽出
non_numeric_cols = df_Base2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all())
# print("nonnumericCol=",non_numeric_cols)
# df_Rover2 = df_Base2.fillna(0)
# df_Base2['iTOWB']=df_Base2['iTOW_B'].astype('int')
# -------------------iTOW_BでSORTする前にiTOW_Bのゴミを完全に排除する--------------------------------------
df_Base2['iTOW_T'] = df_Base2['iTOW_T'].astype('str')
df_Base2['iTOW_T'] = df_Base2['iTOW_T'].str.replace('-', '').str.replace(':', '').str.replace(' ', '')
df_Base2['iTOW_T'] = df_Base2['iTOW_T'].astype("float64")
# ----------------------------------------------------------------------------------------------------
print(df_Base2)
df_Base2.info()
# pd.set_option('display.max_rows', None)
df_Base2 = df_Base2.sort_values("iTOW_T", ascending=True) # SORT iTOWBでSORT
print("Sorted df_Base2=", df_Base2)
# ----------CHECK SORT--------------
itowerr = 0
for i in range(1, len(df_Base2) - 1):
itow0 = df_Base2.iloc[i + 1, 0]
itow_1 = df_Base2.iloc[i, 0]
if ((itow0 - itow_1) != 100):
itowerr += 1
# print("BASE***********i=", i, itow0, "itowErr=", itowerr
df_Base2.drop('index', axis=1,inplace=True)
df_Base2.reset_index(drop=True,inplace = True)
return df_Base2
# ============================BASE ITOW 補修終了===============================
def bnohoshu(df):
print("bnohoshu:",df)
print(df.dtypes)
df_BNOH = df#[df.iloc[:, 0] == 'B'] # id列[:,0]が'B'ならばdf_Baseとして抽出
df_BNOH = df_BNOH.round({'bnitow': -1})
print(df_BNOH["bnitow"])
df_BNOH = df_BNOH.fillna(0) # NANをゼロに変換
df_BNOH = df_BNOH.reset_index() # index番号リセット
# ------------df_Baseをlistへ読み出し-----------
list_data = [] # df_Baseの値をlistに読み込む　DFでの処理よりlistで処理したほうが速い
list_data = df_BNOH.to_numpy().tolist() # list_dataに値を代入
# print("list_data=",list_data)
# print("list_data size=", [len(v) for v in list_data]) #
# -----------------------------------------
lbase = len(df_BNOH) # 全行数
headB = df_BNOH.columns # 完成したヘッダ文字LISTを保持
# lastrow = len(df_Base)
for i in range(1, lbase): # 行数ループ　整数iでカウント
# print("range i=",i)
if i > 1: # i-1対策
itow0 = (int)(df_BNOH.iloc[i,1]) # 整数で番地指定しているのでiloc使用
itow_1 = (int)(df_BNOH.iloc[i - 1, 1]) # 1個手前のitow_1
# print(itow0-itow_1)
c = [] # 補間計算データ行リスト
k = 0
if itow0 - itow_1 > 10 and itow0 - itow_1<25: # itow差が120以上あったらエラー
itowadd = itow_1 + 10
print("itowadd=", itowadd, "itow0=", itow0, "itow_1=", itow_1)
for j in range(0, 8): # 列をループして平均値計算
ss = df_BNOH.iloc[i, j]
if j!=1:
a = float(df_BNOH.iloc[i, j])
b = float(df_BNOH.iloc[i - 1, j])
ab=(a + b) / 2
elif j==1:
ab=itowadd
c.append(ab) # 補間行リストに平均値代入
# add c to list_data
list_data.append(c) # 行代入が完了したcをlist_dataに追加　元のlistデータの最高単に追加される
# print("c len=",len(c))
# print("Err made c=",c)
# print("list_data size=", [len(v) for v in list_data])
elif itow0 - itow_1==0:#同じ場合は消す
print("EQUAL:itow0,itow0_1=",itow0,itow_1)
print()
df_BNOH.drop(df_BNOH.index[[i-1]],axis=1 )
# ================All list_data をdf_Base2にしてitow SORTして修正完成=====================================
df_BNOH2 = pd.DataFrame(list_data, columns=headB) # i,jでループ完了したら、list_dataから新たなDF df_Base2を作成
# 数値以外のデータが含まれている行の抽出
non_numeric_rows = df_BNOH2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all(), axis=1)
# print("nonnumericRow=",non_numeric_rows)
# 数値以外のデータが含まれている列の抽出
non_numeric_cols = df_BNOH2.apply(lambda x: not pd.to_numeric(x, errors='coerce').notnull().all())
# print("nonnumericCol=",non_numeric_cols)
# df_Rover2 = df_Base2.fillna(0)
# df_Base2['iTOWB']=df_Base2['iTOW_B'].astype('int')
# -------------------iTOW_BでSORTする前にiTOW_Bのゴミを完全に排除する--------------------------------------
df_BNOH2['bnitow'] = df_BNOH2['bnitow'].astype('str')
df_BNOH2['bnitow'] = df_BNOH2['bnitow'].str.replace('-', '').str.replace(':', '').str.replace(' ', '')
df_BNOH2['bnitow'] = df_BNOH2['bnitow'].astype("float64")
# ----------------------------------------------------------------------------------------------------
print(df_BNOH2)
print(df_BNOH2.dtypes)
df_BNOH2.info()
# pd.set_option('display.max_rows', None)
df_BNOH2 = df_BNOH2.sort_values("bnitow", ascending=True) # SORT iTOWBでSORT
#df_BNOH2.drop_duplicates(inplace=True)
df_BNOH2.reset_index(drop=True,inplace = True)
df_BNOH2.to_csv(r'C:/RTK_Log/BNOH2_' + basename) # CSV save
print("Sorted df_BNOH2=", df_BNOH2)
# ----------CHECK SORT--------------
itowerr = 0
for i in range(1, len(df_BNOH2) - 1):
itow0 = df_BNOH2.iloc[i + 1, 0]
itow_1 = df_BNOH2.iloc[i, 0]
if ((itow0 - itow_1) != 100):
itowerr += 1
# print("BASE***********i=", i, itow0, "itowErr=", itowerr
df_BNOH2.drop('index', axis=1,inplace=True)
df_BNOH2.reset_index(drop=True,inplace = True)
return df_BNOH2
#========================================================================================================
#====================================def main()を最下部で最後に呼び出す====================================
#========================================================================================================
def main():
global result, basename, df_BNO, BRBNflag
# id+PVT33+RELPOSNED 20
# --------------------------------------------MBUX FILES-----------------------------------------------------------------
# UBXデータは、BaseとRoverから同形式で出力されたバイナリーをSTA24システムでUBX値に変換してCSVファイル
# 形式は、Baseは、id列に’B'行で　Roverはid列に’R'行がついている。同じitowが２行となる。　
# PVT３３個　RELPOSNED 20個　計55個ｘ２=110個整数
print("main pass through")
print("hoge")
#basename=""
brbnOK:int=0
btOK:int=0
bnOK:int=0
ubxflag:int = 0
#bnoflag:int = 0
targetitow = 0
df_BNO = pd.DataFrame
df_BTUBX=pd.DataFrame
df_BRBN=pd.DataFrame
df_BNO=pd.DataFrame
list_PVT = ["id", "iTOW", "year", "month", "day", "hour", "min", "sec", "valid", "tAcc", "nano", "fixtye", "flags",
"flags2", "numSV", "lon", "lat", "height", "hMSL",
"hAcc", "vAcc", "velN", "velE", "velD", "gSpeed", "headMot", "sAcc", "headAcc", "pDOP", "flags3",
"reserved1", "headVeh", "magDec", "magAcc", ""] # 33
list_BasePVT = [s + '_B' for s in list_PVT]
list_RoverPVT = [s + '_R' for s in list_PVT]
list_RELP = ["version", "reserved2", "refStationId", "iTOWR", "relPosN", "relPosE", "relPosD", "relPosLength",
"relPosHeading", "relPosHPN",
"relPosHPE", "relPosHPD", "relPosHPLength", "accN", "accE", "accD", "accLength", "accHeading",
"reserverd4", "flagsR", "a"]
list_BaseRELP = [s + '_B' for s in list_RELP]
list_RoverRELP = [s + '_R' for s in list_RELP]
head0 = list_PVT + list_RELP
# print(list_BasePVT)
# print(list_RoverPVT)
# @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
# @@@@@@@@@@@@@@@@@@@@@@@@@@ CONTROL KEY @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
# @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
v = ""
cmnt = "[]"
initflag=0
while v != "e":
v=""
if initflag==1:
print("Files:" + cmnt)
#print("BRBNflag=",BRBNflag)
print("<<<<PUSH KEY::r:read file,h:plot headmot,x:plot xy,v:plot3,g:plot4 e=EXIT>>>> ")
print("[最初に日付データフォルダから 時刻_ubx-MBUX.csv　を読みこんでから、bnoファイルbno_時刻.bin-MBNOを読む]")
"""
while True:
if keyboard.read_key():
v = keyboard.read_key()
print("キーが押されました: " + v)
if v == "e":
sys.exit()
break
"""
v= input(" keyin =>ENTER")
#tn = st.split('-')
# @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
initflag=1
if v == "v":
plotvector3(resultBRBN)
if v == "g":
print("df_BRBNT:",df_BRBNT)
plotvector4(df_BRBNT)
if v == "t":
turncut(result)
# --------------------------------------------MovingBase BNO FILES-----------------
if v == "r":
list_BNO = ["bnitow", "yaw", "pitch", "roll", "ax", "ay", "az", "com1", "com2", "com3"]
#list_BNO = ["bnitow", "yaw", "pitch", "roll", "ax", "ay", "az"]
list_BNO0 = ["bnitow", "yaw", "pitch", "roll", "ax", "ay", "az"]
root = tkinter.Tk()
root.withdraw()
iDir = r'C:/RTK_Log/'
fTyp = [("データファイル", "*.csv;*.xlsx;*.xls"), ("すべてのファイル", "*.*")]
filename = FileDialog.askopenfilename(parent=root, initialdir=iDir, filetypes=fTyp)
# filenameからbasenameをとりだしてファイル種類区別
basename = os.path.basename(filename)
BRBNflag = "BRBN" in basename
# ************************************************************************************************************
# *************************************MovingBase File *******************************************************
# ************************************************************************************************************
if "BRBN" in basename:# BRBNflag == True:
if "BRBNT" in basename:
bnOK=1#BNBRTファイルでRLプロットvectorplot4
brbnOK=0
df_BRBNT = pd.read_csv(filename, low_memory=True)
cols=df_BRBNT.columns
print(cols)
df_BRBNT[cols]=df_BRBNT[cols].astype("float64")
print(df_BRBNT.dtypes)
print("sf_BRBNT:",df_BRBNT)
dflen=len(df_BRBNT)
print(dflen,":[]",df_BRBNT.loc[0,"iTOW_B"],"-",df_BRBNT.loc[dflen-1,"iTOW_B"],"]")
cmnt = "[" + basename + "]" + cmnt
else:
brbnOK=1#BRBNファイルでSKI MBプロットvectorplot3
bnOK=0
resultBRBN = pd.read_csv(filename, low_memory=True)
print(resultBRBN)
targetitow = int(resultBRBN.iloc[0, 1])
print("targetitow=", targetitow)
BRBNflag=True
dflen=len(resultBRBN)
print("resultBRBN=",dflen,":[",resultBRBN.loc[0,"iTOW_B"],"-",resultBRBN.loc[dflen-1,"iTOW_B"],"]")
cmnt = "[" + basename + "]" + cmnt
elif "ubx" in basename and v == "r" and BRBNflag == False:
ubxflag = 1
result = readMBFile(filename, basename, head0)
print("result=", result)
print(f"result memory usage: {result.memory_usage(deep=True).sum() / 1024 ** 2} MB")
cmnt = "[" + basename +"]" + cmnt
v=""
#>>>>>>>>>>bnoファイル処理は、ubxファイル処理済み状態から実行しないといけない>>>>>>
elif "bno" in basename and v == "r": # MB BNO File
v=""
if ubxflag == 1: # resultが準備されていること
head1 = list_BNO
BNOflag = True
#df_BNO = pd.read_csv(filename, names=head1, low_memory=True)
df_BNO = pd.read_csv(filename, header=None,low_memory=True)#filename　読み込み
print("PATH:", filename, "READ:", basename)
#print(df_BNO)
pd.set_option('display.max_columns', 40)
# ----bnitowの異常値修正 異常値は、前の正常値でインクリメントして埋め
df_BNO = df_BNO.drop(df_BNO.index[-1]) # last コメント除去
if len(df_BNO.columns)>8: #長い列はカット
df_BNO=df_BNO.drop(range(7,len(df_BNO.columns)),axis=1 )
print(df_BNO.head())
#df_BNO = df_BNO.drop(["com1", "com2", "com3"], axis=1)
#print("SortMae:",df_BNO)
# df_BNO=bnitowRe(df_BNO)# bnitow=0行をitowインクリメントで埋める関数
df_BNO.columns=list_BNO0
df_BNO = bnitowsort(df_BNO)#>>>>>bnitowの飛び　文字化けを無視して10msecインクリメントでソート
#prnt("Prepaired df_BNO:", df_BNO)
#print(df_BNO.dtypes)
#df_BNO.to_csv(r'C:/RTK_Log/BNOsorted_' + basename) # CSV save
df_BNO.to_csv(filename+"_sorted.csv",index = False) # CSV save
cmnt = "[" + basename + "]" + cmnt
print(f"df_BNO memory usage: {df_BNO.memory_usage(deep=True).sum() / 1024 ** 2} MB")
# -----------result."iTow_B"とdf_BNO120の同期--------------
#df_BNO = df_BNO.drop(df_BNO.index[-1])
#print("itow120mae",df_BNO)
df_BNO120 = itow120df(df_BNO) # 120周期のDF作成
df_BNO120.to_csv(r'C:/RTK_Log/BNO120ed_' + basename) # CSV save
print("df_BNO120 made", df_BNO120.iloc[0,0],"-",df_BNO120.iloc[- 1,0])
print("result=",result.loc[0,"iTOW_B"],"-",result.loc[result.shape[0] - 1,"iTOW_B"])
list_bnoit = [] # listは、数値を入れれば1次元　listを入れれば２次元になる
bnc = []
#if ubxflag == 1: # resultが準備されていること
# start行番号検索
itb0 = result.loc[0, "iTOW_B"]
itb0tail=int(result.iloc[-1,0])
itbn0 = int(df_BNO120.iloc[0, 0])
itbn0tail=int(df_BNO120.iloc[-1,0])
isa = int(itb0 - itbn0)
print("itb0=", itb0,":itb0tail=",itb0tail)
print("itbn0=", itbn0,":itbn0tail=",itbn0tail)
print("isa=", isa)
if itbn0>itb0tail:
print(" itbn0>itb0tail OUT OF RANGE")
exit()
elif itb0>itbn0tail:
print(" itb0>itbn0tail OUT OF RANGE")
exit()
if isa < 10000: # baseが先行ある
basestart = abs(int(isa / 120))
bnostart = 0
else: # bnoが先行
basestart = 0
bnostart = abs(int(isa / 10))
print("isa=", isa, "basestart=", basestart, "bnostart=", bnostart)
print("basestart itow=", result.iloc[basestart, 0], "bnostart itow=", df_BNO120.iloc[0, 0])
targetitow = int(result.iloc[basestart, 0])
# result左端にdf_bno120を追加するために一旦listにして再構築
# 0からbasestart
list_bno = []#listを作成してからDFに変換
r = []#作業用list
if basestart > bnostart: # bnostartの上にbasestart分のゼロDFを作成して合体
list_bno = [[0] * 7] * (basestart) # 上のデータゼロ埋め
# print(list_bno)
blen = len(df_BNO120)
for i in range(0, blen - 1): # 中間にdf_BNO120をlistに追加
r = df_BNO120.iloc[i, :]
list_bno.append(r)
sn = basestart + len(df_BNO120)
rn = len(result)
print("Upper-listbno=", len(list_bno))
print("for sn=", sn, "rn=", rn, "result len=", len(result))
for i in range(sn, rn + 1): # 下のデータ埋め
l0 = [0, 0, 0, 0, 0, 0, 0]
list_bno.append(l0)
#print("listbno=", len(list_bno))
df_BNOL = pd.DataFrame(list_bno, columns=list_BNO0)
df_BNOL.to_csv(r'C:/RTK_Log/df_BNOL_' + basename) # CSV save
# 現在の最大表示行数の出力
pd.set_option('display.max_rows', None)
pd.set_option('display.max_columns', None)
#print(df_BNOL)
print(result["iTOW_B"])
print("len df_BNOL=", len(df_BNOL))
print("len result=", len(result))
print("Check basestart=", basestart, "@iTOW_B=", result.iloc[220, 0], "df_BNOL bnitow=",df_BNOL.loc[220, "bnitow"])
# concat
print("Concat:df_BNOL=", df_BNOL.iloc[0,0],"-",df_BNOL.iloc[- 1,0])
print("Concat:result=",result.loc[0,"iTOW_B"],"-",result.loc[result.shape[0] - 1,"iTOW_B"])
resultBRBN = pd.concat([result, df_BNOL], axis=1)
additions2(resultBRBN)#skid計算,heading-yaw差計算H-y
#直線回帰してlindecorr列に相関係数いれる
count=linereg(resultBRBN,5)#直線回帰で直進部分を計算して"linecorr"に代入
print("linear regression count=",count)
#直線回帰0.999以上で、yaw補正をかける
resultBRBN=BRyawhosei(resultBRBN)
#additions5(resultBRBN)#UDP用yaw補正
pd.set_option('display.max_rows', 50)
pd.set_option('display.max_columns', 100)
print(resultBRBN)
resultBRBN.to_csv(r'C:/RTK_Log/BRBN_' + basename) # CSV save
brbnOK=1
# CPLT用ファイル
df_CPLT = pd.DataFrame()
df_CPLT = resultBRBN[["Heading360", "yaw", "headMotMA3", "diffzero", "roll"]]
#print("df_CPLT")
df_CPLT.to_csv(r'C:/RTK_Log/CPLT_BRBN_' + basename, index=False) # CSV save
BRBNflag=True
elif basestart < bnostart: # bnostartをカットしてresultと合体
print("ERROR============basestart<bnostart============")
"""
#bnostartがresultの何番目に位置するか検索
#indx2=[result["iTOW_B"]== itbn0].index
indx2=result[result["iTOW_B"]< itbn0]
print(indx2)
list_bno = [[0] * 7] * (basestart) # 上のデータゼロ埋め
# print(list_bno)
blen = len(df_BNO120)
for i in range(0, blen - 1): # 中間にdf_BNO120をlistに追加
r = df_BNO120.iloc[i, :]
list_bno.append(r)
sn = basestart + len(df_BNO120)
rn = len(result)
print("Upper-listbno=", len(list_bno))
print("for sn=", sn, "rn=", rn, "result len=", len(result))
for i in range(sn, rn + 1): # 下のデータ埋め
l0 = [0, 0, 0, 0, 0, 0, 0]
list_bno.append(l0)
print("listbno=", len(list_bno))
df_BNOL = pd.DataFrame(list_bno, columns=list_BNO0)
# 現在の最大表示行数の出力
pd.set_option('display.max_rows', None)
pd.set_option('display.max_columns', None)
print(df_BNOL)
print(result["iTOW_B"])
print("len df_BNOL=", len(df_BNOL))
print("len result=", len(result))
print("Check basestart=", basestart, "@iTOW_B=", result.iloc[220, 0], "df_BNOL bnitow=",
df_BNOL.loc[220, "bnitow"])
# concat
resultBRBN = pd.concat([result, df_BNOL], axis=1)
additions2(resultBRBN)
pd.set_option('display.max_rows', 50)
pd.set_option('display.max_columns', 100)
print(resultBRBN)
resultBRBN.to_csv(r'C:/RTK_Log/BRBN_' + basename) # CSV save
"""
else:
print("ERROR=====>UBXファイルを読み込んでからbnoファイルを読み込んでください===========")
# *****************************************************************************************************
# *********************RTK File BootsOn****************************************************************
# *****************************************************************************************************
elif "BTUBX" in basename and v == "r": # BootsOn BTUBX　　ファイル
v=""
#bnohead = list(range(74))
#bnohead[2]="bnitow"
#print(bnohead)
df_BTUBX = pd.read_csv(filename, low_memory=True) # B 74列　U56列といびつなｃｓｖはエラーになるので、７４列のヘッダーを与える
cols=df_BTUBX.columns
df_BTUBX[cols]=df_BTUBX[cols].astype("float64")
#df_BTUBX.columns = bnohead
#----BTUBX抽出------
#df_BTUBX = df_BTUDP[df_BTUDP.iloc[:, 0] == 'U'] # id列[:,0]が'B'ならばdf_RTKとして抽出
df_BTUBX.fillna(0)
df_BTUBX.reset_index(drop=True)
#print("df_BTUBX=",df_BTUBX)
# df_BTUBX=df_BTUBX.drop(df_BTUBX.columns[[56,73]], axis=1)
#df_BTUBX = df_BTUBX.drop(columns=range(56, 74))
#df_BTUBX.columns = head0
df_BTUBX = df_BTUBX.add_suffix('_T', axis=1)
#print("1442df_BTUBX::", df_BTUBX)
#df_BTUBX=df_BTUBX.reset_index(drop=True)
#df_BTUBX.to_csv(r'C:/RTK_Log/BTUBX_' + basename)#補修前DF
#df_BTUBX補修
df_BTUBX=base_hoshu(df_BTUBX)#itow抜け補修
df_BTUBX.reset_index(drop=True)
#print(df_test)
#　BTUBX内角度変換
for i in range(0,df_BTUBX.shape[0]-1):
h=df_BTUBX.loc[i,"headMot_T"]*0.00001
df_BTUBX.loc[i,"headMot_T"] = h
df_BTUBX["headMotMA3_T"] = df_BTUBX["headMot_T"].rolling(3).mean()
df_BTUBX.to_csv(r'C:/RTK_Log/BTUBX_' + basename)#補修前DF
elif "BTBNO" in basename and v == "r": # BootsOn BTBNO　　ファイル
v=""
df_BTBNO = pd.read_csv(filename, low_memory=True)
#----BTBNO抽出して１列に並べなおして、bnitowを10msecでSORTするBTBNO2作成---2024/4/18
#df_BTBNO = df_BTUDP[df_BTUDP.iloc[:, 0] == 'B'] # id列[:,0]が'B'ならばdf_BTBNOとして抽出
#df_BTBNO.to_csv(r'C:/RTK_Log/maeBTBNO_' + basename)
#df_BTBNO2=BTBNO21(df_BTBNO)#BNOデータを１列にまとめる
#df_BTBNO2 = bnitowsort(df_BTBNO2)
#df_BTBNO2=bnohoshu(df_BTBNO2)
#df_BTBNO2=df_BTBNO2.reset_index(drop=True)
#df_BTBNO2=df_BTBNO2.rename(columns={'bnitow': 'bnitow_T'})
df_BTBNO = df_BTBNO.add_suffix('_T', axis=1)
#---- df_BTBNO内の角度変換　0-360度に変換-----------------------------------------------------------------------------------------
for i in range(0,df_BTBNO.shape[0]-1):
#df_BTBNO.loc[i,"yaw_T"]=df_BTBNO.loc[i,"yaw_T"]
if df_BTBNO.loc[i,"yaw_T"]<0:
df_BTBNO.loc[i,"yaw_T"]=df_BTBNO.loc[i,"yaw_T"]+360
#df_BTBNO["yaw_T"] = np.where(df_BTBNO["yaw_T"] <0, df_BTBNO["yaw_T"] - 360, df_BTBNO["yaw_T"])
#print(df_BTBNO2.dtypes)
#print("df_BTBNO2:",df_BTBNO2.head(1),"\n",df_BTBNO2.tail(1))
df_BTBNO.to_csv(r'C:/RTK_Log/BTBNO_' + basename)
#print("1446df_BTBNO len=",len(df_BTBNO))
#print("1447 df_BTUBX len=",len(df_BTUBX))
#----dfBTBNO2を100msec間引き 2024/4/18---
df_BTBNO100=bnitow2100(df_BTBNO)# bnitow%100
df_BTBNO100=df_BTBNO100.reset_index(drop=True)
df_BTBNO100.to_csv(r'C:/RTK_Log/BTBNO100_' + basename)
#---BTubxとBTBNO100 itow 合わせ---------
if df_BTUBX.empty==False:#df_BTUBXが読み込まれている場合
itb0 = df_BTUBX.loc[0, "iTOW_T"]
lastrowUBX=df_BTUBX.shape[0] - 1
itb0tail=int(df_BTUBX.loc[lastrowUBX,"iTOW_T"])
itbn0 = int(df_BTBNO100.loc[0, "bnitow_T"])
lastrowBNO=df_BTBNO100.shape[0]-1
itbn0=itbn0-itbn0%10#端数切捨て
itbn0tail=int(df_BTBNO100.loc[lastrowBNO,"bnitow_T"])
itbn0tail=itbn0tail-itbn0tail%10#端数切捨て
isa = int(itb0 - itbn0)
if itbn0>itbn0tail:
print(" itbn0>itb0tail OUT OF RANGE")
exit()
elif itb0>itbn0tail:
print(" itb0>itbn0tail OUT OF RANGE")
exit()
lensa=len(df_BTUBX)-len(df_BTBNO100)
#df_BTUBXとdfBTBNO100の長さ合わせて合体
#===========スタート位置合わせ===========================================
if itb0 - itbn0 <0: # BNOスタートが遅れ
BNOstart = 0#BNOスタート現状
UBXstart = abs(int(isa / 100))#UBXスタートをBNOのスタート地点にあわせる
df_BTUBX=df_BTUBX.drop(range(0,UBXstart),axis=0)
elif itb0 - itbn0 >0: # UBXスタートが遅れ
UBXstart = 0
BNOstart = abs(int(isa / 100))
df_BTBNO100=df_BTBNO100.drop(range(0,BNOstart),axis=0)
df_BTUBX.reset_index(drop=True)
df_BTBNO100.reset_index(drop=True)
print("Start_df_BTUBX:",df_BTUBX)
print("Start_df_BTBNO100:",df_BTBNO100)
#=================ラスト位置合わせ=======================================
BNOtail = df_BTBNO100.shape[0]-1#BNO最終行INDEX
UBXtail = df_BTUBX.shape[0]-1#UBXの最後をBNOの最終地点にあわせる
isatail = int(itb0tail - itbn0tail)
if UBXtail - BNOtail >0: # UBXの最後が長い
df_BTUBX=df_BTUBX[:BNOtail]
elif UBXtail - BNOtail <0: # BNoの最後が長い
#df_BTBNO100.drop(range(UBXtail,BNOtail),inplace=True)
df_BTBNO=df_BTBNO[:UBXtail-1]
df_BTUBX.reset_index(drop=True,inplace=True)
df_BTBNO100.reset_index(drop=True,inplace=True)
print("Tail_df_BTUBX:",df_BTUBX)
print("Tail_df_BTBNO100:",df_BTBNO100)
#==========BTUBX とBTBNO100 合体========================
df_BTUBNO= pd.concat([df_BTUBX, df_BTBNO100], axis=1)
pd.set_option('display.max_columns', 120) # 120列まで表示
pd.set_option('display.max_rows', 120) # 120列まで表示
print(df_BTUBNO.dtypes)
#=============100msec周期で直線回帰追加====================
df_BTUBNO=lineregBT(df_BTUBNO,5)
df_BTUBNO.to_csv(r'C:/RTK_Log/hoseimaeBTUBNO_' + basename)
#========直線回帰０．９９９以上で補正値きめてyaw補正========================================
df_BTUBNO=BTyawhosei(df_BTUBNO)
df_BTUBNO.to_csv(r'C:/RTK_Log/BTUBNO_' + basename)#UDP合体
# =========BRBNに同期させるために120msec用に100msecを1個間引くdf_BTUBNO600を作成====================
df_BTUBNO600 = df_BTUBNO
drop_index = df_BTUBNO600.index[(df_BTUBNO600["iTOW_T"]) %600== 300]#600msecで300msec余る行を削除するindex
df_BTUBNO600.drop(drop_index, inplace=True)#%600=300の行をdrop_index指定で削除
df_BTUBNO600 = df_BTUBNO600.reset_index(drop=True) # index reset
df_BTUBNO600 = additions4(df_BTUBNO600)
df_BTUBNO600 = yawH2XY(df_BTUBNO600)
#df_BTplot600 = additions5(df_BTplot600)
#--------------先頭行のitowを%600==0にあわせる---------------------------
top600_index = df_BTUBNO600.index[(df_BTUBNO600["iTOW_T"]) % 600 ==0]
trow=top600_index[0]#%600==0の先頭行のindex
#del_index=df_BTplot600.index[0,trow]
df_BTUBNO600.drop(range(0,trow), inplace=True) # 最初の%600=0行までの先頭からの行削除
df_BTUBNO600 = df_BTUBNO600.reset_index(drop=True) # index reset
print("df_BTUBNO600:")
print(df_BTUBNO600)
df_BTUBNO600.to_csv(r'C:/RTK_Log/BTUBNO600_' + basename)
cmnt = "[BTUBNO600_" + basename + "]" + cmnt
#=============BRBNファイルと合体してBRBNTファイル作成====================================================
if brbnOK==1:#BRBNファイル読み込み済み
#if resultBRBN.empty==False and df_BTUBNO.empty==False:
print("=============df_BTplot add to resultBRBN ==============")
#print(resultBRBN)
#print(df_BTplot)
df_BRBNT=RTK2BRBN(resultBRBN, df_BTUBNO600)
df_BRBNT=df_BRBNT.fillna(0)
print(df_BRBNT)
df_BRBNT.to_csv(r'C:/RTK_Log/BRBNT_' + basename)
btOK=1
elif v == "h" and ubxflag == 1:
cmnt = "["+basename+ "]" + cmnt
#plotheadmot(result, basen)
elif v=="i" :# Integral
bnointegral()
#def new_func(df_BTBNO100, BNOstart):
# return df_BTBNO100.drop(range(0,BNOstart),axis=0)
def BTBNO21(df):#BTBNOを縦１列に変換 bnitowのズレをチェックする
#[5:10][12:17][19:24][26:31][33:38][]
print()
listbndata=[]
for i in range(0,len(df)):
bnitow=int(df.iloc[i,2])
itowN=0
for j in range(5,68,7):
#print(df.iloc[i,j-1],df.iloc[i,j],df.iloc[i,j+1],df.iloc[i,j+2],df.iloc[i,j+3],df.iloc[i,j+4],df.iloc[i,j+5])
rowdata=[]
n=bnitow+itowN
rowdata=[n,df.iloc[i,j],df.iloc[i,j+1],df.iloc[i,j+2],df.iloc[i,j+3],df.iloc[i,j+4],df.iloc[i,j+5]]
itowN=itowN+10
#print("i=",i,":",rowdata)
listbndata.append(rowdata)
df_rowdata=pd.DataFrame(listbndata)
df_rowdata.columns=["bnitow","yaw_T","pitch_T","roll_T","ax_T","ay_T","az_T"]
df_rowdata=df_rowdata.astype("float64")
# yaw_T 0-360度に変換
df_rowdata["yaw_T"] = np.where(df_rowdata["yaw_T"] < 0, df_rowdata["yaw_T"]+ 360, df_rowdata["yaw_T"])
df_rowdata.to_csv(r'C:/RTK_Log/BTBNO21_' + basename)
return df_rowdata
main()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment