// 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; }