Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/STA23_kaiten_rev09.cs Secret

Created June 4, 2023 09:02
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save dj1711572002/dc3831aa4b43571e76419d10e18287f5 to your computer and use it in GitHub Desktop.
Save dj1711572002/dc3831aa4b43571e76419d10e18287f5 to your computer and use it in GitHub Desktop.
Download ZIP
Ｃ#　　RTK heading vs BNO055 yaw vs Magnet Encoder Head position accuracy
Raw
STA23_kaiten_rev09.cs
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_Kaiten_rev09
{
//......................................................................................................................................................
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];
//kaiten用配列
static long[] dgv3itowB;
//******************KAITEN用構造体************************************************
struct kaiten
{
public int allNo;
public string[] magrowstr;
};
kaiten[] magrows;
//****************************dgv3データ構造体**************************************
struct dgv3strct
{
public int ttstartrow;// ターンテーブル開始行番号 [0,0]+2になる
public int ttlastNo;//ターン番号の最後
public int[] tnstN2rN;//ターン番号の開始行を格納してある配列
public int[] row2tN;//row番号からターン番号を格納してある配列
public int tstartrow;//ターンデータ開始行番号
public int tendrow;//ターンデータ終了行番号
public double[] colmax;//列の最大値　dgv3 0-41 50-91
public double[] colmin;//列の最小値
};
dgv3strct dg3s;//dgv3 構造体変数名
//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 NY0;
public double EX0;
public double DZ0;
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];////右スキーのターン構造体配列宣言２００個まで収納
Turns[] tL = new Turns[1000];////左スキーのターン構造体配列宣言２００個まで収納
//************************************Graphic parameters*************************************************************************
const int blen = 3000;//処理データ単位３０００行を１ブロックにして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 = 400;// 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としてポイントサイズを１に最小化した
Bitmap p2Bitmap = new Bitmap(pic2w, pic2h);//picturebox2のBitmap
Bitmap p2sBitmap = new Bitmap(pic2w, pic2h);//picturebox2の拡大用Bitmapとしてポイントサイズを１に最小化した
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;
//MAG_timing 補間用
string sizes = null;
static int sizei = 0;//dgv行数
static int sizei3 = 0;
static int sizec = 0;//dgv列数
long lowitow, miditow, hiitow;
static int tcount = 0;
static int mcount = 0;
static long[] target_itow ;//BN0,BN1,BN2のitow
static long[] mag_itow ;
#endregion
#region [[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[[formset]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]
public Form1()
{
InitializeComponent();
}
//......................................................................................................................................................
//......................................................................................................................................................
private void Form1_Load(object sender, EventArgs e)
{
this.Text = "STA23_Kaiten_rev09";
richTextBox1.Visible = false;
dgv1.Visible = false;
}
//..... .................................................................................................................................................
//......................................................................................................................................................
delegate void SetTextCallback(string text);
//......................................................................................................................................................
private void checkBox2_CheckedChanged(object sender, EventArgs e)
{
}
//......................................................................................................................................................
//......................................................................................................................................................
private void button5_Click(object sender, EventArgs e)
{
dgv3saveMag();
}
private void textBox1_TextChanged(object sender, EventArgs e)
{
string mes = textBox1.Text;
string head = mes.Substring(0, 3);
}
//......................................................................................................................................................
private void checkBox1_CheckedChanged(object sender, EventArgs e)
{
if (checkBox1.Checked == true)
{
richTextBox1.Visible = true;
}
else
{
richTextBox1.Visible = false;
}
}
//..............................mHead Add 回転用にdgv1のmHead yawを累積値に変換.......................................................................................................
private void button7_Click(object sender, EventArgs e)
{
mhAdd();
}
private void mhAdd()
{
int i;
double mhadd = 0;
double[] mharry = new double[sizei + 1];
for (i = 1; i < sizei; i++)
{
//---mHead Add
double mh = Convert.ToDouble(dgv1.Rows[i].Cells[32].Value);
double mh_1 = Convert.ToDouble(dgv1.Rows[i - 1].Cells[32].Value);
if (mh - mh_1 < -60)
{
mhadd = mhadd + 360;
}
mharry[i] = mh + mhadd;
}
for (i = 0; i < sizei; i++)
{
dgv1.Rows[i].Cells[32].Value = mharry[i].ToString();
}
}
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;
}
}
}
//[-----------------------------------------------------------------------------------------------------------------------]
//[Kaiten用データ処理 磁気データとBNOとRTKの時刻合わせ補間----------------------------------------------------------------]
//[-----------------------------------------------------------------------------------------------------------------------]
//使うデータ　[Rover]itowR,mHead [BNO]yaw0,yaw1,yaw2
//itowの基本データとBNOの端数補間データとMAGの補間データ3列を作成
private void button4_Click(object sender, EventArgs e)
{
mhAdd();//dgv1のmHをAdd処理してからdgv3補間計算させる
ITP_set();
}
//*******************ITP_insert(long target_itow,int itow_col,int itow_div,int start_row,int end_row)**************
//dgv1の行に挿入して前後のデータを補間計算して代入する
//計算対象列１[Base]：itowB<20>=target_itow,Ny<21>,Ex<22>,Dz<23>,gSpeed<24>,headMot<25>,
//計算対象列２[Rover]：itowR<27>=target_itow,relN<28>,relE<29>,relD<30>,gSpeedR<31>,mHead<32>
//計算対象列３[BNO]:３個をばらして順次コピーする。BNO0<35-45>,BNO1<47-57>,BNO2<59-69>
private void itpcal(long targetitow,int targetNo,int ri_tcount)//補間計算して行string配列datasで返す
{
int[] dcolB = { 20, 21, 22, 23, 24, 25 };//[Base] [Rover]の補間値の列番号配列
int[] dcolR = { 27, 28, 29, 30, 31, 32 };//[Base] [Rover]の補間値の列番号配列
int dcolN = 6;
int itow_div = 120;
int dBNc = 12;
int[,] dBNO = new int[,] { {34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45 }, {46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57 }, {58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69 } };//[BN0]列番号配列
double d0, d1, d2;//d0 補間値　
int i, j, k,n;
int rowL = 0;
int rowU = 0;
//
Debug.Print("*****ITPCAL:targetitow=" + targetitow.ToString() + "targetNo=" + targetNo.ToString() + "ri_tocount=" + ri_tcount.ToString());
//Base itow範囲の検索
long itow_L = targetitow - (targetitow % itow_div);//target_itowの存在範囲の開始itow
long itow_U = itow_L + itow_div;
for (i = 0; i < sizei-3; i++)
{
if (itow_L == Convert.ToInt64(dgv1.Rows[i].Cells[20].Value))
{
rowL= i;
i = sizei;//exit
}
}
//
string[] rowstr = new string[100];
for (i = 0; i < 20; i++)//[m]列0-19をrostr配列に収納
{
rowstr[i] = dgv1.Rows[rowL].Cells[i].Value.ToString();
}
//Base
rowstr[19] = "[Base+]";
rowstr[20] = targetitow.ToString();//itowB代入
rowstr[26] = "[Rover+]";
rowstr[27] = targetitow.ToString();//itowR代入
double ratio = (double)(targetitow % itow_div )/ itow_div;
for (i = 1; i < dcolN; i++)
{
//Base補間して代入
d1 = Convert.ToDouble(dgv1.Rows[rowL].Cells[dcolB[i]].Value);
d2 = Convert.ToDouble(dgv1.Rows[rowL + 1].Cells[dcolB[i]].Value);
d0 = d1 + (d2 - d1) * ratio;
rowstr[20 + i] = d0.ToString();
// Debug.Print("Base_itpcal:titow" + targetitow.ToString() + "rowL=" + rowL.ToString() + "i=" + i.ToString() + "d1=" + d1.ToString() + "d2=" + d2.ToString()+"d0="+d0.ToString()+"ratio="+ratio.ToString());
//Rover補間して代入
d1 = Convert.ToDouble(dgv1.Rows[rowL].Cells[dcolR[i]].Value);
d2 = Convert.ToDouble(dgv1.Rows[rowL + 1].Cells[dcolR[i]].Value);
d0 = d1 + (d2 - d1) * ratio;
rowstr[27 + i] = d0.ToString();
}//colN Loop
//BNO 代入
for (j = 0; j < dBNc; j++)
{
rowstr[34 + j] = dgv1.Rows[ri_tcount].Cells[dBNO[targetNo, j]].Value.ToString();
//Debug.Print("rowstr[34+" + j.ToString() + "]=" + rowstr[34 + j].ToString());
}
dgv3.Rows.Add(rowstr);
}
//ITP計算する条件を決めてitpcalで補間計算して、dgv3に代入して、補間表作成
//target_itowだけ先に作ってからRTK行を挿入して、最後にMAG行挿入
private void ITP_set()
{
dgv3.ColumnCount = 100;
dgv3.Visible = true;
//target_itowがitowdiv範囲に何個はいっているか検索
int ri, i, j,m;
int itow_div=120;
int itowLrow=0;//Base itow row番号
int targetNo=0;//div内の個数
target_itow = new long[sizei*3];//BN0,BN1,BN2のitow
mag_itow = new long[sizei];
double[] ratio = new double[3];
long[] itowall;//基本itow0の間にBNOとMAGのitowを追加した全配列 itowf[ri]
long[] itowBN = new long[3 * (sizei + 1)];
int[] BNOcolN = new int[] { 45, 57, 69 };
int[] ri_tcount = new int[sizei*3];//BNO　　行位置
int[] ri_mcount=new int[sizei*2]; //mag 行位置
//target_itow 配列作成　BNOのitowを全部配列へ格納する
for (ri = 0; ri < sizei; ri++)
{
//BNO　itowを配列格納
target_itow[tcount] = Convert.ToInt64(dgv1.Rows[ri].Cells[45].Value);
Debug.Print("ITPSET:target_itow[" + tcount.ToString() + "]=" + target_itow[tcount].ToString());
ri_tcount[tcount] = ri;
tcount++;
target_itow[tcount] = Convert.ToInt64(dgv1.Rows[ri].Cells[57].Value);
Debug.Print("ITPSET:target_itow[" + (tcount).ToString() + "]=" + target_itow[tcount].ToString());
ri_tcount[tcount] = ri;
tcount++;
target_itow[tcount] = Convert.ToInt64(dgv1.Rows[ri].Cells[69].Value);
Debug.Print("ITPSET:target_itow[" + (tcount ).ToString() + "]=" + target_itow[tcount].ToString());
ri_tcount[tcount] = ri;
tcount++;
//Mag itowを配列mag_itow[]格納
//Debug.Print("mag cell=dgv1.Rows["+ri.ToString() + "].Cells[71].Value.ToString()= " + dgv1.Rows[ri].Cells[71].Value.ToString());
if (Convert.ToInt64(dgv1.Rows[ri].Cells[71].Value) > 0)
{
mag_itow[mcount] = Convert.ToInt64(dgv1.Rows[ri].Cells[71].Value);
Debug.Print("ITPSET:mag_itow[" + (mcount).ToString() + "]=" + mag_itow[mcount].ToString());
ri_mcount[mcount] = ri;
mcount ++;
}
// Debug.Print("mag cell=dgv1.Rows["+ri.ToString() + "].Cells[72].Value.ToString()= " + dgv1.Rows[ri].Cells[72].Value.ToString());
if (Convert.ToInt64(dgv1.Rows[ri].Cells[72].Value) >0)
{
mag_itow[mcount] = Convert.ToInt64(dgv1.Rows[ri].Cells[72].Value);
Debug.Print("ITPSET:mag_itow[" + (mcount).ToString() + "]=" + mag_itow[mcount].ToString());
ri_mcount[mcount] = ri;
mcount ++ ;
}
}// BNo MAG itow　カウント
sizei3 = tcount;//dgv3の行数
int tarcounter = 0;
//BNOがRTK周期にはいっている個数
for(m=0;m<tcount-3;m++)
{
itpcal(target_itow[m],tarcounter%3 , ri_tcount[m]);//補間計算してdgv3代入
tarcounter++;
//Base itow範囲の検索
long itow_L = target_itow[0] - (target_itow[0] % itow_div);//target_itowの存在範囲の開始itow
long itow_U = itow_L + itow_div;
for(i=0;i<sizei;i++)
{
if (itow_L == Convert.ToInt64(dgv1.Rows[i].Cells[20].Value))
{
itowLrow = i;
i = sizei;
}
}
}//tcount loop end
//----------------BNO yaw　Add処理----------------------------
double yawadd = 0;
double[] yawarry = new double[sizei3+1];
for (i = 1; i < sizei3-4; i++)
{
//---mHead Add
double yaw = Convert.ToDouble(dgv3.Rows[i].Cells[36].Value);
double yaw_1 = Convert.ToDouble(dgv3.Rows[i - 1].Cells[36].Value);
if (yaw - yaw_1 < -60)
{
yawadd = yawadd + 360;
}
yawarry[i] = yaw + yawadd;
}
for (i = 0; i < sizei3-4; i++)
{
dgv3.Rows[i].Cells[36].Value = yawarry[i].ToString();
}
}
private void button6_Click(object sender, EventArgs e)
{
//string[,] magrowstr = new string[sizei * 2, 90];//mag発生行の補間データ保存配列
int i;
//for (i = 0; i < mcount; i++)
//{
magcal(mag_itow[0], 0);
// }
}
//*******************************************************************************************************************
private void magcal(long mitow, int magcount)//mag_itow値でmag挿入して補間計算
{
string[,] strarry = new string[100, 100];
int i,k;//magcount counter
for (k = 0; k < mcount; k++)
{
mitow = mag_itow[k];
magrows = new kaiten[sizei * 2];//kaiten構造体magrows　初期化
for (i = 0; i < sizei * 2; i++)//構造体メンバー配列初期化
{
magrows[i].magrowstr = new string[110];
}
int ri3;
int mag_row = 0;
for (ri3 = 0; ri3 < sizei3-2; ri3++)
{
//Debug.Print("MAGCALitow:ri3="+ri3.ToString()+"mitow="+mitow.ToString()+"itowB=" + dgv3.Rows[ri3].Cells[20].Value.ToString());
// if ((mitow - Convert.ToInt64(dgv3.Rows[ri3].Cells[20].Value)) < 45 && (mitow - Convert.ToInt64(dgv3.Rows[ri3].Cells[20].Value)) >= 0)
if (mitow - target_itow[ri3] < 45 && (mitow - target_itow[ri3] >= 0))
{
//dgv3.Rows.Insert(ri3+1);//dgve3挿入
mag_row = ri3 + 1;
dgv3.Rows[ri3 + 1].Cells[20].Value = mag_itow.ToString();
//Debug.Print("MAGCAL:ri3=" + ri3.ToString() + "mag_itow=" + mag_itow.ToString());
ri3 = sizei3;//finished
}
}
int mag_rowL = mag_row - 1;
int mag_rowU = mag_row + 1;
Debug.Print("mag_rowL=" + mag_rowL.ToString() + "mag_rowU=" + mag_rowU.ToString() + "itowL=" + dgv3.Rows[mag_rowL].Cells[20].Value.ToString() + "itowU" + dgv3.Rows[mag_rowU].Cells[20].Value.ToString());
long itowL = target_itow[mag_rowL];// Convert.ToInt64(dgv3.Rows[mag_rowL].Cells[20].Value);
long itowU = target_itow[mag_rowU];//Convert.ToInt64(dgv3.Rows[mag_rowU].Cells[20].Value);
string[] rowstr = new string[100];
for (i = 0; i < 19; i++)//[m]列0-19をrostr配列に収納
{
Debug.Print("MAGCAL:mag_rowL=" + mag_rowL.ToString() + " dgv3.Rows[mag_rowL].Cells[" + i.ToString() + "].Value=" + dgv3.Rows[mag_rowL].Cells[i].Value.ToString());
rowstr[i] = dgv3.Rows[mag_rowL].Cells[i].Value.ToString();
}
//Base
rowstr[19] = "[Base+mag]";
rowstr[20] = mitow.ToString();//itowB代入
rowstr[26] = "[Rover+mag]";
rowstr[27] = mitow.ToString();//itowR代入
rowstr[35] = "[BNOmag]";
rowstr[45] = mitow.ToString();//BNO_itow代入
// double ratio = (double)(targetitow % itow_div) / itow_div;
// MAG補間計算
int[] dcolB = { 20, 21, 22, 23, 24, 25 };//[Base] [Rover]の補間値の列番号配列
int[] dcolR = { 27, 28, 29, 30, 31, 32 };//[Base] [Rover]の補間値の列番号配列
int dcolN = 6;
int itow_div = 120;
int dBNc = 12;
int[] dBNO = { 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45 };//[BN0]列番号配列
double d0, d1, d2;//d0 補間値　
double ratio = (double)(mitow - itowL) / (itowU - itowL);
for (i = 1; i < dcolN; i++)//Base Rover　補間　６列分
{
//Base補間して代入
d1 = Convert.ToDouble(dgv3.Rows[mag_rowL].Cells[dcolB[i]].Value);
d2 = Convert.ToDouble(dgv3.Rows[mag_rowU].Cells[dcolB[i]].Value);
d0 = d1 + (d2 - d1) * ratio;
rowstr[20 + i] = d0.ToString();
// Debug.Print("Base_itpcal:titow" + targetitow.ToString() + "rowL=" + rowL.ToString() + "i=" + i.ToString() + "d1=" + d1.ToString() + "d2=" + d2.ToString()+"d0="+d0.ToString()+"ratio="+ratio.ToString());
//Rover補間して代入
d1 = Convert.ToDouble(dgv3.Rows[mag_rowL].Cells[dcolR[i]].Value);
d2 = Convert.ToDouble(dgv3.Rows[mag_rowU].Cells[dcolR[i]].Value);
d0 = d1 + (d2 - d1) * ratio;
rowstr[27 + i] = d0.ToString();
}//colN Loop
for (i = 1; i < dBNc; i++)
{
//BNO補間して代入
d1 = Convert.ToDouble(dgv3.Rows[mag_rowL].Cells[dBNO[i]].Value);
d2 = Convert.ToDouble(dgv3.Rows[mag_rowU].Cells[dBNO[i]].Value);
d0 = d1 + (d2 - d1) * ratio;
rowstr[34 + i] = d0.ToString();
}
// dgv3.Rows.Add(rowstr);
for (i = 0; i < 46; i++)
{
//magrows[k].magrowstr[i] = rowstr[i];
strarry[k, i] = rowstr[i];
Debug.Print("MAGCOUNT:<magcoutn=" + k.ToString() + ">rowstr[" + i.ToString() + "]=" + rowstr[i] + "[struct kaiten]:magrows[" + magcount.ToString() + ".magrowstr[" + i.ToString() + "]=" + magrows[k].magrowstr[i]);
}
magrows[0].allNo = magcount;//magrows配列のサイズをmagrows[0].allNoに格納
}//k end
//-------------magrows構造体が完成したので、CSVファイル出力する------
int j;
//dgv1 FileName date 取り出し
string dgv1fname = textBox2.Text;//C:\RTK_Log\Kai_20230522_122510.txt
int fdate = dgv1fname.IndexOf(".txt");//30
//int fdate = dgv1fname.IndexOf("_ubx");//右端から数えて
int startsu = fdate - 11;//Kai_20230522_122510 30-11=19
string fn = dgv1fname.Substring(11, startsu);
//Filename Timestamp
// DateTime dt = DateTime.Now;
// string fn = dt.ToString($"{dt:MMddHHmmss}");
string fnstrng = "c:/RTK_Log/mag_" + 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/head3mag_" + fn + ".csv";
int dgvron = mcount;// mag数
int dgvcln = 46;// dgv3.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 + magrows[i].magrowstr[j] + ",";
dgvrow = dgvrow + strarry[i,j] + ",";
}
sw.WriteLine(dgvrow);
}
}
}
private void MAG_timing()
{
//dgv1 行サイズ=sizei 列サイズ=sizec
int i;
//itow列：itowB<20>,itowBN0<45>,itowBN1<57>,itowBN2<69>,MAG0<71>,MAG1<72>でitowBN0基準で補間
//data列：
//ITP_search(long target_itow,int itow_div,int data_col, ref double d0,int itow_col,int start_row,int end_row)
int targetNo = 0;
int row, rowL, rowU;//target_itowをはさむ正規itowの行番号上下
}
//[-----------------------------------------------------------------------------------------------------------------------]
//-----------------Read File ----------------------------------------------------
private void ReadDialog()
{
dgv1.Visible = true;
pictureBox2.Visible = false;
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)
{
pictureBox2.Visible = false;
hScrollBar2.Visible = false;
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;
pictureBox2.Visible = false;
hScrollBar2.Visible = false;
dgv1.Visible = true;
dgv2.Visible = true;
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;
}
//&&&&&&&&&&&&&&&&&&Kaiten File をDGV１へ書き込み&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
if (checkBox15.Checked == true)
{
dgv1.DefaultCellStyle.NullValue = "-";
dgv1.Visible = true;
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 = { };
/* Kaiten Data Header
string head0 = "md, flagsB, flagsR, pdopB, pdopR, haccB, haccR, miscount0, misflags,=,TimePulse,=,TimePulse0,=,itow0,=,bnoN,=,BNOst";
string head1 = ",[Base],itowB, NYB, EXB, DZB, gSpeedB, headMotB";
string head2 = ",[Rover] ,itowR,NYR,EXR,DZR,gSpeedR,mHead,relLm";
string head3 = ",BNO],yaw1,pitch1,roll1,laX,laY,laZ,grX,grY,grZ";
string head4 = ",turnN,BNOsa,BNHosei,Drel";
*/
/*
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);//各行の長さチェックして修正
}
*/
swtch.Stop();
//dgv1 print
dgv1.Visible = true;
dgv1.RowHeadersWidth = 60;
string ss = "0";
int cr = 0;
string datas0 = "";
string datas1 = "";
string datas2 = "";
//4行データを１行にまとめる
//[mode]最後は無し、[Base]最後に","2C 、[Rover]最後無し、[BNO]最後"LF" 0A がついてる
string dum = "";
for (ic = 1; ic < (int)(sline.Length - 5); 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);truct dg3
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);
*/
dgv1.ColumnCount = 90;
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 - 1);//[BNO]の最後の"LF"削除
sline[ic + 3] = sline[ic + 3].Substring(0, sline[ic + 3].Length - 1);//[BNO]の最後の"LF"削除
sline[ic + 4] = sline[ic + 4].Substring(0, sline[ic + 4].Length - 1);//[BNO]の最後の"LF"削除
sline[ic + 5] = sline[ic + 5].Substring(0, sline[ic + 5].Length - 1);//[Mag]の最後の"LF"削除
//mag 　ゼロ追加
//if (sline[ic+5].Length<=5)
// {
sline[ic + 5] = sline[ic + 5] + "0,0,";
//}
ss = sline[ic - 1] + "," + sline[ic] + sline[ic + 1] + "," + sline[ic + 2] + "," + sline[ic +3] + "," + sline[ic + 4] + "," + sline[ic + 5] ;
string[] datas = ss.Split(','); // カンマで区切って配列に格
dgv1.Rows.Add(datas);//dgv1書き込み
sizec = datas.Length;
//label8.Text = label8.Text +"Col="+ sizec.ToString();
//Debug.Print("datas[0]=" + datas[0],"datas1="+datas1);
datas0 = datas[0];
if (datas[0] == "Rover]")
{
dgv1.Rows.Add(datas);//dgv1書き込み
cr++;
}
if (datas1 == "s]" && datas0 == "m]")
{
dgv1.Rows.Add(datas);//dgv1書き込み
cr++;
//Debug.Print("[m] cr=" + cr.ToString());
}
sizei ++;
}
}
}
rowheader(0);
//label8.Text = sizes +"Col="+ sizec.ToString(); ;
swtch.Stop();
// TimeSpan tst = swtch.ElapsedMilliseconds;
sizes ="RowNo=" +sizei.ToString()+"ColNo="+sizec.ToString()+"FileSize="+ filesize.Length.ToString() + " Ellapsed Time=" + swtch.ElapsedMilliseconds.ToString() + "msec";
label8.Text = sizes;
}// Kaiten data dgv1
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% MONITOR用ASCIIファイル読み込み%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//else// Aveファイルでない場合
// {
if (checkBox2.Checked == false && textBox2.Text.IndexOf("dgv") < 0 && textBox2.Text.IndexOf("dgv") < 0 && checkBox15.Checked==false)
{
dgv1.Visible = true;
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行データを１行にまとめる
//[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);truct dg3
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;
}
}
//dgv３へ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 < 26; 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;
//dgv1 FileName date 取り出し
string dgv1fname = textBox1.Text;
//int fdate=dgv1fname.IndexOf(".ubx");
int fdate = dgv1fname.IndexOf("_ubx");//右端から数えて
int startsu = fdate+5;//_ubx\xXXXXXXXXX.csv"
int mojisu = dgv1fname.Length - startsu;
string fn= dgv1fname. Substring(startsu,mojisu);
//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を１８０度回転補正
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***********************************************************************************************
//処理１：nullセルは０代入
//処理２：文字が数値でないセルは、前の行データ代入
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());
//非数値異常値は、１個上のデータで置き換え
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++;//１個飛ばし
}
}
}
}
//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を小数点５桁に変換
{
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;
}
}
private void mathdgv3(int col,int off, 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;
//13-18 21-27
if (col >= 7+off && col < 13+off) { col = 13+off; }
if (col == 19+off)
{
col = 20+off;
}
if (col == 28+off) { col = 29+off; }
int rowsu = ig3(0, 0);
for (im = 1; im < rowsu - 1; im++)
{
//Debug.Print("im=" + im.ToString() + "col=" + col.ToString());
double val = Convert.ToDouble(dgv3.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に終了ターン番号
//２万ドットを超えた場合は、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);//２万ドットの行数デフォルト2000行xdot=10
//２万ドットでおさまる最大ターン番号を求める
if ((endrowN - startrowN) * tR[0].xdot < 20000)//指定範囲がソースBITMAPサイズに収まているか判定
{
label6.Text = "OK prepare to SourceBitmap ";
}
else//２万ドットより小さい場合
{
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";
}
}
//**********************************************************************************************************************
//************************************dgv3から全データを読み取って時系列graphのソースBITMAPを作成************************:
private void makeAllTurnBitmap(int xdot)
{
int startrowN=dg3s.tstartrow;//全ターンデータの先頭行
int endrowN=dg3s.tendrow;//全ターンデータの最終行
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長２万ドット超えないようにする
//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];
}
// }
}
}
//--------------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長２万ドット超えないようにする
//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　左下の１行上
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];////右スキーのターン構造体配列宣言２００個まで収納
Turns[] tL = new Turns[1000];////左スキーのターン構造体配列宣言２００個まで収納
*/
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++)//５回の移動平均で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);
//*******************************************ターン判定**********************************************************
//　微分正負変化点&&ターン長７データ&&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
tR[turnCountR].NY0 = Convert.ToDouble(dgv1.Rows[turnNoR[nuNR]].Cells[14].Value);//Start NY
tR[turnCountR].EX0 = Convert.ToDouble(dgv1.Rows[turnNoR[nuNR]].Cells[15].Value);//Start EX
tR[turnCountR].DZ0 = Convert.ToDouble(dgv1.Rows[turnNoR[nuNR]].Cells[16].Value);//Start DZ
//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() + "Direction=" + tR[tcr].direction.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() + ",direction=," + tR[tcr].direction + ",NY0=," + tR[tcr].NY0.ToString()+ ",EX0=," + tR[tcr].EX0.ToString() + ",DZ0=," + tR[tcr].DZ0.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);//last２行にヘッダーと番号追加
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行目正常個数　２行目異常個数　　３行目％　４行目補助１　５行目補助２
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の１１行したから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)//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のターンテーブル先頭行はデータ最終行＋２行
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
/*
int directionIndex = 0;
string RLdirection = dgv3.Rows[R0turn_row].Cells[19].Value.ToString();
if (RLdirection == "Rturn")
{
directionIndex = 0;//turn番号が2で割って0の時は右ターン左谷足
}
else
{
directionIndex = 1;//turn番号が2で割って1の時は左ターン右谷足
}
*/
for (it=0;it<R0turn_endrow;it++)//row番号からターン番号取得してrow2turnN[行番号]
{
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]開始点ｙ座標
ex0 = dg3(R0turn_startrow, 15);//baseTurn[0]開始点x座標
ny0m = dg3(R0turn_startrow, 21);//RoverTurn[0]開始点ｙ座標
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[]の８個代入
{
//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;
hScrollBar2.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;
//turn谷足をBOLDにする処理
//dgv3 turnTableからdirectionを読み込んで、ターン番号の奇数偶数でターンdirectionを決定する
int currentRowN = i + R0turn_startrow;
int currentturnNo = row2turnN[i];
string RLdirections = dgv3.Rows[R0turn_row+currentturnNo].Cells[19].Value.ToString();
textBox12.Text = currentturnNo.ToString();
textBox13.Text = RLdirections;
Debug.Print("turnNo=" + currentturnNo.ToString() + RLdirections);
if (RLdirections=="Lturn")//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);
}
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;
}
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;
//左端座標を求める
int R0turn_row = ig3(0, 0) + 2;//dgv3のターンテーブル先頭行はデータ最終行＋２行
int R0turn_startrow = ig3(R0turn_row +0, 3);
//int currentRowN = rowN + R0turn_startrow;
int currentturnNo = row2turnN[rowN];
string RLdirections = dgv3.Rows[R0turn_row + currentturnNo].Cells[19].Value.ToString();
textBox12.Text = currentturnNo.ToString();
textBox13.Text = RLdirections;
Debug.Print("turnNo=" + currentturnNo.ToString() + RLdirections);
//センター座標centerXからRectangleの左開始X座標求める
if (centerX > pictureBox2.Width / 2)
{
startX = centerX - pictureBox2.Width / 2;
}
else
{
startX = centerX - pictureBox2.Width / 2;
}
startY =(int)( centerY - pictureBox2.Height / 2);
//}
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)
{
int tnrl = row2turnN[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 hmotL = Convert.ToDouble(dgv3.Rows[rncx].Cells[68].Value);
double mheadL = Convert.ToDouble(dgv3.Rows[rncx].Cells[75].Value);
double slip = hmot - mhead;
double slipL = hmotL - mheadL;
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 pitL= Convert.ToDouble(dgv3.Rows[rncx].Cells[80].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 flagsR = Convert.ToInt32(dgv3.Rows[rncx].Cells[1].Value);
// int flagsL = Convert.ToInt32(dgv3.Rows[rncx].Cells[51].Value);
string flagsR = "";
string flagsL ="";
// Debug.Print("flagsR=" + dgv3.Rows[rncx].Cells[1].Value.ToString());
if (dgv3.Rows[rncx].Cells[1].Value != null || dgv3.Rows[rncx].Cells[1].Value !=DBNull.Value)
{
Debug.Print("flagsR=" + dgv3.Rows[rncx].Cells[1].Value.ToString());
flagsR = dgv3.Rows[rncx].Cells[1].Value.ToString();
flagsL = dgv3.Rows[rncx].Cells[51].Value.ToString();
}
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 yaw0L= Convert.ToDouble(dgv3.Rows[rncx].Cells[79].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 = 75;
RectangleF rectall = new RectangleF(0, 0, 1000, 20);//行全体
RectangleF rect = new RectangleF(500, 470, 500, 20);//行番号をセンターライン下に表示
RectangleF rect0 = new RectangleF(0, 0, 120, 20);//hMot位置
RectangleF rect1 = new RectangleF(120, 0, 140, 20);//slip位置
RectangleF rect2 = new RectangleF(240, 0, 160, 20);//mhead位置
RectangleF rect3 = new RectangleF(380 , 0, 100, 20);//gspd位置
RectangleF rect4 = new RectangleF(460, 0, 140, 20);//pitch位置
RectangleF rect5 = new RectangleF(560 , 0,160, 20);//roll*Drel位置
RectangleF rect6 = new RectangleF(710, 0, 140, 20);//yaw0位置
RectangleF rect7 = new RectangleF(840, 0, 160, 20);//flags位置
RectangleF rect8 = new RectangleF(haba * 7, 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　左下の１行上
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")+"*"+hmotL.ToString("F0");
string strmhead = "mhed=" + mhead.ToString("F0")+"*"+mheadL.ToString("F0");
string strslip = "slip=" + slip.ToString("F0")+ "*" + slipL.ToString("F0");
string strgspd = gspd.ToString("F1")+"kmh";
string strpit = "pit=" + pit.ToString("F0")+"*"+pitL.ToString("F0");
string stryw = "yaw=" + yaw0.ToString("F0")+"*"+yaw0L.ToString("F0");
string strdrel = "Drel=" + Drel.ToString("F1");
string strdzb = "dzb =" + dzb.ToString("F0");
string strlens = "lens =" + lens.ToString("F0");
string strflags = "flags=" + flagsR + "*" + flagsL;
//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")+"Drel="+Drel.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(strmhead, fnt, Brushes.Blue, rect2);
g2.DrawString(strgspd, fnt, Brushes.White, rect3);
g2.DrawString(strpit, fnt, Brushes.Yellow, rect4);
//g2.DrawString(strywH, fnt, Brushes.Aqua, rect5);
// g2.DrawString(strdrel, fnt, Brushes.BlanchedAlmond, rect6);
g2.DrawString(strroll, fnt, Brushes.DeepPink, rect5);
g2.DrawString(stryw, fnt, Brushes.White, rect6);
g2.DrawString(strflags, fnt, Brushes.White, rect7);
// g2.DrawString(strflags, fnt, Brushes.White, rect9);
//g2.DrawString(strbno, fnt, Brushes.White, rect10);
}
private void dgv3_DataError(object sender,DataGridViewDataErrorEventArgs e)
{
if (e.Exception != null)
{
MessageBox.Show(this,
string.Format("({0}, {1}) のセルでエラーが発生しました。\n\n説明: {2}",
e.ColumnIndex, e.RowIndex, e.Exception.Message),
"エラーが発生しました",
MessageBoxButtons.OK, MessageBoxIcon.Error);
}
}
//プロット前にグラフサイズを確認調整する。
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
{
if (pictureBox2.Visible == true)
{
pictureBox2.Visible = false;
hScrollBar2.Visible = false;
dgv3.Visible = true;
}
else
{
pictureBox2.Visible = true;
hScrollBar2.Visible = true;
dgv3.Visible = false;
}
}
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++;
}
}
/*
private void button4_Click(object sender, EventArgs e)
{
dgv3structMake();
}
*/
private void dgv3structMake()
{
dg3s.tnstN2rN = new int[500];
dg3s.row2tN = new int[50000];
//======================================================================================
//==========================struct dgv3strct 作成=======================================
dg3s.ttstartrow = ig3(0, 0) + 2;
int tablelastNo = 0;
//----Turn番号 ROW番号　関連付け配列--------------------------
int it = 0;
while (dgv3.Rows[it + dg3s.ttstartrow].Cells[3].Value != null)
{
//tN0rN[]作成
Debug.Print("dg3s.tnstN2rN["+ti(it)+"]="+ti(dg3s.tnstN2rN[it]));
dg3s.tnstN2rN[it] = ig3(it + dg3s.ttstartrow, 3);//turn番号itの開始行番号
it++;
}
//------------------------------------------------------------
tablelastNo = it - 1;
dg3s.ttlastNo = tablelastNo;
int tn0 = 0;
int tn1 = tablelastNo;
// rturnitow
int R0turn_row = ig3(0, 0) + 2;//dgv3のターンテーブル先頭行はデータ最終行＋２行
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
int startrow = 0;
int endrow = 0;
for (it = 0; it < R0turn_endrow; it++)//row番号からターン番号取得してrow2turnN[行番号]
{
//row2turnN[it] = tc;
dg3s.row2tN[it] = tc;
// Debug.Print("[dgv3READ]:dg3s.row2tN[" + ti(it) + "]=" + ti(dg3s.row2tN[it]));
startrow = ig3(R0turn_row + tc, 3);
endrow = ig3(R0turn_row + tc, 5);
if (it >= endrow)
{
tc++;
}
}
dg3s.tstartrow = dg3s.tnstN2rN[0];
dg3s.tendrow = endrow-1;
double maxv = 0;
double minv = 0;
double avev = 0;
int coli = 0;
int off = 0;
//13-18 21-17 29,30,31 63-68 71-77 79,80,81
for (coli = 1; coli < 42; coli++)
{
mathdgv3(coli,off, 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();
}
//======================================================================================
}
//\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
//\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\基準ファイル　読み込み\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
/// </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)
{
}
private void textBox12_TextChanged(object sender, EventArgs e)
{
}
private void checkBox12_CheckedChanged(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
private void dgv3saveMag()
{
int i, j;
//dgv1 FileName date 取り出し
string dgv1fname = textBox2.Text;//C:\RTK_Log\Kai_20230522_122510.txt
int fdate=dgv1fname.IndexOf(".txt");//30
//int fdate = dgv1fname.IndexOf("_ubx");//右端から数えて
int startsu = fdate-11;//Kai_20230522_122510 30-11=19
string fn = dgv1fname.Substring(11, startsu);
//Filename Timestamp
// DateTime dt = DateTime.Now;
// string fn = dt.ToString($"{dt:MMddHHmmss}");
string fnstrng = "c:/RTK_Log/dgv3mag_" + 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/head3mag_" + 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-1; 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++;
}
}
*/
}
}//FORM END
}//NAME SPACE END
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment