// 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];
}
/*-----------------------------------------------------------------------------
* 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
/*-----------------------------------------------------------------------------
* 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));
}
}
}
/*-----------------------------------------------------------------------------
* 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
}
/////////////////////////////////////////////////////////////////////////////
// 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()