Beispiel #1
0
// Constructors
gSatTEME::gSatTEME(const char *pstrName, char *pstrTleLine1, char *pstrTleLine2)
{

	double startmfe, stopmfe, deltamin;
	double ro[3];
	double vo[3];

	m_Position.resize(3);
	m_Vel.resize(3);

	m_SatName = pstrName;

	//set gravitational constants
	getgravconst(CONSTANTS_SET, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2);

	//Parsing TLE_Files and sat variables setting
	twoline2rv(pstrTleLine1, pstrTleLine2, TYPERUN_SET, TYPEINPUT_SET, OPSMODE_SET, CONSTANTS_SET,
	           startmfe, stopmfe, deltamin, satrec);

	// call the propagator to get the initial state vector value
	sgp4(CONSTANTS_SET, satrec,  0.0, ro,  vo);

	m_Position[ 0]= ro[ 0];
	m_Position[ 1]= ro[ 1];
	m_Position[ 2]= ro[ 2];
	m_Vel[ 0]     = vo[ 0];
	m_Vel[ 1]     = vo[ 1];
	m_Vel[ 2]     = vo[ 2];
}
Beispiel #2
0
/*-----------------------------------------------------------------------------
*                           procedure initl
  ----------------------------------------------------------------------------*/
static void initl(double *ainv,double *ao,double *con41,double *con42,double *cosio,
									double *cosio2,double *einv,double *eccsq,double *omeosq,double *posq,
									double *rp,double *rteosq,double *sinio,double *po,double *ecco,
									double *inclo,double *no)
{
		 double tumin,mu,radiusearthkm,xke,j2,j3,j4,j3oj2,x2o3,ak,d1,del,adel;
     /* ----------------------- earth constants ---------------------- */
     // sgp4fix identify constants and allow alternate values
     getgravconst(&tumin,&mu,&radiusearthkm,&xke,&j2,&j3,&j4,&j3oj2);
     x2o3   = 2.0 / 3.0;

     /* ------------- calculate auxillary epoch quantities ---------- */
     *eccsq  = *(ecco) * (*ecco);
     *omeosq = 1.0 - *eccsq;
     *rteosq = sqrt(*omeosq);
     *cosio  = cos(*inclo);
     *cosio2 = (*cosio) * (*cosio);

     /* ------------------ un-kozai the mean motion ----------------- */
     ak    = pow(xke / (*no), x2o3);
     d1    = 0.75 * j2 * (3.0 * (*cosio2) - 1.0) / ((*rteosq) * (*omeosq));
     del   = d1 / (ak * ak);
     adel  = ak * (1.0 - del * del - del * (1.0 / 3.0 + 134.0 * del * del / 81.0));
     del   = d1/(adel * adel);
     *no    = (*no) / (1.0 + del);

     *ao    = pow(xke / (*no), x2o3);
     *sinio = sin(*inclo);
     *po    = (*ao) * (*omeosq);
     *con42 = 1.0 - 5.0 * (*cosio2);
     *con41 = -(*con42)-(*cosio2)-(*cosio2);
     *ainv  = 1.0 / (*ao);
		 *einv  = 1.0 / (*ecco);
     *posq  = (*po) * (*po);
     *rp    = (*ao) * (1.0 - *ecco);
}
void twoline2rv
     (
      char      longstr1[130], char longstr2[130],
      char      typerun,  char typeinput, char opsmode,
      gravconsttype       whichconst,
      double& startmfe, double& stopmfe, double& deltamin,
      sgp4struct &satrec
     )
     {
       const double deg2rad  =   pi / 180.0;         //   0.0174532925199433
       const double xpdotp   =  1440.0 / (2.0 * pi); // 229.1831180523293

       double sec, mu, radiusearthkm, tumin, xke, j2, j3, j4, j3oj2;
       double startsec, stopsec, startdayofyr, stopdayofyr, jdstart, jdstop;
       int startyear, stopyear, startmon, stopmon, startday, stopday,
           starthr, stophr, startmin, stopmin;
       int cardnumb, numb, j;
       long revnum = 0, elnum = 0;
       char classification, intldesg[11];
       int year = 0;
       int mon, day, hr, minute, nexp, ibexp;

       getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );

       satrec.error = 0;

       // set the implied decimal points since doing a formated read
       // fixes for bad input data values (missing, ...)
       for (j = 10; j <= 15; j++)
           if (longstr1[j] == ' ')
               longstr1[j] = '_';

       if (longstr1[44] != ' ')
           longstr1[43] = longstr1[44];
       longstr1[44] = '.';
       if (longstr1[7] == ' ')
           longstr1[7] = 'U';
       if (longstr1[9] == ' ')
           longstr1[9] = '.';
       for (j = 45; j <= 49; j++)
           if (longstr1[j] == ' ')
               longstr1[j] = '0';
       if (longstr1[51] == ' ')
           longstr1[51] = '0';
       if (longstr1[53] != ' ')
           longstr1[52] = longstr1[53];
       longstr1[53] = '.';
       longstr2[25] = '.';
       for (j = 26; j <= 32; j++)
           if (longstr2[j] == ' ')
               longstr2[j] = '0';
       if (longstr1[62] == ' ')
           longstr1[62] = '0';
       if (longstr1[68] == ' ')
           longstr1[68] = '0';

       sscanf(longstr1,"%2d %5ld %1c %10s %2d %12lf %11lf %7lf %2d %7lf %2d %2d %6ld ",
                       &cardnumb,&satrec.noradNum,&classification, intldesg, &satrec.epochyr,
                       &satrec.epochdays,&satrec.ndot, &satrec.nddot, &nexp, &satrec.bstar,
                       &ibexp, &numb, &elnum );

       if (typerun == 'v')  // run for specified times from the file
         {
           if (longstr2[52] == ' ')
               sscanf(longstr2,"%2d %5ld %9lf %9lf %8lf %9lf %9lf %10lf %6ld %lf %lf %lf \n",
                       &cardnumb,&satrec.noradNum, &satrec.inclination,
                       &satrec.rightAscNode,&satrec.eccentricity, &satrec.argpo, &satrec.meanAnomaly, &satrec.meanMotion,
                       &revnum, &startmfe, &stopmfe, &deltamin );
             else
               sscanf(longstr2,"%2d %5ld %9lf %9lf %8lf %9lf %9lf %11lf %6ld %lf %lf %lf \n",
                       &cardnumb,&satrec.noradNum, &satrec.inclination,
                       &satrec.rightAscNode,&satrec.eccentricity, &satrec.argpo, &satrec.meanAnomaly, &satrec.meanMotion,
                       &revnum, &startmfe, &stopmfe, &deltamin );
         }
         else  // simply run -1 day to +1 day or user input times
         {
           if (longstr2[52] == ' ')
               sscanf(longstr2,"%2d %5ld %9lf %9lf %8lf %9lf %9lf %10lf %6ld \n",
                       &cardnumb,&satrec.noradNum, &satrec.inclination,
                       &satrec.rightAscNode,&satrec.eccentricity, &satrec.argpo, &satrec.meanAnomaly, &satrec.meanMotion,
                       &revnum );
             else
               sscanf(longstr2,"%2d %5ld %9lf %9lf %8lf %9lf %9lf %11lf %6ld \n",
                       &cardnumb,&satrec.noradNum, &satrec.inclination,
                       &satrec.rightAscNode,&satrec.eccentricity, &satrec.argpo, &satrec.meanAnomaly, &satrec.meanMotion,
                       &revnum );
         }

       // ---- find no, ndot, nddot ----
       satrec.meanMotionRad   = satrec.meanMotion / xpdotp; //* rad/min
       satrec.nddot= satrec.nddot * pow(10.0, nexp);
       satrec.bstar= satrec.bstar * pow(10.0, ibexp);

       // ---- convert to sgp4 units ----
       satrec.a    = pow( satrec.meanMotionRad*tumin , (-2.0/3.0) );
       satrec.ndot = satrec.ndot  / (xpdotp*1440.0);  //* ? * minperday
       satrec.nddot= satrec.nddot / (xpdotp*1440.0*1440);

       // ---- find standard orbital elements ----
       satrec.inclination = satrec.inclination  * deg2rad;
       satrec.rightAscNode = satrec.rightAscNode  * deg2rad;
       satrec.argpo = satrec.argpo  * deg2rad;
       satrec.meanAnomaly    = satrec.meanAnomaly     * deg2rad;

       satrec.alta = satrec.a*(1.0 + satrec.eccentricity) - 1.0;
       satrec.altp = satrec.a*(1.0 - satrec.eccentricity) - 1.0;

       // ----------------------------------------------------------------
       // find sgp4epoch time of element set
       // remember that sgp4 uses units of days from 0 jan 1950 (sgp4epoch)
       // and minutes from the epoch (time)
       // ----------------------------------------------------------------

       // ---------------- temp fix for years from 1957-2056 -------------------
       // --------- correct fix will occur when year is 4-digit in tle ---------
       if (satrec.epochyr < 57)
           year= satrec.epochyr + 2000;
         else
           year= satrec.epochyr + 1900;

       days2mdhms ( year,satrec.epochdays, mon,day,hr,minute,sec );
       jday( year,mon,day,hr,minute,sec, satrec.jdsatepoch );

       // ---- input start stop times manually
       if ((typerun != 'v') && (typerun != 'c'))
         {
         // ------------- enter start/stop ymd hms values --------------------
           if (typeinput == 'e')
             {
               printf("input start prop year mon day hr min sec \n");
               // make sure there is no space at the end of the format specifiers in scanf!
               scanf( "%i %i %i %i %i %lf",&startyear, &startmon, &startday, &starthr, &startmin, &startsec);
               fflush(stdin);
               jday( startyear,startmon,startday,starthr,startmin,startsec, jdstart );

               printf("input stop prop year mon day hr min sec \n");
               scanf( "%i %i %i %i %i %lf",&stopyear, &stopmon, &stopday, &stophr, &stopmin, &stopsec);
               fflush(stdin);
               jday( stopyear,stopmon,stopday,stophr,stopmin,stopsec, jdstop );

               startmfe = (jdstart - satrec.jdsatepoch) * 1440.0;
               stopmfe  = (jdstop - satrec.jdsatepoch) * 1440.0;

               printf("input time step in minutes \n");
               scanf( "%lf",&deltamin );
             }
           // -------- enter start/stop year and days of year values -----------
           if (typeinput == 'd')
             {
               printf("input start year dayofyr \n");
               scanf( "%i %lf",&startyear, &startdayofyr );
               printf("input stop year dayofyr \n");
               scanf( "%i %lf",&stopyear, &stopdayofyr );

               days2mdhms ( startyear,startdayofyr, mon,day,hr,minute,sec );
               jday( startyear,mon,day,hr,minute,sec, jdstart );
               days2mdhms ( stopyear,stopdayofyr, mon,day,hr,minute,sec );
               jday( stopyear,mon,day,hr,minute,sec, jdstop );

               startmfe = (jdstart - satrec.jdsatepoch) * 1440.0;
               stopmfe  = (jdstop - satrec.jdsatepoch) * 1440.0;

               printf("input time step in minutes \n");
               scanf( "%lf",&deltamin );
             }
           // ------------------ enter start/stop mfe values -------------------
           if (typeinput == 'm')
             {
               printf("input start min from epoch \n");
               scanf( "%lf",&startmfe );
               printf("input stop min from epoch \n");
               scanf( "%lf",&stopmfe );
               printf("input time step in minutes \n");
               scanf( "%lf",&deltamin );
             }
         }

       // ------------ perform complete catalog evaluation, -+ 1 day ----------- 
       if (typerun == 'c')
         {
           startmfe = -1440.0;
           stopmfe  =  1440.0;
           deltamin =    10.0;
         }

       // ---------------- initialize the orbit at sgp4epoch -------------------
       sgp4init( whichconst, opsmode, satrec.noradNum, satrec.jdsatepoch-2433281.5,
               satrec.bstar, satrec.eccentricity, satrec.argpo, satrec.inclination,
               satrec.meanAnomaly, satrec.meanMotionRad, satrec.rightAscNode, satrec);
    } // end twoline2rv
