示例#1
0
文件: Form1.cs 项目: xuanmung/mGCS
        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]);
        }
示例#2
0
 //Constructor
 public FtConversion(LowPassFilter lpf)
 {
     this.mLpf = lpf;
 }