-
-
Save dj1711572002/ff13484e9d512b9aeb402bc475577b09 to your computer and use it in GitHub Desktop.
C# SkiTracer RTK ski data Animation Pgm
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
using System; | |
using System.Collections.Generic; | |
using System.ComponentModel; | |
using System.Data; | |
using System.Drawing; | |
using System.Linq; | |
using System.Text; | |
using System.Threading.Tasks; | |
using System.Windows.Forms; | |
using System.IO; | |
using System.IO.Ports; | |
using System.Diagnostics; | |
using System.Threading; | |
using static System.Windows.Forms.VisualStyles.VisualStyleElement; | |
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Button; | |
using static System.Windows.Forms.AxHost; | |
using System.Drawing.Drawing2D; | |
using System.Drawing.Text; | |
using System.Xml.Linq; | |
using static System.Net.Mime.MediaTypeNames; | |
using static System.Windows.Forms.LinkLabel; | |
//using System.Runtime.Remoting.Channels; | |
using System.Security.Policy; | |
using System.Text.RegularExpressions; | |
using System.Security.Cryptography; | |
using static System.Net.WebRequestMethods; | |
using File = System.IO.File; | |
using System.Runtime.Intrinsics.X86; | |
using static System.Windows.Forms.VisualStyles.VisualStyleElement.ToolBar; | |
using System.Security.Cryptography.X509Certificates; | |
using System.Diagnostics.Eventing.Reader; | |
using System.Windows.Forms.VisualStyles; | |
using System.Reflection.Emit; | |
using static System.Formats.Asn1.AsnWriter; | |
using System.Xml.Schema; | |
using System.Security.AccessControl; | |
using System.Collections; | |
using System.Data.Common; | |
namespace STA23_SkiTracer_rev07 | |
{ | |
//...................................................................................................................................................... | |
public partial class Form1 : Form | |
{ | |
#region [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[struct]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]] | |
//converts string/char into ASCII | |
byte[] encoding = System.Text.ASCIIEncoding.ASCII.GetBytes("Hello, World!"); | |
int j, rN; | |
int stopflag = 0; | |
int selectPos, stopPos;//richtextbox 行位置 | |
string mode, modeset; | |
string[] row0, row1, row2, row3, row4;//data 格納 | |
int writeflag; | |
String name;//filename | |
string[] sline;// = new string[150000]; | |
//****************************dgvデータ構造体************************************** | |
//NAV | |
//%%%%%%%%%struct pvt%%%%%%%%%% | |
struct pvt | |
{ | |
public uint itow; | |
public ushort year; | |
public byte month; | |
public byte day; | |
public byte hour; | |
public byte min; | |
public byte sec; | |
public byte valid; | |
public uint tacc; | |
public int nano; | |
public byte fixtype; | |
public byte flags; | |
public byte flags2; | |
public byte numsv; | |
public int lon; | |
public int lat; | |
public int height; | |
public int hmsl; | |
public uint hacc; | |
public uint vacc; | |
public int veln; | |
public int vele; | |
public int veld; | |
public int gspeed; | |
public int headmot; | |
public uint sacc; | |
public uint headacc; | |
public ushort pdop; | |
public byte flags3; | |
public byte resrv0; | |
public byte resrv1; | |
public byte resrv2; | |
public byte resrv3; | |
public byte resrv4; | |
public int headveh; | |
public short magdec; | |
public ushort magacc; | |
};//struct pvt end | |
//%%%%%%%%struct relp%%%%%%%%%%%%%%%%%%%%% | |
struct relp | |
{ | |
public byte version; | |
public byte reserved1; | |
public ushort refstationid; | |
public uint itow; | |
public int relposn; | |
public int relpose; | |
public int relposd; | |
public int relposlength; | |
public int relposheading; | |
public int reserved2; | |
public sbyte relposhpn; | |
public sbyte relposhpe; | |
public sbyte relposhpd; | |
public sbyte relposhplength; | |
public uint accn; | |
public uint acce; | |
public uint accd; | |
public uint accheading; | |
public uint acclength; | |
public uint reserved3;//4byte | |
public uint flags; | |
};//struct relp end | |
//BNO struct | |
// KIJUN struct | |
struct kijun | |
{ | |
public double rNaveN; | |
public double rEaveN; | |
public double rDaveN; | |
public double rLaveN; | |
public double rNstd; | |
public double rEstd; | |
public double rDstd; | |
public double rLstd; | |
public double rLmaveN; | |
public double rLmstd; | |
public double mHead; | |
public int pday; | |
public int phour; | |
public int pmin; | |
public int psec; | |
}; | |
kijun kave;//基準点 | |
struct bno | |
{ | |
//Euler Angle from Quarternion | |
public float[] eX; | |
public float[] eY; | |
public float[] eZ; | |
//Quzrternion | |
public float[] qW; | |
public float[] qX; | |
public float[] qY; | |
public float[] qZ; | |
//Linear Acc | |
public float[] laX; | |
public float[] laY; | |
public float[] laZ; | |
//Gravity acc | |
public float[] grX; | |
public float[] grY; | |
public float[] grZ; | |
//time stamp | |
public int[] tst;// | |
/* | |
public bno(float[] eX, float[] eY, float[] eZ, float[] qW, float[] qX, float[] qY, float[] qZ, float[] laX, float[] laY, float[] laZ, float[] grX, float[] grY, float[] grZ, int[] tst) | |
{ | |
this.eX = eX; | |
this.eY = eY; | |
this.eZ = eZ; | |
this.qW = qW; | |
this.qX = qX; | |
this.qY = qY; | |
this.qZ = qZ; | |
this.laX = laX; | |
this.laY = laY; | |
this.laZ = laZ; | |
this.grX = grX; | |
this.grY = grY; | |
this.grZ = grZ; | |
this.tst = tst; | |
} | |
*/ | |
} //%%%%%%%%struct dgvchk%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
//生データを検査して、有効データ範囲を得る | |
struct dgvchk | |
{ | |
public int maxrow;//Row.Count | |
public int startrow;// | |
public int endrow;// | |
public int maxrn;//data エポック数 | |
public int startrn;//有効スタートrn | |
public int endrn;//有効エンドrn | |
public int miss;//itow飛び個数 | |
public double missper;//miss% | |
public int[] fmiss;//smode[]範囲内でのflag 67未満の個数 | |
public double[] fmissper;//fmiss% | |
// mode | |
public int[] smode_startNo;//=new int[10];//s] modeの開始番号 配列 | |
public int[] smode_endNo;//=new int[10];//s] modeの終了番号 配列 | |
public int smodeN;//測定ファイル内のsmode範囲の個数 | |
//time | |
public int[] smode_startitow; | |
public int[] smode_enditow; | |
public int[] smode_time;//sec | |
// digit error | |
public string[,] digit_err;//itow+err.string | |
public int[] digit_errN;//[smodeN] | |
//itow error | |
public string[,] itow_error;// 足りないitow.Tostring() | |
public int[] itow_errN;//[smodeN] | |
public double[] itow_errper;//% err | |
// turn data | |
public int[] tnrow;//tnrow[行番号]=ターン番号 配列 dgv1 38列目にも代入 | |
public int[] smodeturnN;// | |
//process status | |
public int dgvsorted;//=0 Not Sort =1 Sort finished | |
public int turnprocessed;//Nutral() passed count | |
public int mathmaxrow; | |
public int mathminrow; | |
public int mathaverow; | |
}; | |
dgvchk dc;//1測定dgvに一個 | |
//****************************位置ポイント構造体************************************ | |
struct basep | |
{ | |
public double Ny; | |
public double Ex; | |
public double Dz; | |
public double Height; | |
public double gSpeed; | |
public double headMot; | |
public double mHead; | |
public int flags; | |
public double pdop; | |
//data n | |
public int rn; | |
public int itow; | |
public int tp; | |
public int miss; | |
}; | |
struct rovrp | |
{ | |
public double Ny; | |
public double Ex; | |
public double Dz; | |
public double Height; | |
public double gSpeed; | |
public double headMot; | |
public double mHead; | |
public int flags; | |
public double pdop; | |
//data n | |
public int rn; | |
public int itow; | |
public int tp; | |
public int miss; | |
}; | |
basep[] bp = new basep[50000]; | |
rovrp[] rp = new rovrp[50000]; | |
//******************* ターン構造体 turns******************************************************** | |
struct Turns | |
{ | |
//Public rNturnNo() ;// Integer //rNturnNo(rN)=turnNo | |
public string skiRL; //左右スキーの指定(0)番に収納 | |
public int turnNo; //ターン番号 | |
public string direction;// //ターン方向 R L | |
public int lastNo;// //ターンの数tR[0]番に収納 | |
public int startNo, endNo;// //開始データ行No | |
public int stitow, enditow;//itow | |
public double stheadmot, endheadmot;//headmot | |
public double stmhead, endmhead;//mhead | |
public double[] NY; | |
public double[] EX; | |
public double[] EZ; | |
public int initX, initY, endX, endY;// //開始座標、終了座標 X:lon Y:lat | |
public double turnTime;// //ターン時間 | |
public double turnTimesum;// //累積ターン経過時間 | |
//ターン弧 | |
public double Radius;// //簡易円近似半径 | |
public double theta;////fallLine 角度 | |
public int minorDia, majorDia;// //ターン半円の縦短径、横長径 | |
public double Aspectratio; //pectratio;// //ターン弧のアスペクト比 | |
public double fallen;// //fallLine長さ | |
public double fallensum;// // fallLineの累計 | |
public double initHeight, endHeight;// | |
public double fallHeight;// //fallLineの標高差 | |
public double fallHeightsum;////fallLine標高差の累計 | |
public double Slopedeg;// //fallLineの斜度deg | |
public double Arclen;// //円弧長 | |
public double Arclensum;////円弧長の累計 | |
//Speed | |
public double[] Speed;////Speed配列 | |
public double inSpeed, outSpeed, aveSpeed, maxSpeed, minSpeed;////ターン中の速度各種 | |
//Heading Skidding | |
public double[] headMot;////headMot角配列 | |
public double[] headSki;// //ski角配列 | |
public double[] Skid;// //Skid角配列 | |
public double maxHeadmot, minHeadmot, dHeadmot;// //headMot角各種 | |
public double maxSkid, minSkid, aveSkid, dSkid;// | |
//G Acceleration | |
public double[] Gacc;////平均加速度 | |
public double Gaccmax, Gaccmin, Gaccave;// | |
//Slope ターン開始点標高と終了点標高と傾斜角 | |
public double startHeight; | |
public double lastHeight; | |
public double tSlope; | |
public int xdot;//makeATurnBitmap()で使ったドット分解能dot/data tR[0].xdotに収納 | |
//turnのsmode位置情報 | |
public int[] smode_turnStartNo;//smode番号での最初のターン番号tR[0].smode_turnStartNo[]に収納 | |
public int[] smode_turnEndNo;//smode番号での最後のターン番号tR[0].smode_turnEndNo[]に収納 | |
//BNO 補正 | |
public double bnosaR;//tR[].bnosaR 左ターンスタートポイントでheadMotとの差 bnosaR=headMot-yaw1 yaw1hosei=yaw1+hoseiR | |
public double bnosaL;//bnosaL | |
}; | |
//-----------Turns 構造体配列定義------------------------------- | |
Turns[] tR = new Turns[1000];////右スキーのターン構造体配列宣言200個まで収納 | |
Turns[] tL = new Turns[1000];////左スキーのターン構造体配列宣言200個まで収納 | |
//************************************Graphic parameters************************************************************************* | |
const int blen = 3000;//処理データ単位3000行を1ブロックにしてBitmapメモリー節約 | |
//int rN; | |
int rowN;//dgv rowCount | |
String fname; | |
public double[] f; | |
public double[] fout; | |
public double[] fkekka; | |
// | |
int[,] dgv = new int[50000, 16]; | |
double[,] dgv16_MA = new double[50000, 5];//FilteredData:M9N_gSpeed_MA(16),M9N_headMot_MA(17),M9N_velN_MA(18),M9N_velE_MA(19),M9N_velD(20) | |
double[,] dgv21_Igr = new double[50000, 8];//積分要素F9P:igr_F9P_gSpeed(21),igr_F9P_velN(22),igr_F9P_velE(23),igr_F9P_velD(24) | |
//積分要素M9N:igr_M9N_gSpeed_MA(25),igr_M9N_velN_MA(26),igr_M9N_velE_MA(27),igr_M9N_velD_MA(28) | |
//bitmap definition | |
//picturebox size----------- | |
const int pic1w = 1000;//pictrueBox1 幅 | |
const int pic1h = 500;//pictureBox1 高さ | |
const int pic2w = 1000; | |
const int pic2h = 400; | |
const int pic3w = 500; | |
const int pic3h = 500; | |
const int bw = 20000;// blen;//180000 ; | |
const int xsize =100000; | |
const int ysize = 5000; | |
const int xssize =1000; | |
const int yssize = 400; | |
int wakuflag = 0; | |
Bitmap sourceBitmap = new Bitmap(bw, pic1h);//X軸bw個 data(10msec周期で300秒) Y軸 | |
Bitmap trimmedBitmap = new Bitmap(pic1w, pic1h);//PictuerBox1に貼りこむBitmap | |
Bitmap xyBitmap = new Bitmap(xsize, ysize);//xyプロットのソース | |
Bitmap xysBitmap = new Bitmap(xssize, yssize);//xyプロットのソース拡大用Bitmapとしてポイントサイズを1に最小化した | |
Bitmap p2Bitmap = new Bitmap(pic2w, pic2h);//picturebox2のBitmap | |
Bitmap p2sBitmap = new Bitmap(pic2w, pic2h);//picturebox2の拡大用Bitmapとしてポイントサイズを1に最小化した | |
Bitmap p2wBitmap = new Bitmap(pic2w, pic2h);//picturebox2の枠用Bitmap | |
Bitmap p3Bitmap = new Bitmap(pic3w, pic3h);//picturebox2のBitmap | |
//-XYPLOT----------------- | |
struct XYRLplot//xy plotでRLに必要な変数を収納 | |
{ | |
public int tn0;//開始ターン番号 | |
public int tn1;//最終ターン番号 | |
public int pw;//picture width | |
public int ph;//picture height | |
//プロット関連データ | |
public int xmax; | |
public int xmin; | |
public int ymax; | |
public int ymin; | |
public int xmaxR;// 4ポイントのx最大値 | |
public int ymaxR;// 4ポイントのy最大値 | |
public int xminR;// 4ポイントのx最小値 | |
public int yminR;// 4ポイントのy最小値 | |
public int xmaxL;// 4ポイントのx最大値 | |
public int ymaxL;// 4ポイントのy最大値 | |
public int xminL;// 4ポイントのx最小値 | |
public int yminL;// 4ポイントのy最小値 | |
public int xmaxRm;// 4ポイントのx最大値 | |
public int ymaxRm;// 4ポイントのy最大値 | |
public int xminRm;// 4ポイントのx最小値 | |
public int yminRm;// 4ポイントのy最小値 | |
public int xmaxLm;// 4ポイントのx最大値 | |
public int ymaxLm;// 4ポイントのy最大値 | |
public int xminLm;// 4ポイントのx最小値 | |
public int yminLm;// 4ポイントのy最小値 | |
public double scale1; | |
public double xscale1; | |
public double yscale1; | |
public double width; | |
public double height; | |
public double rotangle; | |
public double distance; | |
}; | |
XYRLplot xyrldt; | |
XYRLplot rl; | |
//-------------------データブロック構造体---------------------------- | |
public struct data_block | |
{ | |
public int blockSize; | |
public int blockNo; | |
public int startRow; | |
public int endRow; | |
public long maxlon; | |
public long minlon; | |
public long maxlat; | |
public long minlat; | |
public long lonW;//全幅 | |
public long latH;//全高 | |
public int[] nutralN;//ニュートラルポイントの行No 配列 | |
} | |
struct lines | |
{ | |
public string name;//LIne名 選択されたデータ名 | |
public bool available;//Lineが有効:true 無効:false | |
public int colN;//Column番号 | |
public Color clr; | |
public int sa;//data 間隔 | |
public int rN_avail; | |
public int period; | |
public int x; | |
public int x_1; | |
public int y; | |
public int y_1; | |
public double max; | |
public double min; | |
public int baseL;//Graph base Line 1/2 1/1 | |
public int uscale;//headMot=100000 | |
} | |
//===========ソースビットマップへプロットした座標データ配列をGLOBAL化============================================== | |
public static int[] pxRg = new int[10000]; | |
public static int[] pyRg = new int[10000]; | |
public static int[] pxmRg = new int[10000]; | |
public static int[] pymRg = new int[10000]; | |
public static int[] pxLg = new int[10000]; | |
public static int[] pyLg = new int[10000]; | |
public static int[] pxmLg = new int[10000]; | |
public static int[] pymLg = new int[10000]; | |
public static int[] tN0rN = new int[10000];//turn開始番号の行番号格納 | |
public static int[] row2turnN = new int[10000]; | |
//Struct GraphLine | |
/* | |
//---ubx 変換データ--- | |
byte[] PVTdata; | |
byte RELPdata; | |
long[] PVTval;// [33]; | |
long[] PVT1val;// [33]; | |
long[] RELPOSval;// [21]; | |
long[] RELPOS1val;// [21]; | |
long[] PVTBval;// [33];//Base PVT | |
long[] PVTRval;// [33]; | |
long[] RELPOSBval;//[21]; | |
long[] RELPOSRval;// [21]; | |
//---------------------- | |
byte[] RELPOSBdata;// [172];//base line RELPOS | |
byte[] RELPOS1Bdata;// [172];//base line RELPOS | |
*/ | |
//=====-BNO055 imu================ | |
int[] tst = { 0, 1, 2 }; | |
int BNOinn, BNOout; | |
//eX[no]=yaw;eY[no]=pitch;eZ[no]=roll; | |
static float bnHosei;//mHeadに合わせる補正値 mHead=bnHosei+yaw | |
static double yaw, pitch, roll; | |
static byte[] fb; | |
static float[] eX = { };//yaw | |
static float[] eY = { };//pitch | |
static float[] eZ = { };//roll | |
static float[] qW = { }; | |
static float[] qX = { }; | |
static float[] qY = { }; | |
static float[] qZ = { }; | |
static float[] aX = { }; | |
static float[] aY = { }; | |
static float[] aZ = { }; | |
//Linear Acc | |
static float[] laX = { }; | |
static float[] laY = { }; | |
static float[] laZ = { }; | |
//Gravity | |
static float[] grX = { }; | |
static float[] grY = { }; | |
static float[] grZ = { }; | |
public Color[] samples = new Color[] { Color.Red, Color.Blue, Color.Green, Color.Yellow, Color.Aqua, Color.DeepPink, Color.BlanchedAlmond, Color.Orange }; | |
lines[] gline;//gline global | |
//RL para | |
int rloff = 0; | |
#endregion | |
#region [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[formset]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]] | |
public Form1() | |
{ | |
InitializeComponent(); | |
} | |
//...................................................................................................................................................... | |
//...................................................................................................................................................... | |
private void Form1_Load(object sender, EventArgs e) | |
{ | |
this.Text = "STA23_SkiTracer_rev07"; | |
richTextBox1.Visible = false; | |
dgv1.Visible = false; | |
/* | |
string[] ports = SerialPort.GetPortNames(); | |
foreach (string port in ports) | |
{ | |
portComboBox.Items.Add(port); | |
} | |
if (portComboBox.Items.Count > 0) | |
{ | |
// portComboBox.SelectedIndex = 1; | |
rN = 0; | |
} | |
writeflag = 0; | |
*/ | |
//------------------------------ | |
// serialPort1.Encoding = System.Text.Encoding.GetEncoding(932); | |
} | |
//..... ................................................................................................................................................. | |
//...................................................................................................................................................... | |
delegate void SetTextCallback(string text); | |
//...................................................................................................................................................... | |
/* | |
private void Response(string text) | |
{ | |
if (textBox1.InvokeRequired) | |
{ | |
SetTextCallback d = new SetTextCallback(Response); | |
BeginInvoke(d, new object[] { text }); | |
} | |
else | |
{ | |
string[] data = (text).Split(',');//,//);//カンマ区切りにばらす | |
int dnum = data.Length; | |
label6.Text = data[0]; | |
//char[] chararray = text.ToCharArray(); | |
//File.AppendAllLines(@"d:\mydata.csv",chararray, Encoding.GetEncoding("shift-jis")); | |
if (writeflag == 0) | |
{ | |
DateTime dt = DateTime.Now; | |
name = dt.ToString($"{dt:MMddHHmmss}"); | |
} | |
using (StreamWriter sw = new StreamWriter(@"C:\RTK_LOG\" + name + ".csv", true, Encoding.GetEncoding("shift-jis"))) | |
{ | |
writeflag = 1; | |
// sw.WriteLine(text); | |
sw.Write(text); | |
} | |
rN++; | |
textBox1.Text = text; | |
if (checkBox1.Checked == true) | |
{ | |
//if (data[0] == "[m]") | |
//{ | |
richTextBox1.AppendText(text + "<" + dnum.ToString() + ">"); | |
//} | |
} | |
} | |
} | |
*/ | |
private void checkBox2_CheckedChanged(object sender, EventArgs e) | |
{ | |
} | |
/* | |
//...................................................................................................................................................... | |
private void serialPort1_DataReceived(object sender, SerialDataReceivedEventArgs e) | |
{ | |
if (stopflag == 0) | |
{ | |
string str = serialPort1.ReadLine(); | |
Response(str); | |
} | |
} | |
*/ | |
//...................................................................................................................................................... | |
/* | |
private void serialPort2_DataReceived(object sender, SerialDataReceivedEventArgs e) | |
{ | |
if (stopflag == 0) | |
{ | |
string str2 = serialPort2.ReadLine(); | |
Response(str2); | |
} | |
} | |
*/ | |
//...................................................................................................................................................... | |
private void button4_Click(object sender, EventArgs e) | |
{ | |
/* | |
if (serialPort1.IsOpen) | |
{ | |
serialPort1.Write("s");//Start Log | |
label6.Text = "Save modeset"; | |
} | |
*/ | |
} | |
//...................................................................................................................................................... | |
private void button5_Click(object sender, EventArgs e) | |
{ | |
/* | |
if (serialPort1.IsOpen) | |
{ | |
serialPort1.Write("q");//Quit log | |
label6.Text = "Quit modeset"; | |
} | |
*/ | |
} | |
private void textBox1_TextChanged(object sender, EventArgs e) | |
{ | |
string mes = textBox1.Text; | |
string head = mes.Substring(0, 3); | |
} | |
//...................................................................................................................................................... | |
private void button6_Click(object sender, EventArgs e) | |
{ | |
} | |
private void checkBox1_CheckedChanged(object sender, EventArgs e) | |
{ | |
if (checkBox1.Checked == true) | |
{ | |
richTextBox1.Visible = true; | |
} | |
else | |
{ | |
richTextBox1.Visible = false; | |
} | |
} | |
//...................................................................................................................................................... | |
private void button7_Click(object sender, EventArgs e) | |
{ | |
} | |
private void checkBox3_CheckedChanged(object sender, EventArgs e) | |
{ | |
if (checkBox3.Checked == true) | |
{ | |
pictureBox1.Visible = false; | |
dgv1.Visible = true; | |
} | |
else | |
{ | |
dgv1.Visible = false; | |
pictureBox1.Visible = true; | |
} | |
} | |
private void button8_Click(object sender, EventArgs e) | |
{ | |
} | |
//read file | |
private void button9_Click(object sender, EventArgs e) | |
{ | |
//dgv1.Visible = true; | |
ReadDialog(); | |
} | |
#endregion | |
#region [[[[[[[[[[[[[[[[READ DIALOGUE]]]]]]]]]]]]]]]]]]]]]] | |
void rowheader(int n) | |
{ | |
//row header | |
dgv1.RowHeadersWidth = 80; | |
int i; | |
for (i = 0; i < dgv1.Rows.Count; i++) | |
{ | |
dgv1.Rows[i].HeaderCell.Value = (i + n).ToString(); | |
} | |
} | |
void headeradd(int addrow, int dgvn) | |
{ | |
int i; | |
string head0 = "md, flagsB, flagsR, pdopB, pdopR, haccB, haccR, miscount0, count_upB,count_upB0,tp,tp0"; | |
string head1 = ",[Base],itowB, NYB, EXB, DZB, gSpeedB, headMotBZ"; | |
string head2 = ",[Rover] ,itowR,NYR,EXR,DZR,gSpeedR,mHead,bnHosei+eX[0],relLm]"; | |
string head3 = ",BNO],yaw1,pitch1,roll1,laX,laY,laZ,grX,grY,grZ"; | |
string head4 = ",turnN,BNOsa,BNHosei,Drel"; | |
string headspace13 = ",sp1,sp2,sp3,sp4,sp5,sp6,sp7,sp8,"; | |
string headall = head0 + head1 + head2 + head3 + head4; | |
string headdgv3 = head0 + head1 + head2 + head3 + head4+headspace13 + head0 + head1 + head2 + head3 + head4; | |
string[] headArr = headall.Split(','); | |
string[] headArr3 = headdgv3.Split(','); | |
int headlen = headArr.Length; | |
// string head0 = "md(0), flagsB(1), flagsR(2), pdopB(3), pdopR(4), haccB(5), haccR(6), miscount0(7), count_upB(8),count_upB0(9),tp(10),tp0(11)"; | |
//string head1 = ",[Base](12),itowB(13), NYB(14), EXB(15), DZB(16), gSpeedB(17), headMotBZ(18)"; | |
//string head2 = ",[Rover](19) ,itowR(20),NYR(21),EXR(22),DZR(23),gSpeedR(24),mHead(25),bnHosei+eX[0](26),relLm](27)"; | |
//string head3 = ",BNO](28),yaw1(29),pitch1(30),roll1(31),yaw2(32),pitch2(33),roll2(34),yaw3(35),pitch3(36),roll3(37)" | |
if (addrow == 0) | |
{ | |
if (dgvn == 3)//dgv3 | |
{ | |
for (i = 0; i < headArr3.Length; i++) | |
{ | |
dgv3.Columns[i].HeaderText = headArr3[i]; ; | |
dgv3.Rows[0].Cells[i].Value = i; | |
} | |
} | |
else//dgv1 dgv2 | |
{ | |
for (i = 0; i < headlen; i++) | |
{ | |
if (dgvn == 1) | |
{ | |
dgv1.Columns[i].HeaderText = headArr[i]; ; | |
dgv1.Rows[0].Cells[i].Value = i; | |
} | |
if (dgvn == 2) | |
{ | |
dgv2.Columns[i].HeaderText = headArr[i]; ; | |
dgv2.Rows[0].Cells[i].Value = i; | |
} | |
} | |
} | |
} | |
else | |
{ | |
//dgv1.Rows.Add(3); | |
//int hrow = dgv1.Rows.Count -2; | |
for (i = 0; i < headlen; i++) | |
{ | |
// dgv1.Columns[i].HeaderText = headArr[i]; ; | |
dgv1.Rows[dgv1.Rows.Count - 2].Cells[i].Value = headArr[i]; | |
dgv1.Rows[dgv1.Rows.Count - 1].Cells[i].Value = i; | |
} | |
} | |
} | |
//-----------------Read File ---------------------------------------------------- | |
private void ReadDialog() | |
{ | |
sline = new string[150000]; | |
int irn = 0; | |
string sizes = null; | |
var swtch = new System.Diagnostics.Stopwatch(); | |
dgv1.AutoSizeColumnsMode = DataGridViewAutoSizeColumnsMode.None; | |
dgv1.AutoSizeRowsMode = DataGridViewAutoSizeRowsMode.None; | |
dgv1.ColumnHeadersHeightSizeMode = DataGridViewColumnHeadersHeightSizeMode.DisableResizing; | |
dgv3.AutoSizeColumnsMode = DataGridViewAutoSizeColumnsMode.None; | |
dgv3.AutoSizeRowsMode = DataGridViewAutoSizeRowsMode.None; | |
dgv3.ColumnHeadersHeightSizeMode = DataGridViewColumnHeadersHeightSizeMode.DisableResizing; | |
//sourceBitmap_clear(); | |
//OpenFileDialogクラスのインスタンスを作成 | |
OpenFileDialog ofd = new OpenFileDialog(); | |
//ダイアログを表示する | |
if (ofd.ShowDialog() == DialogResult.OK) | |
{ | |
richTextBox1.Clear(); | |
int rowN = 0; | |
int i; | |
int fi = 0; | |
int ic = 0; | |
//OKボタンがクリックされたとき、選択されたファイルを読み取り専用で開く | |
textBox2.Text = ofd.FileName; | |
// Ave ファイルの場合 readkijun()で基準値struct kave読み取り | |
if (textBox2.Text.IndexOf("Ave") >= 0) | |
{ | |
readkijun(textBox2.Text); | |
Debug.Print("[readkijun]:kave.rNaveN=" + kave.rNaveN.ToString() + "kave.rEaveN=" + kave.rEaveN.ToString() + "kave.rDaveN=" + kave.rDaveN.ToString()); | |
} | |
// dgv3ファイルの場合dgvread3()へ | |
if (textBox2.Text.IndexOf("dgv3") >= 0) | |
{ | |
dgv1.Visible = false; | |
dgv2.Visible = false; | |
dgvread3(textBox2.Text); | |
Debug.Print("[dgvread3]:="); | |
} | |
//ubx読み込みdgv RIGHT LEFTファイルの場合readdgv()へ | |
else if (textBox2.Text.IndexOf("dgv") >= 0 && textBox2.Text.IndexOf("ubx")>=0) | |
{ | |
dgv3.Visible = false; | |
readdgv(textBox2.Text); | |
Debug.Print("[readdgv]:="); | |
int dgv1_lastrowN = dgv1.RowCount - 2; | |
int R0turn_row = ig(0, 0) + 11; | |
int TurnNum = dgv1_lastrowN - R0turn_row; | |
} | |
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% MONITOR用ASCIIファイル読み込み%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
//else// Aveファイルでない場合 | |
// { | |
if (checkBox2.Checked == false && textBox2.Text.IndexOf("dgv") < 0 && textBox2.Text.IndexOf("dgv") < 0) | |
{ | |
FileInfo filesize = new FileInfo(ofd.FileName); | |
swtch.Start(); | |
//-----readAllText----- | |
// シフトJISのファイルの読み込み | |
//System.Text.Encoding.RegisterProvider(System.Text.CodePagesEncodingProvider.Instance); // memo: Shift-JISを扱うためのおまじない | |
//string alldata = File.ReadAllText(ofd.FileName, System.Text.Encoding.GetEncoding("Shift_JIS")); | |
string alldata = File.ReadAllText(ofd.FileName); | |
// var wordList = alldata.Replace("\r\n", "\n").Split(new[] { //\n//, //\r// }); | |
string[] sline = alldata.Split('[');//Header [で切取 | |
int ni = 0; | |
string[] tsline = { }; | |
string[] ttsline = { }; | |
/* | |
//binary check---------------------------------------------------- | |
//[mode]最後は無し、[Base]最後に","2C 、[Rover]最後無し、[BNO]最後"LF" 0A がついてる | |
for (ic = 1; ic < (int)(sline.Length ); ic++) | |
{ | |
byte[] data = System.Text.Encoding.ASCII.GetBytes(sline[ic]); | |
int iic; | |
string csline = ""; | |
for (iic = 0; iic < data.Length-2; iic++) | |
{ | |
csline = csline + data[iic].ToString("X") + "-"; | |
} | |
Debug.Print("Read sline=" + sline[ic]); | |
Debug.Print("Binary csline=" + csline); | |
Debug.Print("---------------------------------------------------------------------------------------------"); | |
} | |
//------------------------------------------------------------ | |
*/ | |
for (ic = 1; ic < (int)(sline.Length - 2); ic++) | |
{ | |
//Debug.Print("sline[" + ic.ToString() + "]=" + sline[ic]); | |
tsline = sline[ic].Split("\r");//行内に複数ある\rと余計なコメントを除去 | |
sline[ic] = tsline[0];//再度sline[]に代入 | |
//sline[ic]=slinLengthCheck(sline[ic],ic);//各行の長さチェックして修正 | |
} | |
for (ic = 1; ic < (int)(tsline.Length - 2); ic++) | |
{ | |
//Debug.Print("sline[" + ic.ToString() + "]=" + sline[ic]); | |
ttsline = tsline[ic].Split("\n");//行内に複数ある\nと余計なコメントを除去 | |
sline[ic] = ttsline[0];//再度sline[]に代入 | |
sline[ic] = slinLengthCheck(sline[ic], ic);//各行の長さチェックして修正 | |
} | |
//string[] sline = alldata.Split("\r\n"); | |
//Debug.Print("sline[0]=" + sline[0]); | |
// Debug.Print("sline[1]=" + sline[1]); | |
swtch.Stop(); | |
sizes = sline.Length + "行," + filesize.Length.ToString() + "byte Ellapsed Time=" + swtch.ElapsedMilliseconds.ToString() + "msec"; | |
label8.Text = sizes; | |
if (checkBox3.Checked == true)//dgv visible | |
{ | |
dgv1.Visible = true; | |
dgv1.RowHeadersWidth = 60; | |
//dgv1.TopLeftHeaderCell.Value = "左上"; | |
// richTextBox1.Visible = true; | |
swtch.Reset(); | |
swtch.Start(); | |
Debug.Print("sline.Length=" + sline.Length.ToString()); | |
string ss = "0"; | |
int cr = 0; | |
string datas0 = ""; | |
string datas1 = ""; | |
string datas2 = ""; | |
//4行データを1行にまとめる | |
//[mode]最後は無し、[Base]最後に","2C 、[Rover]最後無し、[BNO]最後"LF" 0A がついてる | |
string dum = ""; | |
for (ic = 1; ic < (int)(sline.Length - 2); ic++) | |
{ | |
//Debug.Print("sline["+ic.ToString()+"]=" + sline[ic].Substring(0, 3).ToString()); | |
if (sline[ic].Length > 2) | |
{ | |
if (sline[ic].Substring(0, 2) == "Ba") | |
{ | |
dgv1.Rows[cr].HeaderCell.Value = cr.ToString(); | |
/* | |
sline[ic - 1] = sline[ic - 1].Substring(0, sline[ic - 1].Length - 2); | |
sline[ic] = sline[ic].Substring(0, sline[ic].Length - 2); | |
sline[ic + 1] = sline[ic + 1].Substring(0, sline[ic + 1].Length - 2); | |
sline[ic + 2] = sline[ic + 2].Substring(0, sline[ic + 2].Length - 3); | |
*/ | |
datas2 = datas1; | |
datas1 = datas0; | |
sline[ic - 1] = sline[ic - 1].Substring(0, sline[ic - 1].Length - 2);//[mode]の最後の"LF"削除 | |
sline[ic] = sline[ic].Substring(0, sline[ic].Length - 1);//[Base]の最後の"LF"削除 | |
sline[ic + 1] = sline[ic + 1].Substring(0, sline[ic + 1].Length - 1);//[Rover]の最後の"LF"削除 | |
sline[ic + 2] = sline[ic + 2].Substring(0, sline[ic + 2].Length - 4);//[BNO]の最後の"LF"削除 | |
ss = sline[ic - 1] + "," + sline[ic] + sline[ic + 1] + "," + sline[ic + 2]; | |
string[] datas = ss.Split(','); // カンマで区切って配列に格 | |
//Debug.Print("datas[0]=" + datas[0],"datas1="+datas1); | |
datas0 = datas[0]; | |
if (datas[0] == "s]") | |
{ | |
dgv1.Rows.Add(datas);//dgv1書き込み | |
cr++; | |
} | |
if (datas1 == "s]" && datas0 == "m]") | |
{ | |
dgv1.Rows.Add(datas);//dgv1書き込み | |
cr++; | |
//Debug.Print("[m] cr=" + cr.ToString()); | |
} | |
} | |
} | |
} | |
rowheader(0); | |
swtch.Stop(); | |
// TimeSpan tst = swtch.ElapsedMilliseconds; | |
label8.Text = sizes + "dgv:" + swtch.ElapsedMilliseconds.ToString() + "msec"; | |
} | |
}//if Checkbox2 Not checked ASCII | |
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Binary UBX FILE ubxread()で処理%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
//Base[PVT,RELPOSNED],Rover[PVT,RELPOSNED],M9N[PVT,PVT,PVT],BNO[0,1,2] | |
//UBX FILE Read & UBX conversion | |
if (textBox2.Text.IndexOf("ubx") >= 0 && textBox2.Text.IndexOf("dgv") < 0 && textBox2.Text.IndexOf("dgv") < 0) | |
{ | |
string fn = textBox2.Text; | |
int epochN = 0; | |
epochN = ubxread(fn);//Choosed Binary File Read & Conversion returun epoch Num | |
rowheader(0); | |
} | |
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Binary File Read & Conversion END %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
// int dgv1_lastrowN = dgv1.RowCount-2; | |
//int R0turn_row = ig(0, 0) + 11; | |
// int TurnNum = dgv1_lastrowN - R0turn_row; | |
}//File read End ******************************************************************************************************************* | |
button11.Enabled = true; | |
} //ReadDialog() end | |
//slineのデータ数をチェックして不足しているLINEはゼロで埋めて、多いLINEはカット | |
//[md]13個 [Base] 7 [Rover] 9個 [BNO] 10個 | |
private string slinLengthCheck(string sl, int ic)//修正したsline返す | |
{ | |
int i; | |
string sl0 = ""; | |
string[] datas = sl.Split(','); // カンマで区切って配列に格納 | |
double id;//string cellstr = dgv1.Rows[iir].Cells[iic].Value.ToString(); | |
//Debug.Print("******slinLengthCheck start: sline[" + ic.ToString() + "]="+sl); | |
if (double.TryParse(datas[0], out id))//文字セルが数値かどうかチェック | |
{ | |
sl0 = "Rover]," + sl; | |
Debug.Print("********************************TOP Error sline[" + ic.ToString() + "]=" + sl + "Repaired sl0=" + sl0); | |
sl = sl0; | |
} | |
else if (sl.Length > 5) //[mode]行 | |
{ | |
if (datas[0] == "m]" || datas[0] == "p]" | datas[0] == "s]") | |
{ | |
if (datas.Length == 13) { }//sl OK | |
else if (datas.Length < 12)//不足 | |
{ | |
Debug.Print("row=" + ic.ToString() + "LengthSmallError [mode]=" + sl); | |
for (i = 0; i < 13 - datas.Length; i++) | |
{ | |
sl = sl + ",0"; | |
} | |
Debug.Print("LengthRepaired" + sl); | |
} | |
else if (datas.Length > 13)// 多い場合カット | |
{ | |
Debug.Print("row=" + ic.ToString() + "LengthLargeError [mode]=" + sl); | |
for (i = 0; i < 13; i++) | |
{ | |
sl0 = sl0 + datas[i] + ","; | |
} | |
sl = sl0; | |
Debug.Print("LengthRepaired" + sl); | |
} | |
} | |
if (datas[0].Substring(0, 2) == "Ba") //[Base]行 | |
{ | |
int baselen = 8; | |
if (datas.Length == baselen) { }//sl OK | |
else if (datas.Length < baselen)//不足 | |
{ | |
Debug.Print("row=" + ic.ToString() + "LengthSmallError [Base]=" + sl); | |
for (i = 0; i < baselen - datas.Length; i++) | |
{ | |
sl = sl + ",0"; | |
} | |
Debug.Print("LengthRepaired" + sl); | |
} | |
else if (datas.Length > baselen)// 多い場合カット | |
{ | |
Debug.Print("row=" + ic.ToString() + "LengthLargeError [Base]=" + sl); | |
for (i = 0; i < baselen; i++) | |
{ | |
sl0 = sl0 + datas[i] + ","; | |
} | |
sl = sl0; | |
Debug.Print("LengthRepaired" + sl); | |
} | |
} | |
if (datas[0].Substring(0, 2) == "Ro")//[Rover]行 | |
{ | |
if (datas.Length == 9) { }//sl OK | |
else if (datas.Length < 9)//不足 | |
{ | |
Debug.Print("row=" + ic.ToString() + "LengthSmallError [Rover]=" + sl); | |
for (i = 0; i < 9 - datas.Length; i++) | |
{ | |
sl = sl + ",0"; | |
} | |
Debug.Print("LengthRepaired" + sl); | |
} | |
else if (datas.Length > 9)// 多い場合カット | |
{ | |
Debug.Print("row=" + ic.ToString() + "LengthLargeError [Rover]=" + sl); | |
for (i = 0; i < 9; i++) | |
{ | |
sl0 = sl0 + datas[i] + ","; | |
} | |
sl = sl0; | |
Debug.Print("LengthRepaired" + sl); | |
} | |
} | |
if (datas[0].Substring(0, 2) == "BN")//[BNO]行 | |
{ | |
int bnolen = 11; | |
if (datas.Length == bnolen) { }//sl OK | |
else if (datas.Length < bnolen)//不足 | |
{ | |
Debug.Print("row=" + ic.ToString() + "LengthSmallError [BNO]=" + sl); | |
for (i = 0; i < bnolen - datas.Length; i++) | |
{ | |
sl = sl + ",0"; | |
} | |
Debug.Print("LengthRepaired" + sl); | |
} | |
else if (datas.Length > bnolen)// 多い場合カット | |
{ | |
Debug.Print("row=" + ic.ToString() + "LengthLargeError [BNO]=" + sl); | |
for (i = 0; i < bnolen; i++) | |
{ | |
sl0 = sl0 + datas[i] + ","; | |
} | |
sl = sl0; | |
Debug.Print("LengthRepaired" + sl); | |
} | |
} | |
} | |
return sl; | |
} | |
#endregion | |
#region [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[DGV3MAKE]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]] | |
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&& | |
// &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&& DGV3MAKE &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&& | |
//&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&& | |
private void button2_Click(object sender, EventArgs e)//dgb3にdgv1とdgv2のターン範囲を同期コピー | |
{ | |
int[] xR = new int[10000]; | |
int[] yR = new int[10000]; | |
int[] xmR = new int[10000]; | |
int[] ymR = new int[10000]; | |
int[] xL = new int[10000]; | |
int[] yL = new int[10000]; | |
int[] xmL = new int[10000]; | |
int[] ymL = new int[10000]; | |
int size =0; | |
//dgv1とdgv2が作成済みチェック | |
if (textBox1.Text!="" && textBox4.Text!="") | |
{ | |
dgv3make(); | |
} | |
} | |
//private void dgv3make(int size, int[] xR, int[] yR, int[] xmR, int[] ymR, int[] xL, int[] yL, int[] xmL, int[] ymL) | |
private void dgv3make()//dgv1,dgv2がそろったらdgv3を作成 | |
{ | |
int tn0 = 0;//開始Turn番号 | |
int dgv1_lastrowN = dgv1.RowCount - 2; | |
int R0turn_row = ig(0, 0) + 11; | |
int TurnNum = dgv1_lastrowN - R0turn_row; | |
int tn1 = TurnNum;//最終Turn番号 | |
int R0turn_startrow = ig(R0turn_row + tn0, 3);//ターンテーブル開始行 | |
int R0turn_endrow = ig(R0turn_row + tn1, 5);//ターンテーブル終了行 | |
int R0itow_start = ig(R0turn_row + tn0, 7); | |
int R0itow_end = ig(R0turn_row + tn1, 9); | |
int R0datarowN = R0turn_endrow - R0turn_startrow; | |
//Left Row start end row探す | |
int g2lastrow = ig(0, 0); | |
int L0turn_startrow = 0;//dgv2Left のRight同期開始行 | |
int L0turn_endrow = 0;//dgv2Left のRight同期終了行 | |
int i; | |
for (i = 0; i < g2lastrow - 3; i++)//dgv2のLeft itowの開始行と終了行探す | |
{ | |
if (ig2(i, 13) == R0itow_start) { L0turn_startrow = i; } | |
if (ig2(i, 13) == R0itow_end) | |
{ | |
L0turn_endrow = i; | |
i = g2lastrow; | |
} | |
} | |
//dgv3へcopy | |
dgv3.RowCount = R0datarowN + 2; | |
dgv3.ColumnCount = 100; | |
//回転前にxR,yRを原点(0,0)に移動するための移動量 dx dy | |
//int dy = (int)Convert.ToDouble(dgv1.Rows[R0turn_startrow ].Cells[14].Value); | |
// int dx = (int)Convert.ToDouble(dgv1.Rows[R0turn_startrow].Cells[15].Value); | |
for (i=0;i<R0datarowN+1;i++) | |
{ | |
Debug.Print("dgv3make:i=" + ti(i)+ "dgv1.Rows[R0turn_startrow + i]=["+ti(R0turn_startrow + i)+ "] dgv2.Rows[L0turn_startrow + i]=["+ti(L0turn_startrow + i)); | |
for (int ic = 0; ic < 50; ic++) | |
{ | |
dgv3.Rows[i+1].Cells[ic].Value = dgv1.Rows[R0turn_startrow + i].Cells[ic].Value; | |
dgv3.Rows[i+1].Cells[ic+50].Value = dgv2.Rows[L0turn_startrow + i].Cells[ic].Value; | |
} | |
//Debug.Print("yL[" + ti(i) + "]=" + ti(yL[i])); | |
} | |
//ターンテーブル後付け | |
int dgv3Turnstart_row = dgv3.RowCount; | |
dgv3.Rows.Add(TurnNum+1); | |
for (i=0;i< TurnNum+1;i++) | |
{ | |
for (int ic = 0; ic < 18; ic++) | |
{ | |
dgv3.Rows[i + dgv3Turnstart_row].Cells[ic].Value = dgv1.Rows[R0turn_row + i].Cells[ic].Value; | |
} | |
} | |
//Turn RowNo書き換え | |
int drow = ig3(dgv3Turnstart_row,3)-1;//最初のターン開始行番号を基準 | |
for (i = 0; i < TurnNum + 1; i++) | |
{ | |
dgv3.Rows[i + dgv3Turnstart_row].Cells[3].Value = ti(ig3(i + dgv3Turnstart_row, 3) - drow); | |
dgv3.Rows[i + dgv3Turnstart_row].Cells[5].Value = ti(ig3(i + dgv3Turnstart_row, 5) - drow); | |
} | |
//ヘッダー後付け | |
headeradd(0, 3); | |
dgv3.Visible = true; | |
dgvsave3(); | |
} | |
private void dgvsave3() | |
{ | |
int i, j; | |
//Filename Timestamp | |
DateTime dt = DateTime.Now; | |
string fn = dt.ToString($"{dt:MMddHHmmss}"); | |
string fnstrng = "c:/RTK_Log/dgv3_" + fn + ".csv"; | |
/* | |
//---RL SAVE---------------------------------- | |
if (textBox2.Text.IndexOf("Left") >= 0) | |
{ | |
fnstrng = textBox2.Text + "_L_dgv.csv"; | |
} | |
else if (textBox2.Text.IndexOf("Right") >= 0) | |
{ | |
fnstrng = textBox2.Text + "_R_dgv.csv"; | |
} | |
*/ | |
string hdstrng = "c:/RTK_Log/head3_" + fn + ".csv"; | |
int dgvron = dgv3.RowCount; | |
int dgvcln = dgv3.ColumnCount; | |
Debug.Print("dgvron=" + dgvron.ToString() + ",dgvcln=" + dgvcln.ToString()); | |
using (StreamWriter sw = new StreamWriter(@fnstrng, false, Encoding.UTF8)) | |
{ | |
for (i = 1; i < dgvron ; i++) | |
{ | |
string dgvrow = ""; | |
for (j = 0; j < dgvcln; j++) | |
{ | |
dgvrow = dgvrow + dgv3.Rows[i].Cells[j].Value + ","; | |
} | |
sw.WriteLine(dgvrow); | |
} | |
} | |
if (comboBox2.Items.Count == 0) | |
{ | |
int ico = 0; | |
int tablestartrow = ig3(0, 0) + 2; | |
while (dgv3.Rows[ico + tablestartrow].Cells[1].Value != null) | |
{ | |
comboBox2.Items.Add(ti(ico)); | |
comboBox3.Items.Add(ti(ico)); | |
ico++; | |
} | |
} | |
} | |
//Turn No 選択 コンボ準備 dgv3のターンテーブル読み取り | |
private void comboBox2_SelectedIndexChanged(object sender, EventArgs e) | |
{ | |
} | |
private void comboBox2_MouseClick(object sender, MouseEventArgs e) | |
{ | |
int turnnum; | |
} | |
#endregion | |
#region [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[SORT DGV]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]] | |
private void button11_Click(object sender, EventArgs e)//-------sortdgv 起動------- | |
{ | |
sortdgv(); | |
button12.Enabled = true; | |
int im; | |
//richTextBox1 write | |
richTextBox1.Visible = true; | |
for (im = 0; im < dc.smodeN; im++) | |
{ | |
string mes = "smodeNo=" + im.ToString() + "startRow=" + dc.smode_startNo[im].ToString() + "endRow=" + dc.smode_endNo[im].ToString() + "time=" + dc.smode_time[im].ToString() + "\r\n"; | |
richTextBox1.AppendText(mes); | |
} | |
//slip 追加 DZB反転単位 FlLine用Drel追加Cells[43] | |
for (im = 0; im < dgv1.Rows.Count - 10; im++)//slipを全dgvデータ計算代入 | |
{ | |
double id; | |
//Drel 作成 | |
if (im > 3) | |
{ | |
double dzb = 0; | |
double dzr = 0; | |
dzb = (Convert.ToDouble(dgv1.Rows[im].Cells[16].Value) + Convert.ToDouble(dgv1.Rows[im - 1].Cells[16].Value) + Convert.ToDouble(dgv1.Rows[im - 2].Cells[16].Value)) / 3; // 3個移動平均 | |
if (dgv1.Rows[im].Cells[23].Value != null) | |
{ | |
dzr = (Convert.ToDouble(dgv1.Rows[im].Cells[23].Value) + Convert.ToDouble(dgv1.Rows[im - 1].Cells[23].Value) + Convert.ToDouble(dgv1.Rows[im - 2].Cells[23].Value)) / 3; | |
double drel = dzb - dzr;//下りマイナスcm | |
dgv1.Rows[im].Cells[41].Value = (drel).ToString(); | |
} | |
} | |
} | |
// dgv LAST datacheck MaxMinAve | |
datacheck(); | |
int coli; | |
double maxv = 0; | |
double minv = 0; | |
double avev = 0; | |
dgv1.Rows.Add(3); | |
dgv1.Rows[dgv1.Rows.Count - 3].Cells[0].Value = "Max"; | |
dgv1.Rows[dgv1.Rows.Count - 2].Cells[0].Value = "Min"; | |
dgv1.Rows[dgv1.Rows.Count - 1].Cells[0].Value = "Ave"; | |
for (coli = 1; coli < 42; coli++) | |
{ | |
mathdgv(coli, ref maxv, ref minv, ref avev); | |
Debug.Print("coli=" + coli.ToString() + "maxv=" + maxv.ToString()); | |
dgv1.Rows[dgv1.Rows.Count - 3].Cells[coli].Value = maxv.ToString(); | |
dgv1.Rows[dgv1.Rows.Count - 2].Cells[coli].Value = minv.ToString(); | |
dgv1.Rows[dgv1.Rows.Count - 1].Cells[coli].Value = avev.ToString(); | |
} | |
// | |
}//=============================Turn NUTRAL END============================================== | |
//++++++++++++++++++++++++++++++++++++++SORT DGV ++++++++++++++++++++++++++++++++++++++ | |
///++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
///SORT DVG<summary> | |
/// / | |
/// </summary> | |
void sortdgv()//生データを整理してから各有効データの行番号を抽出 | |
{ | |
var swtch0 = new System.Diagnostics.Stopwatch(); | |
swtch0.Reset(); | |
swtch0.Start(); | |
dgv1.DefaultCellStyle.NullValue = "0"; | |
dgv1.DefaultCellStyle.DataSourceNullValue = "0"; | |
DateTime now = DateTime.Now; | |
Debug.Print("sortdgv In:" + now.ToString()); | |
//行チェック 列ごとに前後チェック | |
//string[,] itow_error = new string[10, 1000];//エラー行列を記録[smodeN,irr] | |
int colitowb = 13;//itowBの列番号 | |
int colitowr = 20;//itowRの列番号 | |
int iir = 0; | |
int iic = 1; | |
int erri = 0; | |
int itowB0, itowB0_1; | |
dc.digit_err = new string[10, 1000]; | |
dc.itow_error = new string[10, 1000];//エラー行列を記録[smodeN,irr] | |
dc.itow_errN = new int[10]; | |
dc.itow_errper = new double[10]; | |
dc.smode_startNo = new int[10]; | |
dc.smode_endNo = new int[10]; | |
dc.smode_startitow = new int[10]; | |
dc.smode_enditow = new int[10]; | |
dc.smode_time = new int[10]; | |
dc.tnrow = new int[60000];//turn番号収納配列確保 | |
dc.tnrow[dgv1.RowCount] = -1; | |
//*********Null削除**************************** | |
dc.maxrow = dgv1.RowCount; | |
for (iir = 1; iir < dc.maxrow - 2; iir++)//行ループ | |
{ | |
for (iic = 0; iic < 38; iic++) | |
{ | |
if (dgv1.Rows[iir].Cells[iic].Value == null) | |
{ | |
dgv1.Rows[iir].Cells[iic].Value = "0"; | |
//dgv1.Rows[iir].Cells[iic].Value = dgv1.Rows[iir - 1].Cells[iic].Value; | |
Debug.Print("null repaired 0 [" + iir.ToString() + "],[" + iic.ToString() + "]" + millis()); | |
} | |
} | |
// if (dgv1.Rows[iir].Cells[0].Value == null) | |
// { | |
// dgv1.Rows.RemoveAt(iir); | |
// Debug.Print("Null Remove iir=" + iir.ToString() + "Cells[0]=Null"); | |
//} | |
} | |
//**********Null削除END******************************* | |
//各データの異常反転基準値配列を作る、<criteria[]は正数、>criteria[]は負数 | |
double[] criteria = new double[38] { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38 }; | |
//1:flags 131 67 変化 | |
dc.maxrow = dgv1.RowCount; | |
//最初にmHeadを180度回転補正 | |
mhead180(25, 2, dgv1.Rows.Count - 2);//mHead | |
mhead180(26, 2, dgv1.Rows.Count - 2);//BNHOSEI yaw1 | |
mhead180(29, 2, dgv1.Rows.Count - 2);//yaw1 | |
//mhead180(32, 2, dgv1.Rows.Count - 2);//yaw2 | |
//mhead180(35, 2, dgv1.Rows.Count - 2);//yaw3 | |
hmotd();//headMotをDoubleに変換 | |
//************************smode範囲 check***************************************************** | |
//dc.smode_startNo[], dc.smode_endNo[],dc.smodeNを得る | |
string modes = "+[s]"; | |
int smodenum = 0; | |
int mode_startflag = 0; | |
int mode_startNo = 0; | |
int mode_endNo = 0; | |
//smode Range Count itowが1秒以上離れたら別範囲************************************** | |
smodesearch(); | |
//Debug.Print("smode count dc.smode_endNo[" + smodenum.ToString() + "]=" + dc.smode_endNo[smodenum].ToString() + "dc_smodeN=" + dc.smodeN.ToString()); | |
mode_endNo = dc.maxrow - 2; | |
DateTime now1 = DateTime.Now; | |
//Debug.Print("smode SearchFinished :" + now1.ToString()); | |
//Smode end*********************************************************************************************************************************** | |
//Digit error repaire*********************************************************************************************** | |
//処理1:nullセルは0代入 | |
//処理2:文字が数値でないセルは、前の行データ代入 | |
int smodeitr = 0; | |
while (smodeitr <= dc.smodeN)//smodeの個数だけスキャン | |
{ | |
DateTime now3 = DateTime.Now; | |
// Debug.Print("loop end :" +now3.ToString()+ now3.Millisecond.ToString()); | |
for (iir = dc.smode_startNo[smodeitr]; iir < dc.smode_endNo[smodeitr]; iir++)//行ループ 2行から開始してiir-1行目とiir行目比較 | |
{ // public static bool TryParse( string s, out データ型名 result)を使って文字から数値変換できるかチェック | |
//nullチェック | |
string itows = dgv1.Rows[iir].Cells[13].Value.ToString(); | |
for (iic = 1; iic < 38; iic++) | |
{ | |
if (dgv1.Rows[iir].Cells[iic].Value == null)//nullチェック | |
{ | |
//dgv1.Rows[iir].Cells[iic].Value = "0"; | |
dgv1.Rows[iir].Cells[iic].Value = dgv1.Rows[iir - 1].Cells[iic].Value; | |
Debug.Print("null repaired 0 [" + iir.ToString() + "],[" + iic.ToString() + "]" + millis()); | |
} | |
} | |
int derr = 0; | |
for (iic = 1; iic < 38; iic++) | |
{ | |
if (iic != 12 && iic != 19 && iic != 28) | |
{ | |
double i; | |
//string cellstr = dgv1.Rows[iir].Cells[iic].Value.ToString(); | |
if (double.TryParse(dgv1.Rows[iir].Cells[iic].Value.ToString(), out i))//文字セルが数値かどうかチェック | |
{ | |
//itow_error[smodeitr, erri] = iir.ToString() + "," + itowsa.ToString(); | |
//Debug.Print("Inseted:itow_erri[" + smodeitr.ToString() + "," + erri.ToString() + "] =" + itow_error[smodeitr, erri] + ", itowB=" + dgv1.Rows[iir].Cells[13].Value + "," + dgv1.Rows[iir - 1].Cells[13].Value); | |
} | |
else | |
{ | |
// Debug.Print("数値に変換できません=" + "Rows[" + iir.ToString() + "]Cells[" + iic.ToString() + "]=" + dgv1.Rows[iir].Cells[iic].Value.ToString()+","+millis()); | |
//非数値異常値は、1個上のデータで置き換え | |
dc.digit_err[smodeitr, derr] = itows + "," + dgv1.Rows[iir].Cells[iic].Value.ToString(); | |
// Debug.Print("dc_err="+dc.digit_err[smodeitr, derr]); | |
derr++; | |
if (checkBox4.Checked == true) | |
{ | |
int iiic; | |
for (iiic = 1; iiic < 38; iiic++) | |
{ | |
Debug.Print("Rows[" + iir.ToString() + "]Cells[" + iiic.ToString() + "]=" + dgv1.Rows[iir].Cells[iiic].Value.ToString() + "=>Repaired=" + dgv1.Rows[iir].Cells[iiic].Value.ToString()); | |
dgv1.Rows[iir].Cells[iiic].Value = dgv1.Rows[iir - 1].Cells[iiic].Value; | |
} | |
} | |
// erri++;//エラー扱い | |
// iir++;//1個飛ばし | |
} | |
} | |
} | |
} | |
//increment | |
smodeitr++; | |
erri = 0; | |
}//while modeitr end | |
//===============================itow Insert=========================================================== | |
//if (checkBox4.Checked == true) | |
//{ | |
smodeitr = 0; | |
while (smodeitr < dc.smodeN)//smodeの個数だけスキャン | |
{ | |
Debug.Print("itowInsert START"); | |
for (iir = dc.smode_startNo[smodeitr] + 1; iir < dc.smode_endNo[smodeitr]; iir++)//行ループ 2行から開始してiir-1行目とiir行目比較 | |
{ | |
itowB0 = Convert.ToInt32(dgv1.Rows[iir].Cells[13].Value); | |
itowB0_1 = Convert.ToInt32(dgv1.Rows[iir - 1].Cells[13].Value); | |
int itowsa = itowB0 - itowB0_1; | |
if (itowsa > 124 && itowsa < 244) | |
{ | |
dc.itow_error[smodeitr, erri] = (itowB0 + 120).ToString() + "," + itowsa.ToString(); | |
dc.itow_errN[smodeitr] = erri; | |
rinsert(iir, itowsa); | |
//Debug.Print("124<<244iir=" + iir.ToString() + "sa=" + itowsa.ToString() + "Inserted:itow_erri[" + smodeitr.ToString() + "," + erri.ToString() + "] =" + dc.itow_error[smodeitr, erri] + ", itowB=" + dgv1.Rows[iir].Cells[13].Value + "," + dgv1.Rows[iir - 1].Cells[13].Value + "," + millis()); | |
erri++; | |
} | |
else if (itowsa > 244) | |
{ | |
//Debug.Print(">244]iir=" + iir.ToString() + "sa=" + itowsa.ToString() + "Inserted:itow_erri[" + smodeitr.ToString() + "," + erri.ToString() + "] =" + dc.itow_error[smodeitr, erri] + ", itowB=" + dgv1.Rows[iir].Cells[13].Value + "," + dgv1.Rows[iir - 1].Cells[13].Value + "," + millis()); | |
} | |
} | |
smodeitr++; | |
erri = 0; | |
// }//itow insert end | |
} | |
smodesearch();//最後の行数でsmode再実施 | |
//Percent 計算 | |
//itow error | |
//public string[,] itow_error;// 足りないitow.Tostring() | |
//public int[] itow_errN;//[smodeN] | |
//public double[] itow_errper;//% err | |
for (smodeitr = 0; smodeitr < dc.smodeN; smodeitr++) | |
{ | |
dc.itow_errper[smodeitr] = (double)dc.itow_errN[smodeitr] / (dc.smode_endNo[smodeitr] - dc.smode_startNo[smodeitr]) * 100; | |
// Debug.Print("dc.itow_errper[" + smodeitr.ToString() + "]=" + dc.itow_errper[smodeitr].ToString("F2")+"%"); | |
dc.smode_startitow[smodeitr] = Convert.ToInt32(dgv1.Rows[dc.smode_startNo[smodeitr]].Cells[13].Value); | |
dc.smode_enditow[smodeitr] = Convert.ToInt32(dgv1.Rows[dc.smode_endNo[smodeitr]].Cells[13].Value); | |
dc.smode_time[smodeitr] = dc.smode_enditow[smodeitr] - dc.smode_startitow[smodeitr]; | |
} | |
//}//checkbox4 end | |
for (iir = 1; iir < dc.maxrow - 2; iir++)//行ループ | |
{ | |
dgv1.Rows[iir].HeaderCell.Value = iir.ToString(); | |
} | |
dc.dgvsorted = 1; | |
dc.turnprocessed = 0; | |
//finished | |
label9.Text = "sortdgv:smodeN=" + dc.smodeN.ToString() + "erri=" + erri.ToString(); | |
Debug.Print("Sort dgv FINISHED"); | |
}//sortdgv end | |
//******************************************************************************************************************************************** | |
private void smodesearch() | |
{ | |
//*********************smode Range Count itowが1秒以上離れたら別範囲************************************** | |
int iir; | |
int mode_startflag = 0; | |
int mode_startNo = 0; | |
int smodenum = 0; | |
int mode_endNo = 0; | |
dc.maxrow = dgv1.RowCount; | |
for (iir = 1; iir < dc.maxrow - 2; iir++)//行ループ | |
{ | |
int itowd = Convert.ToInt32(dgv1.Rows[iir].Cells[13].Value); | |
int itowd_1 = Convert.ToInt32(dgv1.Rows[iir - 1].Cells[13].Value); | |
//if (md.Substring(0, 2) == "s]" && mode_startflag == 0) | |
if (itowd - itowd_1 < 124 && mode_startflag == 0 && iir <= dc.maxrow - 4) | |
{ | |
mode_startflag = 1; | |
mode_startNo = iir; | |
dc.smode_startNo[smodenum] = iir; | |
dc.smodeN = smodenum; | |
//Debug.Print("dc.smode_startNo[" + smodenum.ToString() + "]=" + dc.smode_startNo[smodenum].ToString()); | |
} | |
else if ((itowd - itowd_1 > 1000 || iir >= dc.maxrow - 4) && mode_startflag == 1) | |
{ | |
mode_endNo = iir; | |
mode_startflag = 0; | |
dc.smode_endNo[smodenum] = iir; | |
dc.smodeN = smodenum + 1; | |
Debug.Print("dc.smode_endNo[" + smodenum.ToString() + "]=" + dc.smode_endNo[smodenum].ToString() + "dc_smodeN=" + dc.smodeN.ToString()); | |
smodenum++; | |
} | |
/* | |
else | |
{ | |
mode_endNo = iir; | |
mode_startflag = 1; | |
dc.smode_endNo[smodenum] = iir; | |
dc.smodeN = smodenum + 1; | |
//Debug.Print("dc.smode_endNo[" + smodenum.ToString() + "]=" + dc.smode_endNo[smodenum].ToString() + "dc_smodeN=" + dc.smodeN.ToString()); | |
//smodenum++; | |
} | |
*/ | |
//dc.maxrow = dgv1.RowCount; | |
} | |
} | |
private string millis() | |
{ | |
DateTime now3 = DateTime.Now; | |
string mil = now3.ToString() + ":" + now3.Millisecond.ToString(); | |
return mil; | |
} | |
private void rinsert(int rn, int sa) | |
{ | |
//itow | |
if (sa <= 240) | |
{ | |
//try | |
// { | |
int cii; | |
dgv1.Rows.Insert(rn); | |
Debug.Print("rinsert [=" + rn.ToString() + "]"); | |
//文字固定位置 | |
dgv1.Rows[rn].Cells[0].Value = "[s]"; | |
dgv1.Rows[rn].Cells[12].Value = "[Base]"; | |
dgv1.Rows[rn].Cells[19].Value = "[Rover]"; | |
dgv1.Rows[rn].Cells[28].Value = "[BNO]"; | |
// | |
dgv1.Rows[rn].Cells[13].Value = Convert.ToInt32(dgv1.Rows[rn - 1].Cells[13].Value) + 120;//抜けたitow | |
for (cii = 14; cii < 19; cii++) | |
{ | |
double ave = (Convert.ToDouble(dgv1.Rows[rn - 1].Cells[cii].Value) + Convert.ToDouble(dgv1.Rows[rn + 1].Cells[cii].Value)) / 2; | |
dgv1.Rows[rn].Cells[cii].Value = ave.ToString("F2"); | |
} | |
for (cii = 20; cii < 28; cii++) | |
{ | |
double ave = (Convert.ToDouble(dgv1.Rows[rn - 1].Cells[cii].Value) + Convert.ToDouble(dgv1.Rows[rn + 1].Cells[cii].Value)) / 2; | |
dgv1.Rows[rn].Cells[cii].Value = ave.ToString("F2"); | |
} | |
for (cii = 29; cii < 37; cii++) | |
{ | |
double ave = (Convert.ToDouble(dgv1.Rows[rn - 1].Cells[cii].Value) + Convert.ToDouble(dgv1.Rows[rn + 1].Cells[cii].Value)) / 2; | |
dgv1.Rows[rn].Cells[cii].Value = ave.ToString("F2"); | |
if (cii == 29) | |
{ | |
Debug.Print("rinsert:rn=" + rn.ToString() + "[29]yaw1 ave=" + ave.ToString()); | |
} | |
} | |
} | |
} | |
//\\\\\\\\\\\\\\\\\\\\\角度検査してdgv最下部へ記入\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\ | |
// string head0 = "md(0), flagsB(1), flagsR(2), pdopB(3), pdopR(4), haccB(5), haccR(6), miscount0(7), count_upB(8),count_upB0(9),tp(10),tp0(11)"; | |
//string head1 = ",[Base](12),itowB(13), NYB(14), EXB(15), DZB(16), gSpeedB(17), headMotBZ(18)"; | |
//string head2 = ",[Rover](19) ,itowR(20),NYR(21),EXR(22),DZR(23),gSpeedR(24),mHead(25),bnHosei+eX[0](26),relLm](27)"; | |
//string head3 = ",BNO](28),yaw1(29),pitch1(30),roll1(31),laX(32),laY(33),laZ(34),grX(35),grY(36),grZ(37)" | |
private void anglesdgv()//停止位置と直進でのmHeadとYaw合わせと360度をまたぐ場合 | |
{ | |
//範囲の抽出 | |
int rnn; | |
int hmotcol = 18; | |
int mheadcol = 25; | |
int yawcol = 29; | |
int pitchcol = 30; | |
int rollcol = 31; | |
double hmotMax = 1000; | |
double hmotMin = 0; | |
double mheadMax = 1000; | |
double mheadMin = 0; | |
double yawMax = 1000; | |
double yawMin = 0; | |
double pitchMax = 1000; | |
double pitchMin = -1000; | |
double rollMax = 1000; | |
double rollMin = -1000; | |
for (rnn = 0; rnn < dgv1.Rows.Count; rnn++) | |
{ | |
// col=18;//headMot | |
} | |
} | |
//Base-Rover角度が180度逆になっている場合は修正 | |
private void mhead180(int col, int startr, int endr)//rover haeding 180度回転 | |
{ | |
int hii; | |
for (hii = startr; hii < endr; hii++) | |
{ | |
double d; | |
string sh = dgv1.Rows[hii].Cells[col].Value.ToString(); | |
if (double.TryParse(sh, out d)) | |
{ | |
double head = Convert.ToDouble(dgv1.Rows[hii].Cells[col].Value) + 180; | |
if (head > 360) | |
{ | |
head = head - 360; | |
} | |
dgv1.Rows[hii].Cells[col].Value = head.ToString("F2"); | |
//変換出来たら、dにその数値が入る | |
// Console.WriteLine("{0} は数値 {1} に変換できます。", sh, d); | |
} | |
else | |
{ | |
//Console.WriteLine("{0} は数字ではありません。", sh); | |
Debug.Print("Not Numerical format Cell= " + hii.ToString() + "sh=" + sh); | |
} | |
} | |
} | |
private void hmotd()//headMotを小数点5桁に変換 | |
{ | |
int ir; | |
double d; | |
for (ir = 1; ir < dc.maxrow - 2; ir++)//行ループ | |
{ | |
string sh = dgv1.Rows[ir].Cells[18].Value.ToString(); | |
//Debug.Print("hmotd: ir="+ir.ToString()+"sh=" + sh); | |
double hmotd = 0; | |
if (double.TryParse(sh, out d)) | |
{ | |
if (Convert.ToDouble(dgv1.Rows[ir].Cells[18].Value) > 10000) | |
{ | |
hmotd = Convert.ToDouble(dgv1.Rows[ir].Cells[18].Value) * 0.00001; | |
} | |
else | |
{ | |
hmotd = Convert.ToDouble(dgv1.Rows[ir].Cells[18].Value) * 1; | |
} | |
dgv1.Rows[ir].Cells[18].Value = hmotd.ToString("F2"); | |
//変換出来たら、dにその数値が入る | |
Console.WriteLine("{0} は数値 {1} に変換できます。", sh, d); | |
} | |
else | |
{ | |
Console.WriteLine("{0} は数字ではありません。", sh); | |
Debug.Print("Not Numerical format Cell= " + ir.ToString() + "sh=" + sh); | |
} | |
//double hmotd = Convert.ToDouble(dgv1.Rows[ir].Cells[18].Value) * 0.00001; | |
//dgv1.Rows[ir].Cells[18].Value = hmotd.ToString(); | |
} | |
} | |
private void dgv1_CellClick(object sender, DataGridViewCellEventArgs e)//セル座標表示 | |
{ | |
label7.Text = "row=" + dgv1.CurrentCell.RowIndex.ToString() + "column=" + dgv1.CurrentCell.ColumnIndex.ToString(); | |
} | |
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@dgv Math@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
//dgv全体をMathで計算 Max Min Ave | |
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
private void mathdgv(int col, ref double maxv, ref double minv, ref double avev) | |
{ | |
int im; | |
maxv = -1000000000; | |
int maxvn = 0; | |
double maxv_1 = -1000000000; | |
minv = 10000000000; | |
int minvn = 0; | |
double minv_1 = 10000000000; | |
double sumv = 0; | |
avev = 0; | |
if (col >= 7 && col < 13) { col = 13; } | |
if (col == 19) | |
{ | |
col = 20; | |
} | |
if (col == 28) { col = 29; } | |
for (im = 1; im < dgv1.Rows.Count - 11; im++) | |
{ | |
//Debug.Print("im=" + im.ToString() + "col=" + col.ToString()); | |
double val = Convert.ToDouble(dgv1.Rows[im].Cells[col].Value); | |
maxv_1 = maxv; | |
maxv = Math.Max(val, maxv); | |
if (maxv > maxv_1) | |
{ | |
maxvn++; | |
//Debug.Print("dgv[" + im.ToString() + "][" + col.ToString() + "],maxv=" + maxv.ToString() + "maxvn=" + maxvn.ToString()); | |
} | |
minv_1 = minv; | |
minv = Math.Min(val, minv); | |
if (minv > minv_1) { minvn++; } | |
sumv = sumv + val; | |
avev = sumv / im; | |
} | |
} | |
#endregion | |
#region[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[ DGV SAVE]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]] | |
//====================================================== | |
//===========dgvSave=================================== | |
//==================================================== | |
private void button10_Click(object sender, EventArgs e) | |
{ | |
dgvsave(); | |
} | |
private void dgvsave() | |
{ | |
int i, j; | |
//Filename Timestamp | |
DateTime dt = DateTime.Now; | |
string fn = dt.ToString($"{dt:MMddHHmmss}"); | |
string fnstrng = "c:/RTK_Log/dgv_" + fn + ".csv"; | |
//---RL SAVE---------------------------------- | |
if (textBox2.Text.IndexOf("Left") >= 0) | |
{ | |
fnstrng = textBox2.Text + "_L_dgv.csv"; | |
} | |
else if (textBox2.Text.IndexOf("Right") >= 0) | |
{ | |
fnstrng =textBox2.Text + "_R_dgv.csv"; | |
} | |
string hdstrng = "c:/RTK_Log/head_" + fn + ".csv"; | |
int dgvron = dgv1.RowCount; | |
int dgvcln = dgv1.ColumnCount; | |
Debug.Print("dgvron=" + dgvron.ToString() + ",dgvcln=" + dgvcln.ToString()); | |
using (StreamWriter sw = new StreamWriter(@fnstrng, false, Encoding.UTF8)) | |
{ | |
for (i = 1; i < dgvron - 1; i++) | |
{ | |
string dgvrow = ""; | |
for (j = 0; j < dgvcln; j++) | |
{ | |
dgvrow = dgvrow + dgv1.Rows[i].Cells[j].Value + ","; | |
} | |
sw.WriteLine(dgvrow); | |
} | |
} | |
//-----------HeaderText save---------------------- | |
//DataGridView1.Columns[0].HeaderTex | |
if (dgv1.Columns[2].HeaderText != "") | |
{ | |
using (StreamWriter sw = new StreamWriter(@hdstrng, false, Encoding.UTF8)) | |
{ | |
string dgvhead = ""; | |
for (j = 0; j < dgvcln; j++) | |
{ | |
dgvhead = dgvhead + dgv1.Columns[j].HeaderCell.Value + ","; | |
//Debug.Print("i=" + i.ToString() + ",dgvhead=" + dgvhead); | |
} | |
sw.WriteLine(dgvhead); | |
} | |
} | |
} | |
#endregion | |
#region [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[ TIME SERIES PLOT ]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]] | |
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@GRAPH @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
//@@@@@@@@@@@@@@@@指定されたターン構造体番号をプロット makeATurnBitmap()-@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
// | |
//Plot ボタン | |
private void button13_Click(object sender, EventArgs e)//PLOTボタンsourceBitmap作成してプロット | |
{ | |
dgv1.Visible = false; | |
hScrollBar1.Visible = true; | |
pictureBox1.Visible = true; | |
tR[0].xdot = Convert.ToInt32(textBox8.Text); | |
//=======================make sourceBitmap 範囲指定計算============================================ | |
//textBox10に開始ターン番号、textBox11に終了ターン番号 | |
//2万ドットを超えた場合は、textBox11に許容値をいれる。 | |
int ti = 0; | |
if (textBox10.Text != "" && textBox11.Text != "")//TEXTBOXに範囲を入力しないと描画ができない | |
{ | |
int start_tN = Convert.ToInt32(textBox10.Text); | |
int end_tN = Convert.ToInt32(textBox11.Text); | |
int startrowN = tR[start_tN].startNo; | |
int endrowN = tR[end_tN].endNo; | |
int totalsu; | |
int totaldot; | |
int max_end_tN = 0; | |
int max_endrowN = 0; | |
int limit_rowN = Convert.ToInt32(20000 / tR[0].xdot);//2万ドットの行数デフォルト2000行xdot=10 | |
//2万ドットでおさまる最大ターン番号を求める | |
if ((endrowN - startrowN) * tR[0].xdot < 20000)//指定範囲がソースBITMAPサイズに収まているか判定 | |
{ | |
label6.Text = "OK prepare to SourceBitmap "; | |
} | |
else//2万ドットより小さい場合 | |
{ | |
label6.Text = "Size Over go to auto Range Setting !!"; | |
for (ti = 0; ti < tR[0].lastNo; ti++) | |
{ | |
if (tR[start_tN + ti].endNo - tR[start_tN].startNo > limit_rowN) | |
{ | |
label6.Text = "[AutoSET]startrowN=" + tR[start_tN].startNo.ToString() + ",endrowN=" + tR[start_tN + ti].endNo.ToString(); | |
textBox11.Text = (start_tN + ti).ToString(); | |
endrowN = tR[start_tN + ti].endNo; | |
ti = tR[0].lastNo;//LOOP/終わり | |
} | |
} | |
} | |
// } | |
//} | |
richTextBox1.AppendText("[PLOTButton]:startrowN=" + startrowN.ToString() + "endrowN=" + endrowN.ToString() + "limit_rowN=" + limit_rowN.ToString() + "\r\n"); | |
//label6.Text = "startrowN=" + startrowN.ToString() + "endrowN=" + endrowN.ToString() + "limit_rowN=" + limit_rowN.ToString(); | |
makeATurnBitmap(28, startrowN, endrowN);//ソースBITMAP作成 | |
//int ir; | |
//pictureBox1.Visible = true; | |
//---------------------hScrollBarの設定------------------------------------------ | |
hScrollBar1.Minimum = 0;// tR[1].startNo;位置をゼロとする | |
hScrollBar1.Maximum = (endrowN - startrowN) * tR[0].xdot;// //(tR[tR[0].lastNo].endNo - tR[1].startNo) * tR[0].xdot;//sourceBitmapの全ドット数 | |
hScrollBar1.Value = (endrowN - startrowN) * tR[0].xdot;//初期設定は、最大値右端にセット | |
textBox3.Text = Convert.ToInt32(hScrollBar1.Value / tR[0].xdot).ToString(); | |
richTextBox1.AppendText("plot:hscro;;Bar.minimum=0=" + tR[1].startNo.ToString() + "hscrollBar.Max=" + hScrollBar1.Maximum.ToString() + "hscrollBar.Value=cx=" + hScrollBar1.Value.ToString() + "\r\n"); | |
sourceCut(hScrollBar1.Value);//dot座標を渡す | |
//xyplot | |
//xyplot(); | |
} | |
else | |
{ | |
label6.Text = "INPUT TextBox10 start TurnNo, TextBox11 end TurnNo"; | |
} | |
} | |
//--------------makeATurnBitmap() ------------------------------------------------ | |
private void makeATurnBitmap(int xdot, int startrowN, int endrowN)//指定ターン番号の行範囲を渡す | |
{ | |
int mi; | |
int psa = 1;//120msec 1行 | |
gline = new lines[8]; | |
int gsel = 0;//gline selector 0:headMot,1:mHead,2:gSpeed,3:pitch,4:yaw,5:DZB,6:relLm | |
//------ | |
tR[0].xdot = Convert.ToInt32(textBox8.Text); | |
//周期 | |
gline[0].period = 120;//headMot RED | |
gline[1].period = 120;//mHead BLUE | |
gline[2].period = 120;//gSpeed GREEN | |
gline[3].period = 40;//pitch YELLOW | |
gline[4].period = 40;//yawhosei AQUA | |
gline[5].period = 120;//roll | |
gline[6].period = 120;//Drel//relLm ALMOND | |
gline[7].period = 120;//slip Orange | |
//列番号 | |
gline[0].colN = 18;//headMot | |
gline[1].colN = 25;//mHead | |
gline[2].colN = 17;//gSpeed | |
gline[3].colN = 30;//pitch1=30,pitch2=33,pitch3=36 | |
gline[4].colN = 29;//bnhosei26 yaw129;//yaw1=29,yaw2=32,yaw3=35 | |
gline[5].colN = 31;//roll | |
gline[6].colN = 41;//Drel | |
gline[7].colN = 28;//slipを新たに作成 BNO0をつぶす | |
//max値手入力 | |
gline[0].max = 360;//headMot1deg | |
gline[1].max = 360;//mHead1deg | |
gline[2].max = 12000;//gSpeed 43kmh= | |
gline[3].max = 90;//pitch IMU EulerAngle 0.1deg | |
gline[4].max = 360;//yawhosei IMU EulerAngle 0.1deg | |
gline[5].max = 180;//roll | |
gline[6].max = 50;//Drel | |
gline[7].max = 180;//slipを新たに作成 BNO0をつぶす | |
//min値手入力 | |
gline[0].min = 0;//headMot | |
gline[1].min = 0;//mHead | |
gline[2].min = 0;//gSpeed 43kmh | |
gline[3].min = -180;//pitch1=30,pitch2=33,pitch3=36 | |
gline[4].min = 0;//yawhosei1=29,yaw2=32,yaw3=35 | |
gline[5].min = -180;//roll | |
gline[6].min = 0;//Drel | |
gline[7].max = -90;//slipを新たに作成 BNO0をつぶす | |
//Auto Max Min | |
int rowmax = dc.mathmaxrow;//列最大値 | |
int rowmin = dc.mathminrow;//列最小値 | |
int[] plusminus = new int[8]; | |
for (mi = 0; mi < 8; mi++) | |
{ | |
//Debug.Print("**MAXSET:max[="+rowmax.ToString()+"],["+mi.ToString(9+"]=") + dgv1.Rows[rowmax].Cells[gline[mi].colN].Value.ToString()); | |
double dmax = Convert.ToDouble(dgv1.Rows[rowmax].Cells[gline[mi].colN].Value); | |
double dmin = Convert.ToDouble(dgv1.Rows[rowmin].Cells[gline[mi].colN].Value); | |
gline[mi].max = dmax; | |
gline[mi].min = dmin; | |
gline[mi].baseL = 1; | |
Debug.Print("max-minSET:gline[" + mi.ToString() + "].max=" + gline[mi].max.ToString() + ".min=" + gline[mi].min); | |
if (gline[mi].max > 0 && gline[mi].min >= 0) { plusminus[mi] = 1; gline[mi].baseL = 1; } | |
if (gline[mi].max * gline[mi].min < 0) { plusminus[mi] = 1; gline[mi].baseL = 1; } | |
if (gline[mi].max <= 0 && gline[mi].min < 0) { plusminus[mi] = -1; gline[mi].baseL = 1; } | |
} | |
//gline[5]のrollはMax min 1.2倍しておくぎりぎりでピークが見えないから | |
gline[5].max = (int)1.0 * gline[5].max; | |
gline[5].min = (int)1.0 * gline[5].min; | |
//固定座標は、headMot mHead yaw | |
gline[0].max = 360;//headMot1deg | |
gline[1].max = 360;//mHead1deg | |
//gline[3].max = 90;//pitch IMU EulerAngle 0.1deg | |
gline[4].max = 360;//yaw | |
gline[6].max = 50;//Drel | |
gline[0].min = 0;//headMot | |
gline[1].min = 0;//mHead | |
//gline[3].min = -180;//pitch1=30,pitch2=33,pitch3=36 | |
gline[4].min = 0;//yaw | |
gline[6].min = 0;//Drel | |
//gline[3].baseL = 2; | |
//gline[5].baseL = 2;//roll | |
//gline[6].baseL = 2;//Drel | |
gline[7].baseL = 2;//slip ベースラインセンター | |
//----------------------------------------------------------- | |
int irn; | |
int pw = 1000;// pictureBox1.Width;//1000dot | |
int ph = 500;// pictureBox1.Height;//500dot | |
int velMax = 10000;//mm/sec | |
int velMin = -10000; | |
int headMax = 36000000; | |
int headMin = 0; | |
//======PLOT範囲決め=============================== | |
//bitmap長2万ドット超えないようにする | |
//textbox10のsmode番号を範囲とする | |
// public int[] smode_startNo;//=new int[10];//s] modeの開始番号 配列 | |
//public int[] smode_endNo;//=new int[10];//s] modeの終了番号 配列 | |
// public int smodeN;//ファイル内のsmode個数 | |
Graphics g1 = Graphics.FromImage(sourceBitmap); | |
g1.Clear(Color.Black); | |
for (gsel = 0; gsel < 7; gsel++) | |
{ | |
// if (gline[gsel].available == true) | |
//{ | |
//Turn構造体から範囲取得 | |
int pmsec = gline[gsel].period; | |
// int psa = gline[gsel].sa; | |
int pcoln = gline[gsel].colN; | |
int yrange = (int)(gline[gsel].max - gline[gsel].min); | |
double yscale = (double)ph / (gline[gsel].max - gline[gsel].min);//500/(360-0)= | |
int itow0 = 0; | |
int y0, y0_1; | |
int smn = Convert.ToInt32(textBox10.Text);//plot指定smode番号 | |
for (irn = 1; irn < endrowN - startrowN; irn++) | |
{ | |
Pen p = new Pen(samples[gsel], 1); | |
Brush b1 = new SolidBrush(samples[gsel]); | |
//グラフ化する範囲をyd minを差し引く | |
double yorg = Convert.ToDouble(dgv1.Rows[irn + startrowN].Cells[pcoln].Value); | |
double yorg_1 = Convert.ToDouble(dgv1.Rows[irn + startrowN - 1].Cells[pcoln].Value); | |
double yd = Convert.ToDouble(dgv1.Rows[irn + startrowN].Cells[pcoln].Value) - (double)gline[gsel].min; | |
double yd_1 = Convert.ToDouble(dgv1.Rows[irn + startrowN - 1].Cells[pcoln].Value) - (double)gline[gsel].min; | |
if (gsel == 4)//bnohosei | |
{ | |
int tn = dc.tnrow[irn + startrowN]; | |
yd = Convert.ToDouble(dgv1.Rows[irn + startrowN].Cells[pcoln].Value) + tR[tn].bnosaR; | |
yd_1 = Convert.ToDouble(dgv1.Rows[irn + startrowN - 1].Cells[pcoln].Value) + tR[tn].bnosaR; | |
} | |
y0 = Convert.ToInt32(yd); | |
y0_1 = Convert.ToInt32(yd_1); | |
int px = irn * tR[0].xdot; | |
int px_1 = (irn - psa) * tR[0].xdot; | |
int py0 = (int)ph / gline[gsel].baseL - (int)((double)yd * yscale); | |
int py0_1 = (int)ph / gline[gsel].baseL - (int)((double)yd_1 * yscale); | |
Debug.Print("---------------dgv[" + (irn + startrowN).ToString() + "][" + pcoln.ToString() + "]=" + yorg.ToString("F2") + "max=" + gline[gsel].max.ToString() + ",min=" + gline[gsel].min.ToString()); | |
Debug.Print("yscale=" + yscale.ToString() + ",yrange=" + yrange.ToString() + "yorg=" + yorg.ToString() + "yd=" + yd.ToString() + "y0=" + y0.ToString() + ",py0=" + py0.ToString()); | |
g1.DrawLine(p, px, py0, px_1, py0_1); | |
Color c = samples[gsel]; | |
} | |
// } | |
} | |
// pictureBox1.Image = trimmedBitmap; | |
}//makeATurnBitmap() end | |
//***************TimeScale Graph Source Bit Map make******************************************************************************* | |
/*参照構造体 lines[] gline | |
struct lines | |
{ | |
public string name;//LIne名 選択されたデータ名 | |
public bool available;//Lineが有効:true 無効:false | |
public int colN;//Column番号 | |
public Color clr; | |
public int sa;//data 間隔 | |
public int rN_avail; | |
public int period; | |
public int x; | |
public int x_1; | |
public int y; | |
public int y_1; | |
public int max; | |
public int min; | |
public int baseL;//Graph base Line 1/2 1/1 | |
public int uscale;//headMot=100000 | |
} | |
//Struct GraphLine | |
public Color[] samples = new Color[] { Color.Red, Color.Blue, Color.Green, Color.Yellow, Color.Aqua, Color.DeepPink, Color.BlanchedAlmond }; | |
lines[] gline;//gline global | |
*/ | |
//*******************gline naming****************************************** | |
// soukrceBitamapを切り取って | |
private void hScrollBar1_Scroll(object sender, ScrollEventArgs e) | |
{ | |
int cx = hScrollBar1.Value; | |
int pW = pictureBox1.Width; | |
// int dot = 1;// Convert.ToInt32(textBox10.Text); | |
// int posn = (int)cx / dot; | |
//textBox3.Text = posn.ToString(); | |
int xdot = timeScale(); | |
// int rcx = (int)((cx - pW / 2) / xdot); | |
//textBox13.Text = rcx.ToString(); | |
// Debug.Print("cx=" + cx.ToString() + "pW=" + pW.ToString() + "dot=" + dot.ToString() + "posn=" + posn.ToString() + "xdot=" + xdot.ToString() + "rcx=" + rcx.ToString()); | |
sourceCut(cx);//sourceBitmapからcxからcx-1000ドット切り取って、目盛り線と文字を書き込んでpictureBox1に表示 | |
/* | |
//----------センターカーソルのY値読み込み 表示-------------------------------------------------------- | |
string g0s = (Convert.ToDouble(dgv1.Rows[rcx].Cells[gline[0].colN].Value) / gline[0].uscale).ToString(); | |
string g1s = (Convert.ToDouble(dgv1.Rows[rcx].Cells[gline[1].colN].Value) / gline[1].uscale).ToString(); | |
string g2s = (Convert.ToDouble(dgv1.Rows[rcx].Cells[gline[2].colN].Value) / gline[2].uscale).ToString(); | |
string g3s = (Convert.ToDouble(dgv1.Rows[rcx].Cells[gline[3].colN].Value) / gline[3].uscale).ToString(); | |
string g4s = (Convert.ToDouble(dgv1.Rows[rcx].Cells[gline[4].colN].Value) / gline[4].uscale).ToString(); | |
string g5s = (Convert.ToDouble(dgv1.Rows[rcx].Cells[gline[5].colN].Value) / gline[5].uscale).ToString(); | |
*/ | |
} | |
/* | |
*/ | |
/// <summary> | |
/// ===================================SOURCE CUT=============================================================== | |
/// </summary> | |
/// <param name="cx"></param> | |
private void sourceCut(int cx)//cxはスクロールバー現在位置 | |
{ | |
Graphics g2 = Graphics.FromImage(trimmedBitmap); | |
g2.Clear(Color.Black); | |
int pW = 1000; | |
int startX = 0; | |
if (cx > pW) | |
{ | |
startX = cx - pW; | |
} | |
else | |
{ | |
startX = 0; | |
} | |
/* | |
if (checkBox10.Checked == true) | |
{ | |
pictureBox1.Visible = true; | |
} | |
else | |
{ | |
pictureBox1.Visible = false; | |
} | |
*/ | |
Rectangle srcRect = new Rectangle(startX, 0, pW, 500); | |
Rectangle desRect = new Rectangle(0, 0, pW, 500); | |
g2.DrawImage(sourceBitmap, desRect, srcRect, GraphicsUnit.Pixel); | |
//Center Cursor | |
g2.DrawLine(Pens.White, pW / 2, 0, pW / 2, pictureBox1.Height); | |
g2.DrawLine(Pens.White, 0, pictureBox1.Height / 2, pW, pictureBox1.Height / 2); | |
// =================センターカーソル座標での各数値表示==================================== | |
//0:headMot,1:mHead,2:gSpeed,3:pitch,4:yaw1,5:DZB,6:relLm,7:slip | |
//----sourceBitmapX座標の cxから行番号を計算 ([0].endNo-[1]startNo)*xdotが全幅 cx=0がtR[1].startNo行になる------ | |
int start_turnN = Convert.ToInt32(textBox10.Text); | |
int rncx = (int)(cx / tR[0].xdot) - (int)(500 / tR[0].xdot) + tR[start_turnN].startNo;// (int)(cx / tR[0].xdot); | |
double hmot = Convert.ToDouble(dgv1.Rows[rncx].Cells[18].Value); | |
double mhead = Convert.ToDouble(dgv1.Rows[rncx].Cells[25].Value); | |
double slip = hmot - mhead; | |
double gspd = Convert.ToDouble(dgv1.Rows[rncx].Cells[17].Value) / 1000000 * 3600;//mm/sec=>kmh | |
double pit = Convert.ToDouble(dgv1.Rows[rncx].Cells[30].Value); | |
double ywhosei = Convert.ToDouble(dgv1.Rows[rncx].Cells[39].Value); | |
double Drel = Convert.ToDouble(dgv1.Rows[rncx].Cells[41].Value); | |
double dzb = Convert.ToDouble(dgv1.Rows[rncx].Cells[16].Value); | |
double lens = Convert.ToDouble(dgv1.Rows[rncx].Cells[27].Value); | |
int flagsB = Convert.ToInt32(dgv1.Rows[rncx].Cells[1].Value); | |
int flagsR = Convert.ToInt32(dgv1.Rows[rncx].Cells[2].Value); | |
double roll = Convert.ToDouble(dgv1.Rows[rncx].Cells[31].Value); | |
int haccb = Convert.ToInt32(dgv1.Rows[rncx].Cells[5].Value); | |
double yaw0 = Convert.ToDouble(dgv1.Rows[rncx].Cells[29].Value); | |
double pitch0 = Convert.ToDouble(dgv1.Rows[rncx].Cells[30].Value); | |
double roll0 = Convert.ToDouble(dgv1.Rows[rncx].Cells[31].Value); | |
double grx0 = Convert.ToDouble(dgv1.Rows[rncx].Cells[35].Value); | |
double gry0 = Convert.ToDouble(dgv1.Rows[rncx].Cells[36].Value); | |
double grz0 = Convert.ToDouble(dgv1.Rows[rncx].Cells[37].Value); | |
// | |
// Debug.Print("rncx=" + rncx.ToString() + "hmot=" + hmot.ToString()); | |
//文字列を表示する範囲を指定する | |
// 0:headMot,1:mHead,2:gSpeed,3:pitch,4:yaw1,5:DZB,6:relLm | |
int haba = 110; | |
RectangleF rectall = new RectangleF(0, 0, 1000, 20);//行全体 | |
RectangleF rect = new RectangleF(500, 470, 500, 20);//行番号をセンターライン下に表示 | |
RectangleF rect0 = new RectangleF(0, 0, haba, 20);//hMot位置 | |
RectangleF rect1 = new RectangleF(haba, 0, haba, 20);//slip位置 | |
RectangleF rect2 = new RectangleF(haba * 2, 0, haba, 20);//gspd位置 | |
RectangleF rect3 = new RectangleF(haba * 3, 0, haba, 20);//pit位置 | |
RectangleF rect4 = new RectangleF(haba * 4, 0, haba, 20);//mhead位置 | |
RectangleF rect5 = new RectangleF(haba * 5, 0, haba, 20);//ywhosei位置 | |
RectangleF rect6 = new RectangleF(haba * 6, 0, haba, 20);//yaw1位置 | |
RectangleF rect7 = new RectangleF(haba * 7, 0, haba, 20);//dzb位置 | |
RectangleF rect8 = new RectangleF(haba * 8, 0, haba, 20);//length位置 | |
RectangleF rect9 = new RectangleF(0, 470, haba * 4, 20);//flagsB,flagsR,,hacc 左下 | |
RectangleF rect10 = new RectangleF(0, 450, haba * 7, 20);//BNO yaw,pitch,roll,grX,grY,grZ 左下の1行上 | |
g2.FillRectangle(Brushes.Black, rect);//rectの四角を描く | |
g2.FillRectangle(Brushes.Black, rectall);//rectの四角を描く | |
//フォントオブジェクトの作成 | |
Font fnt = new Font("MS UI Gothic", 16); | |
// string | |
//itowから経過時間表示をrncxの後に追加 | |
double timeitow = (Convert.ToDouble(dgv1.Rows[rncx].Cells[13].Value) - Convert.ToDouble(dgv1.Rows[2].Cells[13].Value)) / 1000; | |
string timeitows = timeitow.ToString("F2"); | |
string itowbstr = dgv1.Rows[rncx].Cells[13].Value.ToString(); | |
//----------------------------------- | |
string strrowN = timeitows + "sec" + "[" + rncx.ToString() + "]" + "TurnNo=" + dc.tnrow[rncx].ToString() + ":" + tR[dc.tnrow[rncx]].direction + "@itowB=" + itowbstr; | |
string strhmot = "hmot=" + hmot.ToString("F0"); | |
string strmhead = "mhed=" + mhead.ToString("F0"); | |
string strslip = "slip=" + slip.ToString("F0"); | |
string strgspd = "gspd=" + gspd.ToString("F1"); | |
string strpit = "pit1=" + pit.ToString("F0"); | |
string strywH = "yawH=" + ywhosei.ToString("F0"); | |
string strdrel = "Drel=" + Drel.ToString("F1"); | |
string strdzb = "dzb =" + dzb.ToString("F0"); | |
string strlens = "lens =" + lens.ToString("F0"); | |
string strflags = "flagsB=" + flagsB.ToString() + "flagsR=" + flagsR.ToString() + "roll=" + roll.ToString() + "hacc=" + haccb.ToString(); | |
string strbno = "BNO:yaw=" + yaw0.ToString("F1") + "pitch=" + pitch0.ToString("F1") + "roll=" + roll0.ToString("F1") + "grX=" + grx0.ToString("F2") + "grY=" + gry0.ToString("F2") + "grZ=" + grz0.ToString("F2"); | |
string strroll = "roll=" + roll.ToString("F0"); | |
//描画 public Color[] samples = new Color[] { Color.Red, Color.Blue, Color.Green, Color.Yellow, Color.Aqua, Color.DeepPink, Color.BlanchedAlmond }; | |
g2.DrawString(strrowN, fnt, Brushes.White, rect);//rowN描画 | |
g2.DrawString(strhmot, fnt, Brushes.Red, rect0);//hMot描画 | |
g2.DrawString(strslip, fnt, Brushes.Orange, rect1); | |
g2.DrawString(strgspd, fnt, Brushes.Green, rect2); | |
g2.DrawString(strpit, fnt, Brushes.Yellow, rect3); | |
g2.DrawString(strmhead, fnt, Brushes.Blue, rect4); | |
g2.DrawString(strywH, fnt, Brushes.Aqua, rect5); | |
g2.DrawString(strdrel, fnt, Brushes.BlanchedAlmond, rect6); | |
g2.DrawString(strroll, fnt, Brushes.DeepPink, rect7); | |
g2.DrawString(strlens, fnt, Brushes.White, rect8); | |
g2.DrawString(strflags, fnt, Brushes.White, rect9); | |
g2.DrawString(strbno, fnt, Brushes.White, rect10); | |
/* | |
// Font fnt = new Font("MS UI Gothic", 15); | |
//文字列を表示する範囲を指定する | |
// RectangleF rect = new RectangleF(10, 30, 150, 100); | |
//rectの四角を描く | |
g2.FillRectangle(Brushes.Black, rect); | |
//dgv ブロックNo 行番号 | |
string gyon = "rowN=" + pointrowN.ToString("D"); | |
g2.DrawString(gyon, fnt, Brushes.White, rect); | |
//gSpeed | |
float gspeedkph = (float)dgv[pointrowN, 2] / 1000000 * 3600; | |
float gspeedmps = (float)dgv[pointrowN, 2] / 1000; | |
string speeds = "gSpeed=" + ((float)dgv[pointrowN, 2] / 100000).ToString(); | |
g2.DrawString(gspeedkph.ToString("F2") + " km/h", fnt, Brushes.White, 10, 50); | |
g2.DrawString(gspeedmps.ToString("F2") + " m/sec", fnt, Brushes.White, 10, 70); | |
*/ | |
pictureBox1.Image = trimmedBitmap; | |
} | |
//******************************************************************************************************* | |
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
private int timeScale() | |
{ | |
int dot = 1;// Convert.ToInt32(textBox1.Text); | |
// if (dot * dgv1.RowCount >bw) | |
if (dot * rowN > bw) | |
{ | |
//dot = (int)(bw / dgv1.RowCount); | |
dot = (int)(bw / rowN); | |
if (dot == 0) { dot = 1; } | |
} | |
return dot; | |
} | |
//=========================================================================================================================== | |
//======================TURNカット Nutral()================================================================================= | |
//headMOTの極値をニュートラル(切り替え)ポイントとして抽出して、配列データとしてもって、ターングラフに重ねる | |
//headMOTを5回MAして 微分してプラスマイナス変化点をポイントとする | |
//=========================================================================================================================== | |
//=========================================================================================================================== | |
//******************* ターン構造体 turns******************************************************** | |
/*TURNS 構造体メモ | |
struct Turns | |
{ | |
//Public rNturnNo() ;// Integer //rNturnNo(rN)=turnNo | |
public string skiRL; //左右スキーの指定(0)番に収納 | |
public int turnNo; //ターン番号 | |
public string direction;// //ターン方向 R L | |
public int lastNo;// //ターンの数(0)番に収納 | |
public int startNo, endNo;// //開始データ行No | |
public double[] NY; | |
public double[] EX; | |
public double[] EZ; | |
public int initX, initY, endX, endY;// //開始座標、終了座標 X:lon Y:lat | |
public double turnTime;// //ターン時間 | |
public double turnTimesum;// //累積ターン経過時間 | |
//ターン弧 | |
public double Radius;// //簡易円近似半径 | |
public double theta;////fallLine 角度 | |
public int minorDia, majorDia;// //ターン半円の縦短径、横長径 | |
public double Aspectratio; //pectratio;// //ターン弧のアスペクト比 | |
public double fallen;// //fallLine長さ | |
public double fallensum;// // fallLineの累計 | |
public double initHeight, endHeight;// | |
public double fallHeight;// //fallLineの標高差 | |
public double fallHeightsum;////fallLine標高差の累計 | |
public double Slopedeg;// //fallLineの斜度deg | |
public double Arclen;// //円弧長 | |
public double Arclensum;////円弧長の累計 | |
//Speed | |
public double[] Speed;////Speed配列 | |
public double inSpeed, outSpeed, aveSpeed, maxSpeed, minSpeed;////ターン中の速度各種 | |
//Heading Skidding | |
public double[] headMot;////headMot角配列 | |
public double[] headSki;// //ski角配列 | |
public double[] Skid;// //Skid角配列 | |
public double maxHeadmot, minHeadmot, dHeadmot;// //headMot角各種 | |
public double maxSkid, minSkid, aveSkid, dSkid;// | |
//G Acceleration | |
public double[] Gacc;////平均加速度 | |
public double Gaccmax, Gaccmin, Gaccave;// | |
//Slope ターン開始点標高と終了点標高と傾斜角 | |
public double startHeight; | |
public double lastHeight; | |
public double tSlope; | |
public int xdot;//makeATurnBitmap()で使ったドット分解能dot/data tR[0].xdotに収納 | |
//turnのsmode位置情報 | |
public int[] smode_turnStartNo;//smode番号での最初のターン番号tR[0].smode_turnStartNo[]に収納 | |
public int[] smode_turnEndNo;//smode番号での最後のターン番号tR[0].smode_turnEndNo[]に収納 | |
//BNO 補正 | |
public double bnosaR;//tR[].bnosaR 左ターンスタートポイントでheadMotとの差 bnosaR=headMot-yaw1 yaw1hosei=yaw1+hoseiR | |
public double bnosaL;//bnosaL | |
}; | |
//-----------Turns 構造体配列定義------------------------------- | |
Turns[] tR = new Turns[1000];////右スキーのターン構造体配列宣言200個まで収納 | |
Turns[] tL = new Turns[1000];////左スキーのターン構造体配列宣言200個まで収納 | |
*/ | |
private void button12_Click(object sender, EventArgs e)//Turn Search Nutral() | |
{ | |
int tsNo; | |
if (dc.dgvsorted == 1 && dc.turnprocessed == 0) | |
{ | |
for (tsNo = 0; tsNo < dc.smodeN + 1; tsNo++) | |
{ | |
Nutral(dc.smode_startNo[tsNo], dc.smode_endNo[tsNo]); | |
} | |
} | |
else | |
{ | |
tsNo = Convert.ToInt32(textBox10.Text); | |
Nutral(dc.smode_startNo[tsNo], dc.smode_endNo[tsNo]); | |
} | |
button13.Enabled = true; | |
//==================BNO Nutral補正Cells[39]へ代入=============================== | |
int irr; | |
int turnlastN = Convert.ToInt32(textBox9.Text); | |
for (irr = 1; irr < turnlastN; irr++) | |
{ | |
Debug.Print("-------------------dc.tnrow[" + irr.ToString() + "]=" + dc.tnrow[irr].ToString()); | |
if (dc.tnrow[irr] > 0) | |
{ | |
int tn = dc.tnrow[irr]; | |
dgv1.Rows[irr].Cells[39].Value = (Convert.ToDouble(dgv1.Rows[irr].Cells[29].Value) + tR[tn].bnosaR).ToString(); | |
dgv1.Rows[irr].Cells[40].Value = tR[tn].bnosaR.ToString(); | |
//dgv1.Rows[irr].Cells[41].Value = dgv1.Rows[irr].Cells[29].Value; | |
//dgv1.Rows[irr].Cells[42].Value = tn.ToString(); | |
Debug.Print("BNOHOSEI:irr=" + irr.ToString() + "tR[" + tn.ToString() + "].bnosaR=" + tR[tn].bnosaR.ToString()); | |
} | |
} | |
tR[0].lastNo = 0;//ターン番号累計初期化 | |
} | |
//bnohosei dgv Cells[38]のターン番号を見ながらCells[39]セルにbnohosei値を代入する | |
#endregion | |
#region <<<<<<<<<<<<<<<<<<<<<<<<<NUTRAL>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> | |
private void Nutral(int stNo, int endNo)//smode中のTurn NutralPointを探す | |
{ | |
// int stNo = Convert.ToInt32(textBox1.Text); | |
// int endNo = Convert.ToInt32(textBox2.Text); | |
int ic, i5; | |
int icR_1, icL_1; | |
double sum5R, sum5L; | |
double headR = 0; | |
double headL = 0; | |
string gspeedRs = ""; | |
int nuNR, nuNL; | |
int numN = 0; | |
int num = endNo - stNo; | |
double[] MA5headR = new double[endNo]; | |
double[] DifheadR = new double[endNo]; | |
double[] MA5headL = new double[endNo]; | |
double[] DifheadL = new double[endNo]; | |
int turnCountR, turnCountL; | |
int pared = 0; | |
turnCountR = tR[0].lastNo;// 前のターンのターン数累計からスタート | |
turnCountL = 0; | |
nuNR = 0;//turn counter RIGHT | |
nuNL = 0;//turn counter LEFT | |
icR_1 = -8; | |
icL_1 = -8; | |
int[] NutralR = new int[endNo]; | |
int[] NutralL = new int[endNo]; | |
int[] turnNoR = new int[endNo]; | |
int[] turnNoL = new int[endNo]; | |
int TurnNumR, TurnNumL; | |
//loop ic------------------------------- | |
Debug.Print("===========================================NUTRAL::stNo=" + stNo.ToString() + "endNo=" + endNo.ToString()); | |
for (ic = stNo; ic < endNo - 5; ic++) | |
{ | |
numN += 1; | |
sum5R = 0; | |
sum5L = 0; | |
for (i5 = ic; i5 < 4 + ic; i5++)//5回の移動平均でheadMotを平均化 | |
{ | |
int col_lon = 0; | |
int col2_lon = 0; | |
int headMotBR = 18; | |
int headMotBL = 25; | |
// headR = (double)(dgv1.Rows[i5].Cells[col_lon + 10].Value )* 0.00001; | |
// headL = (double)(dgv1.Rows[i5].Cells[col2_lon + 10].Value)* 0.00001; | |
headR = Convert.ToDouble(dgv1.Rows[i5].Cells[headMotBR].Value); | |
//Debug.Print("headR=" + headR.ToString()); | |
headL = Convert.ToDouble(dgv1.Rows[i5].Cells[headMotBL].Value); | |
sum5R = sum5R + headR; | |
sum5L = sum5L + headL; | |
}// for i5 end | |
MA5headR[ic] = sum5R / 5; | |
MA5headL[ic] = sum5L / 5; | |
// gspeedRs = dgv1.Rows[i5].Cells[24].Value.ToString(); | |
DifheadR[ic] = MA5headR[ic] - MA5headR[ic - 1]; | |
DifheadL[ic] = MA5headL[ic] - MA5headL[ic - 1]; | |
//Debug.Print("DifheadR[,"+ic.ToString()+",]=,"+DifheadR[ic].ToString()+",hedR=,"+headR.ToString()+",gSpeedRs=,"+gspeedRs); | |
//*******************************************ターン判定********************************************************** | |
// 微分正負変化点&&ターン長7データ&&gspeed >5000 時速18kmh以上で切る | |
double gsp = Convert.ToDouble(dgv1.Rows[ic].Cells[17].Value); | |
//Debug.Print("if DIff:DifheadR*L[" + ic.ToString() + "] =" + (DifheadR[ic] * DifheadR[ic - 1]).ToString() + "gsp=" + gsp.ToString()); | |
if ((DifheadR[ic] * DifheadR[ic - 1]) < 0 && ic - icR_1 > 7 && gsp > 3000) //前回から8個以上離れて正負逆転した場合NutralPointと判断 | |
{ | |
//Debug.Print("ifNUTRAL:gsp=" + gsp.ToString()); | |
icR_1 = ic; | |
if (DifheadR[ic] < 0) | |
{ | |
NutralR[ic] = -1; //左谷足ターン | |
tR[turnCountR].direction = "Rturn"; | |
} | |
else | |
{ | |
NutralR[ic] = 1;//右谷足ターン | |
tR[turnCountR].direction = "Lturn"; | |
} | |
turnNoR[nuNR] = ic;//ニュートラルポイントでの行番号記録 | |
//*****ターン構造体記録**************************** | |
//nuNR=1でtr.turnNo=0,nuNR=2でtr.turnNo=1 | |
if (nuNR > 0) | |
{ | |
tR[turnCountR].turnNo = turnCountR; //構造体へターン番号記録 | |
tR[turnCountR].startNo = turnNoR[nuNR - 1];//構造体へスタート行番号記録 | |
tR[turnCountR].endNo = turnNoR[nuNR]; //構造体へスタート行番号記録3-17-11-14.ubx_dgv | |
tR[turnCountR].stitow = Convert.ToInt32(dgv1.Rows[turnNoR[nuNR - 1]].Cells[13].Value);//StartPanel itow | |
tR[turnCountR].enditow = Convert.ToInt32(dgv1.Rows[turnNoR[nuNR]].Cells[13].Value);//StartPanel itow | |
tR[turnCountR].stheadmot = Convert.ToDouble(dgv1.Rows[turnNoR[nuNR - 1]].Cells[18].Value);//StartPanel itow | |
tR[turnCountR].endheadmot = Convert.ToDouble(dgv1.Rows[turnNoR[nuNR]].Cells[18].Value);//StartPanel itow | |
tR[turnCountR].stmhead = Convert.ToDouble(dgv1.Rows[turnNoR[nuNR - 1]].Cells[25].Value);//StartPanel itow | |
tR[turnCountR].endmhead = Convert.ToDouble(dgv1.Rows[turnNoR[nuNR]].Cells[25].Value);//StartPanel itow | |
//RichTextBox2.AppendText("tR(" + CStr(turnCountR) + ") = " + CStr(turnCountR) + ":tr.startNo=" + CStr(tR(turnCountR).startNo) + ",tr.endNo=" + CStr(tR(turnCountR).endNo) + vbCrLf); | |
dgv1.Rows[ic].Cells[38].Value = turnCountR.ToString();// | |
//bno | |
int strow = tR[turnCountR].startNo; | |
tR[turnCountR].bnosaR = Convert.ToDouble(dgv1.Rows[strow].Cells[18].Value) - Convert.ToDouble(dgv1.Rows[strow].Cells[29].Value); | |
//richTextBox1.AppendText("tR[" + turnCountR.ToString() + "].bnosa=" + tR[turnCountR].bnosaR.ToString() + "hMot[" + strow.ToString()+"]="+ dgv1.Rows[strow].Cells[18].Value.ToString() + "yaw1[" +strow.ToString() + "]=" + dgv1.Rows[strow].Cells[29].Value.ToString() + "\r\n"); | |
Debug.Print("Nutral:ic=" + ic.ToString()); | |
Debug.Print("NutralBNO:tR[" + turnCountR.ToString() + "].bnosaR=" + tR[turnCountR].bnosaR.ToString() + "\r\n"); | |
Debug.Print("hMot=" + dgv1.Rows[strow].Cells[18].Value.ToString() + "\r\n"); | |
Debug.Print("yaw1=" + dgv1.Rows[strow].Cells[29].Value.ToString() + "\r\n"); | |
int tcr = turnCountR; | |
//string nutralstr = "turnCountR[," + turnCountR.ToString() + ",]:startNo=," + tR[turnCountR].startNo.ToString() + ",endNo," + tR[turnCountR].endNo.ToString() + ",dc.tnrow=," + dc.tnrow[ic].ToString() + ",gspeed=," + gsp.ToString() + ",tR[].bnosaR=," + tR[turnCountR].bnosaR.ToString() + "\r\n"; | |
string nutralstr = "tR[" + tcr.ToString() + ",]:startNo=," + tR[tcr].startNo.ToString() + ",endNo," + tR[tcr].endNo.ToString() + "stitow=" + tR[tcr].stitow.ToString() + "enditow=" + tR[tcr].enditow.ToString() + "stheadmot=" + tR[tcr].stheadmot.ToString() + "endheadmot=" + tR[tcr].endheadmot.ToString() + "endmhead=" + tR[tcr].endmhead.ToString() + "\r\n"; | |
//string nutralstrdgv = "turnCountR[," + turnCountR.ToString() + ",dc.tnrow=," + dc.tnrow[ic].ToString() + ",gspeed=," + gsp.ToString() + ",tR[].bnosaR=," + tR[turnCountR].bnosaR.ToString(); | |
string nutralstrdgv="tR[," + tcr.ToString() + ",]:startNo=," + tR[tcr].startNo.ToString() + ",endNo," + tR[tcr].endNo.ToString() +",stitow=,"+tR[tcr].stitow.ToString()+ ",enditow=," +tR[tcr].enditow.ToString() + ",stheadmot=," + tR[tcr].stheadmot.ToString()+ ",endheadmot=," + tR[tcr].endheadmot.ToString() + ",stmhead=," + tR[tcr].stmhead.ToString()+ ",endmhead=," + tR[tcr].endmhead.ToString(); | |
string[] nutralarry = new string[100]; | |
nutralarry = nutralstrdgv.Split(','); | |
richTextBox1.AppendText(nutralstr); | |
dgv1.Rows.Add(nutralarry); | |
turnCountR++; | |
turnCountL++; | |
} | |
else | |
{ | |
NutralR[ic] = turnCountR + 2;// // ターンNOを2個多く記憶 | |
} | |
nuNR += 1;//turn count プラス | |
// Debug.Print("NUTRAL_P:ic=" + ic.ToString() + "nuNR,turnCountR=" + nuNR.ToString() + "," + turnCountR.ToString() + "dc.tnrow=" + dc.tnrow[ic].ToString() + "gspeed=" + gsp.ToString()); | |
}//if DifheadR end | |
else if (gsp < 2000)//ターン gsp<2000でない行 | |
{ | |
dc.tnrow[ic] = -1;// tnrow-1でターン終わり | |
dgv1.Rows[ic].Cells[38].Value = "-1";//; turnCountR.ToString(); | |
// Debug.Print("NOT_NUTRAL:ic=" + ic.ToString() + "nuNR,turnCountR=" + nuNR.ToString() + "," + turnCountR.ToString() + "dc.tnrow=" + dc.tnrow[ic].ToString() + "gspeed=" + gsp.ToString()); | |
} | |
else if (gsp >= 2000)//ターン中 | |
{ | |
dc.tnrow[ic] = turnCountR; | |
dgv1.Rows[ic].Cells[38].Value = turnCountR.ToString(); | |
// Debug.Print("IN TURN:ic=" + ic.ToString() + "nuNR,turnCountR=" + nuNR.ToString() + "," + turnCountR.ToString() + "dc.tnrow=" + dc.tnrow[ic].ToString() + "gspeed=" + gsp.ToString()); | |
} | |
//else if(gsp>230) | |
//{ | |
// Debug.Print("*******************Nowhere:ic=" + ic.ToString() + "nuNR,turnCountR=" + nuNR.ToString() + "," + turnCountR.ToString() + "dc.tnrow=" + dc.tnrow[ic].ToString() + "gspeed=" + gsp.ToString()); | |
//} | |
/* | |
if (DifheadL[ic] * DifheadL[ic - 1] < 0 && ic - icL_1 > 7 )// //前回から8個以上離れて正負逆転した場合NutralPointと判断 | |
{ | |
icL_1 = ic; | |
if( DifheadL[ic] < 0 ) | |
{ | |
NutralL[ic-stNo] = -1; | |
tL[turnCountL].direction = "Rturn"; | |
} | |
else | |
{ | |
NutralL[ic-stNo] = 1; | |
tL[turnCountL].direction = "Lturn"; | |
} | |
//CheckBox12.Checked = True; | |
turnNoL[nuNL-stNo] = ic; | |
//*****ターン構造体記録**************************** | |
//nuNR=1でtr.turnNo=0,nuNR=2でtr.turnNo=1 | |
if (nuNL > 0 ) | |
{ | |
tL[turnCountL].turnNo = turnCountL;// //構造体へターン番号記録 | |
tL[turnCountL].startNo = turnNoL[nuNL - 1];// //構造体へスタート行番号記録 | |
tL[turnCountL].endNo = turnNoL[nuNL];// //構造体へスタート行番号記録 | |
//RichTextBox2.AppendText("tL(" + CStr(turnCountL) + ") = " + CStr(turnCountL) + ":tL.startNo=" + CStr(tL(turnCountL).startNo) + ",tL.endNo=" + CStr(tL(turnCountL).endNo) + vbCrLf) | |
turnCountL += 1; | |
} | |
//************************************************* | |
//RichTextBox2.AppendText("NutralheadL(" + CStr(ic) + ")=" + CStr(1) + "TurnNoL(" + CStr(nuNL) + ")=" + CStr(ic) + "DifheadL(ic)=" + CStr(DifheadL(ic)) + vbCrLf) | |
nuNL += 1; | |
}//if DifheadL | |
else | |
{ | |
NutralL[ic-stNo] = turnCountL + 2;// // ターンNOを2個多く記憶 | |
}//if DifheadL end | |
*/ | |
TurnNumR = nuNR; | |
/* | |
//*********************BNO補正**************************************************** | |
if (turnCountR > 1) | |
{ | |
if (tR[turnCountR - 1].direction == "Lturn")//右谷足(Lturn)時に補正 | |
{ //tR[turnCountR].bnosaR = Convert.ToDouble(dgv1.Rows[ic].Cells[18].Value) - Convert.ToDouble(dgv1.Rows[ic].Cells[29].Value); | |
double wa = Convert.ToDouble(dgv1.Rows[ic].Cells[29].Value) + tR[turnCountR - 1].bnosaR; | |
dgv1.Rows[ic].Cells[39].Value = wa.ToString(); | |
Debug.Print("BNOhosei:ic="+ic.ToString() +"TurnCountR="+(turnCountR-1).ToString()+":Direction:" +tR[turnCountR - 1].direction + ":bnosa=" + tR[turnCountR - 1].bnosaR.ToString() + "bnohosei[39]=" + wa.ToString("F3") + ":mHead=" + dgv1.Rows[ic].Cells[25].Value.ToString()); | |
} | |
} | |
//*************************************************************************************** | |
*/ | |
//bnosaを足して補正済みyawhosei1をdgv1Rows[ic].Cells[39]へ記録 | |
dc.tnrow[ic] = turnCountR; | |
int iend; | |
for (iend = ic; iend < Convert.ToInt32(dgv1.Rows[0].Cells[0].Value); iend++) | |
{ | |
dc.tnrow[iend] = turnCountR; | |
} | |
//richTextBox1.AppendText("dc.tnrow[" + ic.ToString() + "]=" + dc.tnrow[ic].ToString()+"\r\n"); | |
Debug.Print("dc.tnrow[" + ic.ToString() + "]=" + dc.tnrow[ic].ToString()); | |
tR[0].lastNo = turnCountR - 1;// //ターン最後番号をtR(0).lastNoに収納 | |
textBox9.Text = tR[0].lastNo.ToString(); | |
TurnNumL = nuNL; | |
tL[0].lastNo = turnCountL - 1;// //ターン最後番号をtL(0).lastNoに収納 | |
}//for ic | |
}// Nutral end | |
//===================================================================================================== | |
//=================bnofit() Nutral処理後のBNO処理 dgv1.Rows[]Cells[38]に新たに追加==================== | |
//=================BNO yaw角をNutralポイントで、headMot角に一致させる、Nutral毎に回繰り返す============ | |
//===================================================================================================== | |
//:::::::::::BTMON Binary Dataから実データ変換してDGVへ代入============================================ | |
// SD data BNO ASCII UPLOAD Transform | |
//--------bnbuf[180]=header4byte(BNO)+ Euler12(4bytex3)+Quaternion16(4bytex4)+LinearAcc(4bytex3)+Gravity(4bytex3)+time4(4bytex1)=36 byte 60x3=180byte------- | |
// | |
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^SD DATA ASCII UPLOAD^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ | |
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ | |
#endregion | |
#region [[[[[[[[[[[[[[[[[[[[[[[[[[[DATA CHECK]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]] | |
//*************************DATA CHECK*************************************:: | |
//角度が合っているか修正 headMot中心にmHeadとyaw,pitch,rollが正規に値になっているか修正 | |
//信頼性に関わるパラメータの集計をdgv最終行に記入 | |
//Max Min aveも追加 | |
private void button14_Click(object sender, EventArgs e)//DataCHECK | |
{ | |
datacheck(); | |
int coli; | |
double maxv = 0; | |
double minv = 0; | |
double avev = 0; | |
dgv1.Rows.Add(3); | |
dgv1.Rows[dgv1.Rows.Count - 3].Cells[0].Value = "Max"; | |
dgv1.Rows[dgv1.Rows.Count - 2].Cells[0].Value = "Min"; | |
dgv1.Rows[dgv1.Rows.Count - 1].Cells[0].Value = "Ave"; | |
for (coli = 1; coli < 42; coli++) | |
{ | |
mathdgv(coli, ref maxv, ref minv, ref avev); | |
dgv1.Rows[dgv1.Rows.Count - 3].Cells[coli].Value = maxv.ToString(); | |
dgv1.Rows[dgv1.Rows.Count - 2].Cells[coli].Value = minv.ToString(); | |
dgv1.Rows[dgv1.Rows.Count - 1].Cells[coli].Value = avev.ToString(); | |
} | |
} | |
void datacheck() | |
{ | |
// string header = "md,flagsB,flagsR, pdopB,pdopR, haccB,haccR,miscount0,count_upB,count_upB0,ttp,tp0,[Base],itowB,NYB, EXB,DZB,gSpeedB,headMotBZ,[Rover] ,itowR,NYR,EXR,DZR,gSpeedR,mHead,bnHosei+eX[0],relLm],BNO],yaw1,pitch1,roll1,laX,laY,laZ,grX,grY,grZ,turnN,BNOsa,BNHosei,Drel,if(mHead+360)-180,yaw1補正,Gspeed kmh,L_SLIP,LR_Para,DZB*0.01"; | |
//dgv1.Rows.Add(header); | |
headeradd(1, 1);//last2行にヘッダーと番号追加 | |
int bflagcol = 1; | |
int rflagcol = 2; | |
int bpdopcol = 3; | |
int rpdopcol = 4; | |
int bhacccol = 5; | |
int rhacccol = 6; | |
int bitowcol = 13; | |
int ritowcol = 20; | |
//itow error | |
//flag error | |
dgv1.Rows.Add(5);//1行目正常個数 2行目異常個数 3行目% 4行目補助1 5行目補助2 | |
int r0 = dgv1.Rows.Count - 5; | |
int r1 = dgv1.Rows.Count - 4; | |
int r2 = dgv1.Rows.Count - 3; | |
int r3 = dgv1.Rows.Count - 2; | |
int r4 = dgv1.Rows.Count - 1; | |
dgv1.Rows[r0].Cells[0].Value = (dgv1.Rows.Count - 8).ToString(); | |
dgv1.Rows[0].Cells[0].Value = dgv1.Rows[r0].Cells[0].Value;//dgv1[0][0]にデータ総行数を記録 | |
dc.mathmaxrow = Convert.ToInt32(dgv1.Rows[0].Cells[0].Value) + 8; | |
dc.mathminrow = Convert.ToInt32(dgv1.Rows[0].Cells[0].Value) + 9; | |
dc.mathaverow = Convert.ToInt32(dgv1.Rows[0].Cells[0].Value) + 10; | |
dgv1.Rows[r1].Cells[0].Value = "r1"; | |
dgv1.Rows[r2].Cells[0].Value = "r2"; | |
dgv1.Rows[r3].Cells[0].Value = "r3"; | |
dgv1.Rows[r4].Cells[0].Value = "r4"; | |
int lastN = Convert.ToInt32(dgv1.Rows[r0].Cells[0].Value);//[r0][0]に総データ数が書いてある | |
int i; | |
int bflagsN = 0; | |
int rflagsN = 0; | |
int bpdopN = 0; | |
int rpdopN = 0; | |
int bhaccN = 0; | |
int rhaccN = 0; | |
int bitowN = 0; | |
int ritowN = 0; | |
for (i = 1; i < lastN - 1; i++) | |
{ | |
if ((string)dgv1.Rows[i].Cells[bflagcol].Value == "131") //flagsb | |
{ | |
bflagsN++; | |
} | |
if ((string)dgv1.Rows[i].Cells[rflagcol].Value == "131") //flagsr | |
{ | |
rflagsN++; | |
} | |
if (Convert.ToInt32(dgv1.Rows[i].Cells[bpdopcol].Value) < 200) //pdopb | |
{ | |
bpdopN++; | |
} | |
if (Convert.ToInt32(dgv1.Rows[i].Cells[rpdopcol].Value) < 200) //pdopr | |
{ | |
rpdopN++; | |
} | |
if (Convert.ToInt32(dgv1.Rows[i].Cells[bhacccol].Value) < 50) //haccb | |
{ | |
bhaccN++; | |
} | |
if (Convert.ToInt32(dgv1.Rows[i].Cells[rhacccol].Value) < 50) //haccr | |
{ | |
rhaccN++; | |
} | |
if (Convert.ToInt32(dgv1.Rows[i + 1].Cells[bitowcol].Value) - Convert.ToInt32(dgv1.Rows[i].Cells[bitowcol].Value) < 125) //itowb | |
{ | |
bitowN++; | |
} | |
// Debug.Print("datacheck:i=" + i.ToString() + "dgv1.Rows[i + 1].Cells[ritowcol].Value=" + dgv1.Rows[i + 1].Cells[ritowcol].Value.ToString()); | |
if (Convert.ToDouble(dgv1.Rows[i + 1].Cells[ritowcol].Value) - Convert.ToDouble(dgv1.Rows[i].Cells[ritowcol].Value) < 125) //itowr | |
{ | |
ritowN++; | |
} | |
} | |
//result flagsb | |
dgv1.Rows[r0].Cells[bflagcol].Value = bflagsN.ToString();//flagsb Error % | |
dgv1.Rows[r1].Cells[bflagcol].Value = (((float)(lastN - bflagsN) / lastN) * 100).ToString("F3") + "%"; | |
//flagsr | |
dgv1.Rows[r0].Cells[rflagcol].Value = bflagsN.ToString();//flagsr Error % | |
dgv1.Rows[r1].Cells[rflagcol].Value = (((float)(lastN - rflagsN) / lastN) * 100).ToString("F3") + "%"; | |
//pdopb | |
dgv1.Rows[r0].Cells[bpdopcol].Value = bpdopN.ToString();//pdopb Error% | |
dgv1.Rows[r1].Cells[bpdopcol].Value = (((float)(lastN - bpdopN) / lastN) * 100).ToString("F3") + "%"; | |
//pdopr | |
dgv1.Rows[r0].Cells[rpdopcol].Value = rpdopN.ToString();//pdopr % | |
dgv1.Rows[r1].Cells[rpdopcol].Value = (((float)(lastN - rpdopN) / lastN) * 100).ToString("F3") + "%"; | |
//haccb | |
dgv1.Rows[r0].Cells[bhacccol].Value = bhaccN.ToString();//pdopr % | |
dgv1.Rows[r1].Cells[bhacccol].Value = (((float)(lastN - bhaccN) / lastN) * 100).ToString("F3") + "%"; | |
//haccr | |
dgv1.Rows[r0].Cells[rhacccol].Value = rhaccN.ToString();//pdopr % | |
dgv1.Rows[r1].Cells[rhacccol].Value = (((float)(lastN - rhaccN) / lastN) * 100).ToString("F3") + "%"; | |
//itowb | |
dgv1.Rows[r0].Cells[bitowcol].Value = bitowN.ToString();//pdopr % | |
dgv1.Rows[r1].Cells[bitowcol].Value = (((float)(lastN - bitowN) / lastN) * 100).ToString("F3") + "%"; | |
//itowr | |
dgv1.Rows[r0].Cells[ritowcol].Value = ritowN.ToString();//pdopr % | |
dgv1.Rows[r1].Cells[ritowcol].Value = (((float)(lastN - ritowN) / lastN) * 100).ToString("F3") + "%"; | |
} | |
#endregion | |
#region [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[XYPLOT]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]] | |
//((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((( | |
//((((((((((((((((((((((((((((((XYPLOT)))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))) | |
//)))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))))) | |
private void button1_Click(object sender, EventArgs e)//XYPLOT 起動 | |
{ | |
dgv1.Visible = false; | |
dgv2.Visible = false; | |
//int tstN = Convert.ToInt32(textBox5.Text); | |
//int tendN = Convert.ToInt32(textBox6.Text); | |
int tstN = Convert.ToInt32(comboBox2.SelectedItem); | |
int tendN = Convert.ToInt32(comboBox3.SelectedItem); | |
if (checkBox6.Checked == true) | |
{ | |
// XYPLOTsrl(tstN, tendN); | |
XYPLOT3rl(tstN, tendN); | |
} | |
else | |
{ | |
XYPLOTs(tstN, tendN); | |
} | |
}//button1 clickend | |
int Rn(int n,int roff) | |
{ | |
return n + roff; | |
} | |
int Ln(int n,int loff) | |
{ | |
return n+loff; | |
} | |
//---------------------------------------------------------------------------------------------------------------- | |
//-------------------XYPLOTs dgv1専用---------------------------------------------------------------------------- | |
//---------------------------------------------------------------------------------------------------------------- | |
void XYPLOTs(int tn0,int tn1)//dgv1単独用プロット1 | |
{ | |
Graphics g1 = Graphics.FromImage(xyBitmap); | |
tR = new Turns[300]; | |
int ri, ir; | |
int i = 0; | |
double ny0, ex0;//ny [ri][14] ex[][15] | |
double ny, ex;//ny [ri][14] ex[][15] | |
double ny0m, ex0m;//ny [ri][14] ex[][15] | |
double nym, exm;//ny [ri][14] ex[][15] | |
int[] x = new int[10000]; | |
int[] y = new int[10000]; | |
int[] xm = new int[10000]; | |
int[] ym = new int[10000]; | |
// Graphics g1 = Graphics.FromImage(xyBitmap); | |
//turnデータ行番号探索 | |
int lastrn1 = ig(0, 0);// Convert.ToInt32(dgv1.Rows[0].Cells[0].Value); | |
int row0 = lastrn1 + 11; | |
//int row0_2 = lastrn2 + 11; | |
int Rstartrow = ig(row0, 3); | |
//lastrnの11行したからturn構造体データ | |
int tst0 = lastrn1 + 11+tn0; | |
int tst1 = lastrn1 + 11 + tn1; | |
//RL 同期 | |
// int tst2 = lastrn2 + 11; | |
// | |
tR[tn0].startNo = ig(tst0, 3);// Convert.ToInt32( dgv1.Rows[tst1].Cells[3].Value); | |
tR[tn1].endNo = ig(tst1, 5); | |
//初期基準位置 | |
ny0 = dg(tR[tn0].startNo, 14); | |
ex0 = dg(tR[tn0].startNo, 15); | |
ny0m = dg(tR[tn0].startNo, 21); | |
ex0m = dg(tR[tn0].startNo, 22); | |
ir = 0; | |
for (ri = tR[tn0].startNo; ri < tR[tn1].endNo; ri++) | |
{ | |
//Base基準座標------------------ | |
ny = dg(ri, 14); | |
y[ir] = (int)(ny - ny0); | |
ex = dg(ri, 15); | |
x[ir] = (int)(ex - ex0); | |
//RoverをBase基準座標-------- | |
nym = dg(ri, 21); | |
ym[ir] = (int)(nym - ny0); | |
exm = dg(ri, 22); | |
xm[ir] = (int)(exm - ex0); | |
Debug.Print("ri[" + ti(ri) + "]:" + "[ex,ny]=[" + td(ex) + "," + td(ny) + "]" + "[" + ti(ir) + "][x,y]=[" + ti(x[ir]) + "," + ti(y[ir]) + "]"); | |
Debug.Print("ri[" + ti(ri) + "]:" + "[exm,nym]=[" + td(exm) + "," + td(nym) + "]" + "[" + ti(ir) + "][xm,ym]=[" + ti(xm[ir]) + "," + ti(ym[ir]) + "]"); | |
ir++; | |
} | |
int xoff = 0; | |
int yoff = 0; | |
int width = 0; | |
int height = 0; | |
int[] px = new int[10000]; | |
int[] py = new int[10000]; | |
int[] pxm = new int[10000]; | |
int[] pym = new int[10000]; | |
xyrange(ir, x, y,xm,ym, ref xoff, ref yoff, ref width, ref height, px, py,pxm,pym); | |
pictureBox2.Visible = true; | |
g1.Clear(Color.Black); | |
for (i = 1; i < ir; i++) | |
{ | |
g1.DrawLine(Pens.Red, px[i], py[i], px[i - 1], py[i - 1]); | |
g1.DrawLine(Pens.Green, pxm[i], pym[i], pxm[i - 1], pym[i - 1]); | |
g1.DrawLine(Pens.Yellow, px[i], py[i], pxm[i], pym[i ]); | |
} | |
pictureBox2.Image = xyBitmap; | |
} | |
//----------------------------------------------------------------------------------------------------------------- | |
//---------------------XYPLOT3rl dgv3専用---------------------------------------------------------------------- | |
//---------------------------------------------------------------------------------------------------------------- | |
void XYPLOT3rl(int tn0, int tn1)//TurnNo0-1指定してdgv3からソースビットマップxyBitMap作成 処理選択 | |
{ | |
Debug.Print("XYPLOT3rl:****************************tn0=" + ti(tn0) + "tn1=" + ti(tn1) + "*******************************"); | |
// XYRLplot rl = new XYRLplot(); | |
tL = new Turns[300]; | |
int ri, ir; | |
int i = 0; | |
double ny0, ex0;//ny [ri][14] ex[][15] | |
double ny0m, ex0m;//ny [ri][14] ex[][15] | |
double nyR, exR;//ny [ri][14] ex[][15] | |
double nymR, exmR;//ny [ri][14] ex[][15] | |
double nyL, exL;//ny [ri][14] ex[][15] | |
double nymL, exmL;//ny [ri][14] ex[][15] | |
int[] xR = new int[10000]; | |
int[] yR = new int[10000]; | |
int[] xmR = new int[10000]; | |
int[] ymR = new int[10000]; | |
int[] xL = new int[10000]; | |
int[] yL = new int[10000]; | |
int[] xmL = new int[10000]; | |
int[] ymL = new int[10000]; | |
int tablestartrow = ig3(0, 0) + 2; | |
int tablelastNo = 0; | |
//=====================Turn番号 ROW番号 関連付け配列======================= | |
int it = 0; | |
while (dgv3.Rows[it + tablestartrow].Cells[1].Value != null) | |
{ | |
//tN0rN[]作成 | |
tN0rN[it] = ig3(it+ tablestartrow, 3);//turn番号itの開始行番号 | |
it++; | |
Debug.Print("turnNo=" + ti(it) + "tR[" + ti(it) + "].direction=" + tR[it].direction); | |
} | |
//------------------------------------------------------------------------- | |
tablelastNo = it - 1; | |
tn0 = 0; | |
tn1 = tablelastNo; | |
// rturnitow | |
int R0turn_row = ig3(0, 0) + 2;//dgv3のターンテーブル先頭行はデータ最終行+2行 | |
int R0turn_startrow = ig3(R0turn_row + tn0, 3); | |
int R0turn_endrow = ig3(R0turn_row + tn1, 5); | |
// int turnLastN = ig3(R0turn_endrow, 1); | |
// int[] row2turnN = new int[10000]; | |
int tc = 0;//turn count | |
for(it=0;it<R0turn_endrow;it++)//row番号からターン番号取得 | |
{ | |
row2turnN[it] = tc; | |
Debug.Print("row2turnN[" + ti(it) + "]=" + ti(row2turnN[it])); | |
int startrow = ig3(R0turn_row + tc, 3); | |
int endrow = ig3(R0turn_row + tc, 5); | |
if (it>=endrow) | |
{ | |
tc++; | |
} | |
} | |
hScrollBar2.Maximum = ig3(0, 0);//tablelastNo;//ターン番号を入力 xymx[0] - xymx[2];//X軸座標であるxRの最大値-xRの最小値 | |
hScrollBar2.Minimum = 0; | |
//int R0itow_start = ig3(R0turn_row + tn0, 7); | |
//int R0itow_end = ig3(R0turn_row + tn1, 9); | |
int size = ig3(0, 0);//dgv3のデータ範囲がターンの行数 | |
Debug.Print("XYPLOT3rl:R0turn_row=" + ti(R0turn_row) + "R0turn_startrow="+ti(R0turn_startrow)+ "R0turn_endrow="+ti(R0turn_endrow)); | |
//---------------dgv3から配列へcopy----------------------------------------------------------------------------------------------- | |
//tR[tn0].startNo = R0turn_startrow; | |
// tR[tn1].endNo = R0turn_endrow; | |
ny0 = dg3(R0turn_startrow, 14);//baseTurn[0]開始点y座標 | |
ex0 = dg3(R0turn_startrow, 15);//baseTurn[0]開始点x座標 | |
ny0m = dg3(R0turn_startrow, 21);//RoverTurn[0]開始点y座標 | |
ex0m = dg3(R0turn_startrow, 22);//RoverTurn[0]開始点x座標 | |
ir = 0; | |
for (ri = R0turn_startrow; ri < R0turn_endrow; ri++)//生座標をR0基準で整数化xbR[]ybR[]xmR[]ymR[] xbL[] ybL[] xmL[] ymL[]の8個代入 | |
{ | |
//Right(dgv1) Base基準座標------------------ | |
nyR = dg3(ri, 14); | |
yR[ir] = (int)(nyR - ny0); | |
exR = dg3(ri, 15); | |
xR[ir] = (int)(exR - ex0); | |
//Right(dgv1) RoverをBase基準座標-------- | |
nymR = dg3(ri, 21); | |
ymR[ir] = (int)(nymR - ny0); | |
exmR = dg3(ri, 22); | |
xmR[ir] = (int)(exmR - ex0); | |
//Left(dgv2) Base基準座標 dgv1行+iofする------------------ | |
nyL = dg3(ri , 14+50); | |
yL[ir] = (int)(nyL - ny0); | |
exL = dg3(ri , 15+50); | |
xL[ir] = (int)(exL - ex0); | |
//Left(dgv2) RoverをBase基準座標-------- | |
nymL = dg3(ri , 21+50); | |
ymL[ir] = (int)(nymL - ny0); | |
exmL = dg3(ri , 22+50); | |
xmL[ir] = (int)(exmL - ex0); | |
//Debug.Print("xyR:ri=[," + ti(ri) + ",]:ir=" + ti(ir) + "[ex,ny]=[," + td(exR) + "," + td(nyR) + ",]" + "[," + ti(ir) + ",][x,y]=[," + ti(xR[ir]) + "," + ti(yR[ir]) + ",]"); | |
// Debug.Print("xymR:ri=[," + ti(ri) + ",]:ir=" + ti(ir) + "[exm,nym]=[," + td(exmR) + "," + td(nymR) + "," + "[," + ti(ir) + ",][xm,ym]=[," + ti(xmR[ir]) + "," + ti(ymR[ir]) + ",]"); | |
// Debug.Print("xyL:ri=[," + ti(ri ) + ",]:ir=" + ti(ir) + "[exL,ny]=[," + td(exL) + "," + td(nyL) + ",]" + "[" + ti(ir) + ",][x,y]=[," + ti(xL[ir]) + "," + ti(yL[ir]) + ",]"); | |
//Debug.Print("xym:ri=[," + ti(ri ) + ",]:ir=" + ti(ir) + "[exm,nym]=[," + td(exmL) + "," + td(nymL) + ",]" + "[," + ti(ir) + ",][xm,ym]=[," + ti(xmL[ir]) + "," + ti(ymL[ir]) + ",]"); | |
ir++; | |
} | |
size = ir; | |
//FullAuto 全景を一括表示 (ZeroShift=>Rotation=>MaxMin=>AutoScale=>FullPLOT) | |
if (checkBox7.Checked==true || checkBox8.Checked==true)//FullAuto | |
{ | |
//rotate8dgv()でZeroShift=>Rotationした座標作成 | |
rotate8dgv(size, xR, yR, xmR, ymR, xL, yL, xmL,ymL); | |
//xyrangeRL()でMaxMinでScale計算でソースビットマップにおさまる座標作成 | |
double scale1 = 1; | |
int[] xymx = {0,0,0,0};//xmax ymax xmin ymin | |
xyrangeRL( ref scale1,xymx, size, xR, yR, xmR,ymR, xL, yL, xmL, ymL); | |
//ソースビットマップサイズ記入xymx[]={xmax,ymax,xmin,ymin} | |
int srcwidth = xymx[0] - xymx[2]; | |
int srcheight = xymx[1] - xymx[3]; | |
//Bitmap xybitmap = new Bitmap(srcwidth, srcheight);//計算結果のサイズで再定義 | |
textBox5.Text = ti(srcwidth); | |
textBox6.Text = ti(srcheight); | |
Debug.Print("XYplotrl3:rl.scale1=" + rl.scale1.ToString()); | |
//********************RL プロット座標生成*************************************************** | |
//scaleは、モードによって決められている Whole Viewモードは、可変スケール、スクロールモードはscale1=1固定 | |
//int yoff = 500; //ph;//Y軸反転 | |
// int xoff = 0;// (int)( pw / 2); | |
//モードによるスケール定義 | |
double xscale = 0; | |
double yscale = 0; | |
if (checkBox8.Checked == true)//スクロールモード | |
{ | |
//ソースビットマップへの書き込みスケールを0.5に固定、0.5dot/cm=1dot/2cm=2cm/dot | |
xscale = 0.5; | |
yscale = 0.5;// (double) pictureBox2.Height / srcheight; | |
label5.Text = "Smode:xscale=" + td(xscale) + "," + "yscale=" + td(yscale); | |
} | |
else | |
{ | |
xscale = 1; | |
yscale = 1;// pictureBox2.Height / srcheight; | |
label5.Text = "Wmode:xscale=" + td(xscale) + "," + "yscale=" + td(yscale); | |
} | |
//xymx[]={xmax,ymax,xmin,ymin} | |
// int yoff = 500; //ph;//Y軸反転 | |
//int xoff = 0;// (int)( pw / 2); | |
Debug.Print("xyrangeRL0():xscale=," + xscale.ToString("F3") + "yscale=" + yscale.ToString("F3")); | |
Debug.Print("xymx[xmax,ymax,xmin,ymin]=" + ti(xymx[0]) + "," + ti(xymx[1]) + "," + ti(xymx[2]) + "," + ti(xymx[3])); | |
for (i = 0; i < size; i++) | |
{ | |
//Right 座標変換 元配列置換 | |
xR[i] = Convert.ToInt32((double)(xR[i] - xymx[2]) * xscale); | |
yR[i] = Convert.ToInt32((double)(yR[i] - xymx[3]) * yscale); | |
xmR[i] = Convert.ToInt32((double)(xmR[i] - xymx[2]) * xscale); | |
ymR[i] = Convert.ToInt32((double)(ymR[i] - xymx[3]) * yscale); | |
//Left 座標変換 元配列置換 | |
xL[i] = Convert.ToInt32((double)(xL[i] - xymx[2]) * xscale); | |
yL[i] = Convert.ToInt32((double)(yL[i] - xymx[3]) * yscale); | |
xmL[i] = Convert.ToInt32((double)(xmL[i] - xymx[2]) * xscale); | |
ymL[i] = Convert.ToInt32((double)(ymL[i] - xymx[3]) * yscale); | |
// string xyrange1 = ",[" + ti(i) + "][xR,yR]=," + "[," + ti(xmR[i]) + "," + ti(ymR[i]) + "][xmR,ymR]=," + "[," + ti(xmR[i]) + "," + ti(ymR[i]); | |
Debug.Print("[" + ti(i) + "][xR,yR]=," + "[," + ti(xR[i]) + "," + ti(yR[i]) + ",]"); | |
Debug.Print("[" +ti(i) + "][xmR,ymR]=," + "[," + ti(xmR[i]) + "," + ti(ymR[i]) + ",]"); | |
Debug.Print("[" + ti(i) + "][xL,yL]=," + "[," + ti(xL[i]) + "," + ti(yL[i]) + ","); | |
Debug.Print("[" + ti(i) + "][xmL,ymL]=," + "[," + ti(xmL[i]) + "," + ti(ymL[i]) + ",]"); | |
} | |
//====================================================================================================================== | |
//=====================グラフィクス===================================================================================== | |
//======================================================================================================================= | |
//xyrangeRL:rl.xmax=249,rl.xmin=-4451rl.ymax=98356,rl.ymin=-84,x-width=4700,y-height= 98440,rl.scale1=1.0158472165786265 | |
dgv3.Visible = false; | |
pictureBox2.Visible = true; | |
Graphics g1 = Graphics.FromImage(xyBitmap); | |
Graphics g2 = Graphics.FromImage(xysBitmap); | |
g1.Clear(Color.Black); | |
g2.Clear(Color.Black); | |
//--------------------------------------------------------------------------- | |
//----------ソースビットマップ xyBitmapへプロット--------------------------- | |
//---------------------------------------------------------------------------- | |
for (i = 1; i < ir; i++) | |
{ | |
//プロット位置調整 | |
int xad = 0;// pictureBox2.Width / 2; | |
int yad = 0; | |
int h =xyBitmap.Height;//rl.ph; | |
//px,py座標 | |
int pxR = xR[i] + xad; | |
int pxmR = xmR[i] + xad; | |
int pxL = xL[i] + xad; | |
int pxmL = xmL[i] + xad; | |
int pyR =h- yR[i] + yad; | |
int pymR =h- ymR[i] + yad; | |
int pyL =h- yL[i] + yad; | |
int pymL =h- ymL[i] + yad; | |
// | |
int pxR_1 = xR[i - 1] + xad; | |
int pxmR_1 = xmR[i - 1] + xad; | |
int pxL_1 = xL[i - 1] + xad; | |
int pxmL_1 = xmL[i - 1] + xad; | |
int pyR_1 =h- yR[i - 1] + yad; | |
int pymR_1 =h- ymR[i - 1] + yad; | |
int pyL_1 = h-yL[i - 1] + yad; | |
int pymL_1 = h- ymL[i - 1] + yad; | |
if (row2turnN[i]%2==0)//Right PLOT | |
{ | |
g1.DrawLine(Pens.Green, pxR, pyR, pxR_1, pyR_1); | |
g1.DrawLine(Pens.Green, pxmR, pymR, pxmR_1, pymR_1); | |
Pen p0 = new Pen(Color.Red, 3); | |
g1.DrawLine(p0, pxR, pyR, pxmR, pymR); | |
Pen p1 = new Pen(Color.Yellow, 1); | |
g1.DrawLine(p1, pxL, pyL, pxmL, pymL); | |
} | |
else //LEFT PLOT | |
{ | |
g1.DrawLine(Pens.Blue, pxL, pyL, pxL_1, pyL_1); | |
g1.DrawLine(Pens.Blue, pxmL, pymL, pxmL_1, pymL_1); | |
Pen p1 = new Pen(Color.Yellow, 3); | |
g1.DrawLine(p1, pxL, pyL, pxmL, pymL); | |
Pen p0 = new Pen(Color.Red, 1); | |
g1.DrawLine(p0, pxR, pyR, pxmR, pymR); | |
} | |
/* | |
if (i % 3 == 0) | |
{ | |
Pen p0 = new Pen(Color.Red, 2); | |
g1.DrawLine(p0, pxR, pyR, pxmR, pymR); | |
g1.DrawLine(p0, pxL, pyL, pxmL, pymL); | |
//R-LLIne | |
g1.DrawLine(Pens.Red, pxR, pyR, pxL, pyL); | |
g1.DrawLine(Pens.Red, pxmR, pymR, pxmL, pymL); | |
} | |
if (i % 3 ==1) | |
{ | |
g1.DrawLine(Pens.Yellow, pxR, pyR, pxmR, pymR); | |
g1.DrawLine(Pens.Yellow, pxL, pyL, pxmL, pymL); | |
//R-LLIne | |
g1.DrawLine(Pens.Yellow, pxR, pyR, pxL, pyL); | |
g1.DrawLine(Pens.Yellow, pxmR, pymR, pxmL, pymL); | |
} | |
if (i % 3 == 2) | |
{ | |
g1.DrawLine(Pens.Pink, pxR, pyR, pxmR, pymR); | |
g1.DrawLine(Pens.Pink, pxL, pyL, pxmL, pymL); | |
//R-LLIne | |
g1.DrawLine(Pens.Pink, pxR, pyR, pxL, pyL); | |
g1.DrawLine(Pens.Pink, pxmR, pymR, pxmL, pymL); | |
} | |
*/ | |
pxRg[i] = pxR; | |
pyRg[i] = pyR; | |
pxmRg[i] = pxmR; | |
pymRg[i] = pymR; | |
pxLg[i] = pxL; | |
pyLg[i] = pyL; | |
pxmLg[i] = pxmL; | |
pymLg[i] = pymL; | |
Debug.Print("xyRDRAW:ri[," + ti(i) + ",]:" + "[xR,yR]=[," + ti(xR[i]) + "," + ti(yR[i]) + ",]" + "xad=," + ti(xad) + ",yad=," + ti(yad)); | |
Debug.Print("xymRDRAW:ri[," + ti(i) + ",]:" + "[xmR,ymR]=[," + ti(xmR[i]) + "," + ti(ymR[i]) + ",]"); | |
Debug.Print("xyLDRAW:ri[," + ti(i) + ",]:" + "[xL,yL]=[," + ti(xL[i]) + "," + ti(yL[i]) + ",]"); | |
Debug.Print("xymLDRAW:ri[," + ti(i) + ",]:" + "[xmL,ymL]=[," + ti(xmL[i]) + "," + ti(ymL[i]) + ",]"); | |
} | |
//センタリング用ン配列作成 | |
//------------------------------------------------------------------------------------------------------------------- | |
//-----------------------ソースビットマップ 書き込み終了------------------------------------------------------------ | |
//------------------------------------------------------------------------------------------------------------------- | |
//hScrollBar2にスクロールモード用数値入力 | |
//xymx[]={xmax ymax xmin ymin} | |
//モード選択 | |
if (checkBox8.Checked == true)//スクロールモード | |
{ | |
//smodeCut(2);//Turn番号入力 | |
smodeCutrn(60); | |
} | |
else//Whole View モード | |
{ | |
//----------------------xycut------------------------------------ | |
//xyBitmap(1000x100000)から切り取る | |
double magni = rl.scale1; | |
int lefttopx = 0; | |
int lefttopy = 0; | |
int magniw = 1000;// Convert.ToInt32((double)pic3w / magni); | |
int magnih = 400;// Convert.ToInt32((double)pic3h / magni); | |
Rectangle srcxyRect = new Rectangle(lefttopx, lefttopy, magniw, magnih);//xyBitmapを縮小 | |
Rectangle desxyRect = new Rectangle(0, 0, magniw, magnih); | |
g2.DrawImage(xyBitmap, desxyRect, srcxyRect, GraphicsUnit.Pixel); | |
pictureBox2.Image = xysBitmap; | |
} | |
} | |
} | |
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
//-----------------------1ソースビットマップ切取関数 smodeCut(centerターン番号) | |
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ | |
private void smodeCut(int tn)//ターン番号tn | |
{ | |
pictureBox2.Visible = true; | |
int srcheight = Convert.ToInt32(textBox6.Text); | |
// int cx = hScrollBar2.Value; | |
Graphics g2 = Graphics.FromImage(xysBitmap); | |
textBox7.Text = ti(tn); | |
int pW = pictureBox2.Width;// 1000; | |
int pH = pictureBox2.Height; | |
//tnターン番号を行番号に変更 | |
int rowN = tN0rN[tn]; | |
//指定ターン番号tnをセンター座標へもってくる | |
int centerX = pxRg[rowN]; | |
int centerY = pyRg[rowN]; | |
int startX = 0; | |
int startY = 0; | |
//センター座標centerXからRectangleの左開始X座標求める | |
if (centerX > pictureBox2.Width / 2) | |
{ | |
startX = centerX - pictureBox2.Width / 2; | |
} | |
else | |
{ | |
startX = 0; | |
} | |
//センター座標centerYからRectanleの左開始Y座標を求める | |
// if (centerY <(int)( pictureBox2.Height / 2)) | |
// { | |
// startY = pictureBox2.Height / 2; | |
//} | |
// else | |
// { | |
startY = centerY-250; | |
//} | |
richTextBox1.Visible = true; | |
string smodeMon = "[smdeCut]TurnNo="+ti(tn)+":pxRg[" +ti(rowN)+"]="+ ti(pxRg[rowN]) + "pyRg[" + ti(rowN) + "]=" + ti(pyRg[rowN])+"startX="+ti(startX)+"startY="+ti(startY)+"\r\n"; | |
richTextBox1.AppendText(smodeMon); | |
Debug.Print(smodeMon); | |
Rectangle srcRect = new Rectangle(startX,startY, pW, pH); | |
Rectangle desRect = new Rectangle(0, 0, pW, pH); | |
g2.DrawImage(xyBitmap, desRect, srcRect, GraphicsUnit.Pixel);//xyBitmapからsrcRect切り取ってxysBitmapへdesRect分コピー | |
//Center Cursor | |
g2.DrawLine(Pens.White, pW / 2, 0, pW / 2, pictureBox2.Height); | |
g2.DrawLine(Pens.White, 0, pictureBox2.Height / 2, pW, pictureBox2.Height / 2); | |
pictureBox2.Image = xysBitmap; | |
} | |
private void smodeCutrn(int rn) | |
{ | |
pictureBox2.Visible = true; | |
int srcheight = Convert.ToInt32(textBox6.Text); | |
// int cx = hScrollBar2.Value; | |
Graphics g2 = Graphics.FromImage(xysBitmap); | |
textBox7.Text = ti(rn); | |
int pW = pictureBox2.Width;// 1000; | |
int pH = pictureBox2.Height; | |
//tnターン番号を行番号に変更 | |
int tn = row2turnN[rn]; | |
int rowN = rn; | |
//指定ターン番号tnをセンター座標へもってくる | |
int centerX = pxRg[rowN]; | |
int centerY = pyRg[rowN]; | |
int startX = 0; | |
int startY = 0; | |
//左端座標を求める | |
//センター座標centerXからRectangleの左開始X座標求める | |
if (centerX > pictureBox2.Width / 2) | |
{ | |
startX = centerX - pictureBox2.Width / 2; | |
} | |
else | |
{ | |
startX = centerX - pictureBox2.Width / 2; | |
} | |
//センター座標centerYからRectanleの左開始Y座標を求める | |
// if (centerY <(int)( pictureBox2.Height / 2)) | |
// { | |
// startY = pictureBox2.Height / 2; | |
//} | |
// else | |
// { | |
startY = centerY - 200; | |
//} | |
richTextBox1.Visible = true; | |
string smodeMon = "[smdeCut]TurnNo=" + ti(tn) + ":pxRg[" + ti(rowN) + "]=" + ti(pxRg[rowN]) + "pyRg[" + ti(rowN) + "]=" + ti(pyRg[rowN]) + "startX=" + ti(startX) + "startY=" + ti(startY) + "\r\n"; | |
richTextBox1.AppendText(smodeMon); | |
Debug.Print(smodeMon); | |
Rectangle srcRect = new Rectangle(startX, startY, pW, pH); | |
Rectangle desRect = new Rectangle(0, 0, pW, pH); | |
g2.DrawImage(xyBitmap, desRect, srcRect, GraphicsUnit.Pixel);//xyBitmapからsrcRect切り取ってxysBitmapへdesRect分コピー | |
//Center Cursor | |
g2.DrawLine(Pens.White, pW / 2, 0, pW / 2, pictureBox2.Height); | |
g2.DrawLine(Pens.White, 0, pictureBox2.Height / 2, pW, pictureBox2.Height / 2); | |
g3drawstrings(rn); | |
pictureBox2.Image = xysBitmap; | |
} | |
private void g3drawstrings(int rncx) | |
{ | |
Graphics g2 = Graphics.FromImage(xysBitmap); | |
double hmot = Convert.ToDouble(dgv3.Rows[rncx].Cells[18].Value); | |
double mhead = Convert.ToDouble(dgv3.Rows[rncx].Cells[25].Value); | |
double slip = hmot - mhead; | |
double gspd = Convert.ToDouble(dgv3.Rows[rncx].Cells[17].Value) / 1000000 * 3600;//mm/sec=>kmh | |
double pit = Convert.ToDouble(dgv3.Rows[rncx].Cells[30].Value); | |
//double ywhosei = Convert.ToDouble(dgv3.Rows[rncx].Cells[39].Value); | |
//double Drel = Convert.ToDouble(dgv3.Rows[rncx].Cells[41].Value); | |
double dzb = Convert.ToDouble(dgv3.Rows[rncx].Cells[16].Value); | |
double lens = Convert.ToDouble(dgv3.Rows[rncx].Cells[27].Value); | |
int flagsB=0 ; | |
int flagsR=0; | |
if (dgv3.Rows[rncx].Cells[1].Value != null) | |
{ | |
//flagsB = Convert.ToInt32(dgv3.Rows[rncx].Cells[1].Value); | |
//flagsR = Convert.ToInt32(dgv3.Rows[rncx].Cells[2].Value); | |
} | |
double roll = Convert.ToDouble(dgv3.Rows[rncx].Cells[31].Value); | |
//int haccb = Convert.ToInt32(dgv3.Rows[rncx].Cells[5].Value); | |
double yaw0 = Convert.ToDouble(dgv3.Rows[rncx].Cells[29].Value); | |
double pitch0 = Convert.ToDouble(dgv3.Rows[rncx].Cells[30].Value); | |
double roll0 = Convert.ToDouble(dgv3.Rows[rncx].Cells[31].Value); | |
double grx0 = Convert.ToDouble(dgv3.Rows[rncx].Cells[35].Value); | |
double gry0 = Convert.ToDouble(dgv3.Rows[rncx].Cells[36].Value); | |
// double grz0 = Convert.ToDouble(dgv3.Rows[rncx].Cells[37].Value); | |
// | |
// Debug.Print("rncx=" + rncx.ToString() + "hmot=" + hmot.ToString()); | |
//文字列を表示する範囲を指定する | |
// 0:headMot,1:mHead,2:gSpeed,3:pitch,4:yaw1,5:DZB,6:relLm | |
int haba = 110; | |
RectangleF rectall = new RectangleF(0, 0, 1000, 20);//行全体 | |
RectangleF rect = new RectangleF(500, 470, 500, 20);//行番号をセンターライン下に表示 | |
RectangleF rect0 = new RectangleF(0, 0, haba, 20);//hMot位置 | |
RectangleF rect1 = new RectangleF(haba, 0, haba, 20);//slip位置 | |
RectangleF rect2 = new RectangleF(haba * 2, 0, haba, 20);//gspd位置 | |
RectangleF rect3 = new RectangleF(haba * 3, 0, haba, 20);//pit位置 | |
RectangleF rect4 = new RectangleF(haba * 4, 0, haba, 20);//mhead位置 | |
RectangleF rect5 = new RectangleF(haba * 5, 0, haba, 20);//ywhosei位置 | |
RectangleF rect6 = new RectangleF(haba * 6, 0, haba, 20);//yaw1位置 | |
RectangleF rect7 = new RectangleF(haba * 7, 0, haba, 20);//dzb位置 | |
RectangleF rect8 = new RectangleF(haba * 8, 0, haba, 20);//length位置 | |
RectangleF rect9 = new RectangleF(0, 470, haba * 4, 20);//flagsB,flagsR,,hacc 左下 | |
RectangleF rect10 = new RectangleF(0, 450, haba * 7, 20);//BNO yaw,pitch,roll,grX,grY,grZ 左下の1行上 | |
g2.FillRectangle(Brushes.Black, rect);//rectの四角を描く | |
g2.FillRectangle(Brushes.Black, rectall);//rectの四角を描く | |
//フォントオブジェクトの作成 | |
Font fnt = new Font("MS UI Gothic", 16); | |
// string | |
//itowから経過時間表示をrncxの後に追加 | |
//double timeitow = (Convert.ToDouble(dgv3.Rows[rncx].Cells[13].Value) - Convert.ToDouble(dgv3.Rows[2].Cells[13].Value)) / 1000; | |
//string timeitows = timeitow.ToString("F2"); | |
//string itowbstr = (dgv3.Rows[rncx].Cells[13].Value).ToString(); | |
//----------------------------------- | |
//string strrowN = timeitows + "sec" + "[" + rncx.ToString() + "]" + "TurnNo=" + dc.tnrow[rncx].ToString() ;// + tR[dc.tnrow[rncx]].direction + "@itowB=" + itowbstr; | |
string strhmot = "hmot=" + hmot.ToString("F0"); | |
string strmhead = "mhed=" + mhead.ToString("F0"); | |
string strslip = "slip=" + slip.ToString("F0"); | |
string strgspd = "gspd=" + gspd.ToString("F1"); | |
string strpit = "pit1=" + pit.ToString("F0"); | |
//string strywH = "yawH=" + ywhosei.ToString("F0"); | |
//string strdrel = "Drel=" + Drel.ToString("F1"); | |
string strdzb = "dzb =" + dzb.ToString("F0"); | |
string strlens = "lens =" + lens.ToString("F0"); | |
////string strflags = "flagsB=" + flagsB.ToString() + "flagsR=" + flagsR.ToString() + "roll=" + roll.ToString() + "hacc=" + haccb.ToString(); | |
//string strbno = "BNO:yaw=" + yaw0.ToString("F1") + "pitch=" + pitch0.ToString("F1") + "roll=" + roll0.ToString("F1") + "grX=" + grx0.ToString("F2") + "grY=" + gry0.ToString("F2") + "grZ=" + grz0.ToString("F2"); | |
string strroll = "roll=" + roll.ToString("F0"); | |
//描画 public Color[] samples = new Color[] { Color.Red, Color.Blue, Color.Green, Color.Yellow, Color.Aqua, Color.DeepPink, Color.BlanchedAlmond }; | |
//g2.DrawString(strrowN, fnt, Brushes.White, rect);//rowN描画 | |
g2.DrawString(strhmot, fnt, Brushes.Red, rect0);//hMot描画 | |
g2.DrawString(strslip, fnt, Brushes.Orange, rect1); | |
g2.DrawString(strgspd, fnt, Brushes.Green, rect2); | |
g2.DrawString(strpit, fnt, Brushes.Yellow, rect3); | |
g2.DrawString(strmhead, fnt, Brushes.Blue, rect4); | |
//g2.DrawString(strywH, fnt, Brushes.Aqua, rect5); | |
// g2.DrawString(strdrel, fnt, Brushes.BlanchedAlmond, rect6); | |
g2.DrawString(strroll, fnt, Brushes.DeepPink, rect7); | |
g2.DrawString(strlens, fnt, Brushes.White, rect8); | |
// g2.DrawString(strflags, fnt, Brushes.White, rect9); | |
//g2.DrawString(strbno, fnt, Brushes.White, rect10); | |
} | |
//プロット前にグラフサイズを確認調整する。 | |
private void button3_Click(object sender, EventArgs e) | |
{ | |
}//button3 end | |
//xyrangeは、データの範囲を求める | |
void xyrange(int size, int[] x, int[] y, int[] xm, int[] ym, ref int xoff, ref int yoff, ref int width, ref int height, int[] px, int[] py, int[] pxm, int[] pym) | |
{ | |
int i; | |
int xmax = -10000; | |
int xmin = 10000; | |
int ymax = -10000; | |
int ymin = 10000; | |
int pw = 1000;// pictureBox2.Width; | |
int ph = 500;// pictureBox2.Height; | |
double scale1 = 1; | |
for (i = 0; i < size; i++) | |
{ | |
if (x[i] > xmax) { xmax = x[i]; } | |
if (x[i] < xmin) { xmin = x[i]; } | |
if (y[i] < ymin) { ymin = y[i]; } | |
if (y[i] > ymax) { ymax = y[i]; } | |
} | |
//=>最小値xmin決定 | |
Debug.Print("************xyrangeR:xmax=" + ti(xmax) + "xmin=" + ti(xmin) + "ymax=" + ti(ymax) + "ymin=" + ti(ymin)); | |
width = xmax - xmin; | |
height = ymax - ymin; | |
if (width > pw || height > ph) | |
{ | |
if (width > height) | |
{ | |
scale1 = (double)pw / width; | |
} | |
else if (width < height) | |
{ | |
scale1 = (double)ph / height; | |
} | |
else | |
{ | |
scale1 = 1; | |
} | |
} | |
for (i = 0; i < size; i++) | |
{ | |
px[i] = Convert.ToInt32((x[i] - xmin) * scale1); | |
py[i] = ph - Convert.ToInt32((y[i] - ymin) * scale1); | |
pxm[i] = Convert.ToInt32((xm[i] - xmin) * scale1); | |
pym[i] = ph - Convert.ToInt32((ym[i] - ymin) * scale1); | |
Debug.Print("L:scale=" + scale1.ToString("F3") + "[" + ti(i) + "]" + "[px,py]=" + "[" + ti(px[i]) + "," + ti(py[i]) + "]"); | |
Debug.Print("L:scale=" + scale1.ToString("F3") + "[" + ti(i) + "]" + "[pxm,pym]=" + "[" + ti(pxm[i]) + "," + ti(pym[i]) + "]"); | |
} | |
} | |
void xyrangeRL(ref double scale1,int[] marry, int size,int[] xR, int[]yR, int[]xmR, int[]ymR, int[]xL, int[]yL,int[] xmL, int[] ymL)//左右データをプロット座標に変換して構造体に収納してXYPLOTに渡す | |
{ | |
int i; | |
rl.xminR = 10000; | |
rl.ymaxR = -10000; | |
rl.yminR = 10000; | |
//最大値検索:Base:xmaxR,xmaxL Rover:xmaxRm,xmaxLm | |
//最小値検索:Base:xminR,xminL Rover:xminRm,xminLm | |
for (i = 0; i < size; i++) | |
{ | |
//-Right Base------------------------------------------ | |
if (xR[i] > rl.xmaxR) { rl.xmaxR = xR[i]; } | |
if (yR[i] > rl.ymaxR) { rl.ymaxR = yR[i]; } | |
if (xR[i] < rl.xminR) { rl.xminR = xR[i]; } | |
if (yR[i] < rl.yminR) { rl.yminR = yR[i]; } | |
//Right Rover---------------------------------------- | |
if (xmR[i] > rl.xmaxRm) { rl.xmaxRm = xmR[i]; } | |
if (ymR[i] > rl.ymaxRm) { rl.ymaxRm = ymR[i]; } | |
if (xmR[i] < rl.xminRm) { rl.xminRm = xmR[i]; } | |
if (ymR[i] < rl.yminRm) { rl.yminRm = ymR[i]; } | |
//Left Base------------------------------------------- | |
if (xL[i] > rl.xmaxL) { rl.xmaxL = xL[i]; } | |
if (yL[i] > rl.ymaxL) { rl.ymaxL = yL[i]; } | |
if (xL[i] < rl.xminL) { rl.xminL = xL[i]; } | |
if (yL[i] < rl.yminL) { rl.yminL = yL[i]; } | |
//Left Rover---------------------------------------- | |
if (xmL[i] > rl.xmaxLm) { rl.xmaxLm = xmL[i]; } | |
if (ymL[i] > rl.ymaxLm) { rl.ymaxLm = ymL[i]; } | |
if (xmL[i] < rl.xminLm) { rl.xminLm = xmL[i]; } | |
if (ymL[i] < rl.yminLm) { rl.yminLm = ymL[i]; } | |
} | |
Debug.Print("MaxMin:xmaxR=," + ti(rl.xmaxR) + ",rl.xmaxRm=," + ti(rl.xmaxRm)); | |
richTextBox1.AppendText("MaxMin:xmaxR=," + ti(rl.xmaxR) + ",rl.ymaxR=," + ti(rl.ymaxR)+"\r\n"); | |
Debug.Print("MaxMin:xmaxL=," + ti(rl.xmaxL) + ",rl.xmaxRLm=," + ti(rl.xmaxLm)); | |
richTextBox1.AppendText("MaxMin:xminR=, " + ti(rl.xminR) + ", yminR=, " + ti(rl.yminR)+"\r\n"); | |
//4ポイントのMaxMinから比較 | |
//x最大値決定:Base:xmaxR,xmaxL Rover:xmaxRm,xmaxLm | |
rl.xmax = rl.xmaxR;//仮にxmaxRから開始 | |
if (rl.xmax<= rl.xmaxL)//Base x座標最大値 | |
{ | |
rl.xmax = rl.xmaxL; | |
} | |
if (rl.xmax <= rl.xmaxRm)//Base x座標最大値 | |
{ | |
rl.xmax = rl.xmaxRm; | |
} | |
if(rl.xmax<=rl.xmaxLm)//Base x座標最大値 | |
{ | |
rl.xmax = rl.ymaxLm; | |
} | |
//=>最大値xmax決定 | |
//x最小値決定:Base:xminR,xminL Rover:xminRm,xminLm | |
rl.xmin = rl.xminR;//仮にxmaxRから開始 | |
if (rl.xmin >= rl.xminL)//Base x座標最大値 | |
{ | |
rl.xmin = rl.xminL; | |
} | |
if (rl.xmin >= rl.xminRm)//Base x座標最大値 | |
{ | |
rl.xmin = rl.xminRm; | |
} | |
if (rl.xmin >= rl.xminLm)//Base x座標最大値 | |
{ | |
rl.xmin = rl.xminLm; | |
} | |
//=>x最小値xmin決定 | |
//y軸最大値決定:Base:xmaxR,xmaxL Rover:xmaxRm,xmaxLm | |
rl.ymax = rl.ymaxR;//仮にxmaxRから開始 | |
if (rl.ymax <= rl.ymaxL)//Base x座標最大値 | |
{ | |
rl.ymax = rl.ymaxL; | |
} | |
if (rl.ymax <= rl.ymaxRm)//Base x座標最大値 | |
{ | |
rl.ymax = rl.ymaxRm; | |
} | |
if (rl.ymax <= rl.ymaxLm)//Base x座標最大値 | |
{ | |
rl.ymax = rl.ymaxLm; | |
} | |
//y=>最大値xmax決定 | |
//y最小値決定:Base:xminR,xminL Rover:xminRm,xminLm | |
rl.ymin = rl.yminR;//仮にxmaxRから開始 | |
if (rl.ymin >= rl.yminL)//Base x座標最大値 | |
{ | |
rl.ymin = rl.yminL; | |
} | |
if (rl.ymin >= rl.yminRm)//Base x座標最大値 | |
{ | |
rl.ymin = rl.yminRm; | |
} | |
if (rl.ymin >= rl.yminLm)//Base x座標最大値 | |
{ | |
rl.ymin = rl.yminLm; | |
} | |
//=>最小値xmin決定 | |
marry[0] = rl.xmax; | |
marry[1] = rl.ymax; | |
marry[2] = rl.xmin; | |
marry[3] = rl.ymin; | |
int width = rl.xmax - rl.xmin; | |
int height = rl.ymax - rl.ymin; | |
rl.width = width; | |
rl.height = height; | |
//---------------全体を俯瞰したい場合Whole View=AUTO SCALE ---------------------------------------- | |
// | |
if (checkBox8.Checked == false)//AUTO SCALE 計算 全体がxysBitmapに収まるScaleを計算 | |
{ | |
int pw = xysBitmap.Width;// pictureBox2.Width; | |
int ph = xysBitmap.Height;// pictureBox2.Height; | |
if (width > pw || height > ph) | |
{ | |
if (width > height) | |
{ | |
rl.scale1 = (double)pw / width; | |
} | |
else if (width < height) | |
{ | |
rl.scale1 = (double)ph / height; | |
} | |
else | |
{ | |
rl.scale1 = 1; | |
} | |
} | |
} | |
else//AUTO SCALEでなければ、Scale=1 | |
{ | |
rl.scale1 = 1; | |
}// Auto scale END | |
Debug.Print("************xyrangeRL:rl.xmax=," + ti(rl.xmax) + ",rl.xmin=," + ti(rl.xmin) + ",rl.ymax=," + ti(rl.ymax) +",rl.ymin=," + ti(rl.ymin) + ",x-width=," + ti(rl.xmax - rl.xmin) +",y-height=, "+ti(rl.ymax-rl.ymin)+",rl.scale1=," + td(rl.scale1)); ; | |
scale1 = rl.scale1; | |
string xyrange0 = "xyrangeRL():width=" + ti(width) + "cm,height=" + ti(height) + ",cm,scale=," + rl.scale1.ToString("F3"); | |
richTextBox1.AppendText(xyrange0+"\r\n"); | |
Debug.Print(xyrange0); | |
}//==============================xyrangeRL END ==============================================//===============================座標回転VB===================================================== | |
//hscrollBar2でxyBitmapを切り取る | |
private void hScrollBar2_Scroll(object sender, ScrollEventArgs e) | |
{ | |
int cx = hScrollBar2.Value; | |
textBox7.Text = ti(cx); | |
smodeCutrn(cx); | |
} | |
private void maxmin8(XYRLplot rl, int size, int[] xR, int[] yR, int[] xmR, int[] ymR, int[] xL, int[] yL, int[] xmL, int[] ymL) | |
{ | |
int i; | |
rl.xminR = 10000; | |
rl.ymaxR = -10000; | |
rl.yminR = 10000; | |
//最大値検索:Base:xmaxR,xmaxL Rover:xmaxRm,xmaxLm | |
//最小値検索:Base:xminR,xminL Rover:xminRm,xminLm | |
for (i = 0; i < size; i++) | |
{ | |
//-Right Base------------------------------------------ | |
if (xR[i] > rl.xmaxR) { rl.xmaxR = xR[i]; } | |
if (yR[i] > rl.ymaxR) { rl.ymaxR = yR[i]; } | |
if (xR[i] < rl.xminR) { rl.xminR = xR[i]; } | |
if (yR[i] < rl.yminR) { rl.yminR = yR[i]; } | |
//Right Rover---------------------------------------- | |
if (xmR[i] > rl.xmaxRm) { rl.xmaxRm = xmR[i]; } | |
if (ymR[i] > rl.ymaxRm) { rl.ymaxRm = ymR[i]; } | |
if (xmR[i] < rl.xminRm) { rl.xminRm = xmR[i]; } | |
if (ymR[i] < rl.yminRm) { rl.yminRm = ymR[i]; } | |
//Left Base------------------------------------------- | |
if (xL[i] > rl.xmaxL) { rl.xmaxL = xL[i]; } | |
if (yL[i] > rl.ymaxL) { rl.ymaxL = yL[i]; } | |
if (xL[i] < rl.xminL) { rl.xminL = xL[i]; } | |
if (yL[i] < rl.yminL) { rl.yminL = yL[i]; } | |
//Left Rover---------------------------------------- | |
if (xmL[i] > rl.xmaxLm) { rl.xmaxLm = xmL[i]; } | |
if (ymL[i] > rl.ymaxLm) { rl.ymaxLm = ymL[i]; } | |
if (xmL[i] < rl.xminLm) { rl.xminLm = xmL[i]; } | |
if (ymL[i] < rl.yminLm) { rl.yminLm = ymL[i]; } | |
} | |
Debug.Print("MaxMin:xmaxR=" + ti(rl.xmaxR) + "rl.xmaxRm=" + ti(rl.xmaxRm)); | |
Debug.Print("MaxMin:xmaxL=" + ti(rl.xmaxL) + "rl.xmaxRLm=" + ti(rl.xmaxLm)); | |
//4ポイントのMaxMinから比較 | |
//x最大値決定:Base:xmaxR,xmaxL Rover:xmaxRm,xmaxLm | |
rl.xmax = rl.xmaxR;//仮にxmaxRから開始 | |
if (rl.xmax <= rl.xmaxL)//Base x座標最大値 | |
{ | |
rl.xmax = rl.xmaxL; | |
} | |
if (rl.xmax <= rl.xmaxRm)//Base x座標最大値 | |
{ | |
rl.xmax = rl.xmaxRm; | |
} | |
if (rl.xmax <= rl.xmaxLm)//Base x座標最大値 | |
{ | |
rl.xmax = rl.ymaxLm; | |
} | |
//=>最大値xmax決定 | |
//x最小値決定:Base:xminR,xminL Rover:xminRm,xminLm | |
rl.xmin = rl.xminR;//仮にxmaxRから開始 | |
if (rl.xmin >= rl.xminL)//Base x座標最大値 | |
{ | |
rl.xmin = rl.xminL; | |
} | |
if (rl.xmin >= rl.xminRm)//Base x座標最大値 | |
{ | |
rl.xmin = rl.xminRm; | |
} | |
if (rl.xmin >= rl.xminLm)//Base x座標最大値 | |
{ | |
rl.xmin = rl.xminLm; | |
} | |
//=>x最小値xmin決定 | |
//y軸最大値決定:Base:xmaxR,xmaxL Rover:xmaxRm,xmaxLm | |
rl.ymax = rl.ymaxR;//仮にxmaxRから開始 | |
if (rl.ymax <= rl.ymaxL)//Base x座標最大値 | |
{ | |
rl.ymax = rl.ymaxL; | |
} | |
if (rl.ymax <= rl.ymaxRm)//Base x座標最大値 | |
{ | |
rl.ymax = rl.ymaxRm; | |
} | |
if (rl.ymax <= rl.ymaxLm)//Base x座標最大値 | |
{ | |
rl.ymax = rl.ymaxLm; | |
} | |
//y=>最大値xmax決定 | |
//y最小値決定:Base:xminR,xminL Rover:xminRm,xminLm | |
rl.ymin = rl.yminR;//仮にxmaxRから開始 | |
if (rl.ymin >= rl.yminL)//Base x座標最大値 | |
{ | |
rl.ymin = rl.yminL; | |
} | |
if (rl.ymin >= rl.yminRm)//Base x座標最大値 | |
{ | |
rl.ymin = rl.yminRm; | |
} | |
if (rl.ymin >= rl.yminLm)//Base x座標最大値 | |
{ | |
rl.ymin = rl.yminLm; | |
} | |
//=>最小値xmin決定 | |
int width = rl.xmax - rl.xmin; | |
int height = rl.ymax - rl.ymin; | |
int pw = pictureBox2.Width; | |
int ph = pictureBox2.Height; | |
if (width > pw || height > ph) | |
{ | |
if (width > height) | |
{ | |
rl.scale1 = (double)pw / width; | |
} | |
else if (width < height) | |
{ | |
rl.scale1 = (double)ph / height; | |
} | |
else | |
{ | |
rl.scale1 = 1; | |
} | |
} | |
Debug.Print("************xyrangeRL:rl.xmax=" + ti(rl.xmax) + "rl.xmin=" + ti(rl.xmin) + "rl.ymax=" + ti(rl.ymax) + "rl.ymin=" + ti(rl.ymin) + "rl.scale1=" + td(rl.scale1)); ; | |
} | |
//*++++++++++++++++++++++++++++++++++++++座標回転++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
/* | |
'strNとendrNの傾斜角------------------------------------------------- | |
'Dim thetaL As Double | |
thetaRb = Math.Atan((endPY - startPY) / (endPX - startPX)) | |
'thetaL = Math.Atan((endPY2 - startPY2) / (endPX2 - startPX2)) | |
If CheckBox25.Checked = False Then | |
thetaRb = 0 | |
'thetaL = 0 | |
End If | |
TextBox15.Text = CStr(thetaRb * 180 / Math.PI) | |
Dim Sinth As Double = Math.Sin(-thetaRb) | |
Dim Costh As Double = Math.Cos(-thetaRb) | |
'--Right Ski--------------- | |
Rpxb(iu) = CInt((RNlon * CosthR - RNlat * SinthR)) ' theata だけ座標回転 | |
Rpyb(iu) = CInt((RNlon * SinthR + RNlat * CosthR)) ' theata だけ座標回転 | |
Rpxr(iu) = CInt(((RNlon + RNrelE) * CosthR - (RNlat + RNrelN) * SinthR)) ' theata だけ座標回転 | |
Rpyr(iu) = CInt(((RNlon + RNrelE) * SinthR + (RNlat + RNrelN) * CosthR)) ' theata だけ座標回転 | |
*/ | |
private void rotate8(XYRLplot rl,int size,int[] xR,int[] yR, int[]xmR,int[] ymR,int[] xL, int[]yL,int[] xmL, int[]ymL) | |
{ | |
//始点ー終点角度 | |
double xstart = (double)xR[0]; | |
double xend = (double)xR[size - 1]; | |
double ystart = (double)yR[0]; | |
double yend = (double)yR[size- 1]; | |
double ang=Math.PI+Math.Atan2((xstart- xend),(ystart- yend) );//グラフY座標が180度逆なので、補正 | |
rl.rotangle = Math.PI/2-ang;// ang+Math.PI; | |
//距離計算 | |
rl.distance = Math.Sqrt((xend - xstart) * (xend - xstart) + (yend - ystart) * (yend - ystart)); | |
Debug.Print("xR[0],yR[0]=" + ti(xR[0]) + "," + ti(yR[0]) + "xR[size-1],yR[size-1]=" + ti(xR[size - 1]) + "," + ti(yR[size-1])); | |
Debug.Print("ang="+td(ang * 180 / Math.PI) +"rl.rotangle deg=" + td(rl.rotangle*180/Math.PI) + "rl.distance=" + td(rl.distance)); | |
//回転座標変換 | |
double SinthR = Math.Sin(rl.rotangle); | |
double CosthR = Math.Cos(rl.rotangle); | |
int i; | |
for(i=0;i<size; i++) | |
{ | |
//RIGHT | |
int rotxR = (int)(((double)xR[i] * CosthR - yR[i] * SinthR));// xR rotangle座標回転 | |
int rotyR = (int)(((double)xR[i] * SinthR + yR[i] * CosthR));// yR rotangle座標回転 | |
int rotxmR = (int)(((double)xmR[i] * CosthR - ymR[i] * SinthR));// xmR rotangle座標回転 | |
int rotymR = (int)(((double)xmR[i] * SinthR + ymR[i] * CosthR));// ymR rotangle座標回転 | |
//LEFT | |
int rotxL = (int)(((double)xL[i] * CosthR - yL[i] * SinthR));// xR rotangle座標回転 | |
int rotyL = (int)(((double)xL[i] * SinthR + yL[i] * CosthR));// yR rotangle座標回転 | |
int rotxmL = (int)(((double)xmL[i] * CosthR - ymL[i] * SinthR));// xmR rotangle座標回転 | |
int rotymL = (int)(((double)xmL[i] * SinthR + ymL[i] * CosthR));// ymR rotangle座標回転 | |
//配列へ代入 | |
xR[i] = rotxR; | |
yR[i] = rotyR; | |
xmR[i] = rotxmR; | |
ymR[i] = rotymR; | |
xL[i] = rotxL; | |
yL[i] = rotyL; | |
xmL[i] = rotxmL; | |
ymL[i] = rotymL; | |
} | |
Debug.Print("RotateCheck:xR[0],yR[0],xR[size-1],yR[size-1]=" + ti(xR[0]) + "," + ti(yR[0]) + "," + ti(xR[size - 1]) + "," + ti(yR[size - 1])); | |
//Rpxr(iu) = CInt(((RNlon + RNrelE) * CosthR - (RNlat + RNrelN) * SinthR)) ' theata だけ座標回転 | |
//Rpyr(iu) = CInt(((RNlon + RNrelE) * SinthR + (RNlat + RNrelN) * CosthR)) ' theata だけ座標回転 | |
} | |
private void rotate8dgv( int size, int[] xR, int[] yR, int[] xmR, int[] ymR, int[] xL, int[] yL, int[] xmL, int[] ymL) | |
{ | |
//始点ー終点角度 | |
double xstart = (double)xR[0]; | |
double xend = (double)xR[size - 1]; | |
double ystart = (double)yR[0]; | |
double yend = (double)yR[size - 1]; | |
double ang = Math.Atan2(yend, xend);//グラフY座標が180度逆なので、補正 | |
//double rotangle =-Math.PI/2-ang;// 下向きに回転 | |
double rotangle = - ang;// x軸向きに回転 | |
//距離計算 | |
double distance = Math.Sqrt((xend - xstart) * (xend - xstart) + (yend - ystart) * (yend - ystart)); | |
Debug.Print("xR[0],yR[0]=," + ti(xR[0]) + "," + ti(yR[0]) + ",xR[size-1],yR[size-1]=," + ti(xR[size - 1]) + "," + ti(yR[size - 1])); | |
string rotate8dgv0 = "xR[0],yR[0]=," + ti(xR[0]) + "," + ti(yR[0]) + ",xR[size-1],yR[size-1]=," + ti(xR[size - 1]) + "," + ti(yR[size - 1]); | |
Debug.Print("ang=," + td(ang * 180 / Math.PI) + ",rotangle deg=," + td(rotangle * 180 / Math.PI) + ",distance=," + td(distance)); | |
string rotate8dgv1 = "ang=," + td(ang * 180 / Math.PI) + ",rotangle deg=," + td(rotangle * 180 / Math.PI) + ",distance=," + td(distance); | |
//回転座標変換 | |
double SinthR = Math.Sin(rotangle); | |
double CosthR = Math.Cos(rotangle); | |
int i; | |
for (i = 0; i < size; i++) | |
{ | |
//RIGHT | |
int rotxR = (int)(((double)xR[i] * CosthR - yR[i] * SinthR));// xR rotangle座標回転 | |
int rotyR = (int)(((double)xR[i] * SinthR + yR[i] * CosthR));// yR rotangle座標回転 | |
int rotxmR = (int)(((double)xmR[i] * CosthR - ymR[i] * SinthR));// xmR rotangle座標回転 | |
int rotymR = (int)(((double)xmR[i] * SinthR + ymR[i] * CosthR));// ymR rotangle座標回転 | |
//LEFT | |
int rotxL = (int)(((double)xL[i] * CosthR - yL[i] * SinthR));// xR rotangle座標回転 | |
int rotyL = (int)(((double)xL[i] * SinthR + yL[i] * CosthR));// yR rotangle座標回転 | |
int rotxmL = (int)(((double)xmL[i] * CosthR - ymL[i] * SinthR));// xmR rotangle座標回転 | |
int rotymL = (int)(((double)xmL[i] * SinthR + ymL[i] * CosthR));// ymR rotangle座標回転 | |
//配列へ代入 | |
xR[i] = rotxR; | |
yR[i] = rotyR; | |
xmR[i] = rotxmR; | |
ymR[i] = rotymR; | |
xL[i] = rotxL; | |
yL[i] = rotyL; | |
xmL[i] = rotxmL; | |
ymL[i] = rotymL; | |
// Debug.Print("yL[" + ti(i) + "]=" + ti(yL[i])); | |
} | |
Debug.Print("RotateCheck:xR[0],yR[0],xR[size-1],yR[size-1]=," + ti(xR[0]) + "," + ti(yR[0]) + "," + ti(xR[size - 1]) + "," + ti(yR[size - 1])); | |
string rotate8dgv2 = "RotateCheck:xR[0],yR[0],xR[size-1],yR[size-1]=," + ti(xR[0]) + "," + ti(yR[0]) + "," + ti(xR[size - 1]) + "," + ti(yR[size - 1]); | |
richTextBox1.AppendText(rotate8dgv0 + "\r\n"); | |
richTextBox1.AppendText(rotate8dgv1 + "\r\n"); | |
richTextBox1.AppendText(rotate8dgv2 + "\r\n"); | |
//Rpxr(iu) = CInt(((RNlon + RNrelE) * CosthR - (RNlat + RNrelN) * SinthR)) ' theata だけ座標回転 | |
//Rpyr(iu) = CInt(((RNlon + RNrelE) * SinthR + (RNlat + RNrelN) * CosthR)) ' theata だけ座標回転 | |
} | |
//================================================================================================== | |
private void vScrollBar1_Scroll(object sender, ScrollEventArgs e) | |
{ | |
int cy =vScrollBar1.Value; | |
double scale = 1; | |
int xdot = timeScale(); | |
// Debug.Print("cx=" + cx.ToString() + "pW=" + pW.ToString() + "dot=" + dot.ToString() + "posn=" + posn.ToString() + "xdot=" + xdot.ToString() + "rcx=" + rcx.ToString()); | |
xycut(cy,scale);//sourceBitmapからcxからcx-1000ドット切り取って、目盛り線と文字を書き込んでpictureBox1に表示 | |
} | |
private void xycut(int cy,double magni)//xyBitmap(1000x100000)から切り取って。PictureBox2(1000x600)に表示 | |
{ | |
int cp2x = 1000; | |
int cp2y = 10000; | |
int cutcenterx = cp2x * (int)(xsize / pic2w);// | |
int cutcentery = cp2y * (int)(ysize / pic2h);// | |
//int lefttopx = cutcenterx - pic2w/2; | |
//int lefttopy= cutcentery - pic2h/2; | |
//magniでカット面積縮小 | |
int lefttopx = (int)(cutcenterx - (pic2w / 2) / magni); | |
int lefttopy = (int)(cutcentery - (pic2h / 2) / magni); | |
// Debug.Print("lefttopx=" + lefttopx.ToString() + "lefttopy=" + lefttopy.ToString()); | |
Graphics g3 = Graphics.FromImage(xysBitmap); | |
g3.Clear(Color.Black); | |
// magni = 10; | |
if (magni == 0) { magni = 1; } | |
int magniw = Convert.ToInt32((double)pic3w / magni); | |
int magnih = Convert.ToInt32((double)pic3h / magni); | |
Rectangle srcxyRect = new Rectangle(lefttopx, lefttopy, magniw, magnih); | |
Rectangle desxyRect = new Rectangle(0, 0, pic3w, pic3h); | |
g3.DrawImage(xyBitmap, desxyRect, srcxyRect, GraphicsUnit.Pixel); | |
//---------------縮尺目盛り------------ | |
Brush b2 = new SolidBrush(Color.White); | |
g3.FillEllipse(b2, 10, 10, 10, 10); | |
g3.FillEllipse(b2, 110, 10, 10, 10); | |
Pen p2 = new Pen(Color.White, 2); | |
g3.DrawLine(p2, 15, 15, 115, 15); | |
double scale0 = 1; | |
string shukumoji = (scale0 / magni).ToString("n1") + "m"; | |
//フォントオブジェクトの作成 | |
Font fnt = new Font("MS UI Gothic", 16); | |
//文字列を位置(0,0)、青色で表示 | |
g3.DrawString(shukumoji, fnt, Brushes.White, 130, 8); | |
pictureBox2.Image = p2Bitmap; | |
} | |
//*******************************Small functions******************************************************** | |
string ti(int i) | |
{ | |
string s = i.ToString(); | |
return s; | |
} | |
string td(double d ) | |
{ | |
string s = d.ToString(); | |
return s; | |
} | |
int ig(int rN,int cN) | |
{ | |
int result= Convert.ToInt32(dgv1.Rows[rN].Cells[cN].Value); | |
return result; | |
} | |
int ig2(int rN, int cN) | |
{ | |
//Debug.Print("ig2:dgv2[rN,cN]=[" + ti(rN) + "][" + ti(cN) + "]=" + dgv2.Rows[rN].Cells[cN].Value.ToString()); | |
int result = Convert.ToInt32(dgv2.Rows[rN].Cells[cN].Value); | |
return result; | |
} | |
private void checkBox8_CheckedChanged(object sender, EventArgs e) | |
{ | |
if (checkBox8.Checked == false) | |
{ | |
checkBox7.Checked = true; | |
} | |
else | |
{ | |
checkBox7.Checked = false; | |
} | |
} | |
int ig3(int rN, int cN) | |
{ | |
// Debug.Print("ig3:dgv3[rN,cN]=[" + ti(rN) + "][" + ti(cN) + "]=" ); | |
// Debug.Print("ig3:dgv3[rN,cN]=[" + ti(rN) + "][" + ti(cN) + "]=" + dgv3.Rows[rN].Cells[cN].Value.ToString()); | |
int result = Convert.ToInt32(dgv3.Rows[rN].Cells[cN].Value); | |
return result; | |
} | |
double dg(int rN, int cN) | |
{ | |
double result = Convert.ToDouble(dgv1.Rows[rN].Cells[cN].Value); | |
return result; | |
} | |
double dg2(int rN, int cN) | |
{ | |
Debug.Print("dg2:dgv2[rN,cN]=[" + ti(rN) + "][" + ti(cN) + "]=" + dgv2.Rows[rN].Cells[cN].Value.ToString()); | |
double result = Convert.ToDouble(dgv2.Rows[rN].Cells[cN].Value); | |
return result; | |
} | |
double dg3(int rN, int cN) | |
{ | |
//Debug.Print("dg3:dgv3[rN,cN]=[" + ti(rN) + "][" + ti(cN) + "]=" + dgv3.Rows[rN].Cells[cN].Value.ToString()); | |
double result = Convert.ToDouble(dgv3.Rows[rN].Cells[cN].Value); | |
return result; | |
} | |
#endregion]]]]]]]]]]]]]]]]]]]]]]] | |
#region[[[[[[[[[[[[[[[[[[[[[[[[[[[BINARY & DGV FILE READ]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]] | |
//\\\\\\\\\\\\\\\\\\\\\\\dgvファイル READ\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\ | |
private void readdgv(string fname) | |
{ | |
int ir,iu; | |
int slast =0; | |
hScrollBar1.Visible = false; | |
if (checkBox4.Checked == false ) | |
{ | |
dgv1.Visible = true; | |
// DataGridView初期化(データクリア) | |
dgv1.Columns.Clear(); | |
dgv1.Rows.Clear(); | |
//dgv1.RowCount = 1; | |
dgv1.ColumnCount = 50; | |
dgv1.RowHeadersWidth = 60; | |
dgv1.TopLeftHeaderCell.Value = "Right_dgv"; | |
textBox1.Visible = true; | |
textBox1.Text = fname; | |
string str = File.ReadAllText(fname); | |
String[] sline = new string[200]; | |
dgv1.Rows.Add(); | |
sline = str.Split("\r\n"); | |
for (ir = 0; ir < sline.Length; ir++) | |
{ | |
string[] datast = sline[ir].Split(','); | |
if (datast[0] == "[s]") | |
{ | |
slast= ir; | |
//Debug.Print("ir=" + ir.ToString() + ":" +datast[0]); | |
} | |
//Debug.Print("ir=" + ir.ToString() + ":" + sline[ir]); | |
dgv1.Rows.Add(datast); | |
dgv1.Rows[ir].HeaderCell.Value = (ir ).ToString(); | |
} | |
headeradd(0, 1); | |
dgv1.Rows[0].Cells[0].Value =( slast+1).ToString(); | |
if (comboBox2.Items.Count == 0) | |
{ | |
int ico = 0; | |
int tablestartrow = ig(0, 0) + 11; | |
while (dgv1.Rows[ico + tablestartrow].Cells[1].Value != null) | |
{ | |
comboBox2.Items.Add(ti(ico)); | |
comboBox3.Items.Add(ti(ico)); | |
ico++; | |
} | |
} | |
} | |
else | |
{ | |
dgv2.Visible = true; | |
// DataGridView初期化(データクリア) | |
dgv2.Columns.Clear(); | |
dgv2.Rows.Clear(); | |
dgv2.RowCount = 2; | |
dgv2.ColumnCount = 50; | |
dgv2.RowHeadersWidth=60; | |
dgv2.TopLeftHeaderCell.Value = "Left_dgv"; | |
textBox4.Visible = true; | |
textBox4.Text = fname; | |
string str = File.ReadAllText(fname); | |
String[] sline = new string[200]; | |
sline = str.Split("\r\n"); | |
for (ir = 0; ir < sline.Length; ir++) | |
{ | |
string[] datasL = sline[ir].Split(','); | |
if (datasL[0] == "[s]") | |
{ | |
slast = ir; | |
//Debug.Print("ir=" + ir.ToString() + ":" +datast[0]); | |
} | |
// Debug.Print("ir=" + ir.ToString() + ":" + sline[ir]); | |
dgv2.Rows.Add(datasL); | |
dgv2.Rows[ir].HeaderCell.Value = (ir ).ToString(); | |
} | |
headeradd(0, 2); | |
dgv2.Rows[0].Cells[0].Value = (slast+1).ToString(); | |
if (comboBox2.Items.Count == 0) | |
{ | |
int ico = 0; | |
int tablestartrow = ig3(0, 0) + 11; | |
while (dgv2.Rows[ico + tablestartrow].Cells[1].Value != null) | |
{ | |
comboBox2.Items.Add(ti(ico)); | |
comboBox3.Items.Add(ti(ico)); | |
ico++; | |
} | |
} | |
} | |
Debug.Print("rnd readdgv"); | |
} | |
private void checkBox5_CheckedChanged(object sender, EventArgs e)//dgv3 READ | |
{ | |
} | |
private void dgvread3(string fname)//dgv3 FILE読み込み | |
{ | |
int ir; | |
int slast=0; | |
dgv3.Visible = true; | |
dgv3.ColumnCount = 100; | |
dgv3.Rows.Add(); | |
// DataGridView初期化(データクリア) | |
//dgv3.RowCount = 1; | |
//dgv3.ColumnCount =100; | |
// dgv3.Columns.Clear(); | |
//dgv3.Rows.Clear(); | |
//dgv3.RowCount = 1; | |
//dgv3.ColumnCount =100; | |
dgv3.TopLeftHeaderCell.Value = "RL_dgv3"; | |
//textBox1.Visible = true; | |
//textBox1.Text = fname; | |
string str = File.ReadAllText(fname); | |
String[] sline = new string[200]; | |
sline = str.Split("\r\n"); | |
for (ir = 0; ir < sline.Length; ir++) | |
{ | |
string[] datast = sline[ir].Split(','); | |
if (datast[0] == "[s]") | |
{ | |
slast = ir; | |
//Debug.Print("ir=" + ir.ToString() + ":" +datast[0]); | |
} | |
//Debug.Print("ir=" + ir.ToString() + ":" + sline[ir]); | |
dgv3.Rows.Add(datast); | |
dgv3.Rows[ir].HeaderCell.Value = (ir).ToString(); | |
} | |
headeradd(0, 3); | |
dgv3.Rows[0].Cells[0].Value = (slast + 1).ToString(); | |
dgv3.RowHeadersWidth = 60; | |
//読み取り後combobox2,3をクリアして新規ITEM作成 | |
comboBox2.Items.Clear(); | |
comboBox3.Items.Clear(); | |
int i = 0; | |
int tablestartrow = ig3(0, 0) + 2; | |
while (dgv3.Rows[i + tablestartrow].Cells[1].Value != null) | |
{ | |
comboBox2.Items.Add(ti(i)); | |
comboBox3.Items.Add(ti(i)); | |
i++; | |
} | |
} | |
//\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\ | |
//\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\基準ファイル 読み込み\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\ | |
/// </summary> | |
/// <param name="fname"></param> | |
void readkijun(string fname) | |
{ | |
//--------再起動時の基準位置読み込み kijun.txtを読み込む preprocessが新たに更新されない限りこのkijun値がデフォルト------- | |
string str = File.ReadAllText(fname); | |
String[] kdatastr = new string[200]; | |
double[] kdata = new double[200]; | |
kdatastr = str.Split(','); | |
int index = kdatastr.Length; | |
int i; | |
for (i = 0; i < index; i++) | |
{ | |
kdata[i] = Convert.ToDouble(kdatastr[i]); | |
} | |
//----------------------------------------------- | |
//rNaveN,rEaveN,rDaveN,rLaveN,rNstd,rEstd,rDstd,rLstd,rLmaveN,rLmstd,mHead); | |
kave.rNaveN = kdata[0]; | |
kave.rEaveN = kdata[1]; | |
kave.rDaveN = kdata[2]; | |
kave.rLaveN = kdata[3]; | |
kave.rNstd = kdata[4]; | |
kave.rEstd = kdata[5]; | |
kave.rDstd = kdata[6]; | |
kave.rLstd = kdata[7]; | |
kave.rLmaveN = kdata[8]; | |
kave.rLmstd = kdata[9]; | |
kave.mHead = kdata[10]; | |
kave.pday = (int)kdata[11]; | |
kave.phour = (int)kdata[12] + 9; | |
kave.pmin = (int)kdata[13]; | |
kave.psec = (int)kdata[14]; | |
//Serial.printf("KijunData:rNaveN=%4.2f,rEaveN=%4.2f,rDaveN=%4.2f,rLaveN=%4.2f,rNstd=%4.2f,rEstd=%4.2f,rDstd=%4.2f,rLstd=%4.2f,rLmaveN=%4.2f,rLmstd=%4.2f,mHead=%4.2f,date=%d:%d:%d:%d\n\r", rNaveN, rEaveN, rDaveN, rLaveN, rNstd, rEstd, rDstd, rLstd, rLmaveN, rLmstd, mHead, pday, phour, pmin, psec); | |
// Serial2.printf("KijunData:%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,<%d:%d:%d:%d>,\n\r", rNaveN, rEaveN, rDaveN, rLaveN, rNstd, rEstd, rDstd, rLstd, rLmaveN, rLmstd, mHead, pday, phour, pmin, psec); | |
} | |
private void dgv3_CellClick(object sender, DataGridViewCellEventArgs e) | |
{ | |
label7.Text = "row=" + dgv3.CurrentCell.RowIndex.ToString() + "column=" + dgv3.CurrentCell.ColumnIndex.ToString(); | |
} | |
private void checkBox7_CheckedChanged(object sender, EventArgs e) | |
{ | |
if (checkBox7.Checked == true) | |
{ | |
checkBox8.Checked = false; | |
} | |
else | |
{ | |
checkBox8.Checked = true; | |
} | |
} | |
private void textBox3_TextChanged(object sender, EventArgs e) | |
{ | |
} | |
int ubxread(String fname)//u keyin | |
{ | |
dgv1.Visible = true; | |
int fb = 0; | |
int nc = 0; ; | |
int epochn = 0; | |
char[] cd = new char[4]; | |
char c; | |
int crn = 0; | |
//data buffer | |
byte[] dBuf1pvt = new byte[100]; | |
byte[] dBuf1relp = new byte[72]; | |
byte[] dBuf4pvt = new byte[100]; | |
byte[] dBuf4relp = new byte[72]; | |
byte[] dBuf5_0 = new byte[100]; | |
byte[] dBuf5_1 = new byte[100]; | |
byte[] dBuf5_2 = new byte[100]; | |
byte[] bnbuf0 = new byte[60]; | |
byte[] bnbuf1 = new byte[60]; | |
byte[] bnbuf2 = new byte[60]; | |
dgv1.Columns.Clear(); | |
dgv1.Rows.Clear(); | |
dgv1.RowCount = 2; | |
dgv1.ColumnCount = 46; | |
dgv1.RowHeadersWidth = 60; | |
dgv1.TopLeftHeaderCell.Value = "Binary_read"; | |
textBox1.Visible = true; | |
textBox1.Text = fname; | |
// if (openflag == 1) | |
//{ | |
// myFileA.close(); | |
//} | |
//=============FILE streqm================================== | |
//string path = fname; | |
FileStream fs = File.OpenRead(fname); | |
if (fs.Length > 0) | |
{ | |
while (fb < fs.Length) | |
{ | |
fs.Read(dBuf1pvt, 0, 100); | |
fs.Read(dBuf1relp, 0, 72); | |
fs.Read(dBuf4pvt, 0, 100); | |
fs.Read(dBuf4relp, 0, 72); | |
fs.Read(dBuf5_0, 0, 100); | |
fs.Read(dBuf5_1, 0, 100); | |
fs.Read(dBuf5_2, 0, 100); | |
fs.Read(bnbuf0, 0, 60); | |
fs.Read(bnbuf1, 0, 60); | |
fs.Read(bnbuf2, 0, 60); | |
fb = fb + 172 + 172 + 300 + 180; | |
epochn++; | |
//Base conversion | |
pvt basepvt = new pvt(); | |
PVTcnv(dBuf1pvt, ref basepvt); | |
relp baserelp = new relp(); | |
RELPOScnv(dBuf1relp, ref baserelp); | |
//Rover conbersion | |
pvt roverpvt = new pvt(); | |
PVTcnv(dBuf4pvt, ref roverpvt); | |
relp roverrelp = new relp(); | |
RELPOScnv(dBuf4relp, ref roverrelp); | |
//M9N conversion | |
pvt[] M9Npvt = new pvt[3]; | |
PVTcnv(dBuf5_0, ref M9Npvt[0]); | |
PVTcnv(dBuf5_1, ref M9Npvt[1]); | |
PVTcnv(dBuf5_2, ref M9Npvt[2]); | |
//BNO conversion | |
//bno bnodata0 = new bno(); | |
//bno bnodata1 = new bno(); | |
//bno bnodata2 = new bno(); | |
bnocnv(bnbuf0); | |
bnocnv(bnbuf1); | |
bnocnv(bnbuf2); | |
//------Position HighPrecision Calculation---------------- | |
//基準位置ゼロにした現在位置 | |
// Debug.Print("[ubxread]:baserelp.relposn=" + baserelp.relposn.ToString()); | |
double relN = (baserelp.relposn + baserelp.relposhpn * 0.01) - kave.rNaveN; | |
double relE = (baserelp.relpose + baserelp.relposhpe * 0.01) - kave.rEaveN; | |
double relD = (baserelp.relposd + baserelp.relposhpd * 0.01) - kave.rDaveN; | |
double relL = (baserelp.relposlength + baserelp.relposhplength * 0.01) - kave.rLaveN; ; | |
// | |
double rrelN = (roverrelp.relposn + roverrelp.relposhpn * 0.01) + relN; | |
double rrelE = (roverrelp.relpose + roverrelp.relposhpe * 0.01) + relE; | |
double rrelD = (roverrelp.relposd + roverrelp.relposhpd * 0.01) + relD; | |
//double rrelL = (baserelp.relposlength + baserelp.relposhplength * 0.01) - kave.rLaveN; ; | |
dgv1.Rows.Add(); | |
//[0]md,[1]flagsB,[2]flagsR,[3]pdopB,[4]pdopR,[5]haccB,[6]haccR,[7]miscount0,[8]count_upB,[9]count_upB0,[10]tp,[11]tp0,[12][Base], | |
crn++; | |
dgv1.Rows[crn].Cells[0].Value = "[s]"; | |
dgv1.Rows[crn].Cells[1].Value = basepvt.flags.ToString(); | |
dgv1.Rows[crn].Cells[2].Value = roverpvt.flags.ToString(); | |
dgv1.Rows[crn].Cells[3].Value = basepvt.pdop.ToString(); | |
dgv1.Rows[crn].Cells[4].Value = roverpvt.pdop.ToString(); | |
dgv1.Rows[crn].Cells[5].Value = basepvt.hacc.ToString(); | |
dgv1.Rows[crn].Cells[6].Value = roverpvt.hacc.ToString(); | |
dgv1.Rows[crn].Cells[7].Value = "miscount0"; | |
dgv1.Rows[crn].Cells[8].Value = "countupB"; | |
dgv1.Rows[crn].Cells[9].Value = "countupB0"; | |
dgv1.Rows[crn].Cells[10].Value = "tp"; | |
dgv1.Rows[crn].Cells[11].Value = "tp0"; | |
dgv1.Rows[crn].Cells[12].Value = "Base"; | |
//[13]itowB,[14]NYB,[15]EXB,[16]DZB,[17]gSpeedB,[18]headMotBZ,[19][Rover],[20]itowR,[21]NYR,[22]EXR,[23] DZR, | |
dgv1.Rows[crn].Cells[13].Value = basepvt.itow.ToString(); | |
dgv1.Rows[crn].Cells[14].Value = relN.ToString(); | |
dgv1.Rows[crn].Cells[15].Value = relE.ToString(); | |
dgv1.Rows[crn].Cells[16].Value = relD.ToString(); | |
dgv1.Rows[crn].Cells[17].Value = basepvt.gspeed.ToString(); | |
double hmot = Convert.ToDouble(basepvt.headmot) / 100000; | |
dgv1.Rows[crn].Cells[18].Value = hmot.ToString(); | |
dgv1.Rows[crn].Cells[19].Value = "Rover"; | |
dgv1.Rows[crn].Cells[20].Value = baserelp.itow.ToString(); | |
dgv1.Rows[crn].Cells[21].Value = rrelN.ToString(); | |
dgv1.Rows[crn].Cells[22].Value = rrelE.ToString(); | |
dgv1.Rows[crn].Cells[23].Value = rrelD.ToString(); | |
// [24]gSpeedR,[25]mHead,[26]bnHosei + eX[0],[27]relLm],[28][BNO],[29]yaw1,[30]pitch1,[31]roll1,[32] lax0,[33]lay0,[34]laz0,[35]grx0,[36]gry0,[37]grz0 | |
dgv1.Rows[crn].Cells[24].Value = roverpvt.gspeed.ToString(); | |
double head = Convert.ToDouble(roverrelp.relposheading) / 100000; | |
dgv1.Rows[crn].Cells[25].Value = head.ToString(); | |
dgv1.Rows[crn].Cells[26].Value = eX[0].ToString(); | |
dgv1.Rows[crn].Cells[27].Value = roverrelp.relposlength.ToString(); | |
dgv1.Rows[crn].Cells[28].Value = "BNO"; | |
dgv1.Rows[crn].Cells[29].Value = eX[0].ToString(); | |
dgv1.Rows[crn].Cells[30].Value = eY[0].ToString(); | |
dgv1.Rows[crn].Cells[31].Value = eZ[0].ToString(); | |
dgv1.Rows[crn].Cells[32].Value = laX[0].ToString(); | |
dgv1.Rows[crn].Cells[33].Value = laY[0].ToString(); | |
dgv1.Rows[crn].Cells[34].Value = laZ[0].ToString(); | |
dgv1.Rows[crn].Cells[35].Value = grX[0].ToString(); | |
dgv1.Rows[crn].Cells[36].Value = grY[0].ToString(); | |
dgv1.Rows[crn].Cells[37].Value = grZ[0].ToString(); | |
} //while end | |
} // if length end | |
else //OPEN ERROR | |
{ | |
// Serial.println("UPLOAD: Read File Open Eror"); | |
} | |
//---------KIJIUN 点読み込------------------------- | |
//read finished | |
fs.Dispose(); | |
//ASCII Data | |
return epochn; | |
} //ubxcnv() end | |
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% | |
int ubxmondgv(int i) | |
{ | |
//dgvの受信データをパラメータに代入 | |
//("[%c],%d,%d,%4.2f,%4.2f,%d,%d,%d,%d,%d,%d,%d\n\r",md,flagsB,flagsR,pdopB,pdopR,haccB,haccR,miscount0,count_upB,count_upB-count_upB0,tp,tp0); | |
//"[Base],%d,%4.2f,%4.2f,%4.2f,%d,%d,\n\r",itowB,NYB,EXB,DZB,gSpeedB,headMotB); | |
//"[Rover],%d,%4.2f,%4.2f,%4.2f,%d,%4.2f,%4.2f,%4.1f\n\r",itowR,NYR,EXR,DZR,gSpeedR,mHead,bnHosei+eX[0],relLm); | |
//"[BNO],%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%4.1f%,\n\r",eX[0],eY[0],eZ[0],eX[1],eY[1],eZ[1],eX[2],eY[2],eZ[2]); | |
//[0]md,[1]flagsB,[2]flagsR,[3]pdopB,[4]pdopR,[5]haccB,[6]haccR,[7]miscount0,[8]count_upB,[9]count_upB0,[10]tp,[11]tp0,[12][Base],[13]itowB,[14]NYB,[15]EXB,[16]DZB,[17]gSpeedB,[18]headMotBZ,[19][Rover],[20]itowR,[21]NYR,[22]EXR DZR,[23]gSpeedR,[24]mHead,[25]bnHosei+eX[0],[26]relLm],[27][BNO],[28]yaw1,[29]pitch1,[30]roll1,[31] lax0,[32]lay0,[33]laz0,[34]grx0,[35]gry0,[36]grz0 | |
dgv1.Rows.Add(); | |
int crn = dgv1.CurrentRow.Index; | |
return i; | |
} | |
int toint32(byte[] buf, int n) | |
{ | |
int result; | |
result = buf[n] + buf[n + 1] * 256 * buf[n + 2] * 256 * 256 + buf[n + 3] * 256 * 256 * 256 + buf[n + 3] * 256 * 256 * 256; | |
return result; | |
} | |
void bnocnv(byte[] bnbuf) | |
{ | |
eX = new float[3]; | |
eY = new float[3]; | |
eZ = new float[3]; | |
qW = new float[3]; | |
qX = new float[3]; | |
qY = new float[3]; | |
qZ = new float[3]; | |
laX = new float[3]; | |
laY = new float[3]; | |
laZ = new float[3]; | |
grX = new float[3]; | |
grY = new float[3]; | |
grZ = new float[3]; | |
tst = new int[3]; | |
int bi = 0; | |
int iib, nnb; | |
int bl = 60;//bn data block length 60byte | |
int hd = 4; | |
for (bi = 0; bi < 3; bi++) | |
{ | |
/* | |
bnbuf[0 + bi * bl] = (byte)'B'; | |
bnbuf[1 + bi * bl] = (byte)'N'; | |
bnbuf[2 + bi * bl] = (byte)'O'; | |
bnbuf[3 + bi * bl] =(byte) bi; | |
*/ | |
//public static int ToInt32 (byte[] value, int startIndex); | |
//---EULER | |
nnb = 4;//eX | |
int iex = Toint32(bnbuf[7], bnbuf[6], bnbuf[5], bnbuf[4]); | |
eX[bi] = (float)iex / 1000000; | |
// Debug.Print("[bnocnv]:b.eX[bi]=" + b.eX[bi].ToString()); | |
nnb = 8;//eY | |
int iey = Toint32(bnbuf[11], bnbuf[10], bnbuf[9], bnbuf[8]); | |
eY[bi] = (float)iey / 1000000; | |
nnb = 12;//eZ | |
int iez = Toint32(bnbuf[15], bnbuf[14], bnbuf[13], bnbuf[12]); | |
eZ[bi] = (float)iez / 1000000; | |
//---Quarternion | |
nnb = 16;//qW | |
int iqw = Toint32(bnbuf[19], bnbuf[18], bnbuf[17], bnbuf[16]); | |
qW[bi] = (float)iex / 1000000; | |
nnb = 20;//eX | |
int iqx = Toint32(bnbuf[23], bnbuf[22], bnbuf[21], bnbuf[20]); | |
qX[bi] = (float)iqx / 1000000; | |
nnb = 24;//qY | |
int iqy = Toint32(bnbuf[27], bnbuf[26], bnbuf[25], bnbuf[24]); | |
qY[bi] = (float)iey / 1000000; | |
nnb = 28;//qZ | |
int iqz = Toint32(bnbuf[31], bnbuf[30], bnbuf[29], bnbuf[28]); | |
qZ[bi] = (float)iqz / 1000000; | |
//---LinearAcc | |
nnb = 32;//laX | |
int ilax = Toint32(bnbuf[35], bnbuf[34], bnbuf[33], bnbuf[32]); | |
laX[bi] = (float)ilax / 1000000; | |
nnb = 36;//laY | |
int ilay = Toint32(bnbuf[39], bnbuf[38], bnbuf[37], bnbuf[36]); | |
laY[bi] = (float)ilay / 1000000; | |
nnb = 40;//laZ | |
int ilaz = Toint32(bnbuf[43], bnbuf[42], bnbuf[41], bnbuf[40]); | |
laZ[bi] = (float)ilaz / 1000000; | |
//---Gravity | |
nnb = 44;//grX | |
int igrx = Toint32(bnbuf[47], bnbuf[46], bnbuf[45], bnbuf[44]); | |
grX[bi] = (float)igrx / 1000000; | |
nnb = 48;//grY | |
int igry = Toint32(bnbuf[51], bnbuf[50], bnbuf[49], bnbuf[48]); | |
grY[bi] = (float)igry / 1000000; | |
nnb = 52;//grZ | |
int igrz = Toint32(bnbuf[55], bnbuf[54], bnbuf[53], bnbuf[52]); | |
grZ[bi] = (float)igrz / 1000000; | |
//---tst time | |
nnb = 56; | |
tst[bi] = Toint32(bnbuf[59], bnbuf[58], bnbuf[57], bnbuf[56]); | |
} | |
}// bnoBintoDbl | |
//+++PVTcnv++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
int PVTcnv(byte[] d, ref pvt p) | |
{ | |
//PVT header[0-6] | |
//0:itow[6-9] | |
p.itow = (uint)Toint32(d[9], d[8], d[7], d[6]); | |
//Serial.printf("PVTcnv:itow=%d\n\r",pvt[0]); | |
//1:year[10-12] | |
p.year = (ushort)(d[10] + d[11] * 256); | |
//2:month[12] | |
p.month = d[12]; | |
//3:day[13] | |
p.day = d[13]; | |
//4:hour[14] | |
p.hour = d[14]; | |
//Serial.printf("PVTcnv:hour=%d\n\r",pvt[4]); | |
//5:min[15] | |
p.min = d[15]; | |
//Serial.printf("PVTcnv:min=%d\n\r",pvt[5]); | |
//6:sec[16] | |
p.sec = d[16]; | |
//Serial.printf("PVTcnv:sec=%d\n\r",pvt[6]); | |
//7:valid[17] | |
p.valid = d[17]; | |
//8:tAcc[18-21] | |
p.tacc = (uint)Toint32(d[21], d[20], d[19], d[18]); | |
//9:nano[22-25] | |
p.nano = Toint32(d[25], d[24], d[23], d[22]); | |
//10:fixType[26] | |
p.fixtype = d[26]; | |
//11:flags[27] This is Fix status 131 | |
p.flags = d[27]; | |
//12:flags2[28] | |
p.flags2 = d[28]; | |
//13:numSV[29] | |
p.numsv = d[29]; | |
//14:lon[30-33] | |
p.lon = Toint32(d[33], d[32], d[31], d[30]); | |
//15:lat[34-37] | |
p.lat = Toint32(d[37], d[36], d[35], d[34]); | |
//16:height[38-41] | |
p.height = Toint32(d[41], d[40], d[39], d[38]); | |
//17:hMSL[42-45] | |
p.hmsl = Toint32(d[45], d[44], d[43], d[42]); | |
//18:hAcc[46-49] | |
p.hacc = (uint)Toint32(d[49], d[48], d[47], d[46]); | |
if (p.hacc > 1000) | |
{ | |
p.hacc = 0; | |
} | |
//19:vAcc[50-53] | |
p.vacc = (uint)Toint32(d[53], d[52], d[51], d[50]); | |
//20:velN[54-57] | |
p.veln = Toint32(d[57], d[56], d[55], d[54]); | |
//21:velE[58-61] | |
p.vele = Toint32(d[61], d[60], d[59], d[58]); | |
//22:velD[62-65] | |
p.veld = Toint32(d[65], d[64], d[63], d[62]); | |
//23:gSpeed[66-69] | |
p.gspeed = Toint32(d[69], d[68], d[67], d[66]); | |
//24:headMot[70-73] | |
p.headmot = Toint32(d[73], d[72], d[71], d[70]); | |
//25:sAcc[74-77] | |
p.sacc = (uint)Toint32(d[77], d[76], d[75], d[74]); | |
//26:headAcc[78-81] | |
p.headacc = (uint)Toint32(d[81], d[80], d[79], d[78]); | |
//27:pDOP[82-83] | |
p.pdop = (ushort)(d[82] + d[83] * 256); | |
//28:flags3[84] | |
p.flags3 = d[84]; | |
//29:reserved1[85] | |
p.resrv0 = d[85]; | |
//30:headVeh[86-89] | |
p.headveh = Toint32(d[89], d[88], d[87], d[86]); | |
//31:magDec[90-91] | |
p.magdec = (short)(d[90] + d[91] * 256); | |
//32:magAcc[92-93] | |
p.magacc = (ushort)(d[92] + d[93] * 256); | |
return (int)p.itow; | |
}//PVTcnv() end | |
//--RELPOScnv++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
int RELPOScnv(byte[] d, ref relp r) | |
{ | |
//RELPOS header[0-5] | |
//Debug.Print("{RELPOScnv]:d[0][1][2][3]" + d[0].ToString("X") + d[1].ToString("X") + d[2].ToString("X") + d[3].ToString("X")); | |
int s = 0; | |
//0:ver[6] | |
r.version = d[s + 6]; | |
//1:reserved1[7] | |
r.reserved1 = d[s + 7]; | |
//2:refStationId[8-9] | |
r.refstationid = d[s + 8]; | |
//3:itow[10-13] | |
//Debug.Print("{RELPOScnv ITOW]:d[10][11][12][13]" + d[10].ToString("X") + d[11].ToString("X") + d[12].ToString("X") + d[13].ToString("X")); | |
r.itow = (uint)Toint32(d[s + 13], d[s + 12], d[s + 11], d[s + 10]); | |
//Serial.printf("RELPOScnv:itow=%d\n\r",relpos[3]); | |
//4:relposN[14-17] | |
r.relposn = Toint32(d[s + 17], d[s + 16], d[s + 15], d[s + 14]); | |
//5:relposE[18-21] | |
r.relpose = Toint32(d[s + 21], d[s + 20], d[s + 19], d[s + 18]); | |
//6:relposD[22-25] | |
r.relposd = Toint32(d[s + 25], d[s + 24], d[s + 23], d[s + 22]); | |
//7:relposLength[26-29] | |
r.relposlength = Toint32(d[s + 29], d[s + 28], d[s + 27], d[s + 26]); | |
//Serial.printf("RELPOScnv:Lenghth=relpos[7]=%d\n\r",relpos[7]); | |
//8:relposHeading[30-33] | |
r.relposheading = Toint32(d[s + 33], d[s + 32], d[s + 31], d[s + 30]); | |
//Serial.printf("relposHeading[8]=%d,[33]%x,[32]%x,[31]%x,[30]%x,\n\r",relpos[8],d[33],d[32],d[31],d[30]); | |
//9:reserved2[34] | |
r.reserved2 = Toint32(d[s + 37], d[s + 36], d[s + 35], d[s + 34]); | |
//10:relposHPN[35] | |
r.relposhpn = (sbyte)((d[s + 38] & 127) - (d[s + 38] & 128)); | |
//Serial.printf("HPN=%d,d[38]=%x,d[39]=%x,d[40]=%x,d[41]=%x,&127=%dx,&128=%d\n\r",relpos[10],d[38],d[39],d[40],d[41],d[38] && 127,d[38] && 128); | |
//11:relposHPE[36] | |
r.relposhpe = (sbyte)((d[s + 39] & 127) - (d[s + 39] & 128)); | |
//12:relposHPD[37] | |
r.relposhpd = (sbyte)((d[s + 40] & 127) - (d[s + 40] & 128)); | |
//13:relposHPLength[38] | |
r.relposhplength = (sbyte)((d[s + 41] & 127) - (d[s + 41] & 128)); | |
//14:accN[38-41] | |
r.accn = (uint)Toint32(d[s + 41], d[s + 40], d[s + 39], d[s + 38]); | |
//15:accE[42-45] | |
r.acce = (uint)Toint32(d[s + 45], d[s + 44], d[s + 43], d[s + 42]); | |
//16:accD[46-49] | |
r.accd = (uint)Toint32(d[s + 49], d[s + 48], d[s + 47], d[s + 46]); | |
//17:accLength[50-53] | |
r.acclength = (uint)Toint32(d[s + 53], d[s + 52], d[s + 51], d[s + 50]); | |
//18:accHeading[54-57] | |
r.accheading = (uint)Toint32(d[s + 57], d[s + 56], d[s + 55], d[s + 54]); | |
//19:reserved[57-60] | |
r.reserved3 = (uint)Toint32(d[s + 61], d[s + 60], d[s + 59], d[s + 58]); | |
//20:flags[60-63] | |
r.flags = (uint)Toint32(d[s + 65], d[s + 64], d[s + 63], d[s + 62]); | |
return (int)r.itow; | |
} | |
//+++++++++++++4byte Binary to Long ++++++++++++++++++++++++++++++++++++++++++++++ | |
int Toint32(byte b4, byte b3, byte b2, byte b1) | |
{ | |
//pc.printf("ToInt32 IN=%s,%x,%x,%x,%x,b4&0x80=%d\n\r",sen,b4,b3,b2,b1,b4 &0x80); | |
//pc.printf("ToInt32 IN=b4&0x80=%d\n\r",b4 & 0x80); | |
long su; | |
if ((b4 & 0b10000000) == 0b10000000) | |
{ //最上位ビットたっていればマイナス | |
//su = -(256 - (long)b1) + (255 - (long)b2) * 256 + (255 - (long)b3) * 65536 + (255 - (long)b4) * 256 * 256 * 256; | |
//pc.printf("ToInt32-:sen=%s,%d,%d,%d,%d,%d\n\r",sen,b4,b3,b2,b1,su); | |
uint i1 = b1; | |
uint i2 = (uint)b2 << 8; | |
uint i3 = (uint)b3 << 16; | |
uint i4 = (uint)b4 << 24; | |
uint i5 = i1 | i2 | i3 | i4; | |
su = (long)i5; | |
} | |
else | |
{ | |
su = (long)b1 + (long)b2 * 256 + (long)b3 * 65536 + (long)b4 * 256 * 256 * 256; | |
//pc.printf("ToInt32+:sen=%s,%d,%d,%d,%d,%d,%d\n\r",sen,b4,b3,b2,b1,su); | |
} | |
return (int)su; | |
} | |
//================================================================================= | |
//+++++++++++++++i_to_char++++++++++++++++++++++++++++++++++++ | |
// i=IntegerValueData,*d=Array pointer, n=Array start No | |
void i_to_char(int i, byte[] d, int n) | |
{ | |
d = BitConverter.GetBytes(i); | |
/* | |
d[n] = i & 0x000000ff; | |
d[n + 1] = (i & 0x0000ff00) >> 8; | |
d[n + 2] = (i & 0x00ff0000) >> 16; | |
d[n + 3] = (i & 0xff000000) >> 24; | |
*/ | |
// Serial.printf("itochar:i=%d,d[0]=%x,d[1]=%x,d[2]=%x,d[3]=%x\n\r",i,d[0],d[1],d[2],d[3]); | |
} | |
//++++++++++++++++String to CharArray Trans++++++++++++++++++++ | |
void str2char(char[] c, String dataS) | |
{ | |
//String dataS; | |
//dataS="HELLO dataS"; | |
int dataS_len = dataS.Length + 1; | |
// char char_array[dataS_len]; | |
c = dataS.ToCharArray(); | |
} | |
//itow=> HH:MM:SS ================================================== | |
String itowToHMS(int itow) | |
{ | |
String HMS; | |
int DAY, HOUR, MIN, SEC, JHOUR; | |
int DAY_MOD, HOUR_MOD, MIN_MOD;//, SEC_MOD; | |
//DAY = int(itow / 86400000); | |
DAY_MOD = itow % 86400000; | |
HOUR = (int)(DAY_MOD / 3600000); | |
HOUR_MOD = DAY_MOD % 3600000; | |
MIN = (int)(HOUR_MOD / 60000); | |
MIN_MOD = HOUR_MOD % 60000; | |
SEC = (int)(MIN_MOD / 1000); | |
//--UTC=>JST | |
if (HOUR > 15) | |
{ | |
JHOUR = HOUR + 9 - 24; | |
} | |
else | |
{ | |
JHOUR = HOUR + 9; | |
} | |
//=====18sec 進んでいる?補正======= | |
if (SEC < 18) | |
{ | |
SEC = 60 - (18 - SEC); | |
} | |
else | |
{ | |
SEC = SEC - 18; | |
} | |
//------------- | |
//Serial.printf("itowToHMS:JHOUR=%d,MIN=%d,SEC=%d\n\r",JHOUR,MIN,SEC); | |
HMS = JHOUR.ToString() + ":" + MIN.ToString() + ":" + SEC.ToString(); | |
return HMS; | |
}//itowToHMS end===================================================== | |
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX | |
#endregion | |
}//FORM END | |
}//NAME SPACE END | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment