예제 #1
0
        public XYZCoord GetRinexSVPos(string filename, int svPRN, double TOW, out double ionoError, out double svClkBias, out XYZCoord svVel)
        {
            XYZCoord coord = new XYZCoord();

            if (this.m_RinexFile.GetNumEphemerides() == 0)
            {
                this.m_RinexFile.Read(@"..\scripts\TunnelsMeasAndNav\brdc2140.07n");
            }
            RinexEph eph = this.m_RinexFile.SearchRinexArrayList((byte)svPRN, (int)TOW);

            if (eph == null)
            {
                ionoError = 0.0;
                svClkBias = 0.0;
                svVel.X   = svVel.Y = svVel.Z = 0.0;
                return(coord);
            }
            tSVD_SVState st    = new tSVD_SVState();
            tGPSTime     gTime = new tGPSTime();

            gTime.isTOWValid  = true;
            gTime.isWeekValid = true;
            gTime.time        = TOW;
            gTime.week        = (int)eph.weekNo;
            new ComputeSVPos().computeSVState(st, (short)svPRN, eph, gTime);
            coord.X   = st.pos[0];
            coord.Y   = st.pos[1];
            coord.Z   = st.pos[2];
            svVel.X   = st.vel[0];
            svVel.Y   = st.vel[1];
            svVel.Z   = st.vel[2];
            ionoError = st.ionoDelayStd;
            svClkBias = st.clockBias;
            return(coord);
        }
