def state_from_orbit(ra, rp, lat, lng, node_dir, vel_dir, gm):
inc = lat + (np.pi / 2 - lat) * 0.05
a = (ra + rp) / 2
e = (ra - rp) / (ra + rp)
dt = 2 * np.pi * np.sqrt(a**3 / gm) / 6
dt = 616.0809208241426
r = np.array([1, 0, 0])
r = um.vector_axis_angle(r, np.array([0, 0, 1]), lng)
r = rp * um.vector_axis_angle(r, np.cross(r, np.array([0, 0, 1])), lat)
longitude_of_node = lng + node_dir * abs(
um.safe_asin(np.tan(lat) / np.tan(inc)))
print('LAN', np.degrees(longitude_of_node))
print('a', a)
print('e', e)
print('dt', dt)
n = np.array([1, 0, 0])
n = um.vector_axis_angle(n, np.array([0, 0, 1]), longitude_of_node)
normal = vel_dir * um.normalize(np.cross(n, r))
v = np.sqrt((1 + e) / (1 - e) * gm / a) * um.normalize(np.cross(normal, r))
r, v = um.cse_rv(r, v, -dt, gm)
return r, v
def p63_ignition_algorithm(mission):
t = mission.time_initial
tg = mission.guide_time_initial
li = mission.landing_position_initial
unfcp = -um.normalize(mission.velocity_initial)
mission.target = mission.brtg
while True:
r, v = um.cse_rv(mission.position_initial, mission.velocity_initial, t,
mission.gravitational_parameter)
lt = um.vector_axis_angle(li, np.array([0, 1, 0]),
mission.body_rotation[1] * t)
for i in range(3):
v += mission.aftrim * unfcp * mission.ttrim
afcp, tg, rg, vg = guide(tg, 0, r, v, lt, mission)
unfcp = um.normalize(afcp)
dt = np.dot(
np.array([
rg[0] - mission.brig[0][0], rg[1]**2,
rg[2] - mission.brig[0][2],
um.magnitude(vg) - um.magnitude(mission.brig[1])
]), mission.kgt) / (vg[2] + mission.kgt[0] * vg[0])
if abs(dt) > 1:
dt *= 1 / abs(dt)
t -= dt
if abs(dt) < 1 / 128:
break
t -= (mission.tullage + mission.ttrim)
mission.time = t
mission.guide_time = tg
def find_orbit(self, p, plane_change_flag):
r = self.vessel.position(self.ref_frame)
v = self.vessel.velocity(self.ref_frame)
rt = self.conn.space_center.target_vessel.position(self.ref_frame)
vt = self.conn.space_center.target_vessel.velocity(self.ref_frame)
rt2, vt2 = um.cse_rv(rt, vt, p, self.moon.gravitational_parameter)
if plane_change_flag:
v1 = np.sqrt(self.moon.gravitational_parameter *
(2 / um.magnitude(r) - 2 /
(um.magnitude(r) + um.magnitude(rt))))
v1 *= um.normalize(np.cross(np.cross(r, v), r))
else:
v1, v2 = um.cse_rr(r, rt2, p, -1,
self.moon.gravitational_parameter)
delta_velocity = v1 - v
uk.burn(self.conn,
self.ui,
self.ref_frame,
delta_velocity,
stopping_time=(0.1, 0.5, 0.1))
def descent_orbit(self):
target_time = self.calculate_descent_orbit_time()
self.conn.space_center.warp_to(self.conn.space_center.ut +
target_time - 15)
position = np.array(self.vessel.position(self.ref_frame))
velocity = np.array(self.vessel.velocity(self.ref_frame))
speed = np.sqrt(self.moon.gravitational_parameter *
(2 / np.linalg.norm(position) - 2 /
(np.linalg.norm(position) +
self.moon.equatorial_radius + self.periapsis)))
direction = um.normalize(
np.cross(
np.cross(np.array(self.vessel.position(self.ref_frame)),
np.array(self.vessel.velocity(self.ref_frame))),
np.array(self.vessel.position(self.ref_frame))))
delta_velocity = speed * direction - velocity
uk.burn(self.conn,
self.ui,
self.ref_frame,
delta_velocity,
stopping_time=(0.1, 0.5, 0.1))
def calculate_descent_orbit_time(self):
ref_axis = np.array(
self.vessel.orbit.reference_plane_normal(self.ref_frame))
normal = um.normalize(
np.cross(self.vessel.position(self.ref_frame),
self.vessel.velocity(self.ref_frame)))
node = np.cross(normal, ref_axis)
landing_site = self.moon.surface_position(self.latitude,
self.longitude,
self.ref_frame)
angle_dir = 1
if np.dot(node, landing_site) > 0:
angle_dir = -1
node = -node
theta = um.safe_asin(
np.sin(np.radians(self.latitude)) / np.sin(self.inclination))
target = um.vector_axis_angle(node, normal, theta * angle_dir)
target_angle = um.vector_angle(self.vessel.position(self.ref_frame),
target)
delta_direction = np.dot(
normal, np.cross(target, self.vessel.position(self.ref_frame)))
if delta_direction > 0:
target_angle = 2 * np.pi - target_angle
target_time = target_angle * self.vessel.orbit.period / (2 * np.pi)
return target_time
def guide(tg, dt, r, v, l, mission):
cgx = um.normalize(l)
cgy = um.normalize(
np.cross(
l, r - l - mission.k *
(v - np.cross(mission.body_rotation, r)) * tg / 4))
cgz = um.normalize(np.cross(cgx, cgy))
cg = np.array([cgx, cgy, cgz])
rg = np.matmul(cg, r - l)
vg = np.matmul(cg, v - np.cross(mission.body_rotation, r))
a = mission.target[3][2]
b = 6 * mission.target[2][2]
c = 18 * mission.target[1][2] + 6 * vg[2]
d = 24 * (mission.target[0][2] - rg[2])
tg += dt
while True:
dt = -(a * tg**3 + b * tg**2 + c * tg + d) / (3 * a * tg**2 +
2 * b * tg + c)
if abs(dt) > 60:
dt *= 60 / abs(dt)
tg = tg + dt
if abs(dt) < 1 / 128:
break
tp = tg + mission.leadtime
a = (3 * (tp / tg)**2 - 2 * (tp / tg)) * 12 / tg**2
b = (4 * (tp / tg)**2 - 3 * (tp / tg)) * 6 / tg
c = (2 * (tp / tg)**2 - (tp / tg)) * 6 / tg
d = (6 * (tp / tg)**2 - 6 * (tp / tg) + 1)
acg1 = a * (mission.target[0] - rg)
acg2 = b * mission.target[1]
acg3 = c * vg
acg4 = d * mission.target[2]
acg = acg1 + acg2 + acg3 + acg4
g = -mission.gravitational_parameter / um.magnitude(r)**3 * r
afcp = np.matmul(np.transpose(cg), acg) - g
return afcp, tg, rg, vg
def simulate(mission, target, time_limit, delta_time, afcp_limit,
guide_time_flag):
mission.target = target
t0 = mission.time
rg_out = []
thrott_out = []
while True:
lt = um.vector_axis_angle(mission.landing_position_initial,
np.array([0, 1, 0]),
mission.body_rotation[1] * mission.time)
afcp, mission.guide_time, rg, vg = guide(mission.guide_time,
delta_time, mission.position,
mission.velocity, lt, mission)
maxafcp = mission.maxthrust / mission.mass
if afcp_limit == -1:
if um.magnitude(afcp) > maxafcp * mission.minthrottle:
afcp = um.normalize(afcp) * maxafcp
elif not mission.throttflag:
mission.throttflag = True
mission.throtta = mission.guide_time
else:
afcp = um.normalize(afcp) * afcp_limit
mission.position, mission.velocity = runge_kutta(
mission.position, mission.velocity, afcp, delta_time,
mission.gravitational_parameter)
mission.mass -= um.magnitude(
afcp) * mission.mass / mission.vexh * delta_time
mission.time += delta_time
rg_out.append(rg)
thrott_out.append([mission.guide_time, um.magnitude(afcp) / maxafcp])
if guide_time_flag and mission.guide_time > time_limit:
break
if not guide_time_flag and mission.time - t0 > time_limit:
break
return rg_out, thrott_out
def match_speed(self, d, v):
while um.magnitude(self.target.position(
self.vessel.reference_frame)) > d:
pass
cv = np.array(self.vessel.velocity(self.ref_frame)) - np.array(
self.target.velocity(self.ref_frame))
cp = np.array(self.vessel.position(self.ref_frame)) - np.array(
self.target.position(self.ref_frame))
desv = um.normalize(-cp) * v
delta_velocity = desv - cv
uk.burn(self.conn,
self.ui,
self.ref_frame,
delta_velocity,
stopping_time=(0.25, 0.5, 0.25))
def change_inclination(self):
sma_1 = (self.vessel.orbit.semi_major_axis + self.apoapsis +
self.moon.equatorial_radius) / 2
sma_2 = self.apoapsis + self.moon.equatorial_radius
sma_3 = (self.apoapsis +
self.periapsis) / 2 + self.moon.equatorial_radius
period_1 = 2 * np.pi * (sma_1**3 /
self.moon.gravitational_parameter)**0.5
period_2 = 2 * np.pi * (sma_2**3 /
self.moon.gravitational_parameter)**0.5
period_3 = 2 * np.pi * (sma_3**3 /
self.moon.gravitational_parameter)**0.5
self.lan = self.longitude + (
period_1 / 2 + period_2 +
period_3 / 2) / self.moon.orbit.period * 360 + np.degrees(
self.moon.rotation_angle)
delta_longitude = um.safe_asin(
np.tan(np.radians(self.latitude)) / np.tan(self.inclination))
closest_normal = self.calculate_landing_normal(np.radians(self.lan),
delta_longitude)
angle_to_maneuver = self.calculate_angle_to_maneuver(closest_normal)
self.conn.space_center.warp_to(self.conn.space_center.ut +
angle_to_maneuver /
(2 * np.pi) * self.vessel.orbit.period -
15)
attitude = um.normalize(
np.cross(closest_normal,
np.array(self.vessel.position(self.ref_frame))))
delta_velocity = attitude * np.sqrt(
self.moon.gravitational_parameter *
(2 / np.linalg.norm(self.vessel.position(self.ref_frame)) -
1 / sma_1)) - self.vessel.velocity(self.ref_frame)
uk.burn(self.conn,
self.ui,
self.ref_frame,
delta_velocity,
stopping_time=(1.0, 0.5, 1.0))
def burn_wait(self, angle_burn):
vessel_angle = self.vessel.orbit.true_anomaly + self.vessel.orbit.argument_of_periapsis
while vessel_angle > 2 * np.pi:
vessel_angle -= 2 * np.pi
while angle_burn > 2 * np.pi:
angle_burn -= 2 * np.pi
if angle_burn < vessel_angle:
delta_true_anomaly = 2 * np.pi + angle_burn - vessel_angle
else:
delta_true_anomaly = angle_burn - vessel_angle
position = np.array(self.vessel.position(self.ref_frame))
velocity = np.array(self.vessel.velocity(self.ref_frame))
normal = np.cross(position, velocity)
attitude = um.normalize(np.cross(normal, position))
attitude = um.vector_axis_angle(attitude, normal, delta_true_anomaly)
time_wait = delta_true_anomaly / (2 * np.pi) * self.vessel.orbit.period
return attitude, time_wait
def circularize(self):
self.conn.space_center.warp_to(self.conn.space_center.ut +
self.vessel.orbit.time_to_periapsis -
15)
circular_speed = np.sqrt(
self.moon.gravitational_parameter /
np.linalg.norm(np.array(self.vessel.position(self.ref_frame))))
circular_direction = um.normalize(
np.cross(
np.cross(np.array(self.vessel.position(self.ref_frame)),
np.array(self.vessel.velocity(self.ref_frame))),
np.array(self.vessel.position(self.ref_frame))))
delta_velocity = circular_speed * circular_direction - np.array(
self.vessel.velocity(self.ref_frame))
uk.burn(self.conn,
self.ui,
self.ref_frame,
delta_velocity,
stopping_time=(1.0, 0.5, 1.0))
def match_speed_mono(self, d, v):
while um.magnitude(self.target.position(
self.vessel.reference_frame)) > d:
pass
cv = np.array(self.vessel.velocity(self.ref_frame)) - np.array(
self.target.velocity(self.ref_frame))
cp = np.array(self.vessel.position(self.ref_frame)) - np.array(
self.target.position(self.ref_frame))
desv = um.normalize(-cp) * v
delta_velocity = desv - cv
uk.burn(self.conn,
self.ui,
self.ref_frame,
delta_velocity,
stopping_time=(0.25, 0.5, 0.25),
main_engine=False,
specified_thrust=4000,
specified_isp=240,
facing=-1)
self.vessel_attitude()
def execute(self):
ldg.p63_ignition_algorithm(self)
burn_time = self.time + self.time_initial_ut
self.vessel.auto_pilot.engage()
self.vessel.auto_pilot.reference_frame = self.ref_frame
self.conn.space_center.rails_warp_factor = 4
while self.conn.space_center.ut < burn_time - 90:
self.ui.update_phase_current_time(
int(abs(self.conn.space_center.ut - burn_time)))
self.conn.space_center.rails_warp_factor = 2
while self.conn.space_center.ut < burn_time - 30:
self.ui.update_phase_current_time(
int(abs(self.conn.space_center.ut - burn_time)))
self.conn.space_center.rails_warp_factor = 0
self.ui.update_phase_current('P63')
self.ui.update_phase_next('P64')
while True:
position = np.array(self.vessel.position(self.ref_frame))
velocity = np.array(self.vessel.velocity(self.ref_frame))
landing_site = (
self.moon.equatorial_radius +
self.inputs[11]) / self.moon.equatorial_radius * np.array(
self.moon.surface_position(self.latitude, self.longitude,
self.ref_frame))
afcp, tg, rg, vg = ldg.guide(self.guide_time, 0, position,
velocity, landing_site, self)
self.vessel.auto_pilot.target_direction = afcp
maxafcp = self.vessel.max_thrust / self.vessel.mass
ts = um.magnitude(afcp) / maxafcp
if self.conn.space_center.ut > burn_time:
if ts > 1:
ts = 1
self.vessel.control.throttle = ts
self.ui.update_phase_current_time(int(abs(tg - self.tbrf)))
else:
self.ui.update_phase_current_time(
int(abs(self.conn.space_center.ut - burn_time)))
if tg > self.tbrf:
break
print('swap to approach phase')
self.ui.update_phase_current('P64')
self.ui.update_phase_next('P66')
self.target = self.aptg
self.guidetime = self.tapi
while tg < self.tapf and self.vessel.flight().surface_altitude > 30:
self.ui.update_phase_current_time(int(abs(tg - self.tapf)))
position = np.array(self.vessel.position(self.ref_frame))
velocity = np.array(self.vessel.velocity(self.ref_frame))
landing_site = (
self.moon.equatorial_radius +
self.inputs[11]) / self.moon.equatorial_radius * np.array(
self.moon.surface_position(self.latitude, self.longitude,
self.ref_frame))
afcp, tg, rg, vg = ldg.guide(self.guide_time, 0, position,
velocity, landing_site, self)
self.vessel.auto_pilot.target_direction = afcp
maxafcp = self.vessel.max_thrust / self.vessel.mass
self.vessel.control.throttle = um.magnitude(afcp) / maxafcp
self.ui.update_phase_current('P88')
self.ui.update_phase_next('N/A')
self.ui.update_phase_current_time(0)
self.vessel.control.throttle = 0.00
ref_frame = self.moon.reference_frame
gm = self.moon.gravitational_parameter
self.vessel.auto_pilot.engage()
self.vessel.auto_pilot.reference_frame = ref_frame
self.vessel.control.gear = True
while self.vessel.flight().surface_altitude > 1.5:
r = np.array(self.vessel.position(self.moon.reference_frame))
vs = np.array(self.vessel.flight(ref_frame).vertical_speed)
fa = np.array(self.vessel.direction(ref_frame))
g = -gm / um.magnitude(r)**3 * r
up = np.array(
self.conn.space_center.transform_direction(
[1, 0, 0], self.vessel.surface_reference_frame,
self.moon.reference_frame))
v = np.array(self.vessel.velocity(self.moon.reference_frame))
a = um.vector_exclude(up, -v)
afyz = a / 10
if self.vessel.flight().surface_altitude > 5:
if um.magnitude(afyz) > 0.35 * um.magnitude(g):
afyz = 0.35 * g * um.normalize(afyz)
if vs < 0:
afx = (((-1.00 - vs) / 1.5) * up - g) / np.cos(
um.vector_angle(up, fa))
else:
afx = 0
afcp = afyz + afx
else:
if um.magnitude(afyz) > 0.35 * um.magnitude(g):
afyz = 0.35 * g * um.normalize(afyz)
if vs < 0:
afx = (((-0.50 - vs) / 1.5) * up - g) / np.cos(
um.vector_angle(up, fa))
else:
afx = 0
afcp = afyz + afx
maxafcp = self.vessel.max_thrust / self.vessel.mass
self.vessel.auto_pilot.target_direction = afcp
self.vessel.control.throttle = um.magnitude(afcp) / maxafcp
self.vessel.control.throttle = 0
self.vessel.auto_pilot.disengage()