Beispiel #4
0
/*-----------------------------------------------------------------------------
*                             procedure sgp4init
*              this procedure initializes variables for sgp4.
  ----------------------------------------------------------------------------*/
void sgp4init(elsetrec *satrec)
{
     /* --------------------- local variables ------------------------ */
			double tumin,mu,radiusearthkm,xke,j2,j3,j4,j3oj2,ss,qzms2ttemp,
						qzms2t,x2o3,ainv,ao,con42,cosio,cosio2,einv,eccsq,omeosq,posq,
						rp,rteosq,sinio,po,sfour,qzms24,perige,qzms24temp,pinvsq,tsi,etasq,
						eeta,psisq,coef,coef1,cc2,cc3,cosio4,temp1,temp2,temp3,xhdot1,xpidot,
						delmotemp,cc1sq,temp;
     double temp4 = 1.5e-12;

     /* ----------- set all near earth variables to zero ------------ */
     satrec->isimp   = 0;   satrec->aycof  = 0.0;
     satrec->con41   = 0.0; satrec->cc1    = 0.0; satrec->cc4      = 0.0;
     satrec->cc5     = 0.0; satrec->d2     = 0.0; satrec->d3       = 0.0;
     satrec->d4      = 0.0; satrec->delmo  = 0.0; satrec->eta      = 0.0;
     satrec->argpdot = 0.0; satrec->omgcof = 0.0; satrec->sinmao   = 0.0;
     satrec->t       = 0.0; satrec->t2cof  = 0.0; satrec->t3cof    = 0.0;
     satrec->t4cof   = 0.0; satrec->t5cof  = 0.0; satrec->x1mth2   = 0.0;
     satrec->x7thm1  = 0.0; satrec->mdot   = 0.0; satrec->nodedot  = 0.0;
     satrec->xlcof   = 0.0; satrec->xmcof  = 0.0; satrec->nodecf   = 0.0;

     /* ------------------------ earth constants ----------------------- */
     // sgp4fix identify constants and allow alternate values
     getgravconst(&tumin,&mu,&radiusearthkm,&xke,&j2,&j3,&j4,&j3oj2);
     ss = 78.0 / radiusearthkm + 1.0;
     // sgp4fix use multiply for speed instead of pow
     qzms2ttemp = (120.0 - 78.0) / radiusearthkm;
     qzms2t = qzms2ttemp * qzms2ttemp * qzms2ttemp * qzms2ttemp;
     x2o3   =  2.0 / 3.0;

     satrec->t	 = 0.0;

		 initl(&ainv,&ao,&satrec->con41,&con42,&cosio,&cosio2,&einv,&eccsq,&omeosq,&posq,
					&rp,&rteosq,&sinio,&po,&satrec->ecco, &satrec->inclo, &satrec->no);

     if ((omeosq >= 0.0 ) || ( satrec->no >= 0.0))
       {
         satrec->isimp = 0;
         if (rp < (220.0 / radiusearthkm + 1.0))
             satrec->isimp = 1;
         sfour  = ss;
         qzms24 = qzms2t;
         perige = (rp - 1.0) * radiusearthkm;

         /* - for perigees below 156 km, s and qoms2t are altered - */
         if (perige < 156.0)
           {
             sfour = perige - 78.0;
             if (perige < 98.0)
                 sfour = 20.0;
             // sgp4fix use multiply for speed instead of pow
             qzms24temp =  (120.0 - sfour) / radiusearthkm;
             qzms24 = qzms24temp * qzms24temp * qzms24temp * qzms24temp;
             sfour  = sfour / radiusearthkm + 1.0;
           }
         pinvsq = 1.0 / posq;

         tsi  = 1.0 / (ao - sfour);
         satrec->eta  = ao * satrec->ecco * tsi;
         etasq = satrec->eta * satrec->eta;
         eeta  = satrec->ecco * satrec->eta;
         psisq = fabs(1.0 - etasq);
         coef  = qzms24 * pow(tsi, 4.0);
         coef1 = coef / pow(psisq, 3.5);
         cc2   = coef1 * satrec->no * (ao * (1.0 + 1.5 * etasq + eeta *
                        (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec->con41 *
                        (8.0 + 3.0 * etasq * (8.0 + etasq)));
         satrec->cc1   = satrec->bstar * cc2;
         cc3   = 0.0;
         if (satrec->ecco > 1.0e-4)
             cc3 = -2.0 * coef * tsi * j3oj2 * satrec->no * sinio / satrec->ecco;
         satrec->x1mth2 = 1.0 - cosio2;
         satrec->cc4    = 2.0* satrec->no * coef1 * ao * omeosq *
                           (satrec->eta * (2.0 + 0.5 * etasq) + satrec->ecco *
                           (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) *
                           (-3.0 * satrec->con41 * (1.0 - 2.0 * eeta + etasq *
                           (1.5 - 0.5 * eeta)) + 0.75 * satrec->x1mth2 *
                           (2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec->argpo)));
         satrec->cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 *
                        (etasq + eeta) + eeta * etasq);
         cosio4 = cosio2 * cosio2;
         temp1  = 1.5 * j2 * pinvsq * satrec->no;
         temp2  = 0.5 * temp1 * j2 * pinvsq;
         temp3  = -0.46875 * j4 * pinvsq * pinvsq * satrec->no;
         satrec->mdot     = satrec->no + 0.5 * temp1 * rteosq * satrec->con41 + 0.0625 *
                            temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4);
         satrec->argpdot  = -0.5 * temp1 * con42 + 0.0625 * temp2 *
                             (7.0 - 114.0 * cosio2 + 395.0 * cosio4) +
                             temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4);
         xhdot1            = -temp1 * cosio;
         satrec->nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) +
                              2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio;
         xpidot            =  satrec->argpdot+ satrec->nodedot;
         satrec->omgcof   = satrec->bstar * cc3 * cos(satrec->argpo);
         satrec->xmcof    = 0.0;
         if (satrec->ecco > 1.0e-4)
             satrec->xmcof = -x2o3 * coef * satrec->bstar / eeta;
         satrec->nodecf = 3.5 * omeosq * xhdot1 * satrec->cc1;
         satrec->t2cof   = 1.5 * satrec->cc1;
         // sgp4fix for divide by zero with xinco = 180 deg
         if (fabs(cosio+1.0) > 1.5e-12)
             satrec->xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio);
           else
             satrec->xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4;
         satrec->aycof   = -0.5 * j3oj2 * sinio;
         // sgp4fix use multiply for speed instead of pow
         delmotemp = 1.0 + satrec->eta * cos(satrec->mo);
         satrec->delmo   = delmotemp * delmotemp * delmotemp;
         satrec->sinmao  = sin(satrec->mo);
         satrec->x7thm1  = 7.0 * cosio2 - 1.0;

       /* ----------- set variables if not deep space ----------- */
       if (satrec->isimp != 1)
         {
           cc1sq          = satrec->cc1 * satrec->cc1;
           satrec->d2    = 4.0 * ao * tsi * cc1sq;
           temp           = satrec->d2 * tsi * satrec->cc1 / 3.0;
           satrec->d3    = (17.0 * ao + sfour) * temp;
           satrec->d4    = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) *
                            satrec->cc1;
           satrec->t3cof = satrec->d2 + 2.0 * cc1sq;
           satrec->t4cof = 0.25 * (3.0 * satrec->d3 + satrec->cc1 *
                            (12.0 * satrec->d2 + 10.0 * cc1sq));
           satrec->t5cof = 0.2 * (3.0 * satrec->d4 +
                            12.0 * satrec->cc1 * satrec->d3 +
                            6.0 * satrec->d2 * satrec->d2 +
                            15.0 * cc1sq * (2.0 * satrec->d2 + cc1sq));
         }
       } 
}
Beispiel #5
0
/*-----------------------------------------------------------------------------
*                             procedure sgp4
  ----------------------------------------------------------------------------*/