예제 #2
0
        public void computeSVState(tSVD_SVState st, short svid, RinexEph eph, tGPSTime gTime)
        {
            double num50 = 0.0;
            double ntcld = 0.0;
            double num52 = 0.0;
            double num53 = 0.0;
            double num54 = 0.0;
            double num55 = 0.0;
            double num56 = 0.0;
            tSVD_EphemerisConsts constptr = new tSVD_EphemerisConsts();

            st.svid = (byte)svid;
            double num59 = ((SECONDS_IN_WEEK * (gTime.week - eph.weekNo)) + gTime.time) - eph.toe;

            if ((eph.status & EPHSTATUS_EPH_AVAIL) != 0)
            {
                if (eph.age == 0)
                {
                    st.dataAvail = 1;
                }
                else if ((eph.age & EPH_AGE_CGEE_INDICATOR) != 0)
                {
                    st.dataAvail = 6;
                }
                else
                {
                    st.dataAvail = 5;
                }
            }
            else if ((eph.status & EPHSTATUS_ALM_AVAIL) != 0)
            {
                if ((num59 < 302400.0) && (num59 > -302400.0))
                {
                    st.dataAvail = 4;
                }
                else
                {
                    st.dataAvail = 2;
                }
            }
            else
            {
                st.dataAvail = 0;
                return;
            }
            this.setEphemerisConstants(ref constptr, svid, ref eph, ref gTime);
            num59 = ((SECONDS_IN_WEEK * (gTime.week - eph.weekNo)) + gTime.time) - eph.toe;
            double a     = eph.m0 + (constptr.ntcn * num59);
            double num14 = Math.Sin(a);
            double num25 = Math.Cos(a);
            double num15 = num14 * num14;
            double num16 = num15 * num14;
            double num17 = num16 * num14;
            double num18 = num17 * num14;
            double num19 = num18 * num14;
            double num20 = num19 * num14;
            double num23 = constptr.ntck * (((((((constptr.ntcs1 * num14) + ((constptr.ntcs2 * num14) * num25)) + (constptr.ntcs3 * num16)) + ((constptr.ntcs4 * num16) * num25)) + (constptr.ntcs5 * num18)) + ((constptr.ntcs6 * num18) * num25)) + (constptr.ntcs7 * num20));
            double num28 = (((((num25 + (constptr.ntcc2 * num15)) + ((constptr.ntcc3 * num15) * num25)) + (constptr.ntcc4 * num17)) + ((constptr.ntcc5 * num17) * num25)) + (constptr.ntcc6 * num19)) + ((constptr.ntcc7 * num19) * num25);
            double num21 = (num23 * constptr.ntccw) + (num28 * constptr.ntcsw);
            double num26 = (num28 * constptr.ntccw) - (num23 * constptr.ntcsw);
            double num12 = (2.0 * num21) * num26;
            double num24 = (num26 * num26) - (num21 * num21);
            double num45 = (eph.Cus * num12) + (eph.Cuc * num24);
            double num13 = num45;
            double num48 = 1.0 - ((num13 * num13) / 2.0);
            double num22 = (num21 * num48) + (num26 * num13);
            double num27 = (num26 * num48) - (num21 * num13);
            double num   = 1.0 + (eph.ecc * num28);
            double num8  = constptr.ntcrx / num;
            double num10 = (eph.Crs * num12) + (eph.Crc * num24);
            double num37 = num8 + num10;
            double num39 = num37 * num27;

            num50 = num37 * num22;
            num52 = (eph.Cis * num12) + (eph.Cic * num24);
            double num29 = (eph.i0 + num52) + (eph.idot * num59);

            ntcld = constptr.ntcld;
            double d = (eph.omega0 + (ntcld * num59)) - constptr.ntcl2;

            num53     = Math.Cos(d);
            num55     = Math.Sin(d);
            num54     = Math.Sin(num29);
            num56     = Math.Cos(num29);
            st.pos[0] = (num39 * num53) - ((num50 * num56) * num55);
            st.pos[1] = (num39 * num55) + ((num50 * num56) * num53);
            st.pos[2] = num50 * num54;
            double num5  = ((constptr.ntcn * num) * num) / constptr.ntck2;
            double num44 = (eph.Cus * num24) - (eph.Cuc * num12);
            double num3  = 2.0 * num5;
            double num42 = num5 + (num3 * num44);
            double num11 = (eph.Crs * num24) - (eph.Crc * num12);
            double num57 = ((constptr.sqrta_2 * eph.ecc) * constptr.ntcn) / constptr.ntck;
            double num38 = (num57 * num23) + (num3 * num11);
            double num40 = (num38 * num27) - (num50 * num42);
            double num41 = (num38 * num22) + (num39 * num42);
            double num32 = (eph.Cis * num24) - (eph.Cic * num12);
            double num30 = (num3 * num32) + eph.idot;
            double num33 = num40 - ((num50 * ntcld) * num56);
            double num35 = ((num41 * num56) - (st.pos[2] * num30)) + (num39 * ntcld);

            st.vel[0] = (num33 * num53) - (num35 * num55);
            st.vel[1] = (num33 * num55) + (num35 * num53);
            st.vel[2] = (num41 * num54) + ((num50 * num30) * num56);
            double num2  = num3 * num3;
            double num6  = (-(((num3 * eph.ecc) * constptr.ntcn) * num23) / constptr.ntck2) * num;
            double num9  = (((num57 * num5) * num28) + ((2.0 * num6) * num11)) - (num2 * num10);
            double num43 = (num6 + ((2.0 * num6) * num44)) - (num2 * num45);
            double num46 = (((num9 * num27) - ((num38 * num42) * num22)) - (num41 * num42)) - (num43 * num50);
            double num47 = (((num9 * num22) + ((num38 * num42) * num27)) + (num40 * num42)) + (num43 * num39);
            double num31 = ((2.0 * num6) * num32) - (num2 * num52);
            double num34 = (num46 - ((num41 * ntcld) * num56)) + (((num50 * num30) * ntcld) * num54);
            double num36 = ((((num47 * num56) - ((num41 * num30) * num54)) - (st.vel[2] * num30)) - (st.pos[2] * num31)) + (num40 * ntcld);

            st.acc[0] = (float)(((-ntcld * st.vel[1]) + (num34 * num53)) - (num36 * num55));
            st.acc[1] = (float)(((ntcld * st.vel[0]) + (num34 * num55)) + (num36 * num53));
            st.acc[2] = (float)((((num47 * num54) + (((2.0 * num41) * num30) * num56)) + ((num50 * num31) * num56)) - ((num30 * num30) * st.pos[2]));
            st.jrk[0] = (float)((((-3.0 * NLERTE) * NLERTE) * ((float)st.vel[0])) + ((2.0 * NLERTE) * st.acc[1]));
            st.jrk[1] = (float)((((-3.0 * NLERTE) * NLERTE) * st.vel[1]) - ((2.0 * NLERTE) * st.acc[0]));
            st.jrk[2] = (float)(((-4.0 * NLERTE) * NLERTE) * st.vel[2]);
            double num49 = constptr.ntecos / (1.0 + (eph.ecc * num28));

            st.tcr  = (float)((num49 * constptr.ntck) * num23);
            st.rcd  = (float)((num49 * constptr.ntcn) * (eph.ecc + num28));
            st.slw  = 0;
            st.tow  = (int)eph.toe;
            st.wno  = (short)eph.weekNo;
            st.cct  = (int)eph.toc;
            st.tgd  = eph.tgd;
            st.iode = (byte)(((byte)eph.iode) & 0xff);
            st.af0  = eph.af0;
            st.af1  = eph.af1;
            st.af2  = eph.af2;
            st.gct  = gTime.time;
            st.gcw  = (short)gTime.week;
            if (st.dataAvail == 5)
            {
                st.posVar = g_posVar[eph.age];
                st.clkVar = g_svClkVar[eph.age];
                st.ephAge = eph.age;
            }
            else if (st.dataAvail == 6)
            {
                if (eph.age == 1)
                {
                    st.posVar = g_posVar[3];
                }
                else if (eph.age == 2)
                {
                    st.posVar = g_posVar[4];
                }
                else if (eph.age == 3)
                {
                    st.posVar = g_posVar[6];
                }
                else
                {
                    st.posVar = g_posVar[eph.age];
                }
                st.clkVar = g_svClkVar[eph.age];
                st.ephAge = eph.age;
            }
            else
            {
                st.posVar = 0f;
                st.clkVar = 0f;
                st.ephAge = 0;
            }
            double num58 = ((SECONDS_PER_WEEK * (st.gcw - st.wno)) + st.gct) - st.cct;

            st.clockBias  = ((st.af0 + (num58 * (st.af1 + (num58 * st.af2)))) + st.tcr) - st.tgd;
            st.clockDrift = (st.af1 + ((2.0 * st.af2) * num58)) + st.rcd;
        }
