You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
204 lines
7.1 KiB
204 lines
7.1 KiB
#!/usr/bin/env python3 |
|
""" |
|
Pre-compute Hohmann transfer rendezvous parameters for test validation. |
|
Replicates the exact rendezvous module phasing logic from src/rendezvous.cpp. |
|
Hardcoded from tests/test_rendezvous.toml — no TOML parser needed. |
|
|
|
Usage: python3 tests/compute_rendezvous_params.py |
|
""" |
|
|
|
import math |
|
import sys |
|
|
|
G = 6.67430e-11 |
|
|
|
# Central body |
|
EARTH_MASS = 5.972e24 |
|
|
|
# Spacecraft orbits (from test_rendezvous.toml) |
|
TARGET_R = 6.771e6 # 400 km altitude |
|
TARGET_NU = 0.0 |
|
|
|
CHASER_R = 6.671e6 # 300 km altitude |
|
CHASER_NU = 4.71238898038469 # 270 degrees |
|
|
|
MU = G * EARTH_MASS |
|
|
|
|
|
def calc_mean_motion(radius, mass): |
|
"""n = sqrt(mu / a^3)""" |
|
return math.sqrt(MU / (radius ** 3)) |
|
|
|
|
|
def hohmann_transfer_time(r1, r2, mass): |
|
"""Half orbit of transfer ellipse.""" |
|
a_transfer = (r1 + r2) / 2.0 |
|
T_transfer = 2.0 * math.pi * math.sqrt(a_transfer ** 3 / MU) |
|
return T_transfer / 2.0 |
|
|
|
|
|
def required_separation(r1, r2, mass): |
|
""" |
|
Required angular separation at first burn. |
|
chaser_pos - target_pos = target_angle - pi |
|
""" |
|
transfer_time = hohmann_transfer_time(r1, r2, mass) |
|
n2 = calc_mean_motion(r2, mass) |
|
target_angle = n2 * transfer_time |
|
return target_angle - math.pi |
|
|
|
|
|
def normalize_angle_2pi(angle): |
|
"""Normalize to [0, 2*pi).""" |
|
while angle < 0.0: |
|
angle += 2.0 * math.pi |
|
while angle >= 2.0 * math.pi: |
|
angle -= 2.0 * math.pi |
|
return angle |
|
|
|
|
|
def normalize_angle_pi(angle): |
|
"""Normalize to [-pi, pi].""" |
|
angle = normalize_angle_2pi(angle) |
|
while angle > math.pi: |
|
angle -= 2.0 * math.pi |
|
while angle < -math.pi: |
|
angle += 2.0 * math.pi |
|
return angle |
|
|
|
|
|
def calculate_wait_time_for_hohmann(r1, r2, angular_separation, mass): |
|
""" |
|
Wait time before Hohmann transfer. |
|
Positive = wait, negative = transfer already late. |
|
""" |
|
required_sep = required_separation(r1, r2, mass) |
|
n1 = calc_mean_motion(r1, mass) |
|
n2 = calc_mean_motion(r2, mass) |
|
rel_angular_vel = n1 - n2 |
|
|
|
current_sep = normalize_angle_pi(angular_separation) |
|
required_sep = normalize_angle_pi(required_sep) |
|
|
|
angle_to_close = required_sep - current_sep |
|
|
|
return angle_to_close / rel_angular_vel |
|
|
|
|
|
def calculate_relative_orbit_period(r1, r2, mass): |
|
"""Time between consecutive phasing opportunities.""" |
|
n1 = calc_mean_motion(r1, mass) |
|
n2 = calc_mean_motion(r2, mass) |
|
rel_angular_vel = abs(n1 - n2) |
|
return 2.0 * math.pi / rel_angular_vel |
|
|
|
|
|
def calculate_next_hohmann_wait_time(r1, r2, angular_separation, mass, min_wait_time): |
|
""" |
|
Like calculate_wait_time_for_hohmann, but advances to next phasing |
|
opportunity if wait_time < min_wait_time. Always returns non-negative. |
|
""" |
|
wait_time = calculate_wait_time_for_hohmann(r1, r2, angular_separation, mass) |
|
rel_period = calculate_relative_orbit_period(r1, r2, mass) |
|
|
|
while wait_time < min_wait_time: |
|
wait_time += rel_period |
|
|
|
return wait_time |
|
|
|
|
|
def main(): |
|
print(f"Central body: Earth, mass = {EARTH_MASS:.6e} kg") |
|
print(f"mu = {MU:.6e} m^3/s^2") |
|
|
|
print(f"\n=== INITIAL ORBITAL ELEMENTS ===") |
|
print(f"Chaser_Lower: r = {CHASER_R:.6e} m, nu = {CHASER_NU:.6f} rad ({math.degrees(CHASER_NU):.2f} deg)") |
|
print(f"Target: r = {TARGET_R:.6e} m, nu = {TARGET_NU:.6f} rad ({math.degrees(TARGET_NU):.2f} deg)") |
|
|
|
# Angular separation: chaser - target |
|
angular_sep = CHASER_NU - TARGET_NU |
|
angular_sep = normalize_angle_pi(angular_sep) |
|
print(f"\nAngular separation (chaser - target): {angular_sep:.6f} rad ({math.degrees(angular_sep):.2f} deg)") |
|
|
|
# Mean motions |
|
n1 = calc_mean_motion(CHASER_R, EARTH_MASS) |
|
n2 = calc_mean_motion(TARGET_R, EARTH_MASS) |
|
print(f"\nMean motions:") |
|
print(f" n1 (chaser): {n1:.10f} rad/s") |
|
print(f" n2 (target): {n2:.10f} rad/s") |
|
print(f" n1 - n2: {n1 - n2:.10f} rad/s") |
|
|
|
# Orbital periods |
|
p_chaser = 2.0 * math.pi / n1 |
|
p_target = 2.0 * math.pi / n2 |
|
print(f"\nOrbital periods:") |
|
print(f" Chaser: {p_chaser:.2f} s ({p_chaser/3600:.2f} h)") |
|
print(f" Target: {p_target:.2f} s ({p_target/3600:.2f} h)") |
|
|
|
# Hohmann transfer |
|
tt = hohmann_transfer_time(CHASER_R, TARGET_R, EARTH_MASS) |
|
a_t = (CHASER_R + TARGET_R) / 2.0 |
|
print(f"\n=== HOHMANN TRANSFER ===") |
|
print(f" Transfer semi-major axis: {a_t:.6e} m") |
|
print(f" Transfer time: {tt:.6f} s ({tt/60:.2f} min)") |
|
|
|
# Required separation |
|
req_sep = required_separation(CHASER_R, TARGET_R, EARTH_MASS) |
|
req_sep_norm = normalize_angle_pi(req_sep) |
|
print(f"\n=== REQUIRED SEPARATION ===") |
|
print(f" Raw: {req_sep:.6f} rad ({math.degrees(req_sep):.2f} deg)") |
|
print(f" Norm: {req_sep_norm:.6f} rad ({math.degrees(req_sep_norm):.2f} deg)") |
|
|
|
# Relative orbit period |
|
rel_period = calculate_relative_orbit_period(CHASER_R, TARGET_R, EARTH_MASS) |
|
print(f"\nRelative orbit period: {rel_period:.6f} s ({rel_period/3600:.2f} h)") |
|
|
|
# Detailed phasing calculation |
|
print(f"\n=== PHASING CALCULATION ===") |
|
current_sep = normalize_angle_pi(angular_sep) |
|
print(f" Current separation (normalized): {current_sep:.6f} rad ({math.degrees(current_sep):.2f} deg)") |
|
print(f" Required separation (normalized): {req_sep_norm:.6f} rad ({math.degrees(req_sep_norm):.2f} deg)") |
|
|
|
angle_to_close = req_sep_norm - current_sep |
|
print(f" Angle to close: {angle_to_close:.6f} rad ({math.degrees(angle_to_close):.2f} deg)") |
|
|
|
wait_time = calculate_wait_time_for_hohmann(CHASER_R, TARGET_R, angular_sep, EARTH_MASS) |
|
print(f" Raw wait_time: {wait_time:.6f} s ({wait_time/3600:.2f} h)") |
|
|
|
# Wait times for various DT values |
|
dt_values = [0.1, 0.5, 1.0, 2.0, 5.0, 10.0] |
|
print(f"\n=== WAIT TIME vs DT (via calculate_next_hohmann_wait_time) ===") |
|
for dt in dt_values: |
|
wt = calculate_next_hohmann_wait_time(CHASER_R, TARGET_R, angular_sep, EARTH_MASS, dt) |
|
arrival = wt + tt |
|
steps = int(arrival / dt) + 1 |
|
print(f" DT={dt:6.1f} s: wait={wt:12.2f} s arrival={arrival:12.2f} s steps~{steps}") |
|
|
|
# Recommended values for TIME_STEP = 0.1 |
|
dt = 0.1 |
|
wt = calculate_next_hohmann_wait_time(CHASER_R, TARGET_R, angular_sep, EARTH_MASS, dt) |
|
arrival = wt + tt |
|
max_steps = int(arrival / dt) + 1000 |
|
|
|
print(f"\n=== RECOMMENDED FOR TEST (DT=0.1) ===") |
|
print(f" wait_time: {wt:.2f} s") |
|
print(f" arrival_time: {arrival:.2f} s") |
|
print(f" expected_steps: {int(arrival / dt)}") |
|
print(f" max_steps (with margin): {max_steps}") |
|
print(f" safety_limit (1 yr): {3600.0 * 24.0 * 365.0:.2f} s") |
|
print(f"\n Milestone step indices:") |
|
print(f" just_before_departure: {int(wt / dt)}") |
|
print(f" after_departure: {int(wt / dt) + 1}") |
|
print(f" just_before_arrival: {int(arrival / dt)}") |
|
|
|
# Verify against C++ test output |
|
print(f"\n=== COMPARISON WITH C++ TEST OUTPUT ===") |
|
print(f" Python wait_time: {wt:.2f} s") |
|
print(f" C++ test wait_time: 60062.7 s") |
|
print(f" Python arrival: {arrival:.2f} s") |
|
print(f" C++ test arrival: 62804.5 s") |
|
print(f" Match: {abs(wt - 60062.7) < 0.1 and abs(arrival - 62804.5) < 0.1}") |
|
|
|
|
|
if __name__ == '__main__': |
|
main()
|
|
|