void sgp4(double r[3],double v[3],elsetrec *satrec,double *tsince)
{
			double twopi,x2o3,tumin,mu,radiusearthkm,xke,j2,j3,j4,j3oj2,vkmpersec,
					xmdf,argpdf,nodedf,argpm,mm,t2,nodem,tempa,tempe,templ,delomg,delmtemp,
					delm,temp,t3,t4,nm,em,inclm,am,xlm,emsq,sinim,cosim,ep,xincp,argpp,nodep,
					mp,sinip,cosip,axnl,aynl,xl,u,eo1,tem5,sineo1,coseo1,esine,ecose,el2,pl,
					rl,rdotl,rvdotl,betal,sinu,cosu,su,sin2u,cos2u,temp1,temp2,mrt,xnode,xinc,
					mvt,rvdot,sinsu,cossu,snod,cnod,sini,cosi,xmx,xmy,ux,uy,uz,vx,vy,vz;
			int ktr;
     /* ------------------ set mathematical constants --------------- */
     twopi = 2.0 * PI;
     x2o3  = 2.0 / 3.0;
     // sgp4fix identify constants and allow alternate values
     getgravconst(&tumin,&mu,&radiusearthkm,&xke,&j2,&j3,&j4,&j3oj2);
     vkmpersec = radiusearthkm * xke/60.0;

     /* --------------------- clear sgp4 error flag ----------------- */
     satrec->t = *tsince;

     /* ------- update for secular gravity and atmospheric drag ----- */
     xmdf    = satrec->mo + satrec->mdot * satrec->t;
     argpdf  = satrec->argpo + satrec->argpdot * satrec->t;
     nodedf  = satrec->nodeo + satrec->nodedot * satrec->t;
     argpm   = argpdf;
     mm      = xmdf;
     t2      = satrec->t * satrec->t;
     nodem   = nodedf + satrec->nodecf * t2;
     tempa   = 1.0 - satrec->cc1 * satrec->t;
     tempe   = satrec->bstar * satrec->cc4 * satrec->t;
     templ   = satrec->t2cof * t2;

     if (satrec->isimp != 1)
       {
         delomg = satrec->omgcof * satrec->t;
         // sgp4fix use mutliply for speed instead of pow
         delmtemp =  1.0 + satrec->eta * cos(xmdf);
         delm   = satrec->xmcof *
                  (delmtemp * delmtemp * delmtemp -
                  satrec->delmo);
         temp   = delomg + delm;
         mm     = xmdf + temp;
         argpm  = argpdf - temp;
         t3     = t2 * satrec->t;
         t4     = t3 * satrec->t;
         tempa  = tempa - satrec->d2 * t2 - satrec->d3 * t3 -
                          satrec->d4 * t4;
         tempe  = tempe + satrec->bstar * satrec->cc5 * (sin(mm) -
                          satrec->sinmao);
         templ  = templ + satrec->t3cof * t3 + t4 * (satrec->t4cof +
                          satrec->t * satrec->t5cof);
       }

     nm    = satrec->no;
     em    = satrec->ecco;
     inclm = satrec->inclo;

     am = pow((xke / nm),x2o3) * tempa * tempa;
     nm = xke / pow(am, 1.5);
     em = em - tempe;
			 
     if (em < 1.0e-6)
         em  = 1.0e-6;
     mm     = mm + satrec->no * templ;
     xlm    = mm + argpm + nodem;
     emsq   = em * em;
     temp   = 1.0 - emsq;

     nodem  = fmod(nodem, twopi);
     argpm  = fmod(argpm, twopi);
     xlm    = fmod(xlm, twopi);
     mm     = fmod(xlm - argpm - nodem, twopi);

     /* ----------------- compute extra mean quantities ------------- */
     sinim = sin(inclm);
     cosim = cos(inclm);

     /* -------------------- add lunar-solar periodics -------------- */
     ep     = em;
     xincp  = inclm;
     argpp  = argpm;
     nodep  = nodem;
     mp     = mm;
     sinip  = sinim;
     cosip  = cosim;

     /* -------------------- long period periodics ------------------ */
     axnl = ep * cos(argpp);
     temp = 1.0 / (am * (1.0 - ep * ep));
     aynl = ep* sin(argpp) + temp * satrec->aycof;
     xl   = mp + argpp + nodep + temp * satrec->xlcof * axnl;

     /* --------------------- solve kepler's equation --------------- */
     u    = fmod(xl - nodep, twopi);
     eo1  = u;
     tem5 = 9999.9;
     ktr = 1;
     while (( fabs(tem5) >= 1.0e-12) && (ktr <= 10) )
       {
         sineo1 = sin(eo1);
         coseo1 = cos(eo1);
         tem5   = 1.0 - coseo1 * axnl - sineo1 * aynl;
         tem5   = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5;
         if(fabs(tem5) >= 0.95)
             tem5 = tem5 > 0.0 ? 0.95 : -0.95;
         eo1    = eo1 + tem5;
         ktr = ktr + 1;
       }

     /* ------------- short period preliminary quantities ----------- */
     ecose = axnl*coseo1 + aynl*sineo1;
     esine = axnl*sineo1 - aynl*coseo1;
     el2   = axnl*axnl + aynl*aynl;
     pl    = am*(1.0-el2);
     if (pl < 0.0)
       {
				 *r = 0;
         *(r+1) = 0;
         *(r+2) = 0;
         *v = 0;
         *(v+1) = 0;
         *(v+2) = 0;
       }
       else
       {
         rl     = am * (1.0 - ecose);
         rdotl  = sqrt(am) * esine/rl;
         rvdotl = sqrt(pl) / rl;
         betal  = sqrt(1.0 - el2);
         temp   = esine / (1.0 + betal);
         sinu   = am / rl * (sineo1 - aynl - axnl * temp);
         cosu   = am / rl * (coseo1 - axnl + aynl * temp);
         su     = atan2(sinu, cosu);
         sin2u  = (cosu + cosu) * sinu;
         cos2u  = 1.0 - 2.0 * sinu * sinu;
         temp   = 1.0 / pl;
         temp1  = 0.5 * j2 * temp;
         temp2  = temp1 * temp;

         /* -------------- update for short period periodics ------------ */
         mrt   = rl * (1.0 - 1.5 * temp2 * betal * satrec->con41) +
                 0.5 * temp1 * satrec->x1mth2 * cos2u;
         su    = su - 0.25 * temp2 * satrec->x7thm1 * sin2u;
         xnode = nodep + 1.5 * temp2 * cosip * sin2u;
         xinc  = xincp + 1.5 * temp2 * cosip * sinip * cos2u;
         mvt   = rdotl - nm * temp1 * satrec->x1mth2 * sin2u / xke;
         rvdot = rvdotl + nm * temp1 * (satrec->x1mth2 * cos2u +
                 1.5 * satrec->con41) / xke;

         /* --------------------- orientation vectors ------------------- */
         sinsu =  sin(su);
         cossu =  cos(su);
         snod  =  sin(xnode);
         cnod  =  cos(xnode);
         sini  =  sin(xinc);
         cosi  =  cos(xinc);
         xmx   = -snod * cosi;
         xmy   =  cnod * cosi;
         ux    =  xmx * sinsu + cnod * cossu;
         uy    =  xmy * sinsu + snod * cossu;
         uz    =  sini * sinsu;
         vx    =  xmx * cossu - cnod * sinsu;
         vy    =  xmy * cossu - snod * sinsu;
         vz    =  sini * cossu;

         /* --------- position and velocity (in km and km/sec) ---------- */
         *r = (mrt * ux)* radiusearthkm;
         *(r+1) = (mrt * uy)* radiusearthkm;
         *(r+2) = (mrt * uz)* radiusearthkm;
         *v = (mvt * ux + rvdot * vx) * vkmpersec;
         *(v+1) = (mvt * uy + rvdot * vy) * vkmpersec;
         *(v+2) = (mvt * uz + rvdot * vz) * vkmpersec;
       } 

}
int main()
{
    //INITIALIZE STEPPER MOTOR
    Adafruit_MotorShield AFMS = Adafruit_MotorShield();
    Adafruit_StepperMotor *myMotor = AFMS.getStepper(200, 1);
    AFMS.begin(2400);  // create with the frequency 2.4KHz
    myMotor->setSpeed(1000);  // rpm (this has an upper limit way below 1000 rpm)
    myMotor->release();
    
    //SET UP SOME VARIABLES
    double ro[3];
    double vo[3];
    double recef[3];
    double vecef[3];
    char typerun, typeinput, opsmode;
    gravconsttype  whichconst;
    
    double sec, secC, jd, jdC, tsince, startmfe, stopmfe, deltamin;
    double tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2;
    double latlongh[3]; //lat, long in rad, h in km above ellipsoid
    double siteLat, siteLon, siteAlt, siteLatRad, siteLonRad;
    double razel[3];
    double razelrates[3];
    int  year; int mon; int day; int hr; int min;
    int yearC; int monC; int dayC; int hrC; int minC;
    typedef char str3[4];
    str3 monstr[13];
    elsetrec satrec;
    double steps_per_degree = 1.38889; //Stepper motor steps per degree azimuth
    float elevation;
    
    //VARIABLES FOR STEPPER CALCULATIONS
    float azimuth; //-180 to 0 to 180
    float prevAzimuth;
    float cAzimuth; //From 0 to 359.99
    float prevcAzimuth;
    bool stepperRelative = 0; //Has the stepper direction been initialized?
    float azimuthDatum = 0;
    int stepsFromDatum = 0;
    int stepsNext = 0;
    int dirNext = 1;
    int totalSteps = 0;
    int prevDir = 3; //Initialize at 3 to indicate that there is no previous direction yet (you can't have a "previous direction" until the third step)
    double azError = 0;
    
    //SET REAL TIME CLOCK (Set values manually using custom excel function until I find a way to do it automatically)    
    set_time(1440763200);
    
    //SET VARIABLES
    opsmode = 'i';
    typerun = 'c';
    typeinput = 'e';
    whichconst = wgs72;
    getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
    strcpy(monstr[1], "Jan");
    strcpy(monstr[2], "Feb");
    strcpy(monstr[3], "Mar");
    strcpy(monstr[4], "Apr");
    strcpy(monstr[5], "May");
    strcpy(monstr[6], "Jun");
    strcpy(monstr[7], "Jul");
    strcpy(monstr[8], "Aug");
    strcpy(monstr[9], "Sep");
    strcpy(monstr[10], "Oct");
    strcpy(monstr[11], "Nov");
    strcpy(monstr[12], "Dec");
    
    //ENTER TWO-LINE ELEMENT HERE
    char longstr1[] = "1 25544U 98067A   15239.40934558  .00012538  00000-0  18683-3 0  9996";
    char longstr2[] = "2 25544  51.6452  88.4122 0001595  95.9665 324.8493 15.55497522959124";
    
    //ENTER SITE DETAILS HERE
    siteLat = 30.25; //+North (Austin)
    siteLon = -97.75; //+East (Austin)
    siteAlt = 0.15; //km (Austin)
    siteLatRad = siteLat * pi / 180.0;
    siteLonRad = siteLon * pi / 180.0;
    
    //FREEDOM OF MOVEMENT CHECKS
    
    for (int i = 0; i < 500; i = i + 10) {
        EL_SERVO = Convert_El_to_Servo(-90.0 + 180.0 * i / 500.0);
    }
    wait(1);

    for (int i = 500; i > 0; i = i - 10) {
        EL_SERVO = Convert_El_to_Servo(-90.0 + 180.0 * i / 500.0);
    }    
    wait(1);
    
    /*
    //FREEDOM OF MOVEMENT CHECKS STEPPER
    myMotor->step(500, FORWARD, SINGLE);
    myMotor->step(500, BACKWARD, SINGLE);
    */
    
    //INITIALIZE SATELLITE TRACKING    
    //pc.printf("Initializing satellite orbit...\n");
    twoline2rv(longstr1, longstr2, typerun, typeinput, opsmode, whichconst, startmfe, stopmfe, deltamin, satrec );
    //pc.printf("twoline2rv function complete...\n");
    //Call propogator to get initial state vector value
    sgp4(whichconst, satrec, 0.0, ro, vo);
    //pc.printf("SGP4 at t = 0 to get initial state vector complete...\n"); 
    jd = satrec.jdsatepoch;    
    
    invjday(jd, year, mon, day, hr, min, sec);
    pc.printf("Scenario Epoch   %3i %3s%5i%3i:%2i:%12.9f \n", day, monstr[mon], year, hr, min, sec);
    jdC = getJulianFromUnix(time(NULL));
    invjday( jdC, yearC, monC, dayC, hrC, minC, secC);
    pc.printf("Current Time    %3i %3s%5i%3i:%2i:%12.9f \n", dayC, monstr[monC], yearC, hrC, minC, secC);
    //pc.printf("            Time            PosX            PosY            PosZ              Vx              Vy              Vz\n");
    //pc.printf("            Time             Lat            Long          Height           Range         Azimuth       Elevation\n");
    
    //BEGIN SATELLITE TRACKING
    while(1)
    {
        
        //RUN SGP4 AND COORDINATE TRANSFORMATION COMPUTATIONS
        jdC = getJulianFromUnix(time(NULL));
        tsince = (jdC - jd) * 24.0 * 60.0;
        sgp4(whichconst, satrec, tsince, ro, vo);
        teme2ecef(ro, vo, jdC, recef, vecef);
        ijk2ll(recef, latlongh);
        rv2azel(ro, vo, siteLatRad, siteLonRad, siteAlt, jdC, razel, razelrates);
        
        //CHECK FOR ERRORS
        if (satrec.error > 0)
        {
            pc.printf("# *** error: t:= %f *** code = %3d\n", satrec.t, satrec.error);
        }
        else
        {
            azimuth = razel[1]*180/pi;
            if (azimuth < 0) {
                cAzimuth = 360.0 + azimuth;
            }
            else {
                cAzimuth = azimuth;
            }      
            elevation = razel[2]*180/pi;
            
            //pc.printf("%16.8f%16.8f%16.8f%16.8f%16.8f%16.8f%16.8f\n", satrec.t, recef[0], recef[1], recef[2], vecef[0], vecef[1], vecef[2]);
            //pc.printf("%16.8f%16.8f%16.8f%16.8f%16.8f%16.8f%16.8f\n", satrec.t, latlongh[0]*180/pi, latlongh[1]*180/pi, latlongh[2], razel[0], razel[1]*180/pi, razel[2]*180/pi);
            
            //For first step, initialize the stepper direction assuming its initial position is true north
            if (stepperRelative == 0){
                stepsNext = int(cAzimuth * steps_per_degree);
                dirNext = 2;                
                myMotor->step(stepsNext, dirNext, MICROSTEP); //Turn stepper clockwise to approximate initial azimuth
                stepperRelative = 1;
                azimuthDatum = stepsNext / steps_per_degree;
                prevAzimuth = azimuth;
                prevcAzimuth = cAzimuth;
                
                pc.printf("             Azimuth       Azimuth Datum    Steps from Datum         Total Steps          Steps Next           Direction          Az. Error\n");
            }
            else {
                
                //Determine direction of rotation (note this will be incorrect if azimuth has crossed true north since previous step - this is dealt with later)
                if ( cAzimuth < prevcAzimuth ) {
                    dirNext = 1; //CCW
                }
                else {
                    dirNext = 2; //CW
                }
                
                
                //Check if azimuth has crossed from 360 to 0 degrees or vice versa
                if (abs( (azimuth - prevAzimuth) - (cAzimuth - prevcAzimuth) ) > 0.0001) {
                    
                    //Recalculate direction of rotation
                    if ( cAzimuth > prevcAzimuth ) {
                        dirNext = 1; //CCW
                    }
                    else {
                        dirNext = 2; //CW
                    }
                    
                    //Reset the azimuth datum
                    if (dirNext == 1) {
                        azimuthDatum = cAzimuth + azError + prevcAzimuth;
                    }
                    else {
                        azimuthDatum = cAzimuth - azError + (prevcAzimuth - 360);
                    }
                    
                    //Reset totalSteps
                    totalSteps = 0;
                }
                
                
                //Check if azimuth rate has changed directions
                if (prevDir != 3) { //prevDir of 3 means there is no previous direction yet
                    
                    if (prevDir != dirNext) {
                        
                        //Reset totalSteps
                        totalSteps = 0;
                        
                        //Reset azimuth datum
                        if (dirNext == 1) {
                            azimuthDatum = prevcAzimuth + azError;
                        }
                        else {
                            azimuthDatum = prevcAzimuth - azError;
                        }
                        
                    }
                    
                }
                
                
                stepsFromDatum = int( abs(cAzimuth - azimuthDatum) * steps_per_degree );
                stepsNext = stepsFromDatum - totalSteps;
                totalSteps += stepsNext;
                azError = abs(cAzimuth - azimuthDatum) - (totalSteps / steps_per_degree);
                                
                pc.printf("%20.2f%20.2f%20d%20d%20d%20d%20.2f\n", cAzimuth, azimuthDatum, stepsFromDatum, totalSteps, stepsNext, dirNext, azError);
                
                if (stepsNext > 250) {
                
                    pc.printf("something's probably wrong... too many steps\n\n\n\n");
                    while(1){} // pause
                    
                }
                
                myMotor->step(stepsNext, dirNext, MICROSTEP);               
            }
                        
            EL_SERVO = Convert_El_to_Servo(elevation);
            prevAzimuth = azimuth;
            prevcAzimuth = cAzimuth;
            prevDir = dirNext;
        }
        
        wait(1);
        
    } //indefinite loop
    
}
Beispiel #7
0
/////////////////////////////////////////////////////////////////////////////
// Initialize()
// Initialize the string array.
void QTle::Initialize()
{
   const double deg2rad  =   PI / 180.0;         //   0.0174532925199433
   const double xpdotp   =  1440.0 / (2.0 * PI);  // 229.1831180523293
   int year, mon, day, hr, minute;
   double sec;


   assert(!m_strName.isEmpty());
   assert(!m_strLine1.isEmpty());
   assert(!m_strLine2.isEmpty());
   

   m_Field[FLD_NORADNUM] = m_strLine1.mid(TLE1_COL_SATNUM, TLE1_LEN_SATNUM);
   m_Field[FLD_INTLDESC] = m_strLine1.mid(TLE1_COL_INTLDESC_A,
                                             TLE1_LEN_INTLDESC_A +
                                             TLE1_LEN_INTLDESC_B +   
                                             TLE1_LEN_INTLDESC_C);   
   m_Field[FLD_EPOCHYEAR] = 
         m_strLine1.mid(TLE1_COL_EPOCH_A, TLE1_LEN_EPOCH_A);

   m_Field[FLD_EPOCHDAY] = 
         m_strLine1.mid(TLE1_COL_EPOCH_B, TLE1_LEN_EPOCH_B);
   
   if (m_strLine1[TLE1_COL_MEANMOTIONDT] == '-')
   {
      // value is negative
      m_Field[FLD_MMOTIONDT] = "-0";
   }
   else
      m_Field[FLD_MMOTIONDT] = "0";
   
   m_Field[FLD_MMOTIONDT] += m_strLine1.mid(TLE1_COL_MEANMOTIONDT + 1,
                                               TLE1_LEN_MEANMOTIONDT);
   
   // decimal point assumed; exponential notation
   m_Field[FLD_MMOTIONDT2] = ExpToDecimal(
                                 m_strLine1.mid(TLE1_COL_MEANMOTIONDT2,
                                                   TLE1_LEN_MEANMOTIONDT2));
   // decimal point assumed; exponential notation
   m_Field[FLD_BSTAR] = ExpToDecimal(
                                m_strLine1.mid(TLE1_COL_BSTAR,
                                                       TLE1_LEN_BSTAR));
   //TLE1_COL_EPHEMTYPE      
   //TLE1_LEN_EPHEMTYPE   
   m_Field[FLD_SET] = m_strLine1.mid(TLE1_COL_ELNUM, TLE1_LEN_ELNUM);
   
   //TrimLeft(m_Field[FLD_SET].tr);
   
   //TLE2_COL_SATNUM         
   //TLE2_LEN_SATNUM         
   
   m_Field[FLD_I] = m_strLine2.mid(TLE2_COL_INCLINATION,
                                      TLE2_LEN_INCLINATION).trimmed();
   
   m_Field[FLD_RAAN] = m_strLine2.mid(TLE2_COL_RAASCENDNODE,
                                         TLE2_LEN_RAASCENDNODE).trimmed();
   // decimal point is assumed
   m_Field[FLD_E]   = "0.";
   m_Field[FLD_E]   += m_strLine2.mid(TLE2_COL_ECCENTRICITY,
                                       TLE2_LEN_ECCENTRICITY);
   
   m_Field[FLD_ARGPER] = m_strLine2.mid(TLE2_COL_ARGPERIGEE,
                                           TLE2_LEN_ARGPERIGEE).trimmed();
   
   m_Field[FLD_M]   = m_strLine2.mid(TLE2_COL_MEANANOMALY,
                                      TLE2_LEN_MEANANOMALY).trimmed();
   
   m_Field[FLD_MMOTION]   = m_strLine2.mid(TLE2_COL_MEANMOTION,
                                            TLE2_LEN_MEANMOTION).trimmed();

   m_Field[FLD_ORBITNUM] = m_strLine2.mid(TLE2_COL_REVATEPOCH,
                                             TLE2_LEN_REVATEPOCH).trimmed();

   satnum = m_Field[FLD_NORADNUM].toLong();
   epochyr = m_Field[FLD_EPOCHYEAR].toInt();
   epochdays = m_Field[FLD_EPOCHDAY].toDouble();
   ndot = m_Field[FLD_MMOTIONDT].toDouble();        // mean motion dt
   nddot = m_Field[FLD_MMOTIONDT2].toDouble();      // mean motion dt2
   bstar = m_Field[FLD_BSTAR].toDouble();           // bstar
   inclo = m_Field[FLD_I].toDouble();               // inclination
   nodeo = m_Field[FLD_RAAN].toDouble();            // right ascending node
   ecco = m_Field[FLD_E].toDouble();                // eccenticity
   argpo = m_Field[FLD_ARGPER].toDouble();          // argument of perigee
   mo = m_Field[FLD_M].toDouble();                  // mean anomaly
   no = m_Field[FLD_MMOTION].toDouble();            // mean motion

   getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2, radiusearthkmminor, flattening );

   // ---- find no ----
   no   = no / xpdotp; //* rad/min
   // ---- convert to sgp4 units ----
   a    = pow( no*tumin , (-2.0/3.0) );
   ndot = ndot  / (xpdotp*1440.0);  //* ? * minperday
   nddot= nddot / (xpdotp*1440.0*1440);

   // ---- find standard orbital elements ----
   inclo = inclo  * deg2rad;
   nodeo = nodeo  * deg2rad;
   argpo = argpo  * deg2rad;
   mo    = mo     * deg2rad;

   alta = a*(1.0 + ecco) - 1.0;
   altp = a*(1.0 - ecco) - 1.0;

   // ----------------------------------------------------------------
   // find sgp4epoch time of element set
   // remember that sgp4 uses units of days from 0 jan 1950 (sgp4epoch)
   // and minutes from the epoch (time)
   // ----------------------------------------------------------------

   // ---------------- temp fix for years from 1957-2056 -------------------
   // --------- correct fix will occur when year is 4-digit in tle ---------
   if (epochyr < 57)
       year= epochyr + 2000;
     else
       year= epochyr + 1900;

   days2mdhms ( year, epochdays, mon, day, hr, minute, sec );
   jday( year, mon, day, hr, minute, sec, jdsatepoch );

   const double a1 = pow( xke / no, 2/3);
   const double cosio = cos(inclo);
   const double theta2 = cosio * cosio;
   const double x3thm1 = 3.0 * theta2 - 1.0;
   const double eosq = ecco * ecco;
   const double betao2 = 1.0 - eosq;
   const double betao = sqrt(betao2);
   const double temp = (1.5 * (j2/2)) * x3thm1 / (betao * betao2);
   const double del1 = temp / (a1 * a1);
   const double a0 = a1 * (1.0 - del1 * (1.0 / 3.0 + del1 * (1.0 + del1 * 134.0 / 81.0)));
   const double del0 = temp / (a0 * a0);

   recovered_mean_motion = no/(1.0 + del0);

} // InitStrVars()