private void Form1_Load(object sender, EventArgs e)
{
mInputHelper = new UserInputHelper();
mMatrixHelper = new MatrixHelper();
//Convert lon and lat from [deg] to [rad]
initLonRad = mMatrixHelper.deg2rad(initLon);
initLatRad = mMatrixHelper.deg2rad(initLat);
//Calculate rotation matrix from ECEF to NED
mRotationMatrixEcef2Ned = mMatrixHelper.MatrixCreate(3, 3);
mRotationMatrixEcef2Ned[0][0] = -Math.Sin(initLatRad) * Math.Cos(initLonRad);
mRotationMatrixEcef2Ned[0][1] = -Math.Sin(initLatRad) * Math.Sin(initLonRad);
mRotationMatrixEcef2Ned[0][2] = Math.Cos(initLatRad);
mRotationMatrixEcef2Ned[1][0] = -Math.Sin(initLonRad);
mRotationMatrixEcef2Ned[1][1] = Math.Cos(initLonRad);
mRotationMatrixEcef2Ned[1][2] = 0;
mRotationMatrixEcef2Ned[2][0] = -Math.Cos(initLatRad) * Math.Cos(initLonRad);
mRotationMatrixEcef2Ned[2][1] = -Math.Cos(initLatRad) * Math.Sin(initLonRad);
mRotationMatrixEcef2Ned[2][2] = -Math.Sin(initLatRad);
//mRotationMatrixEcef2Ned[0][0] = -Math.Sin(initLonRad);
//mRotationMatrixEcef2Ned[0][1] = Math.Cos(initLonRad);
//mRotationMatrixEcef2Ned[0][2] = 0;
//mRotationMatrixEcef2Ned[1][0] = -Math.Sin(initLatRad) * Math.Cos(initLonRad);
//mRotationMatrixEcef2Ned[1][1] = -Math.Sin(initLatRad) * Math.Sin(initLonRad);
//mRotationMatrixEcef2Ned[1][2] = Math.Cos(initLatRad);
//mRotationMatrixEcef2Ned[2][0] = Math.Cos(initLatRad) * Math.Cos(initLonRad);
//mRotationMatrixEcef2Ned[2][1] = Math.Cos(initLatRad) * Math.Sin(initLonRad);
//mRotationMatrixEcef2Ned[2][2] = Math.Sin(initLatRad);
//Calculate rotation matrix from NED to ECEF
mRotationMatrixNed2Ecef = mMatrixHelper.MatrixCreate(3, 3);
try
{
mRotationMatrixNed2Ecef = mMatrixHelper.MatrixInverse(mRotationMatrixEcef2Ned);
}
catch
{
MessageBox.Show("Coordinate transform failed");
}
//Declare the ECEF reference postion
posEcefRef = mMatrixHelper.MatrixCreate(3, 1);
posEcefRef[0][0] = initEcefX;
posEcefRef[1][0] = initEcefY;
posEcefRef[2][0] = initEcefZ;
//Init the ECEF reference postion
posEcef = mMatrixHelper.MatrixCreate(3, 1);
posNed = mMatrixHelper.MatrixCreate(3, 1);
//Inquiry parameters from saved internal file (if any)
try
{
if (File.Exists(mParamFileName))
{
StreamReader streamRdr = new StreamReader(mParamFileName);
ftComPort = streamRdr.ReadLine();
ftBaudRate = Convert.ToInt32(streamRdr.ReadLine());
vclComPort = streamRdr.ReadLine();
vclBaudRate = Convert.ToInt32(streamRdr.ReadLine());
gpsSimIp = streamRdr.ReadLine();
gpsSimPort = Convert.ToInt32(streamRdr.ReadLine());
fsIp = streamRdr.ReadLine();
mVehicleMass = Convert.ToDouble(streamRdr.ReadLine());
drag[0] = Convert.ToDouble(streamRdr.ReadLine());
drag[1] = Convert.ToDouble(streamRdr.ReadLine());
forceBias[0] = Convert.ToDouble(streamRdr.ReadLine());
forceBias[1] = Convert.ToDouble(streamRdr.ReadLine());
forceBias[2] = Convert.ToDouble(streamRdr.ReadLine());
streamRdr.Close();
cbxFtComPort.Text = ftComPort;
cbxFtBaudRate.Text = ftBaudRate.ToString();
cbxVclComPort.Text = vclComPort;
cbxVclBaudRate.Text = vclBaudRate.ToString();
tbxGpsSimIP.Text = gpsSimIp;
tbxGpsSimPort.Text = gpsSimPort.ToString();
tbxFsIp.Text = fsIp;
lblMass.Text = mVehicleMass.ToString();
lblDragX.Text = drag[0].ToString();
lblDragY.Text = drag[1].ToString();
tbxDragX.Text = drag[0].ToString();
tbxDragY.Text = drag[1].ToString();
lblFx0.Text = forceBias[0].ToString();
lblFy0.Text = forceBias[1].ToString();
lblFz0.Text = forceBias[2].ToString();
}
else
{
//gpsSimIp = tbxGpsSimIP.Text;
//gpsSimPort = Convert.ToInt32(tbxGpsSimPort.Text);
//fsIp = tbxFsIp.Text;
}
}
catch (Exception x)
{ }
pbarFzCalib.Visible = false;
pbarFxyCalib.Visible = false;
btnRefreshAll.Visible = false;
pnlFtSetting.Visible = false;
mTimer = new System.Windows.Forms.Timer();
mTimer.Interval = 95; //100 mili-seconds
mTimer.Tick += new System.EventHandler(mTimer_Tick);
//mTimer.Start();
mTimePoint = 0;
mTimePoint0 = 0;
sendingStep = 0.0;
mGpsTimePoint = 0;
//Initiate Lowpass filter
mLpf = new LowPassFilter(2.0, mFtSamplingTime);
//Assign position map
mPosMap = new PositionMap(chartPosSim);
mPosMap.setTitle("Vehicle's Position");
//chartPosSim.Titles.Add("Vehicle's Position");
updatePosMapThread = new Thread(new ThreadStart(updatePosMap));
//mPsner = new PseudoPositioning(0.3, 0.28);
mPsner = new PseudoPositioning(drag[0], drag[1]);
}
//Constructor
public FtConversion(LowPassFilter lpf)
{
this.mLpf = lpf;
}