예제 #3
0
        private void setEphemerisConstants(ref tSVD_EphemerisConsts constptr, short svid, ref RinexEph eph, ref tGPSTime gTime)
        {
            constptr.sqrta_2 = eph.sqrta * eph.sqrta;
            if (constptr.sqrta_2 != 0.0)
            {
                constptr.ntcn = (NLSQMU / (constptr.sqrta_2 * eph.sqrta)) + eph.deltan;
            }
            else
            {
                constptr.ntcn = eph.deltan;
            }
            double num  = eph.ecc * eph.ecc;
            double num2 = num * eph.ecc;
            double num3 = num2 * eph.ecc;
            double num4 = num3 * eph.ecc;
            double num5 = num4 * eph.ecc;

            constptr.ntcs1  = ((1.0 + (3.0 * num)) + (5.0 * num3)) + (7.0 * num5);
            constptr.ntcs2  = ((2.0 * eph.ecc) + (4.0 * num2)) + (6.0 * num4);
            constptr.ntcs3  = -(((4.5 * num) + (28.3333333333333 * num3)) + (96.8333333333333 * num5));
            constptr.ntcs4  = -((10.6666666666667 * num2) + (52.0 * num4));
            constptr.ntcs5  = (26.0416666666667 * num3) + (247.975 * num5);
            constptr.ntcs6  = 64.8 * num4;
            constptr.ntcs7  = -163.401388888889 * num5;
            constptr.ntcc2  = -(((2.0 * eph.ecc) + (8.0 * num2)) + (18.0 * num4));
            constptr.ntcc3  = -(((4.5 * num) + (12.5 * num3)) + (24.5 * num5));
            constptr.ntcc4  = (10.6666666666667 * num2) + (78.0 * num4);
            constptr.ntcc5  = (26.0416666666667 * num3) + (151.083333333333 * num5);
            constptr.ntcc6  = -constptr.ntcs6;
            constptr.ntcc7  = constptr.ntcs7;
            constptr.ntc1e2 = 1.0 - num;
            constptr.ntcrx  = constptr.sqrta_2 * constptr.ntc1e2;
            constptr.ntck   = Math.Sqrt(constptr.ntc1e2);
            constptr.ntck2  = constptr.ntck * constptr.ntc1e2;
            constptr.ntcsw  = Math.Sin(eph.omega);
            constptr.ntccw  = Math.Cos(eph.omega);
            constptr.ntcl   = eph.omega0 - (eph.omegaDot * eph.toe);
            constptr.ntcld  = eph.omegaDot - NLERTE;
            constptr.ntcl2  = NLERTE * eph.toe;
            constptr.ntecos = (-NLRF * eph.ecc) * eph.sqrta;
        }