/// <summary>
/// データ取得
/// </summary>
/// <returns>double配列</returns>
public LocSumpLogData[] getScanData()
{
string str;
if (fsr == null) return new LocSumpLogData[0];
long allComNum;
allComNum = getNumCommand();
if (allComNum == 0)
{
allComNum = getNumCommand();
if (allComNum == 0)
{
// 対応するログファイルではない
return null;
}
}
LocSumpLogData[] resultData = new LocSumpLogData[allComNum];
int dataIdx = 0;
// long startMs = 0;
resultData[dataIdx] = new LocSumpLogData();
LocSumpLogData lsData = resultData[dataIdx];
do
{
str = fsr.ReadLine();
if (str == null) break;
if (str.Length == 0) continue;
string[] strSplitData = str.Split('$');
for (int i = 0; i < strSplitData.Length; i++)
{
string[] dataWord = strSplitData[i].Split(',');
switch (dataWord[0])
{
case "A1": // RE
lsData.ms = ParseMS(dataWord[1]);
if (dataWord.Length > 2)
{
lsData.REL = long.Parse(dataWord[2]);
lsData.RER = long.Parse(dataWord[3]);
}
lsData.bRE = true;
break;
case "A2": // Compus
lsData.ms = ParseMS(dataWord[1]);
lsData.CompusDir = int.Parse(dataWord[2]);
lsData.bCompus = true;
break;
case "A3": // GPS
lsData.ms = ParseMS(dataWord[1]);
{
string[] dataGPS = dataWord[2].Split('.');
lsData.GPSLandX = double.Parse(dataGPS[0]) + (double.Parse(dataGPS[1]) / 60.0);
}
{
string[] dataGPS = dataWord[3].Split('.');
lsData.GPSLandY = double.Parse(dataGPS[0]) + (double.Parse(dataGPS[1]) / 60.0);
}
lsData.bGPS = true;
break;
case "A4": // RE PlotX,Y
lsData.ms = ParseMS(dataWord[1]);
lsData.REPlotX = double.Parse(dataWord[2]);
lsData.REPlotY = double.Parse(dataWord[3]);
lsData.REPlotDir = double.Parse(dataWord[4]);
lsData.bREPlot = true;
break;
}
}
if (str.Length > 0 && str.Substring(str.Length - 1, 1) == "$")
{
// １データ完了
if (dataIdx < resultData.Length-1)
{
dataIdx++;
resultData[dataIdx] = new LocSumpLogData();
lsData = resultData[dataIdx];
}
}
}
while (true);
return resultData;
}
/// <summary>
/// データ取得
/// </summary>
/// <returns>double配列</returns>
public LocSumpLogData[] getScanData()
{
string str;
if (fsr == null) return new LocSumpLogData[0];
long allComNum;
allComNum = getNumCommand();
if (allComNum == 0)
{
allComNum = getNumCommand();
if (allComNum == 0)
{
// 対応するログファイルではない
return null;
}
}
LocSumpLogData[] resultData = new LocSumpLogData[allComNum];
LocSumpLogData lsData;
int dataIdx = -1;
do
{
str = fsr.ReadLine();
if (str == null) break;
if (str.Length == 0) continue;
string[] dataWord = str.Split(',');
switch (dataWord[0])
{
case "$GPRMC": // RE
{
double ido = 0.0;
dataIdx++;
resultData[dataIdx] = new LocSumpLogData();
lsData = resultData[dataIdx];
// 定世界時(UTC）での時刻。日本標準時は協定世界時より9時間進んでいる。hhmmss.ss
lsData.ms = ParseMS(dataWord[1]);
// dataWord[2] A,V ステータス。V = 警告、A = 有効
{
// dataWord[3] 緯度。dddmm.mmmm
string[] dataGPS = dataWord[3].Split('.');
//ido = double.Parse(dataGPS[0]);
//lsData.GPSLandY = -(double.Parse(dataGPS[0]) + (double.Parse(dataGPS[1])/60.0)) * GPSScale;
string strDo = dataWord[3].Substring(0, dataGPS[0].Length - 2);
string strHun = dataWord[3].Substring(strDo.Length, dataWord[3].Length - strDo.Length);
lsData.GPSLandY = -(double.Parse(strDo) * 60.0 + (double.Parse(strHun))) * GPSScaleY;//GPSScale;
ido = double.Parse(strDo);
}
// dataWord[4] N,S N = 北緯、South = 南緯
{
// dataWord[5] 経度。dddmm.mmmm
string[] dataGPS = dataWord[5].Split('.');
//lsData.GPSLandX = (double.Parse(dataGPS[0]) + (double.Parse(dataGPS[1])) / 60.0) * (GPSScale * Math.Cos((ido / 100.0) * Math.PI / 180.0));
string strDo = dataWord[5].Substring(0, dataGPS[0].Length - 2);
string strHun = dataWord[5].Substring(strDo.Length, dataWord[5].Length - strDo.Length);
lsData.GPSLandX = (double.Parse(strDo) * 60.0 + (double.Parse(strHun))) * GPSScaleX;//(GPSScale * Math.Cos(ido * Math.PI / 180.0));
}
// dataWord[6] E = 東経、West = 西経
// dataWord[7] 地表における移動の速度。000.0～999.9[knot]
// dataWord[8] 地表における移動の真方位。000.0～359.9度
// dataWord[9] 協定世界時(UTC）での日付。ddmmyy
// dataWord[10] 磁北と真北の間の角度の差。000.0～359.9度
// dataWord[11] 磁北と真北の間の角度の差の方向。E = 東、W = 西
// dataWord[12] モード, N = データなし, A = Autonomous（自律方式）, D = Differential（干渉測位方式）, E = Estimated（推定）
// dataWord[13] チェックサム
lsData.bGPS = true;
}
break;
case "$GPGGA":
// ※未対応
break;
case "$GPGSA":
// ※未対応
break;
}
if (str.Length > 0 && str.Substring(str.Length - 1, 1) == "$")
{
// １データ完了
if (dataIdx < resultData.Length - 1)
{
dataIdx++;
resultData[dataIdx] = new LocSumpLogData();
lsData = resultData[dataIdx];
}
}
}
while (true);
return resultData;
}
/// <summary>
/// ログデータ表示
/// </summary>
/// <param name="index"></param>
private void ShowLogData(int index)
{
if( null == LogData ) return;
LocSumpLogData lsData = LogData[index];
nowLsData = lsData;
// 時間
{
long hour = lsData.ms/(60*60*1000);
long min = (lsData.ms-(hour*(60*60*1000)))/(60*1000);
double ms = (lsData.ms - (((hour * (60 * 60)) + min*60) * 1000))/1000.0;
Lbl_Time.Text = hour.ToString("D2") + ":" + min.ToString("D2") + ":" + ms.ToString("00.0000");
}
// Send Handle,ACC
if (lsData.bSend)
{
if (lsData.bSendACC)
{
Lbl_Handle.Enabled = true;
Lbl_ACC.Enabled = true;
}
if (lsData.bSendLED)
{
Lbl_LED.Enabled = true;
}
Lbl_Handle.Text = lsData.sendHandle.ToString("f3");
Lbl_ACC.Text = lsData.sendACC.ToString("f3");
Lbl_LED.Text = lsData.sendLED.ToString();
drawLsData.sendHandle = lsData.sendHandle;
drawLsData.sendACC = lsData.sendACC;
tb_SendUnknown.Text = lsData.sendUnkownStr;
}
else
{
Lbl_Handle.Enabled = false;
Lbl_ACC.Enabled = false;
Lbl_LED.Enabled = false;
//Lbl_Handle.Text = "ND";
//Lbl_ACC.Text = "ND";
}
// A1 RE
if (lsData.bRE)
{
Lbl_RE.Enabled = true;
Lbl_RE.Text = lsData.REL.ToString() + "," + lsData.RER.ToString();
drawLsData.REL = lsData.REL;
drawLsData.RER = lsData.RER;
}
else
{
Lbl_RE.Enabled = false;
//Lbl_RE.Text = "ND";
}
// A2 Compus
if (lsData.bCompus)
{
Lbl_Compus.Enabled = true;
Lbl_Compus.Text = lsData.CompusDir.ToString();
drawLsData.CompusDir = lsData.CompusDir;
}
else
{
Lbl_Compus.Enabled = false;
//Lbl_Compus.Text = "ND";
}
// A3 GPS
if (lsData.bGPS)
{
Lbl_GPS_X.Enabled = true;
Lbl_GPS_Y.Enabled = true;
Lbl_GPS_X.Text = lsData.GPSLandX.ToString();
Lbl_GPS_Y.Text = lsData.GPSLandY.ToString();
drawLsData.GPSLandX = lsData.GPSLandX;
drawLsData.GPSLandY = lsData.GPSLandY;
}
else
{
Lbl_GPS_X.Enabled = false;
Lbl_GPS_Y.Enabled = false;
//Lbl_GPS_X.Text = "ND";
}
// A4 REPlot
if (lsData.bREPlot)
{
Lbl_REPlotX.Enabled = true;
Lbl_REPlotY.Enabled = true;
Lbl_REPlotDir.Enabled = true;
Lbl_REPlotX.Text = lsData.REPlotX.ToString("f3");
Lbl_REPlotY.Text = lsData.REPlotY.ToString("f3");
Lbl_REPlotDir.Text = lsData.REPlotDir.ToString("f3");
drawLsData.REPlotX = lsData.REPlotX;
drawLsData.REPlotY = lsData.REPlotY;
drawLsData.REPlotDir = lsData.REPlotDir;
}
else
{
Lbl_REPlotX.Enabled = false;
Lbl_REPlotY.Enabled = false;
Lbl_REPlotDir.Enabled = false;
//Lbl_REPlotX.Text = "ND";
}
// LocSump
// R1
if (lsData.bR1)
{
Lbl_R1_X.Enabled = true;
Lbl_R1_Y.Enabled = true;
Lbl_R1_Dir.Enabled = true;
Lbl_R1_X.Text = lsData.R1_X.ToString("f3");
Lbl_R1_Y.Text = lsData.R1_Y.ToString("f3");
Lbl_R1_Dir.Text = lsData.R1_Dir.ToString("f3");
drawLsData.R1_X = lsData.R1_X;
drawLsData.R1_Y = lsData.R1_Y;
drawLsData.R1_Dir = lsData.R1_Dir;
}
else
{
Lbl_R1_X.Enabled = false;
Lbl_R1_Y.Enabled = false;
Lbl_R1_Dir.Enabled = false;
//Lbl_R1_X.Text = "ND";
}
// E1
if (lsData.bE1)
{
Lbl_E1_X.Enabled = true;
Lbl_E1_Y.Enabled = true;
Lbl_E1_Dir.Enabled = true;
Lbl_E1_X.Text = lsData.E1_X.ToString("f3");
Lbl_E1_Y.Text = lsData.E1_Y.ToString("f3");
Lbl_E1_Dir.Text = lsData.E1_Dir.ToString("f3");
drawLsData.E1_X = lsData.E1_X;
drawLsData.E1_Y = lsData.E1_Y;
drawLsData.E1_Dir = lsData.E1_Dir;
}
else
{
Lbl_E1_X.Enabled = false;
Lbl_E1_Y.Enabled = false;
Lbl_E1_Dir.Enabled = false;
//Lbl_R1_X.Text = "ND";
}
tb_ResiveUnknown.Text = lsData.resiveUnkownStr + lsData.locsumpUnkownStr;
UpdateSubWindow();
}
/// <summary>
/// データ取得
/// </summary>
/// <returns>double配列</returns>
public LocSumpLogData[] getScanData()
{
string str;
bool oldVer = false; // 識別子のない旧バージョンファイル
if (fsr == null) return new LocSumpLogData[0];
long allComNum;
allComNum = getNumCommand();
if (allComNum == 0)
{
allComNum = getNumCommand(true);
oldVer = true;
if (allComNum == 0)
{
// 対応するログファイルではない
return null;
}
}
LocSumpLogData[] resultData = new LocSumpLogData[allComNum];
int dataIdx = -1;
long startMs = 0;
do
{
str = fsr.ReadLine();
if (str == null) break;
if (str.Length == 0) continue;
/*
2015/10/17 15:09:08.151
hwSendStr:/AC,0.01,0.12
hwResiveStr:A4,174.8945,-2.71978620181e-14,0.0,0.0$A2,175.0494,343
$
handle:0.0100482320785522 / acc:0.12
R1:X 150/ Y 150/ Dir 0
V1:X 150/ Y 150/ Dir 0
E1:X 150/ Y 150/ Dir 0
*/
if ((!oldVer && IsLineHeader(str)) || (oldVer && IsLineHeader_Time(str)))
{
dataIdx++;
string[] strTimeStump = str.Split(' ');
string[] strTime = strTimeStump[1].Split(':');
// 時、分、秒
long readMS = (int.Parse(strTime[0])*(60*60) + int.Parse(strTime[1])*60)*1000 + (int)(double.Parse(strTime[2])*1000);
resultData[dataIdx] = new LocSumpLogData();
if (dataIdx == 0)
{
startMs = readMS;
resultData[dataIdx].ms = 0;
}
else
{
resultData[dataIdx].ms = readMS - startMs;
}
continue;
}
LocSumpLogData lsData = resultData[dataIdx];
string[] strSplitData = str.Split(':');
switch (strSplitData[0])
{
case "hwSendStr":
{
string[] strSendCommand = strSplitData[1].Split('/');
lsData.bSend = true;
foreach (var OneCommand in strSendCommand)
{
string[] dataWord = OneCommand.Split(',');
switch (dataWord[0])
{
case "AC":
lsData.sendHandle = double.Parse(dataWord[1]);
lsData.sendACC = double.Parse(dataWord[2]);
lsData.bSendACC = true;
break;
case "AL":
lsData.sendLED = int.Parse(dataWord[1]);
lsData.bSendLED = true;
break;
default:// 未対応 コマンド
lsData.sendUnkownStr += OneCommand;
break;
}
}
}
break;
case "hwResiveStr":
{
string readLine = strSplitData[1];
bool bReadEnd = false;
do
{
string[] strSendCommand = readLine.Split('$');
if (readLine[readLine.Length - 1] == '$') bReadEnd = true;
foreach (var OneCommand in strSendCommand)
{
string[] dataWord = OneCommand.Split(',');
switch (dataWord[0])
{
case "AC":
case "AL":
// 送信結果
break;
case "A1": // RE
// dataWord[1] ms
lsData.RER = double.Parse(dataWord[2]);
lsData.REL = double.Parse(dataWord[3]);
lsData.bRE = true;
break;
case "A2": // Compus
// dataWord[1] ms
lsData.CompusDir = int.Parse(dataWord[2]);
lsData.bCompus = true;
break;
case "A3": // GPS
// dataWord[1] ms
{
string[] dataGPS = dataWord[2].Split('.');
lsData.GPSLandX = double.Parse(dataGPS[0]) + (double.Parse(dataGPS[1]) / 60.0);
}
{
string[] dataGPS = dataWord[3].Split('.');
lsData.GPSLandY = double.Parse(dataGPS[0]) + (double.Parse(dataGPS[1]) / 60.0);
}
lsData.bGPS = true;
break;
case "A4": // RE PlotX,Y
// dataWord[1] ms
lsData.REPlotX = double.Parse(dataWord[2]);
lsData.REPlotY = double.Parse(dataWord[3]);
lsData.REPlotDir = double.Parse(dataWord[4]);
lsData.bREPlot = true;
break;
default:
lsData.resiveUnkownStr += OneCommand;
break;
}
}
if (!bReadEnd)
{
readLine = fsr.ReadLine();
}
} while (!bReadEnd);
}
break;
case "handle":
// ※未対応
break;
case "R1":
{
lsData.bR1 = true;
string[] strSendCommand = strSplitData[1].Split('/');
foreach (var OneCommand in strSendCommand)
{
string[] dataWord = OneCommand.Trim().Split(' ');
switch (dataWord[0])
{
case "X":
lsData.R1_X = double.Parse(dataWord[1]);
break;
case "Y":
lsData.R1_Y = double.Parse(dataWord[1]);
break;
case "Dir":
lsData.R1_Dir = double.Parse(dataWord[1]);
break;
default:// ※未対応
break;
}
}
}
break;
case "V1":
// ※未対応 パーティクルフィルタ自己位置推定座標
break;
case "C1":
// ※未対応 コンパス座標
break;
case "E1":
{
lsData.bE1 = true;
string[] strSendCommand = strSplitData[1].Split('/');
foreach (var OneCommand in strSendCommand)
{
string[] dataWord = OneCommand.Trim().Split(' ');
switch (dataWord[0])
{
case "X":
lsData.E1_X = double.Parse(dataWord[1]);
break;
case "Y":
lsData.E1_Y = double.Parse(dataWord[1]);
break;
case "Dir":
lsData.E1_Dir = double.Parse(dataWord[1]);
break;
default:// ※未対応
break;
}
}
}
break;
default:
lsData.locsumpUnkownStr += str;
break;
}
}
while (true);
return resultData;
}
/// <summary>
/// GPS軌跡描画
/// </summary>
/// <param name="g"></param>
private void DrawGPSData(Graphics g, double angle, LocSumpLogData[] lcData )
{
int drawNum = 10; // 色変えの単位
int px = 0;
int py = 0;
double fstX, fstY;
bool bFst = true;
fstX = fstY = 0.0;
double dCos = Math.Cos((angle) * Math.PI / 180.0);
double dSin = Math.Sin((angle) * Math.PI / 180.0);
for (int i = 0; i < lcData.Length / drawNum; i++)
{
int n = i * drawNum;
int en = lcData.Length - n;
if (en > drawNum) en = drawNum;
Point[] drawLinePos = new Point[en+1];
// 前回のポイントから途切れないように。
drawLinePos[0] = new Point(px, py);
for (int iR = 0; iR < en; iR++)
{
if (lcData[iR + n].bGPS)
{
if (bFst)
{
bFst = false;
fstX = lcData[iR + n].GPSLandX;
fstY = lcData[iR + n].GPSLandY;
}
double x = (lcData[iR + n].GPSLandX - fstX);
double y = (lcData[iR + n].GPSLandY - fstY);
px = (int)((x * dCos - y * dSin) * LRF_ScaleOfPixel);
py = (int)((x * dSin + y * dCos) * LRF_ScaleOfPixel);
}
drawLinePos[iR+1] = new Point(px, py);
}
// 色変え
if (i % 2 == 0)
{
g.DrawLines(Pens.Green, drawLinePos);
}
else
{
g.DrawLines(Pens.Lime, drawLinePos);
}
}
}
