2014-09-25 00:48:34 -06:00
# include <stdio.h>
# include <string.h>
# include <stdlib.h>
# include <math.h>
2018-02-27 09:19:34 -07:00
# include <ctype.h>
2014-09-25 00:48:34 -06:00
# include "cpgplot.h"
# include "sgdp4h.h"
# include "satutl.h"
# include <getopt.h>
# define LIM 128
# define XKE 0.07436680 // Guassian Gravitational Constant
# 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 ;
// Dot product
float dot ( xyz_t a , xyz_t b )
{
return a . x * b . x + a . y * b . y + a . z * b . z ;
}
// Return x modulo y [0,y)
double modulo ( double x , double y )
{
x = fmod ( x , y ) ;
if ( x < 0.0 ) x + = y ;
return x ;
}
// Magnitude
double magnitude ( xyz_t a )
{
return sqrt ( dot ( a , a ) ) ;
}
// Cross product
xyz_t cross ( xyz_t a , xyz_t b )
{
xyz_t c ;
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 ;
return c ;
}
// Compute Date from Julian Day
void mjd2date ( double mjd , int * year , int * month , double * day )
{
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 ;
else
* month = e - 13 ;
if ( * month > 2 )
* year = c - 4716 ;
else
* year = c - 4715 ;
return ;
}
// MJD to DOY
double mjd2doy ( double mjd , int * yr )
{
int year , month , k = 2 ;
double day , doy ;
mjd2date ( mjd , & year , & month , & day ) ;
if ( year % 4 = = 0 & & year % 400 ! = 0 )
k = 1 ;
doy = floor ( 275.0 * month / 9.0 ) - k * floor ( ( month + 9.0 ) / 12.0 ) + day - 30 ;
* yr = year ;
return doy ;
}
// Clasical elements
orbit_t classel ( int ep_year , double ep_day , xyz_t r , xyz_t v )
{
int i ;
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 ;
orbit_t orb ;
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 ;
// Elliptic motion
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 ;
// Fill
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 ;
return orb ;
}
orbit_t rv2el ( int satno , double mjd , xyz_t r0 , xyz_t v0 )
{
int i , imode ;
orbit_t orb [ 5 ] , orb1 [ 5 ] ;
xyz_t r , v ;
kep_t kep ;
char line1 [ 70 ] , line2 [ 70 ] ;
int ep_year ;
double ep_day ;
// Epoch
ep_day = mjd2doy ( mjd , & ep_year ) ;
// Initial guess
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 ;
}
2015-04-16 01:30:49 -06:00
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 ) ;
2014-09-25 00:48:34 -06:00
return orb [ i ] ;
}
// Format TLE
void format_tle ( orbit_t orb , char * line1 , char * line2 )
{
int i , csum ;
char sbstar [ ] = " 00000-0 " , bstar [ 13 ] ;
// Format Bstar term
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 ' ;
}
// Print lines
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 ) ;
// Compute checksums
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 ) ;
return ;
}
// Present nfd
void nfd_now ( char * s )
{
time_t rawtime ;
struct tm * ptm ;
// Get UTC time
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 ) ;
return ;
}
// Compute Julian Day from Date
double date2mjd ( int year , int month , double day )
{
int a , b ;
double jd ;
if ( month < 3 ) {
year - - ;
month + = 12 ;
}
a = floor ( year / 100. ) ;
b = 2. - a + floor ( a / 4. ) ;
if ( year < 1582 ) b = 0 ;
if ( year = = 1582 & & month < 10 ) b = 0 ;
2015-04-16 01:30:49 -06:00
if ( year = = 1582 & & month = = 10 & & day < = 4 ) b = 0 ;
2014-09-25 00:48:34 -06:00
jd = floor ( 365.25 * ( year + 4716 ) ) + floor ( 30.6001 * ( month + 1 ) ) + day + b - 1524.5 ;
return jd - 2400000.5 ;
}
// nfd2mjd
double nfd2mjd ( char * date )
{
int year , month , day , hour , min , sec ;
double mjd , dday ;
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 ;
mjd = date2mjd ( year , month , dday ) ;
return mjd ;
}
void usage ( void )
{
printf ( " propagate c:i:t:m: \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 " ) ;
return ;
}
int main ( int argc , char * argv [ ] )
{
int imode , satno = 0 , arg , usecatalog = 0 , satnomin , flag = 0 ;
FILE * file ;
orbit_t orb ;
2018-02-27 09:19:34 -07:00
xyz_t r , v , n , m , nm , r0 , v0 , mr , mv ;
2014-09-25 00:48:34 -06:00
char tlefile [ LIM ] , catalog [ LIM ] , nfd [ 32 ] ;
char line1 [ 80 ] , line2 [ 80 ] , desig [ 20 ] ;
2018-02-27 09:19:34 -07:00
double mjd , ra , de , dr , drmin , dmr , dmv ;
2014-09-25 00:48:34 -06:00
char * env ;
2019-11-02 01:59:53 -06:00
int at_epoch = 0 ;
2014-09-25 00:48:34 -06:00
// Get environment variable
env = getenv ( " ST_TLEDIR " ) ;
sprintf ( tlefile , " %s/classfd.tle " , env ) ;
// Set date
nfd_now ( nfd ) ;
mjd = nfd2mjd ( nfd ) ;
// Decode options
2019-11-02 01:59:53 -06:00
while ( ( arg = getopt ( argc , argv , " C:c:i:t:m:he " ) ) ! = - 1 ) {
2014-09-25 00:48:34 -06:00
switch ( arg ) {
case ' t ' :
strcpy ( nfd , optarg ) ;
mjd = nfd2mjd ( nfd ) ;
break ;
case ' m ' :
mjd = ( double ) atof ( optarg ) ;
break ;
case ' c ' :
strcpy ( tlefile , optarg ) ;
break ;
case ' C ' :
strcpy ( catalog , optarg ) ;
usecatalog = 1 ;
break ;
2019-11-02 01:59:53 -06:00
case ' e ' :
at_epoch = 1 ;
break ;
2014-09-25 00:48:34 -06:00
case ' i ' :
satno = atoi ( optarg ) ;
break ;
case ' h ' :
usage ( ) ;
return 0 ;
break ;
default :
usage ( ) ;
return 0 ;
}
}
// Reloop stderr
freopen ( " /tmp/stderr.txt " , " w " , stderr ) ;
// Open file
file = fopen ( tlefile , " r " ) ;
while ( read_twoline ( file , satno , & orb ) = = 0 ) {
format_tle ( orb , line1 , line2 ) ;
// Propagate
imode = init_sgdp4 ( & orb ) ;
2019-11-02 01:59:53 -06:00
if ( at_epoch = = 1 )
mjd = SGDP4_jd0 - 2400000.5 ;
2018-02-27 09:19:34 -07:00
imode = satpos_xyz ( mjd + 2400000.5 , & r0 , & v0 ) ;
2014-09-25 00:48:34 -06:00
// Compute normal
2018-02-27 09:19:34 -07:00
n = cross ( r0 , v0 ) ;
2014-09-25 00:48:34 -06:00
ra = modulo ( atan2 ( n . y , n . x ) * R2D , 360.0 ) ;
de = asin ( n . z / magnitude ( n ) ) * R2D ;
if ( usecatalog = = 0 )
printf ( " %05d %14.8lf %10.6f %10.6f %10.2f %10.2f %10.2f %10.2f \n " , orb . satno , mjd , ra , de , magnitude ( n ) , n . x , n . y , n . z ) ;
}
fclose ( file ) ;
// Match against a catalog
if ( usecatalog = = 1 ) {
// Open file
file = fopen ( catalog , " r " ) ;
while ( read_twoline ( file , 0 , & orb ) = = 0 ) {
format_tle ( orb , line1 , line2 ) ;
// Propagate
imode = init_sgdp4 ( & orb ) ;
imode = satpos_xyz ( mjd + 2400000.5 , & r , & v ) ;
// Compute normal
m = cross ( r , v ) ;
2018-02-27 09:19:34 -07:00
// Normal difference
2014-09-25 00:48:34 -06:00
nm . x = n . x - m . x ;
nm . y = n . y - m . y ;
nm . z = n . z - m . z ;
dr = magnitude ( nm ) ;
2018-02-27 09:19:34 -07:00
// Position/velocity difference
mr . x = r0 . x - r . x ;
mr . y = r0 . y - r . y ;
mr . z = r0 . z - r . z ;
mv . x = v0 . x - v . x ;
mv . y = v0 . y - v . y ;
mv . z = v0 . z - v . z ;
2014-09-25 00:48:34 -06:00
if ( flag = = 0 | | dr < drmin ) {
drmin = dr ;
2018-02-27 09:19:34 -07:00
dmr = magnitude ( mr ) ;
dmv = magnitude ( mv ) ;
2014-09-25 00:48:34 -06:00
satnomin = orb . satno ;
flag = 1 ;
}
}
2018-02-27 09:19:34 -07:00
printf ( " %05d %05d %8.2f km %8.2f km %10.6f km/s \n " , satno , satnomin , drmin , dmr , dmv ) ;
2014-09-25 00:48:34 -06:00
}
return 0 ;
}