/// <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) == "$") { // 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) == "$") { // 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); } } }