2014-03-06 08:30:59 -07:00
# include <stdio.h>
# include <string.h>
# include <stdlib.h>
# include <math.h>
2018-02-27 14:36:04 -07:00
# include <ctype.h>
2014-03-06 08:30:59 -07:00
# include "sgdp4h.h"
# include "satutl.h"
# include <getopt.h>
# define LIM 128
2022-08-06 22:23:15 -06:00
# define XKE 0.07436680 // Guassian Gravitational Constant
2014-03-06 08:30:59 -07:00
# define XKMPER 6378.135
# define AE 1.0
# define XMNPDA 1440.0
# define R2D 180.0 / M_PI
# define D2R M_PI / 180.0
extern double SGDP4_jd0 ;
2015-04-20 15:46:24 -06:00
// Return x modulo y [0,y)
2022-08-06 22:23:15 -06:00
double
modulo ( double x , double y )
2015-04-20 15:46:24 -06:00
{
2022-08-06 22:23:15 -06:00
x = fmod ( x , y ) ;
if ( x < 0.0 )
x + = y ;
2015-04-20 15:46:24 -06:00
return x ;
}
2014-03-06 08:30:59 -07:00
// Dot product
2022-08-06 22:23:15 -06:00
float
dot ( xyz_t a , xyz_t b )
2014-03-06 08:30:59 -07:00
{
2022-08-06 22:23:15 -06:00
return a . x * b . x + a . y * b . y + a . z * b . z ;
2014-03-06 08:30:59 -07:00
}
// Magnitude
2022-08-06 22:23:15 -06:00
double
magnitude ( xyz_t a )
2014-03-06 08:30:59 -07:00
{
2022-08-06 22:23:15 -06:00
return sqrt ( dot ( a , a ) ) ;
2014-03-06 08:30:59 -07:00
}
// Cross product
2022-08-06 22:23:15 -06:00
xyz_t
cross ( xyz_t a , xyz_t b )
2014-03-06 08:30:59 -07:00
{
xyz_t c ;
2022-08-06 22:23:15 -06:00
c . x = a . y * b . z - a . z * b . y ;
c . y = a . z * b . x - a . x * b . z ;
c . z = a . x * b . y - a . y * b . x ;
2014-03-06 08:30:59 -07:00
return c ;
}
// Compute Date from Julian Day
2022-08-06 22:23:15 -06:00
void
mjd2date ( double mjd , int * year , int * month , double * day )
2014-03-06 08:30:59 -07:00
{
2022-08-06 22:23:15 -06:00
double f , jd ;
int z , alpha , a , b , c , d , e ;
jd = mjd + 2400000.5 ;
jd + = 0.5 ;
z = floor ( jd ) ;
f = fmod ( jd , 1. ) ;
if ( z < 2299161 )
a = z ;
else
{
alpha = floor ( ( z - 1867216.25 ) / 36524.25 ) ;
a = z + 1 + alpha - floor ( alpha / 4. ) ;
}
b = a + 1524 ;
c = floor ( ( b - 122.1 ) / 365.25 ) ;
d = floor ( 365.25 * c ) ;
e = floor ( ( b - d ) / 30.6001 ) ;
* day = b - d - floor ( 30.6001 * e ) + f ;
if ( e < 14 )
* month = e - 1 ;
2014-03-06 08:30:59 -07:00
else
2022-08-06 22:23:15 -06:00
* month = e - 13 ;
2014-03-06 08:30:59 -07:00
2022-08-06 22:23:15 -06:00
if ( * month > 2 )
* year = c - 4716 ;
2014-03-06 08:30:59 -07:00
else
2022-08-06 22:23:15 -06:00
* year = c - 4715 ;
2014-03-06 08:30:59 -07:00
return ;
}
// MJD to DOY
2022-08-06 22:23:15 -06:00
double
mjd2doy ( double mjd , int * yr )
2014-03-06 08:30:59 -07:00
{
2022-08-06 22:23:15 -06:00
int year , month , k = 2 ;
double day , doy ;
2014-03-06 08:30:59 -07:00
2022-08-06 22:23:15 -06:00
mjd2date ( mjd , & year , & month , & day ) ;
2014-03-06 08:30:59 -07:00
2022-08-06 22:23:15 -06:00
if ( year % 4 = = 0 & & year % 400 ! = 0 )
k = 1 ;
2014-03-06 08:30:59 -07:00
2022-08-06 22:23:15 -06:00
doy =
floor ( 275.0 * month / 9.0 ) - k * floor ( ( month + 9.0 ) / 12.0 ) + day - 30 ;
* yr = year ;
2014-03-06 08:30:59 -07:00
return doy ;
}
// Clasical elements
2022-08-06 22:23:15 -06:00
orbit_t
classel ( int ep_year , double ep_day , xyz_t r , xyz_t v )
2014-03-06 08:30:59 -07:00
{
int i ;
2022-08-06 22:23:15 -06:00
double rm , vm , vm2 , rvm , mu = 1.0 ; ;
double chi , xp , yp , sx , cx , b , ee ;
double a , ecc , incl , node , peri , mm , n ;
xyz_t h , e , kk , nn ;
2014-03-06 08:30:59 -07:00
orbit_t orb ;
2022-08-06 22:23:15 -06:00
r . x / = XKMPER ;
r . y / = XKMPER ;
r . z / = XKMPER ;
v . x / = ( XKE * XKMPER / AE * XMNPDA / 86400.0 ) ;
v . y / = ( XKE * XKMPER / AE * XMNPDA / 86400.0 ) ;
v . z / = ( XKE * XKMPER / AE * XMNPDA / 86400.0 ) ;
rm = magnitude ( r ) ;
vm2 = dot ( v , v ) ;
rvm = dot ( r , v ) ;
h = cross ( r , v ) ;
chi = dot ( h , h ) / mu ;
e . x = ( vm2 / mu - 1.0 / rm ) * r . x - rvm / mu * v . x ;
e . y = ( vm2 / mu - 1.0 / rm ) * r . y - rvm / mu * v . y ;
e . z = ( vm2 / mu - 1.0 / rm ) * r . z - rvm / mu * v . z ;
a = pow ( 2.0 / rm - vm2 / mu , - 1 ) ;
ecc = magnitude ( e ) ;
incl = acos ( h . z / magnitude ( h ) ) * R2D ;
kk . x = 0.0 ;
kk . y = 0.0 ;
kk . z = 1.0 ;
nn = cross ( kk , h ) ;
if ( nn . x = = 0.0 & & nn . y = = 0.0 )
nn . x = 1.0 ;
node = atan2 ( nn . y , nn . x ) * R2D ;
if ( node < 0.0 )
node + = 360.0 ;
peri = acos ( dot ( nn , e ) / ( magnitude ( nn ) * ecc ) ) * R2D ;
if ( e . z < 0.0 )
peri = 360.0 - peri ;
if ( peri < 0.0 )
peri + = 360.0 ;
2014-03-06 08:30:59 -07:00
// Elliptic motion
2022-08-06 22:23:15 -06:00
if ( ecc < 1.0 )
{
xp = ( chi - rm ) / ecc ;
yp = rvm / ecc * sqrt ( chi / mu ) ;
b = a * sqrt ( 1.0 - ecc * ecc ) ;
cx = xp / a + ecc ;
sx = yp / b ;
ee = atan2 ( sx , cx ) ;
n = XKE * sqrt ( mu / ( a * a * a ) ) ;
mm = ( ee - ecc * sx ) * R2D ;
}
if ( mm < 0.0 )
mm + = 360.0 ;
2014-03-06 08:30:59 -07:00
// Fill
2022-08-06 22:23:15 -06:00
orb . satno = 0 ;
orb . eqinc = incl * D2R ;
orb . ascn = node * D2R ;
orb . argp = peri * D2R ;
orb . mnan = mm * D2R ;
orb . ecc = ecc ;
orb . rev = XKE * pow ( a , - 3.0 / 2.0 ) * XMNPDA / ( 2.0 * M_PI ) ;
orb . bstar = 0.0 ;
orb . ep_year = ep_year ;
orb . ep_day = ep_day ;
orb . norb = 0 ;
2014-03-06 08:30:59 -07:00
return orb ;
}
2022-08-06 22:23:15 -06:00
orbit_t
rv2el ( int satno , double mjd , xyz_t r0 , xyz_t v0 )
2014-03-06 08:30:59 -07:00
{
2022-08-06 22:23:15 -06:00
int i , imode ;
orbit_t orb [ 5 ] , orb1 [ 5 ] ;
xyz_t r , v ;
2014-03-06 08:30:59 -07:00
kep_t kep ;
2022-08-06 22:23:15 -06:00
char line1 [ 70 ] , line2 [ 70 ] ;
2014-03-06 08:30:59 -07:00
int ep_year ;
double ep_day ;
// Epoch
2022-08-06 22:23:15 -06:00
ep_day = mjd2doy ( mjd , & ep_year ) ;
2014-03-06 08:30:59 -07:00
// Initial guess
2022-08-06 22:23:15 -06:00
orb [ 0 ] = classel ( ep_year , ep_day , r0 , v0 ) ;
orb [ 0 ] . satno = satno ;
for ( i = 0 ; i < 4 ; i + + )
{
// Propagate
imode = init_sgdp4 ( & orb [ i ] ) ;
imode = satpos_xyz ( mjd + 2400000.5 , & r , & v ) ;
// Compute initial orbital elements
orb1 [ i ] = classel ( ep_year , ep_day , r , v ) ;
// Adjust
orb [ i + 1 ] . rev = orb [ i ] . rev + orb [ 0 ] . rev - orb1 [ i ] . rev ;
orb [ i + 1 ] . ascn = orb [ i ] . ascn + orb [ 0 ] . ascn - orb1 [ i ] . ascn ;
orb [ i + 1 ] . argp = orb [ i ] . argp + orb [ 0 ] . argp - orb1 [ i ] . argp ;
orb [ i + 1 ] . mnan = orb [ i ] . mnan + orb [ 0 ] . mnan - orb1 [ i ] . mnan ;
orb [ i + 1 ] . eqinc = orb [ i ] . eqinc + orb [ 0 ] . eqinc - orb1 [ i ] . eqinc ;
orb [ i + 1 ] . ecc = orb [ i ] . ecc + orb [ 0 ] . ecc - orb1 [ i ] . ecc ;
orb [ i + 1 ] . ep_year = orb [ i ] . ep_year ;
orb [ i + 1 ] . ep_day = orb [ i ] . ep_day ;
orb [ i + 1 ] . satno = orb [ i ] . satno ;
orb [ i + 1 ] . norb = orb [ i ] . norb ;
orb [ i + 1 ] . bstar = orb [ i ] . bstar ;
// Keep in range
if ( orb [ i + 1 ] . ecc < 0.0 )
orb [ i + 1 ] . ecc = 0.0 ;
if ( orb [ i + 1 ] . eqinc < 0.0 )
orb [ i + 1 ] . eqinc = 0.0 ;
}
orb [ i ] . mnan = modulo ( orb [ i ] . mnan , 2.0 * M_PI ) ;
orb [ i ] . ascn = modulo ( orb [ i ] . ascn , 2.0 * M_PI ) ;
orb [ i ] . argp = modulo ( orb [ i ] . argp , 2.0 * M_PI ) ;
2015-04-16 01:30:49 -06:00
2014-03-06 08:30:59 -07:00
return orb [ i ] ;
}
// Format TLE
2022-08-06 22:23:15 -06:00
void
format_tle ( orbit_t orb , char * line1 , char * line2 )
2014-03-06 08:30:59 -07:00
{
2022-08-06 22:23:15 -06:00
int i , csum ;
char sbstar [ ] = " 00000-0 " , bstar [ 13 ] ;
2014-03-06 08:30:59 -07:00
// Format Bstar term
2022-08-06 22:23:15 -06:00
if ( fabs ( orb . bstar ) > 1e-9 )
{
sprintf ( bstar , " %11.4e " , 10 * orb . bstar ) ;
sbstar [ 0 ] = bstar [ 0 ] ;
sbstar [ 1 ] = bstar [ 1 ] ;
sbstar [ 2 ] = bstar [ 3 ] ;
sbstar [ 3 ] = bstar [ 4 ] ;
sbstar [ 4 ] = bstar [ 5 ] ;
sbstar [ 5 ] = bstar [ 6 ] ;
sbstar [ 6 ] = bstar [ 8 ] ;
sbstar [ 7 ] = bstar [ 10 ] ;
sbstar [ 8 ] = ' \0 ' ;
}
2014-03-06 08:30:59 -07:00
// Print lines
2022-08-06 22:23:15 -06:00
sprintf ( line1 , " 1 %05dU %-8s %2d%012.8f .00000000 00000-0 %8s 0 0 " ,
orb . satno , orb . desig , orb . ep_year - 2000 , orb . ep_day , sbstar ) ;
sprintf ( line2 , " 2 %05d %8.4f %8.4f %07.0f %8.4f %8.4f %11.8f 0 " ,
orb . satno , DEG ( orb . eqinc ) , DEG ( orb . ascn ) , 1E7 * orb . ecc ,
DEG ( orb . argp ) , DEG ( orb . mnan ) , orb . rev ) ;
2014-03-06 08:30:59 -07:00
// Compute checksums
2022-08-06 22:23:15 -06:00
for ( i = 0 , csum = 0 ; i < strlen ( line1 ) ; i + + )
{
if ( isdigit ( line1 [ i ] ) )
csum + = line1 [ i ] - ' 0 ' ;
else if ( line1 [ i ] = = ' - ' )
csum + + ;
}
sprintf ( line1 , " %s%d " , line1 , csum % 10 ) ;
for ( i = 0 , csum = 0 ; i < strlen ( line2 ) ; i + + )
{
if ( isdigit ( line2 [ i ] ) )
csum + = line2 [ i ] - ' 0 ' ;
else if ( line2 [ i ] = = ' - ' )
csum + + ;
}
sprintf ( line2 , " %s%d " , line2 , csum % 10 ) ;
2014-03-06 08:30:59 -07:00
return ;
}
// Present nfd
2022-08-06 22:23:15 -06:00
void
nfd_now ( char * s )
2014-03-06 08:30:59 -07:00
{
time_t rawtime ;
struct tm * ptm ;
// Get UTC time
2022-08-06 22:23:15 -06:00
time ( & rawtime ) ;
ptm = gmtime ( & rawtime ) ;
sprintf ( s , " %04d-%02d-%02dT%02d:%02d:%02d " , ptm - > tm_year + 1900 ,
ptm - > tm_mon + 1 , ptm - > tm_mday , ptm - > tm_hour , ptm - > tm_min ,
ptm - > tm_sec ) ;
2014-03-06 08:30:59 -07:00
return ;
}
// Compute Julian Day from Date
2022-08-06 22:23:15 -06:00
double
date2mjd ( int year , int month , double day )
2014-03-06 08:30:59 -07:00
{
2022-08-06 22:23:15 -06:00
int a , b ;
2014-03-06 08:30:59 -07:00
double jd ;
2022-08-06 22:23:15 -06:00
if ( month < 3 )
{
year - - ;
month + = 12 ;
}
2014-03-06 08:30:59 -07:00
2022-08-06 22:23:15 -06:00
a = floor ( year / 100. ) ;
b = 2. - a + floor ( a / 4. ) ;
2014-03-06 08:30:59 -07:00
2022-08-06 22:23:15 -06:00
if ( year < 1582 )
b = 0 ;
if ( year = = 1582 & & month < 10 )
b = 0 ;
if ( year = = 1582 & & month = = 10 & & day < = 4 )
b = 0 ;
2014-03-06 08:30:59 -07:00
2022-08-06 22:23:15 -06:00
jd =
floor ( 365.25 * ( year + 4716 ) ) + floor ( 30.6001 * ( month + 1 ) ) + day + b -
1524.5 ;
2014-03-06 08:30:59 -07:00
2022-08-06 22:23:15 -06:00
return jd - 2400000.5 ;
2014-03-06 08:30:59 -07:00
}
2014-12-03 11:08:02 -07:00
// DOY to MJD
2022-08-06 22:23:15 -06:00
double
doy2mjd ( int year , double doy )
2014-12-03 11:08:02 -07:00
{
2022-08-06 22:23:15 -06:00
int month , k = 2 ;
2014-12-03 11:08:02 -07:00
double day ;
2022-08-06 22:23:15 -06:00
if ( year % 4 = = 0 & & year % 400 ! = 0 )
k = 1 ;
2014-12-03 11:08:02 -07:00
2022-08-06 22:23:15 -06:00
month = floor ( 9.0 * ( k + doy ) / 275.0 + 0.98 ) ;
2014-12-03 11:08:02 -07:00
2022-08-06 22:23:15 -06:00
if ( doy < 32 )
month = 1 ;
2014-12-03 11:08:02 -07:00
2022-08-06 22:23:15 -06:00
day =
doy - floor ( 275.0 * month / 9.0 ) + k * floor ( ( month + 9.0 ) / 12.0 ) +
30.0 ;
return date2mjd ( year , month , day ) ;
2014-12-03 11:08:02 -07:00
}
2014-03-06 08:30:59 -07:00
// nfd2mjd
2022-08-06 22:23:15 -06:00
double
nfd2mjd ( char * date )
2014-03-06 08:30:59 -07:00
{
2022-08-06 22:23:15 -06:00
int year , month , day , hour , min , sec ;
double mjd , dday ;
2014-03-06 08:30:59 -07:00
2022-08-06 22:23:15 -06:00
sscanf ( date , " %04d-%02d-%02dT%02d:%02d:%02d " , & year , & month , & day , & hour ,
& min , & sec ) ;
dday = day + hour / 24.0 + min / 1440.0 + sec / 86400.0 ;
2014-03-06 08:30:59 -07:00
2022-08-06 22:23:15 -06:00
mjd = date2mjd ( year , month , dday ) ;
2014-03-06 08:30:59 -07:00
return mjd ;
}
2022-08-06 22:23:15 -06:00
void
usage ( void )
2014-03-06 08:30:59 -07:00
{
2022-08-06 22:23:15 -06:00
printf
( " propagate c:i:t:m:e:d: \n \n Propagates orbital elements to a new epoch using the SGP4/SDP4 model. \n Default operation propagates classfd.tle to now, \n \n -c input catalog \n -i Satellite number \n -t New epoch (YYYY-MM-DDTHH:MM:SS) \n -m New epoch (MJD) \n -e New epoch (YYDDD.ddddddd) \n -d New COSPAR designation \n " ) ;
2014-03-06 08:30:59 -07:00
return ;
}
2022-08-06 22:23:15 -06:00
int
main ( int argc , char * argv [ ] )
2014-03-06 08:30:59 -07:00
{
2022-08-06 22:23:15 -06:00
int imode , satno = 0 , arg , desigflag = 0 ;
2014-03-06 08:30:59 -07:00
FILE * file ;
orbit_t orb ;
2022-08-06 22:23:15 -06:00
xyz_t r , v ;
char tlefile [ LIM ] , nfd [ 32 ] ;
char line1 [ 80 ] , line2 [ 80 ] , desig [ 20 ] ;
double mjd , epoch , ep_day , bstar ;
2014-03-06 08:30:59 -07:00
char * env ;
2014-12-03 11:08:02 -07:00
int ep_year ;
2014-03-06 08:30:59 -07:00
// Get environment variable
2022-08-06 22:23:15 -06:00
env = getenv ( " ST_TLEDIR " ) ;
sprintf ( tlefile , " %s/classfd.tle " , env ) ;
2014-03-06 08:30:59 -07:00
// Set date
2022-08-06 22:23:15 -06:00
nfd_now ( nfd ) ;
mjd = nfd2mjd ( nfd ) ;
2014-03-06 08:30:59 -07:00
// Decode options
2022-08-06 22:23:15 -06:00
while ( ( arg = getopt ( argc , argv , " c:i:t:m:he:d: " ) ) ! = - 1 )
{
switch ( arg )
{
case ' t ' :
strcpy ( nfd , optarg ) ;
mjd = nfd2mjd ( nfd ) ;
break ;
case ' e ' :
epoch = ( double ) atof ( optarg ) ;
ep_year = ( int ) floor ( epoch / 1000.0 ) ;
ep_day = epoch - 1000 * ep_year ;
printf ( " %d %f \n " , ep_year , ep_day ) ;
if ( ep_year < 50 )
ep_year + = 2000 ;
else
ep_year + = 1900 ;
mjd = doy2mjd ( ep_year , ep_day ) ;
break ;
case ' d ' :
strcpy ( desig , optarg ) ;
desigflag = 1 ;
break ;
case ' m ' :
mjd = ( double ) atof ( optarg ) ;
break ;
case ' c ' :
strcpy ( tlefile , optarg ) ;
break ;
case ' i ' :
satno = atoi ( optarg ) ;
break ;
case ' h ' :
usage ( ) ;
return 0 ;
break ;
default :
usage ( ) ;
return 0 ;
}
2014-03-06 08:30:59 -07:00
}
// Open file
2022-08-06 22:23:15 -06:00
file = fopen ( tlefile , " r " ) ;
while ( read_twoline ( file , satno , & orb ) = = 0 )
{
format_tle ( orb , line1 , line2 ) ;
// printf("Input:\n%s\n%s\n",line1,line2);
if ( desigflag = = 0 )
strcpy ( desig , orb . desig ) ;
bstar = orb . bstar ;
// Propagate
imode = init_sgdp4 ( & orb ) ;
imode = satpos_xyz ( mjd + 2400000.5 , & r , & v ) ;
// Convert
orb = rv2el ( orb . satno , mjd , r , v ) ;
// Copy back
strcpy ( orb . desig , desig ) ;
orb . bstar = bstar ;
format_tle ( orb , line1 , line2 ) ;
printf ( " %s \n %s \n " , line1 , line2 ) ;
}
fclose ( file ) ;
2014-03-06 08:30:59 -07:00
return 0 ;
}