// Resets the Position, Velocity in rvfile with Position, Velocity
// corresponding to TLE in tlefile.
void resetRV(const char* rvfile, const char* init_tlefile) {
// Read data from TLE file
FILE *tle_fptr;
tle_fptr = fopen( init_tlefile, "r" );
if (tle_fptr == NULL) {
printf("Error opening file: %s\n", init_tlefile);
return;
}
char tle_line1[130], tle_line2[130];
fgets(tle_line1, 130, tle_fptr);
fgets(tle_line2, 130, tle_fptr);
fclose(tle_fptr);
// Convert TLE data to Time, Position, Velocity data
const gravconsttype GRAV_CONSTS = wgs72; // Apparently the twoline2rv function needs this.
elsetrec satrecord; // This too: and object containing the satellite information.
double startmfe, stopmfe, deltamin;
twoline2rv(tle_line1, tle_line2, 'v', 'm', 'i', GRAV_CONSTS, startmfe, stopmfe, deltamin, satrecord);
// Magic inputs... really sketchy and something to come back to.
// Write Time, Position, Velocity data to rvfile
double rvtime, posn[3], vel[3];
sgp4(GRAV_CONSTS, satrecord, 0.0, posn, vel); // Apparently this is the only way to extract the posn/vel data :S
rvtime = satrecord.jdsatepoch;
writeRV(rvfile, rvtime, posn, vel);
}
// 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];
}
int computeCartesianToTwoLineElementResiduals( const gsl_vector* independentVariables,
void* parameters,
gsl_vector* residuals )
{
const Vector6 targetState
= static_cast< CartesianToTwoLineElementsParameters< Vector6 >* >( parameters )->targetState;
const Tle templateTle
= static_cast< CartesianToTwoLineElementsParameters< Vector6 >* >( parameters )->templateTle;
// Create a TLE object with the mean TLE elements generated by the root-finder.
Tle tle = templateTle;
Real meanInclination = sml::computeModulo( std::fabs( gsl_vector_get( independentVariables, 0 ) ), 180.0 );
Real meanRightAscendingNode = sml::computeModulo( gsl_vector_get( independentVariables, 1 ), 360.0 );
Real meanEccentricity = gsl_vector_get( independentVariables, 2 );
if ( meanEccentricity < 0.0 )
{
meanEccentricity = std::fabs( gsl_vector_get( independentVariables, 2 ) );
}
if ( meanEccentricity > 0.999 )
{
meanEccentricity = 0.99;
}
Real meanArgumentPerigee = sml::computeModulo( gsl_vector_get( independentVariables, 3 ), 360.0 );
Real meanMeanAnomaly = sml::computeModulo( gsl_vector_get( independentVariables, 4 ), 360.0 );
Real meanMeanMotion = std::fabs( gsl_vector_get( independentVariables, 5 ) );
tle.updateMeanElements( meanInclination,
meanRightAscendingNode,
meanEccentricity,
meanArgumentPerigee,
meanMeanAnomaly,
meanMeanMotion );
// Propagate the TLE object to the specified epoch using the SGP4 propagator.
SGP4 sgp4( tle );
Eci cartesianState = sgp4.FindPosition( 0.0 );
// Compute residuals by computing the difference between the Cartesian state generated by the
// SGP4 propagator and the target Cartesian state.
gsl_vector_set( residuals, astro::xPositionIndex, cartesianState.Position( ).x - targetState[ astro::xPositionIndex ] );
gsl_vector_set( residuals, astro::yPositionIndex, cartesianState.Position( ).y - targetState[ astro::yPositionIndex ] );
gsl_vector_set( residuals, astro::zPositionIndex, cartesianState.Position( ).z - targetState[ astro::zPositionIndex ] );
gsl_vector_set( residuals, astro::xVelocityIndex, cartesianState.Velocity( ).x - targetState[ astro::xVelocityIndex ] );
gsl_vector_set( residuals, astro::yVelocityIndex, cartesianState.Velocity( ).y - targetState[ astro::yVelocityIndex ] );
gsl_vector_set( residuals, astro::zVelocityIndex, cartesianState.Velocity( ).z - targetState[ astro::zVelocityIndex ] );
return GSL_SUCCESS;
}
void gSatTEME::setMinSinceKepEpoch(double ai_minSinceKepEpoch)
{
double ro[3];
double vo[3];
gTimeSpan tSince( ai_minSinceKepEpoch/KMIN_PER_DAY);
gTime Epoch(satrec.jdsatepoch);
Epoch += tSince;
// call the propagator to get the initial state vector value
sgp4(CONSTANTS_SET, satrec, ai_minSinceKepEpoch, 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];
m_SubPoint = computeSubPoint( Epoch);
}
void gSatTEME::setEpoch(gTime ai_time)
{
gTime kepEpoch(satrec.jdsatepoch);
gTimeSpan tSince = ai_time - kepEpoch;
double ro[3];
double vo[3];
double dtsince = tSince.getDblSeconds()/KSEC_PER_MIN;
// call the propagator to get the initial state vector value
sgp4(CONSTANTS_SET, satrec, dtsince, 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];
m_SubPoint = computeSubPoint( ai_time);
}
// Propagates and updates the state estimate from tlefile to the current time
// and places the new position and velocity in the corresponding variables.
void currentOrbitState(const char* rvfile, const char* init_tlefile, const char* clockfile, double* rvtime, double posn[3], double vel[3]) {
const gravconsttype GRAV_CONSTS = wgs72;
// Check whether these are the right ones to hard-code.
elsetrec satrecord;
// This is an object which contains the satellite info.
// Initialise satrecord directly from TLE file...
// ... trust me, it's cleaner than using sgp4init()
FILE *tle_fptr;
tle_fptr = fopen( init_tlefile, "r" );
if (tle_fptr == NULL) {
printf("Error opening file: %s\n", init_tlefile);
return;
}
char tle_line1[130], tle_line2[130];
fgets(tle_line1, 130, tle_fptr);
fgets(tle_line2, 130, tle_fptr);
fclose(tle_fptr);
double startmfe, stopmfe, deltamin;
// The twoline2rv() function initialises the satrecord object
twoline2rv(tle_line1, tle_line2, 'v', 'm', 'i', GRAV_CONSTS, startmfe, stopmfe, deltamin, satrecord);
// Again, check whether these are the inputs we want.
// Propagate to new state
*rvtime = readClock(clockfile);
sgp4(GRAV_CONSTS, satrecord, 1440.0*(*rvtime - satrecord.jdsatepoch), posn, vel);
// rvtime, posn, vel will be stored in the pointers as outputs, but we'll write to file as well.
writeRV(rvfile, *rvtime, posn, vel);
}
void sgp_test (void)
{
struct vector pos[5], vel[5];
struct sgp_data satdata;
int satnumber=0, interval, j;
double delta, tsince;
struct tle_ascii sat_data[2];
gettestdata(&sat_data[0],&sat_data[1]);
delta = 360.0;
for (j=0;(j<5);j++)
{
int model=0;
printf("\n\n\n");
switch (j)
{
case 0 :
model = _SGP0;
printf("*** SGP0 ***\n\n");
satnumber = 0;
break;
case 1 :
model = _SGP4;
printf("*** SGP4 ***\n\n");
satnumber = 0;
break;
case 2 :
model = _SDP4;
printf("*** SDP4 ***\n\n");
satnumber = 1;
break;
case 3 :
model = _SGP8;
printf("*** SGP8 ***\n\n");
satnumber = 0;
break;
case 4 :
model = _SDP8;
printf("*** SDP8 ***\n\n");
satnumber = 1;
break;
}
printf("%s",sat_data[satnumber].l[1]);
printf("%s",sat_data[satnumber].l[2]);
printf("\n");
printf("\n");
Convert_Satellite_Data(sat_data[satnumber],&satdata);
for (interval=0;(interval<=4);interval++)
{
tsince = interval * delta;
switch (model)
{
case _SGP0:
sgp0(tsince,&pos[interval],&vel[interval],&satdata);
break;
case _SGP4:
sgp4(tsince,&pos[interval],&vel[interval],&satdata);
break;
case _SDP4:
sdp4(tsince,&pos[interval],&vel[interval],&satdata);
break;
case _SGP8:
sgp8(tsince,&pos[interval],&vel[interval],&satdata);
break;
case _SDP8:
sdp8(tsince,&pos[interval],&vel[interval],&satdata);
break;
}
Convert_Sat_State(&pos[interval],&vel[interval]);
pos[interval].v[3]=tsince;
}
printf(" TSINCE X Y Z \n");
for (interval=0;(interval<=4);interval++)
printf(" %4.1f %8.8f %8.8f %8.8f \n", pos[interval].v[3], pos[interval].v[0],pos[interval].v[1],pos[interval].v[2]);
printf("\n\n\n XDOT YDOT ZDOT \n");
for (interval=0;(interval<=4);interval++)
printf(" %9.8f %9.8f %9.8f \n",vel[interval].v[0],vel[interval].v[1],vel[interval].v[2]);
printf("\n");
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
//The Julan date in TT at the epoch used in the orbital propagation
//algorithm: 0:00 January 1 1950
const double epochDate=2433281.5;
double *deltaT;
size_t numTimes;
double SGP4Elements[7],elementEpochTime;
bool opsMode=0;
char opsChar;
bool gravityModel=0;
gravconsttype gravConstType;
mxArray *retMat;
double *retVec;
mxArray *errorMat;
double *errorVals;
if(nrhs<2||nrhs>6||nrhs==3) {
mexErrMsgTxt("Incorrect number of inputs.");
return;
}
if(nlhs>2) {
mexErrMsgTxt("Wrong number of outputs.");
return;
}
{
const size_t M=mxGetM(prhs[0]);
const size_t N=mxGetN(prhs[0]);
if(!((M==7&&N==1)||(N==7&&M==1))) {
mexErrMsgTxt("The SGP4 elements have the wrong dimensionality.");
return;
}
}
checkRealDoubleArray(prhs[0]);
{
size_t i;
double *theEls;
theEls=(double*)mxGetData(prhs[0]);
//Copy the elements into SGP4Elements, adjusting the units from SI
//to the values used here.
for(i=0;i<5;i++) {
SGP4Elements[i]=theEls[i];
}
//The multipliation by 60 changes the value from radians per second
//to radians per minute as desired by the SGP4 code.
SGP4Elements[5]=theEls[5]*60.0;
SGP4Elements[6]=theEls[6];
}
{
const size_t M=mxGetM(prhs[1]);
const size_t N=mxGetN(prhs[1]);
if((M!=1&&N!=1)||mxIsEmpty(prhs[1])) {
mexErrMsgTxt("The times have the wrong dimensionality.");
return;
}
numTimes=std::max(M,N);
}
checkRealDoubleArray(prhs[1]);
deltaT=(double*)mxGetData(prhs[1]);
if(nrhs>2&&!mxIsEmpty(prhs[2])&&!mxIsEmpty(prhs[3])) {
const double TT1=getDoubleFromMatlab(prhs[2]);
const double TT2=getDoubleFromMatlab(prhs[3]);
if(TT1>TT2) {
elementEpochTime=(TT1-epochDate)+TT2;
} else {
elementEpochTime=(TT2-epochDate)+TT1;
}
} else {
//No time is given. Make sure that the deep space propagator is not
//going to be used. Otherwise, the time value does not matter.
elementEpochTime=0;
if(2*pi/SGP4Elements[5]>=225) {
mexErrMsgTxt("The elements imply the use of the deep space propagator, but no time was given.");
return;
}
}
if(nrhs>4) {
opsMode=getBoolFromMatlab(prhs[4]);
}
if(opsMode==0) {
opsChar='a';
} else {
opsChar='i';
}
if(nrhs>5) {
gravityModel=getBoolFromMatlab(prhs[5]);
}
if(gravityModel==0) {
gravConstType=wgs72;
} else {
gravConstType=wgs84;
}
//Allocate space for the return values
retMat=mxCreateDoubleMatrix(6,numTimes,mxREAL);
retVec=(double*)mxGetData(retMat);
errorMat=mxCreateDoubleMatrix(numTimes,1,mxREAL);
errorVals=(double*)mxGetData(errorMat);
{
//This will be filled with the ephemeris information needed for
//propagating the satellite.
elsetrec satRec;
size_t i;
sgp4init(gravConstType,//Specified WGS-84 or WGS-72
opsChar,
elementEpochTime,//Epoch time of the orbital elements.
SGP4Elements[6],//BSTAR drag term.
SGP4Elements[0],//Eccentricity
SGP4Elements[2],//Argument of perigee
SGP4Elements[1],//Inclination
SGP4Elements[4],//Mean anomaly
SGP4Elements[5],//Mean motion
SGP4Elements[3],//Righ ascension of the ascending node.
satRec);//Gets filled by the function.
//Do the actual propagation.
//The division by 60 transforms the time value from seconds to
//minutes, as the SGP4 code wants the result in minutes.
for(i=0;i<numTimes;i++) {
double *r=retVec+6*i;
double *v=retVec+6*i+3;
int j;
sgp4(gravConstType, satRec, deltaT[i]/60.0, r, v);
//Convert the units from kilometers and kilometers per second
//to meters and meters per second.
for(j=0;j<3;j++) {
r[j]=1000*r[j];
v[j]=1000*v[j];
}
errorVals[i]=(double)satRec.error;
}
}
//Save the result
plhs[0]=retMat;
//If the error value is desired.
if(nlhs>1) {
plhs[1]=errorMat;
} else{
mxDestroyArray(errorMat);
}
}
int computeAtomResiduals( const gsl_vector* independentVariables,
void* parameters,
gsl_vector* residuals )
{
// Store parameters locally.
const Vector3 departurePosition = static_cast< AtomParameters< Real, Vector3 >* >(
parameters )->departurePosition;
const DateTime departureEpoch
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->departureEpoch;
const Vector3 targetPosition = static_cast< AtomParameters< Real, Vector3 >* >(
parameters )->targetPosition;
const Real timeOfFlight
= static_cast< AtomParameters< Real, Vector3 >* >(
parameters )->timeOfFlight;
const Real earthGravitationalParameter
= static_cast< AtomParameters< Real, Vector3 >* >(
parameters )->earthGravitationalParameter;
const Real earthMeanRadius
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->earthMeanRadius;
const Tle referenceTle
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->referenceTle;
const Real absoluteTolerance
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->absoluteTolerance;
const Real relativeTolerance
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->relativeTolerance;
const int maximumIterations
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->maximumIterations;
// Set Departure state [km; km/s].
std::vector< Real > departureVelocity( 3 );
for ( int i = 0; i < 3; i++ )
{
departureVelocity[ i ] = gsl_vector_get( independentVariables, i );
}
std::vector< Real > departureState( 6 );
for ( int i = 0; i < 3; i++ )
{
departureState[ i ] = departurePosition[ i ];
}
for ( int i = 0; i < 3; i++ )
{
departureState[ i + 3 ] = departureVelocity[ i ];
}
// Convert departure state to TLE.
std::string dummyString = "";
int dummyint = 0;
const Tle departureTle = convertCartesianStateToTwoLineElements(
departureState,
departureEpoch,
dummyString,
dummyint,
referenceTle,
earthGravitationalParameter,
earthMeanRadius,
absoluteTolerance,
relativeTolerance,
maximumIterations );
// Propagate departure TLE by time-of-flight using SGP4 propagator.
SGP4 sgp4( departureTle );
DateTime arrivalEpoch = departureEpoch.AddSeconds( timeOfFlight );
Eci arrivalState = sgp4.FindPosition( arrivalEpoch );
// Evaluate system of non-linear equations and store residuals.
gsl_vector_set( residuals, 0,
( arrivalState.Position( ).x - targetPosition[ 0 ] ) / earthMeanRadius );
gsl_vector_set( residuals, 1,
( arrivalState.Position( ).y - targetPosition[ 1 ] ) / earthMeanRadius );
gsl_vector_set( residuals, 2,
( arrivalState.Position( ).z - targetPosition[ 2 ] ) / earthMeanRadius );
return GSL_SUCCESS;
}
const std::pair< Vector3, Vector3 > executeAtomSolver(
const Vector3& departurePosition,
const DateTime& departureEpoch,
const Vector3& arrivalPosition,
const Real timeOfFlight,
const Vector3& departureVelocityGuess,
std::string& solverStatusSummary,
int& numberOfIterations,
const Tle& referenceTle,
const Real earthGravitationalParameter,
const Real earthMeanRadius,
const Real absoluteTolerance,
const Real relativeTolerance,
const int maximumIterations )
{
// Set up parameters for residual function.
AtomParameters< Real, Vector3 > parameters( departurePosition,
departureEpoch,
arrivalPosition,
timeOfFlight,
earthGravitationalParameter,
earthMeanRadius,
referenceTle,
absoluteTolerance,
relativeTolerance,
maximumIterations );
// Set up residual function.
gsl_multiroot_function atomFunction
= {
&computeAtomResiduals< Real, Vector3 >, 3, ¶meters
};
// Set initial guess.
gsl_vector* initialGuess = gsl_vector_alloc( 3 );
for ( int i = 0; i < 3; i++ )
{
gsl_vector_set( initialGuess, i, departureVelocityGuess[ i ] );
}
// Set up solver type (derivative free).
const gsl_multiroot_fsolver_type* solverType = gsl_multiroot_fsolver_hybrids;
// Allocate memory for solver.
gsl_multiroot_fsolver* solver = gsl_multiroot_fsolver_alloc( solverType, 3 );
// Set solver to use residual function with initial guess.
gsl_multiroot_fsolver_set( solver, &atomFunction, initialGuess );
// Declare current solver status and iteration counter.
int solverStatus = false;
int counter = 0;
// Set up buffer to store solver status summary table.
std::ostringstream summary;
// Print header for summary table to buffer.
summary << printAtomSolverStateTableHeader( );
do
{
// Print current state of solver for summary table.
summary << printAtomSolverState( counter, solver );
// Increment iteration counter.
++counter;
// Execute solver iteration.
solverStatus = gsl_multiroot_fsolver_iterate( solver );
// Check if solver is stuck; if it is stuck, break from loop.
if ( solverStatus )
{
std::cerr << "GSL solver status: " << solverStatus << std::endl;
std::cerr << summary.str( ) << std::endl;
std::cerr << std::endl;
throw std::runtime_error( "ERROR: Non-linear solver is stuck!" );
}
// Check if root has been found (within tolerance).
solverStatus = gsl_multiroot_test_delta(
solver->dx, solver->x, absoluteTolerance, relativeTolerance );
} while ( solverStatus == GSL_CONTINUE && counter < maximumIterations );
// Save number of iterations.
numberOfIterations = counter - 1;
// Print final status of solver to buffer.
summary << std::endl;
summary << "Status of non-linear solver: " << gsl_strerror( solverStatus ) << std::endl;
summary << std::endl;
// Write buffer contents to solver status summary string.
solverStatusSummary = summary.str( );
// Store final departure velocity.
Vector3 departureVelocity = departureVelocityGuess;
for ( int i = 0; i < 3; i++ )
{
departureVelocity[ i ] = gsl_vector_get( solver->x, i );
}
// Set departure state [km/s].
std::vector< Real > departureState( 6 );
for ( int i = 0; i < 3; i++ )
{
departureState[ i ] = departurePosition[ i ];
}
for ( int i = 0; i < 3; i++ )
{
departureState[ i + 3 ] = departureVelocity[ i ];
}
// Convert departure state to TLE.
std::string dummyString = "";
int dummyint = 0;
const Tle departureTle = convertCartesianStateToTwoLineElements< Real >(
departureState,
departureEpoch,
dummyString,
dummyint,
referenceTle,
earthGravitationalParameter,
earthMeanRadius,
absoluteTolerance,
relativeTolerance,
maximumIterations );
// Propagate departure TLE by time-of-flight using SGP4 propagator.
SGP4 sgp4( departureTle );
DateTime arrivalEpoch = departureEpoch.AddSeconds( timeOfFlight );
Eci arrivalState = sgp4.FindPosition( arrivalEpoch );
Vector3 arrivalVelocity = departureVelocity;
arrivalVelocity[ 0 ] = arrivalState.Velocity( ).x;
arrivalVelocity[ 1 ] = arrivalState.Velocity( ).y;
arrivalVelocity[ 2 ] = arrivalState.Velocity( ).z;
// Free up memory.
gsl_multiroot_fsolver_free( solver );
gsl_vector_free( initialGuess );
// Return departure and arrival velocities.
return std::make_pair< Vector3, Vector3 >( departureVelocity, arrivalVelocity );
}
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
}
/* ��GPS�������������� */
void GetTLEFromGPS(elsetrec *satrecFromGPS,double orbInfoGPS[6],double *tTime)
{
double MeanKepler[6];
double loops,RMS,PosNorm[3],endwhile;
double PosInTEME[3],VelInTEME[3];
double error[6],JToTEME[3][3],TEMEToJ[3][3];
double PosInJ[3],VelInJ[3],meanorbInfo[6];
int i,j,k,m;
cordMtxJToTEMEGet(JToTEME,tTime);
mtxInv((double *)TEMEToJ,(double *)JToTEME,3);
for (m=0;m<6;m++)
{
meanorbInfo[m] = orbInfoGPS[m];
}
Get_KplInfo(MeanKepler,meanorbInfo);
satrecFromGPS->bstar = 0.0002;
satrecFromGPS->inclo = MeanKepler[2]*PI/180;
satrecFromGPS->nodeo = MeanKepler[3]*PI/180;
satrecFromGPS->ecco = MeanKepler[1];
satrecFromGPS->argpo = MeanKepler[4]*PI/180;
satrecFromGPS->mo = MeanKepler[5]*PI/180;
satrecFromGPS->no = 60*sqrt(GM/(MeanKepler[0]*MeanKepler[0]*MeanKepler[0]));
loops = 0;
RMS = 10000;
endwhile = 1;
while (endwhile !=0)
{
loops = loops+1;
sgp4init(satrecFromGPS);
sgp4(PosInTEME,VelInTEME,satrecFromGPS,0);
mtxMtp((double *)PosInJ,(double *)TEMEToJ,3,3,(double *)PosInTEME,3,1);
mtxMtp((double *)VelInJ,(double *)TEMEToJ,3,3,(double *)VelInTEME,3,1);
for (i=0;i<3;i++)
{
error[i] = orbInfoGPS[i]-PosInJ[i];
PosNorm[i] = error[i];
}
for (j=0;j<3;j++)
{
error[j+3] = orbInfoGPS[j+3]-VelInJ[j];
}
RMS = sqrt(norm(PosNorm,3)/3);
if ((RMS >= 0.01) && (loops <= 20))
{
for (k=0;k<6;k++)
{
meanorbInfo[k] = meanorbInfo[k]+error[k];
}
Get_KplInfo(MeanKepler,meanorbInfo);
satrecFromGPS->bstar = 0.0002;
satrecFromGPS->inclo = MeanKepler[2]*PI/180;
satrecFromGPS->nodeo = MeanKepler[3]*PI/180;
satrecFromGPS->ecco = MeanKepler[1];
satrecFromGPS->argpo = MeanKepler[4]*PI/180;
satrecFromGPS->mo = MeanKepler[5]*PI/180;
satrecFromGPS->no = 60*sqrt(GM/(MeanKepler[0]*MeanKepler[0]*MeanKepler[0]));
}
else
{
endwhile = 0;
}
}
return;
}
