def test_eci_times_astropy():
pytest.importorskip('astropy')
with pytest.raises(AssertionError):
pm.eci2ecef(eci0, [t0, t0])
with pytest.raises(AssertionError):
pm.ecef2eci(eci0, [t0, t0])
eci0s = np.stack((eci0, eci0))
assert pm.ecef2eci(pm.eci2ecef(eci0s, [t0] * 2), [t0] * 2) == approx(eci0s)
def test_eci_times_vallado():
with pytest.raises(AssertionError):
pm.eci2ecef(eci0, [t0, t0], useastropy=False)
with pytest.raises(AssertionError):
pm.ecef2eci(eci0, [t0, t0], useastropy=False)
eci0s = np.stack((eci0, eci0))
assert pm.ecef2eci(pm.eci2ecef(eci0s, [t0] * 2, useastropy=False),
[t0] * 2,
useastropy=False) == approx(eci0s, rel=0.001)
def test_eci_times(useastropy):
with pytest.raises(ValueError):
pm.eci2ecef(*eci0, [t0, t0], useastropy=useastropy)
with pytest.raises(ValueError):
pm.ecef2eci(*eci0, [t0, t0], useastropy=useastropy)
x = [eci0[0]] * 2
y = [eci0[1]] * 2
z = [eci0[2]] * 2
t = [t0] * 2
assert pm.ecef2eci(*pm.eci2ecef(x, y, z, t, useastropy=useastropy),
t, useastropy=useastropy) == approx(eci0, rel=0.001)
def read_ira():
while True:
line = ira.readline()
if len(line) == 0:
return
tu, s, b, alt, x, y, z = line.split(
None, 7) # time_unix, sat, beam, altitude, position(xyz)
s = int(s)
x = int(x) * 4000
y = int(y) * 4000
z = int(z) * 4000
tu = float(tu)
#print(ppm)
#tu = float(tu) * (1-ppm/1e6)
"""
#dist=sqrt( (x-ox)**2+ (y-oy)**2+ (y-oz)**2)
#dist_s=float(dist) / 299792458
#tu -= dist_s
"""
x, y, z = pymap3d.ecef2eci(x, y, z,
datetime.datetime.utcfromtimestamp(tu))
ira_xyzt[s].append([x, y, z, tu])
def testFunction(self, bV, sV, lat, long, alt, time):
jdays = utc2jul.utc2jul(time)
sI = sun_vec.sun_vec(jdays - 2444244.5)
temp = np.matrix([0, 0, -1]).transpose()
temp = pm.ned2ecef(0, 0, -1, 45, 30, 0)
temp = temp / np.linalg.norm(temp)
print(temp)
print("sI:")
print(sI)
print("-----------------------------------------")
print()
gm = wrldmagm.wrldmagm("WMM2015.COF")
bI = np.matrix(gm.wrldmagm(lat, long, alt, decyear.decyear(time)))
bI = bI / np.linalg.norm(bI)
print("bI: (NED)")
print(bI)
print("-----------------------------------------")
print()
bI = np.squeeze(np.asarray(bI))
bI = pm.ned2ecef(bI[0], bI[1], bI[2], lat, long, alt)
bIECEF = np.matrix(bI).transpose()
bIECEF = bI / np.linalg.norm(bI)
print("bI: (ECEF)")
print(bIECEF)
print("-----------------------------------------")
print()
bI = pm.ecef2eci(bI, time)
bI = np.matrix(bI).transpose()
bI = bI / np.linalg.norm(bI)
print("bI: (ECI)")
print(bI)
print("------------------------------")
print()
print("DCM:")
return getDCM.getDCM(bV, sV, bI, sI)
def sunlocate(time):
#inputs
# time: time in MJD
#outputs
#Sun2Earth - unit vector from sun2earth in ECI
import astropy
import astropy.coordinates
import datetime
import julian
from astropy.time import Time
import pymap3d
from pymap3d import geodetic2ecef
import decimal
from astropy import units as u
from astropy.coordinates import Angle
import numpy as np
#get the time in MJD
#mjd = 58562
t2 = Time(time, format='mjd')
#print(t2)
#get the location of the sun in lat long
sun_GEOD = astropy.coordinates.get_sun(t2)
#print(sun_GEOD)
lon_extra = Angle(sun_GEOD.ra)
lat_extra = Angle(sun_GEOD.dec)
#get it as a float
lon = 1 * lon_extra.degree
lat = 1 * lat_extra.degree
#print(lon)
#print(lat)
#print(type(lon))
#a = Angle(lat)
#print(a.degree)
here = type(lon)
#print(here)
#convert the geodetic coordinates to ECEF
sun_ECEF = geodetic2ecef(lat, lon, 0, ell=None, deg=True)
#Convert ECEF to ECI
sun_ECI = pymap3d.ecef2eci(sun_ECEF, t2, useastropy=True)
#get the norm to normalize it
norm_sun_ECI = np.linalg.norm(sun_ECI)
#print(norm_sun_ECI)
Sun2Earth = 1.496e8 * sun_ECI / norm_sun_ECI
return Sun2Earth
def test_eciecef(useastropy):
pytest.importorskip("numpy")
ecef = pm.eci2ecef(*eci0, t1, useastropy=useastropy)
assert ecef == approx(
[649012.04640917, -4697980.55129606, 4250818.82815207], rel=0.001)
assert pm.ecef2eci(*ecef, t1, useastropy=useastropy) == approx(eci0,
rel=0.001)
def test_ecef2eci(use_astropy):
pytest.importorskip("numpy")
if use_astropy:
pytest.importorskip("astropy")
# this example from Matlab ecef2eci docs
ecef = [-5762640, -1682738, 3156028]
utc = datetime(2019, 1, 4, 12)
eci = pm.ecef2eci(*ecef, utc, use_astropy=use_astropy)
rel = 0.0001 if use_astropy else 0.01
assert eci == approx([-2.9818e6, 5.2070e6, 3.1616e6], rel=rel)
def test_eci():
tlla = (42, -82, 200)
teci = (-3.977913815668146e6, -2.582332196263046e6, 4.250818828152067e6)
t = datetime(2013, 1, 15, 12, 0, 5, tzinfo=UTC)
lla = asarray(pm.eci2geodetic(teci, t)).squeeze()
assert_allclose(lla, tlla, rtol=0.2)
assert_allclose(
pm.eci2ecef(teci, t).squeeze(),
[649012.04640917, -4697980.55129606, 4250818.82815207])
assert_allclose(
pm.ecef2eci([649012.04640917, -4697980.55129606, 4250818.82815207],
t).squeeze(), teci)
assert_allclose(
asarray(pm.eci2aer(teci, 42, -100, 0, t)).squeeze(),
[83.73050, -6.614478, 1.473510e6])
def test_eci_vallado():
t = '2013-01-15T12:00:05'
lla = pm.eci2geodetic(eci0, t, useastropy=False)
assert lla == approx(lla0, rel=0.2)
eci1 = pm.eci2ecef(eci0, t, useastropy=False)
assert eci1 == approx(
[649012.04640917, -4697980.55129606, 4250818.82815207], rel=0.001)
assert pm.ecef2eci(eci1, t, useastropy=False) == approx(eci0, rel=0.001)
aer1 = pm.eci2aer(eci0, 42, -100, 0, t, useastropy=False)
assert aer1 == approx([83.73050, -6.614478, 1.473510e6], rel=0.001)
assert pm.aer2eci(*aer1, 42, -100, 0, t,
useastropy=False) == approx(eci0, rel=0.001)
with pytest.raises(ValueError):
pm.aer2eci(aer1[0], aer1[1], -1, 42, -100, 0, t, useastropy=False)
def test_eci_astropy():
pytest.importorskip('astropy')
t = '2013-01-15T12:00:05'
lla = pm.eci2geodetic(eci0, t)
assert lla == approx(lla0, rel=0.2)
eci1 = pm.eci2ecef(eci0, t)
assert eci1 == approx(
[649012.04640917, -4697980.55129606, 4250818.82815207])
assert pm.ecef2eci(eci1, t) == approx(eci0)
aer1 = pm.eci2aer(eci0, 42, -100, 0, t)
assert aer1 == approx([83.73050, -6.614478, 1.473510e6])
assert pm.aer2eci(*aer1, 42, -100, 0, t) == approx(eci0)
with pytest.raises(ValueError):
pm.aer2eci(aer1[0], aer1[1], -1, 42, -100, 0, t)
def test_eci(self):
if numpy is None or astropy is None:
logging.warning('ECI not tested')
return
teci = (-3.977913815668146e6, -2.582332196263046e6,
4.250818828152067e6)
t = datetime(2013, 1, 15, 12, 0, 5, tzinfo=UTC)
lla = numpy.asarray(pm.eci2geodetic(teci, t)).squeeze()
assert_allclose(lla, lla0, rtol=0.2)
assert_allclose(
pm.eci2ecef(teci, t).squeeze(),
[649012.04640917, -4697980.55129606, 4250818.82815207])
assert_allclose(
pm.ecef2eci([649012.04640917, -4697980.55129606, 4250818.82815207],
t).squeeze(), teci)
assert_allclose(
numpy.asarray(pm.eci2aer(teci, 42, -100, 0, t)).squeeze(),
[83.73050, -6.614478, 1.473510e6])
def igrffx(eci_vec, time):
#import igrf12
import numpy
import pyIGRF
import pymap3d
from pymap3d import ecef2eci
import navpy
from navpy import ned2ecef
import datetime
from datetime import datetime
import astropy
#eci_vec is a xyz vector in ECI km
#output B_ECI is a 3 item array in units of T
#get our lat long and alt from ECI
geod = pymap3d.eci2geodetic(1000 * eci_vec, time, useastropy=True)
latitude = geod[0][0] #degrees
longitude = geod[1][0] #degrees
altitude = geod[2] #meters
#call igrf to get b vector in NED
#mag = igrf12.igrf('2019-01-12', glat=latitude, glon=longitude, alt_km=altitude/1000)
b = pyIGRF.igrf_value(latitude, longitude, altitude / 1000, 2019)
#combine NED components back into an array
#NED = numpy.array([b_north,b_east,b_down])
NED = b[3:6]
#convert from NED to ECEF
ECEF = navpy.ned2ecef(NED, latitude, longitude, altitude)
#convert from ECEF to ECI
B_ECI = (pymap3d.ecef2eci(ECEF, time, useastropy=True)) * 10**(-9)
return B_ECI
from Unisim.environment.gravity import SimpleNewton from Unisim.environment.environment import Environment t0 = 0 #% Position, velocity, acceleration x0 = np.zeros(6) lat = 0 long = 0 alt = 242000 time_now = datetime.datetime.now() pos_ecef = pymap3d.geodetic2ecef(lat, long, alt) print(pos_ecef) pos_eci = pymap3d.ecef2eci(pos_ecef[0], pos_ecef[1], pos_ecef[2], time_now) print(pos_eci) x0[0:3] = pos_ecef # Velocity x0[4] = 7800 Sat = Body_RoundEarth(t0, x0) Sat._set_mass(500) atmo = ISA1976() gravity = SimpleNewton() wind = NoWind() Env = Environment(atmo, gravity, wind) Sat.set_environment(Env)
def start():
global epoch
global revnum
global lastmeananom
global lasttime
global lastmeanmot
revnum =0
lasttime=datetime(2018, 4, 4)
lastmeananom=0
lastmeanmot=15.5
gain = 2*(10**(-5))
config = load_config() # Load the data from the YAML.
# If GPS is on, get Cartesian (position, velocity) vectors and UTC time from the GPS.
# Convert Cartesian coordinates and time to a Keplerian Elements array.
# Generate a new TLE using the KOE.
if gps_is_on(): # If we ask for GPS coordinates and the GPS responds:
# Data is a list (cache) of dictionaries representing one timestep.
data = gps_dummy.get_data()
if gps_dummy.data_is_valid(data): # If the data is valid:
i = len(data)-1 # Get the last dictionary in the cache.
# Position state vector.
r = [data[i]['x_pos'], data[i]['y_pos'], data[i]['z_pos']] # ECI frame
# Velocity state vector.
vel = [data[i]['x_vel'], data[i]['y_vel'], data[i]['z_vel']]
epoch = data[i]['time'] # Datetime object representing the epoch.
# Convert state vectors into an array representing the KOE.
koe_array = cart_to_kep(r, vel)
koe_list = koe_array.tolist()
# koe_array = np.insert(koe_array, 0, epoch) # Add the datetime object epoch to the beginning.
koe_list.insert(0, epoch)
# koe_array = np.append(koe_array, data['adcs']['tledata']['bstardrag']) # Append the B-star drag coefficient
koe_list.append(config['adcs']['sc']['bstardrag'])
temp_tle, lastmeanmot, lastmeananom, lasttime = tle_points.propagate(koe_list, lastmeanmot, lastmeananom, lasttime, revnum) # Generate the new TLE.
# print(koe_list)
# print(temp_tle)
tjreverbtle = open(config['adcs']['tlefiles']['tjreverb'], "w") # Open the main TJREVERB TLE for writing.
tjreverbtle.write(temp_tle) # Write the new TLE to TJREVERB TLE.
tjreverbtle.close() # Close the file.
# Backup the TLE data.
backuptle = open(config['adcs']['tlefiles']['backup'], "w")
backuptle.write(temp_tle)
backuptle.close()
lla = tle_dummy.get_lla(epoch) # Pull LLA data from TJREVERB TLE.
else: # If the GPS is on but the data is invalid:
epoch = datetime.utcnow() # Set current time to the system time.
# Uses PyOrbital to propogate the TLE using epoch, which returns its LLA.
lla = tle_dummy.get_lla(epoch)
# If GPS is off, use the system time and TJREVERB TLE to propogate the current LLA.
else: # If we ask for GPS coordinates and the GPS not respond:
epoch = datetime.utcnow() # Set current time to the system time.
# Uses PyOrbital to propogate the TLE using epoch, which returns its LLA.
lla = tle_dummy.get_lla(epoch)
# needed to incremement revnum
print(tle_dummy.get_xyz(epoch)['xyz_pos'])
print(tle_dummy.get_xyz(epoch)['xyz_vel'])
print(tle_dummy.get_lla(epoch))
pos = []
vel = []
for i in tle_dummy.get_xyz(epoch)['xyz_pos']:
pos.append(i*1000)
for j in tle_dummy.get_xyz(epoch)['xyz_vel']:
vel.append(j*1000)
poskep = cart_to_kep(pos, vel)
print("KOE (from newly generated TLE): "+str(poskep))
# if (poskep[4]>0 and oldargp<=0):
# revnum=revnum+1
# oldargp = poskep[4]
# write_config('config_adcs.yaml', utc_to_jul(epoch)) # config['adcs']['sc']['jd0'] = utc_to_jul(epoch)
# Instantiates the WrldMagM object.
gm = WrldMagM((Path(__file__).parent.resolve() /
config['adcs']['wrldmagm']))
# Calculate the magnetic field vector in ECEF. Altitude is multiplied to convert meters to feet.
magECEF = gm.wrldmagm(lla['lat'], lla['lon'], lla['alt'], date.today())
magECEF = np.squeeze(np.asarray(magECEF))
magECI = ecef2eci(magECEF, epoch)
# Magnetic field in inertial frame, converts teslas to nanoteslas.
bI = 1.0*(10e-09) * magECI
bI = bI/np.linalg.norm(bI)
bI = np.asmatrix(bI)
bI = bI.getH()
# Sun vector in intertial frame.
sI = sun_vec(utc_to_jul(epoch)-utc_to_jul(datetime(1980, 1, 6, 0, 0, 0)))
sI = sI/np.linalg.norm(sI) # Normalize sI.
print(bI)
print(sI)
# bV and sV data are taken from the onboard magnetometer and sunsensors.
bV = [1,1,2]
sV = [1,2,1]
dcm = get_dcm(bV, sV, bI, sI)
print("DCM: "+str(dcm))
q = dcm_to_q(dcm)
print("Quaternion: "+str(q))
qref = get_q_ref_nadir(poskep)
print("Reference Quaternion: "+str(qref))
qerr = get_q_err(q, qref)
print("Quaternion Error: "+str(qerr))
thetaerr = get_theta_err(qerr)
print("Theta Error (radians): "+str(thetaerr.getH()))
mmax = .2
mtrans = np.matrix([[1,0,0],[0,1,0],[0,0,1]])
ctcomm=-1*gain*thetaerr.getH()
#print(ctcomm)
magdip = get_mc(ctcomm.getH(),np.matrix([bV]).getH(),mmax,mtrans)
print("Magnetic Dipole (sent to imtq): "+str(magdip))
ctprod = np.cross(magdip,bV)
print("Control Torque Produced: "+str(ctprod))
def RAY_INTERSECT_WGS84_TERRAIN(point,
ray_dir,
t,
DEM_FILE_LOCATION=None,
nom_height=0):
lat, lon, height = RAY_INTERSECT_WGS84(point, ray_dir, t)
s, height_min = Height_From_DEM(lon, lat, DEM_FILE_LOCATION)
if (s == 0):
return RAY_INTERSECT_WGS84(point, ray_dir, t)
#print(Max_Local_Height_From_DEM(lon,lat,DEM_FILE_LOCATION,scale=(5,5)))
s, (max_lat, max_lon,
max_height) = Max_Local_Height_From_DEM(lon,
lat,
DEM_FILE_LOCATION,
scale=(5, 5))
if (s == 0):
return RAY_INTERSECT_WGS84(point, ray_dir, t)
eci_ter = pm.ecef2eci(pm.geodetic2ecef(lat, lon, height), t)
#print([(max_lon,max_lat),max_height, t])
eci_ter_lmax = pm.ecef2eci(pm.geodetic2ecef(max_lat, max_lon, max_height),
t)
#print(mid_llh[2],height_mid
xd = ray_dir[0] #-31.46071792#-
yd = ray_dir[1] #58.59611618#-
zd = ray_dir[2] #27.47631664#-
xc = -point[0]
yc = -point[1]
zc = -point[2]
xn = eci_ter_lmax[0]
yn = eci_ter_lmax[1]
zn = eci_ter_lmax[2]
#print(xn,yn,zn)
t_par = (((xn)**2 + (yn)**2 + (zn)**2) - (xc * xn) - (yc * yn) -
(zc * zn)) / (xd * xn + yd * yn + zd * zn)
rez = np.array([xc + t_par * xd, yc + t_par * yd, zc + t_par * zd])
#print(pm.eci2geodetic((rez),t))
#print(eci_ter)
height_delta = 9999999999
eci_ter_mid = (eci_ter + rez) / 2
height_delta_old = 9898989898
while (abs(height_delta) > 5 and height_delta_old != height_delta):
#print(str(height_delta)+': height delta')
mid_llh = pm.eci2geodetic(eci_ter_mid, t) #latlonheight
s, height_mid = Height_From_DEM(mid_llh[1], mid_llh[0],
DEM_FILE_LOCATION)
if (s == 0):
return RAY_INTERSECT_WGS84(point, ray_dir, t)
#print(mid_llh[2],height_mid[2],mid_llh[2]-height_mid[2])
height_delta_old = height_delta
height_delta = mid_llh[2] - height_mid[2]
if (height_delta < 0):
eci_ter_mid_new = (rez + eci_ter_mid) / 2
eci_ter = eci_ter_mid
eci_ter_mid = eci_ter_mid_new
else:
eci_ter_mid_new = (eci_ter_mid + eci_ter) / 2
rez = eci_ter_mid
eci_ter_mid = eci_ter_mid_new
#print(mid_llh)
return (mid_llh)
def solve_trajectory(rocket):
# Initial Attitude #############################
quat0 = coord.euler2quat(rocket.azimuth0, rocket.elevation0)
DCM_LAUNCHER2NED = coord.DCM_NED2BODY_quat(quat0).transpose()
################################################
# Initial Position #############################
pos0_ECEF = pm.geodetic2ecef(rocket.pos0_LLH[0], rocket.pos0_LLH[1],
rocket.pos0_LLH[2])
pos0_ECI = pm.ecef2eci(pos0_ECEF, rocket.launch_date)
pos0_NED = DCM_LAUNCHER2NED.dot(np.array([rocket.Lcg0, 0.0, 0.0]))
################################################
# onLauncher Trajectory ##################################################
# lower launch lugがランチャレール長を越した時点でランチクリア
# lower lugが抜けた時点での重心各値をメインのソルバへ渡す
time = np.arange(
0.0, 0.5 + np.sqrt(
2.0 * rocket.launcher_rail * rocket.m(0.0) / rocket.ref_thrust),
0.01)
x0 = np.zeros(13)
x0[0:3] = pos0_NED # Pos_NED
x0[3:6] = np.zeros((1, 3)) # Vel_NED
x0[6:9] = np.zeros((1, 3)) # omega
x0[9:13] = quat0 # quat
ode_log = odeint(onlauncher_dynamics,
x0,
time,
args=(rocket, DCM_LAUNCHER2NED.transpose()))
# onLauncher post ######
pos_LAUNCHER = np.array(
[DCM_LAUNCHER2NED.transpose().dot(pos) for pos in ode_log[:, 0:3]])
# ランチクリアまででカット
index_launch_clear = np.argmax(
rocket.launcher_rail <= pos_LAUNCHER[:, 0] - rocket.lower_lug)
# log
rocket.result.time_onlauncher_log = time[:index_launch_clear + 1]
rocket.result.pos_onlauncher_NED_log = ode_log[:index_launch_clear + 1,
0:3]
rocket.result.vel_onlauncher_NED_log = ode_log[:index_launch_clear + 1,
3:6]
rocket.result.omega_onlauncher_log = ode_log[:index_launch_clear + 1, 6:9]
rocket.result.quat_onlauncher_log = ode_log[:index_launch_clear + 1, 9:13]
pos_launch_clear_NED = rocket.result.pos_onlauncher_NED_log[-1, :]
vel_launch_clear_NED = rocket.result.vel_onlauncher_NED_log[-1, :]
omega_launch_clear = rocket.result.omega_onlauncher_log[-1, :]
quat_launch_clear = rocket.result.quat_onlauncher_log[-1, :]
time_launch_clear = rocket.result.time_onlauncher_log[-1]
altitude_launch_clear = -pos_launch_clear_NED[2]
DCM_NED2BODY = coord.DCM_NED2BODY_quat(quat_launch_clear)
# tipoff moment
if rocket.tipoff_exist:
mg_tipoff_NED = np.array([
0, 0,
rocket.m(time_launch_clear) * env.gravity(altitude_launch_clear)
])
mg_tipoff_BODY = DCM_NED2BODY.dot(mg_tipoff_NED)
moment_arm_tipoff = np.array(
[rocket.lower_lug - rocket.Lcg(time_launch_clear), 0, 0])
moment_tipoff = np.cross(mg_tipoff_BODY, moment_arm_tipoff)
Ij_pitch = rocket.Ij_pitch(
time_launch_clear) + rocket.m(time_launch_clear) * np.abs(
rocket.Lcg(time_launch_clear) - rocket.lower_lug)**2
Ij_roll = rocket.Ij_roll(time_launch_clear)
Ij = np.array([Ij_roll, Ij_pitch, Ij_pitch])
omegadot_tipoff = moment_tipoff / Ij # 下部ラグを支点とした機体質量でのチップオフ角加速度。あとで角速度に変換する
else:
omegadot_tipoff = 0.0
################################################
# Main Trajectory ##################################################
def estimate_end(total_impulse):
It = total_impulse
tf = 2.37 * It**0.37
tf_order = len(str(int(tf))) - 1
dt = 10.0**(tf_order - 4)
return 1.4 * tf, dt
if rocket.auto_end:
rocket.end_time, rocket.time_step = estimate_end(rocket.total_impulse)
start_time = time_launch_clear # + rocket.time_step
time = np.arange(start_time, rocket.end_time, rocket.time_step)
pos_launch_clear_ECEF = pm.ned2ecef(pos_launch_clear_NED[0],
pos_launch_clear_NED[1],
pos_launch_clear_NED[2],
rocket.pos0_LLH[0], rocket.pos0_LLH[1],
rocket.pos0_LLH[2])
pos_launch_clear_ECI = coord.DCM_ECI2ECEF(
time_launch_clear).transpose().dot(pos_launch_clear_ECEF)
DCM_NED2ECEF = coord.DCM_ECEF2NED(rocket.pos0_LLH).transpose()
DCM_ECI2ECEF = coord.DCM_ECI2ECEF(time_launch_clear)
x0 = np.zeros(13)
x0[0:3] = pos_launch_clear_ECI
x0[3:6] = coord.vel_ECEF2ECI(DCM_NED2ECEF.dot(vel_launch_clear_NED),
DCM_ECI2ECEF, pos_launch_clear_ECI)
x0[6:9] = omega_launch_clear + omegadot_tipoff * rocket.time_step
x0[9:13] = quat_launch_clear
ode_log = odeint(dynamics_odeint, x0, time, args=(rocket, ))
# Main post ######
date_array = rocket.launch_date + np.array(
[datetime.timedelta(seconds=sec) for sec in time])
pos_ECEF = np.array([
coord.DCM_ECI2ECEF(t).dot(pos)
for pos, t in zip(ode_log[:, 0:3], time)
])
pos_LLH = np.array(
[pm.ecef2geodetic(pos[0], pos[1], pos[2]) for pos in pos_ECEF])
# 着地後の分をカット
index_hard_landing = np.argmax(pos_LLH[:, 2] <= 0.0)
# log
rocket.result.time_log = time[:index_hard_landing + 1]
rocket.result.pos_ECI_log = ode_log[:index_hard_landing + 1, 0:3]
rocket.result.vel_ECI_log = ode_log[:index_hard_landing + 1, 3:6]
rocket.result.omega_log = ode_log[:index_hard_landing + 1, 6:9]
rocket.result.quat_log = ode_log[:index_hard_landing + 1, 9:13]
dynamics_result(rocket)
################################################
# Decent Trajectory ##################################################
if rocket.timer_mode:
index_apogee = np.argmax(rocket.result.time_log > rocket.t_1st)
else:
index_apogee = np.argmax(pos_LLH[:, 2])
time_apogee = rocket.result.time_log[index_apogee]
pos_apogee_ECI = rocket.result.pos_ECI_log[index_apogee]
vel_apogee_ECI = rocket.result.vel_ECI_log[index_apogee]
vel_apogee_ned = np.array([
0, 0,
coord.DCM_ECEF2NED(rocket.pos0_LLH).dot(
coord.vel_ECI2ECEF(vel_apogee_ECI, coord.DCM_ECI2ECEF(time_apogee),
pos_apogee_ECI))[2]
])
vel_apogee_ECI = coord.vel_ECEF2ECI(
coord.DCM_ECEF2NED(rocket.pos0_LLH).transpose().dot(vel_apogee_ned),
coord.DCM_ECI2ECEF(time_apogee), pos_apogee_ECI)
if rocket.payload_exist:
rocket.m_burnout -= rocket.payload.mass
est_landing_payload = pos_LLH[index_apogee, 2] / np.sqrt(
2.0 * rocket.payload.mass * env.gravity(pos_LLH[index_apogee, 2]) /
(env.get_std_density(0.0) * rocket.payload.CdS))
est_landing = pos_LLH[index_apogee, 2] / np.sqrt(
2.0 * rocket.result.m_log[index_apogee] *
env.gravity(pos_LLH[index_apogee, 2]) / (env.get_std_density(0.0) *
(rocket.CdS1 + rocket.CdS2)))
time = np.arange(time_apogee, time_apogee + 1.2 * est_landing, 0.1)
x0 = np.zeros(6)
x0[0:3] = pos_apogee_ECI
x0[3:6] = vel_apogee_ECI
ode_log = odeint(parachute_dynamics, x0, time, args=(rocket, ))
# Decent post ######
date_array = rocket.launch_date + np.array(
[datetime.timedelta(seconds=sec) for sec in time])
pos_ECEF = np.array([
coord.DCM_ECI2ECEF(t).dot(pos)
for pos, t in zip(ode_log[:, 0:3], time)
])
pos_LLH = np.array(
[pm.ecef2geodetic(pos[0], pos[1], pos[2]) for pos in pos_ECEF])
# 着地後の分をカット
index_soft_landing = np.argmax(pos_LLH[:, 2] <= 0.0)
# log
rocket.result.time_decent_log = time[:index_soft_landing + 1]
rocket.result.pos_decent_ECI_log = ode_log[:index_soft_landing + 1, 0:3]
rocket.result.vel_decent_ECI_log = ode_log[:index_soft_landing + 1, 3:6]
# Payload ####
if rocket.payload_exist:
time = np.arange(time_apogee, time_apogee + 1.2 * est_landing_payload,
0.01)
x0 = np.zeros(6)
x0[0:3] = pos_apogee_ECI
x0[3:6] = vel_apogee_ECI
ode_log = odeint(payload_parachute_dynamics, x0, time, args=(rocket, ))
date_array = rocket.launch_date + np.array(
[datetime.timedelta(seconds=sec) for sec in time])
pos_ECEF = np.array([
coord.DCM_ECI2ECEF(t).dot(pos)
for pos, t in zip(ode_log[:, 0:3], time)
])
pos_LLH = np.array(
[pm.ecef2geodetic(pos[0], pos[1], pos[2]) for pos in pos_ECEF])
# 着地後の分をカット
index_payload_landing = np.argmax(pos_LLH[:, 2] <= 0.0)
# log
rocket.payload.result.time_decent_log = time[:index_payload_landing +
1]
rocket.payload.result.pos_decent_LLH_log = pos_LLH[:
index_payload_landing
+ 1, :]
