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, :]