import SpiceyPy as spice
dpr = spice.dpr()
spice.furnsh('.'.join(__file__.split('.')[:-1]+['py']))
target = spice.gcpool('TARGET',0,1,99)[0]
spacecraft = spice.gcpool('SPACECRAFT',0,1,99)[0]
etnearca = spice.gdpool('ETNEARPRG',0,1)[0]
targetID = spice.bods2c(target)
et = etnearca
state,lt = spice.spkezr(spacecraft,et,'J2000','NONE',target)
dt = spice.vdot(state[:3],state[3:]) / spice.vdot(state[3:],state[3:])
oldet = et
et = et - dt
while abs(et-oldet)>0.:
state,lt = spice.spkezr(spacecraft,et,'J2000','NONE',target)
dt = spice.vdot(state[:3],state[3:]) / spice.vdot(state[3:],state[3:])
oldet = et
et = et - dt
utcPRG = spice.et2utc(et,'ISOC',3,99)
mtxJ2kToBF = spice.tipbod('J2000', targetID, et)
velocityPRG_BF = spice.mxv(mtxJ2kToBF,state[3:])
V_prg,ra,Theta = spice.recrad(velocityPRG_BF)
def __init__(self,stateChf,mu,stateDep=None):
"""Make chief-only calculations. Save deputy-based calculations for
Deputy() method below; call it now if stateDep is provided
mu = GM, km**3 s**-2
"""
####################################################################
### Extract ECI Positions and ECI velocities from 6-element ECI states
### Save mu in self object
self.rChfEci,self.velChfEci = stateChf[:3], stateChf[3:]
self.mu = mu
####################################################################
### Equations (1) and (2)
### Convert chief position and velocity to [Rhat|Shat|What]1 matrix
self.mtxEciToRsw1 = RVtoRSW(self.rChfEci,self.velChfEci)
####################################################################
### Transform chief position and velocity to RSW1 frame
(self.rChfRsw1
,self.velChfRsw1
) = [ sp.mxv(self.mtxEciToRsw1,v)
for v in
[self.rChfEci,self.velChfEci]
]
####################################################################
### Equations (3) - postponed to deputy calculation
####################################################################
### Equations (4)
### Eccentricity and semi-major axis
### - vHChf = momentum vector
### - self.pChf = semiparameter or semilatus rectum = distance from
### the primary focus to the orbit, measured
### perpendicular to the semi-major axis
### - self.eccChfSquared = Square of Eccentricity
### - derivation, dot product of vEccChf with itself, will always
### be non-negative
### - square root of (1-self.eccChfSquared) will throw math domain
### exception if eccentricity exceeds unity.
### - reciprocal of (1-self.eccChfSquared) will throw division by
### zero exception if eccentricity is unity
### - self.eccChf = Eccentricity (also Elliptic Modulus, k)
### - self.bOverA = ratio of semi-minor to semi-major axes' lengths
### - self.aChf = length of semi-major axis
### - self.bChf = length of semi-minor axis
vHChf = sp.vcrss(self.rChfRsw1,self.velChfRsw1)
self.pChf = sp.vdot(vHChf,vHChf) / self.mu
vEccChf = sp.vsub(sp.vscl(1./self.mu,sp.vcrss(self.velChfRsw1,vHChf)),sp.vhat(self.rChfRsw1))
self.eccChfSquared = sp.vdot(vEccChf,vEccChf)
self.eccChf = math.sqrt(self.eccChfSquared)
self.bOverA = math.sqrt(1 - self.eccChfSquared)
self.aChf = self.pChf / (1. - self.eccChfSquared)
self.bChf = self.pChf / self.bOverA
self.cChf = self.aChf * self.eccChf
if self.eccChfSquared>0.: self.vEccHatChf = sp.vhat(vEccChf)
else : self.vEccHatChf = sp.vhat(self.rChfRsw1)
### Use SPICE toolkit routine to calculate perfocal distance (rp), eccentricity, and semi-major axis
self.rpChf,self.eccChfSpice,inc,lnode,argp,m0,t0,muTmp = sp.oscelt(stateChf,0.,self.mu)
self.aChfSpice = self.rpChf / (1. - self.eccChfSpice)
### Quadrant arc length; used later
self.quadArc = self.aChf * E(halfpi,self.eccChfSquared)
####################################################################
### Equations (5)
### Chief True anomaly posiiton
### Deputy True anomaly postponed to deputy calculation
self.lambdaPerigee = sp.recrad(self.vEccHatChf)[iRA]
self.trueAnom1 = (twopi - self.lambdaPerigee) % twopi
####################################################################
### Equations (6 through 9) - postponed to deputy calculation
####################################################################
### Equations (9) - chief only
### 9.1) Convert True Anomaly 1 (chief) to Eccentric Anomaly
self.eccAnom1 = self.TrueToEccAnom(self.trueAnom1)
### Get the arc length from periapse to the chief
self.arcChf = self.E(self.eccAnom1)
####################################################################
### Complete conversion to EQCM for Deputy if Deputy state is present
if not (stateDep is None): self.Deputy(stateDep)