Browse Source
Remove src/rendezvous.h, src/rendezvous.cpp, tests/test_rendezvous.cpp, and tests/test_rendezvous.toml. No other modules depend on these files. Also remove RendezvousState enum, RendezvousTarget struct, and rendezvous_target field from Spacecraft in orbital_objects.h.main
6 changed files with 0 additions and 1329 deletions
@ -1,458 +0,0 @@ |
|||||||
#include "rendezvous.h" |
|
||||||
#include <math.h> |
|
||||||
#include <string.h> |
|
||||||
#include <stdlib.h> |
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Utility Functions - LVLH Frame Transformations
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
void cartesian_to_lvlh_basis( |
|
||||||
Vec3 position, |
|
||||||
Vec3 velocity, |
|
||||||
double parent_mass, |
|
||||||
Vec3* out_r_hat, |
|
||||||
Vec3* out_v_hat, |
|
||||||
Vec3* out_h_hat |
|
||||||
) { |
|
||||||
// r_hat: radial direction (from parent to object)
|
|
||||||
*out_r_hat = vec3_normalize(position); |
|
||||||
|
|
||||||
// h_hat: orbit normal (angular momentum direction)
|
|
||||||
Vec3 h = vec3_cross(position, velocity); |
|
||||||
double h_mag = vec3_magnitude(h); |
|
||||||
|
|
||||||
if (h_mag > 1e-10) { |
|
||||||
*out_h_hat = vec3_scale(h, 1.0 / h_mag); |
|
||||||
} else { |
|
||||||
// Degenerate case: set to default z-direction
|
|
||||||
*out_h_hat = (Vec3){.x = 0.0, .y = 1.0, .z = 0.0}; |
|
||||||
} |
|
||||||
|
|
||||||
// v_hat: along-track direction (completes right-handed frame)
|
|
||||||
*out_v_hat = vec3_cross(*out_h_hat, *out_r_hat); |
|
||||||
} |
|
||||||
|
|
||||||
void project_to_lvlh_frame( |
|
||||||
Vec3 rel_pos, |
|
||||||
Vec3 r_hat, |
|
||||||
Vec3 v_hat, |
|
||||||
Vec3 h_hat, |
|
||||||
Vec3 rel_vel, |
|
||||||
LVLHRelativeState* out |
|
||||||
) { |
|
||||||
out->radial = vec3_dot(rel_pos, r_hat); |
|
||||||
out->along_track = vec3_dot(rel_pos, v_hat); |
|
||||||
out->cross_track = vec3_dot(rel_pos, h_hat); |
|
||||||
|
|
||||||
out->v_radial = vec3_dot(rel_vel, r_hat); |
|
||||||
out->v_along_track = vec3_dot(rel_vel, v_hat); |
|
||||||
out->v_cross_track = vec3_dot(rel_vel, h_hat); |
|
||||||
} |
|
||||||
|
|
||||||
void lvlh_to_cartesian( |
|
||||||
LVLHRelativeState* lvlh, |
|
||||||
Vec3 r_hat, |
|
||||||
Vec3 v_hat, |
|
||||||
Vec3 h_hat, |
|
||||||
Vec3 chaser_pos, |
|
||||||
Vec3* out_r_cart, |
|
||||||
Vec3* out_v_cart |
|
||||||
) { |
|
||||||
// Position: r_cart = chaser_pos + lvlh_x*r_hat + lvlh_y*v_hat + lvlh_z*h_hat
|
|
||||||
Vec3 r_radial = vec3_scale(r_hat, lvlh->radial); |
|
||||||
Vec3 r_along = vec3_scale(v_hat, lvlh->along_track); |
|
||||||
Vec3 r_cross = vec3_scale(h_hat, lvlh->cross_track); |
|
||||||
*out_r_cart = vec3_add(vec3_add(r_radial, r_along), r_cross); |
|
||||||
*out_r_cart = vec3_add(chaser_pos, *out_r_cart); |
|
||||||
|
|
||||||
// Velocity: v_cart = lvlh_vx*r_hat + lvlh_vy*v_hat + lvlh_vz*h_hat
|
|
||||||
Vec3 v_radial = vec3_scale(r_hat, lvlh->v_radial); |
|
||||||
Vec3 v_along = vec3_scale(v_hat, lvlh->v_along_track); |
|
||||||
Vec3 v_cross = vec3_scale(h_hat, lvlh->v_cross_track); |
|
||||||
*out_v_cart = vec3_add(vec3_add(v_radial, v_along), v_cross); |
|
||||||
} |
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// CW Validity Functions
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
double compute_mean_motion( |
|
||||||
double parent_mass, |
|
||||||
double orbital_radius |
|
||||||
) { |
|
||||||
double mu = G * parent_mass; |
|
||||||
return sqrt(mu / pow(orbital_radius, 3)); |
|
||||||
} |
|
||||||
|
|
||||||
CWValidityResult check_cw_validity( |
|
||||||
Spacecraft* chaser, |
|
||||||
void* target, |
|
||||||
CelestialBody* parent, |
|
||||||
double current_time |
|
||||||
) { |
|
||||||
CWValidityResult result = {0}; |
|
||||||
|
|
||||||
// Get orbital radius of chaser
|
|
||||||
double orbital_radius = vec3_magnitude(chaser->local_position); |
|
||||||
if (orbital_radius < 1e-10) { |
|
||||||
result.overall_valid = false; |
|
||||||
return result; |
|
||||||
} |
|
||||||
|
|
||||||
// Compute mean motion
|
|
||||||
double n = compute_mean_motion(parent->mass, orbital_radius); |
|
||||||
|
|
||||||
// Get relative state
|
|
||||||
Vec3 rel_pos; |
|
||||||
Vec3 rel_vel; |
|
||||||
|
|
||||||
if (target == NULL) { |
|
||||||
result.overall_valid = false; |
|
||||||
return result; |
|
||||||
} |
|
||||||
|
|
||||||
// Check if target is spacecraft or body
|
|
||||||
bool is_spacecraft = ((Spacecraft*)target)->mass > 0 && |
|
||||||
((Spacecraft*)target)->parent_index >= 0; |
|
||||||
|
|
||||||
if (is_spacecraft) { |
|
||||||
Spacecraft* target_craft = (Spacecraft*)target; |
|
||||||
rel_pos = vec3_sub(target_craft->local_position, chaser->local_position); |
|
||||||
rel_vel = vec3_sub(target_craft->local_velocity, chaser->local_velocity); |
|
||||||
} else { |
|
||||||
CelestialBody* target_body = (CelestialBody*)target; |
|
||||||
rel_pos = vec3_sub(target_body->local_position, chaser->local_position); |
|
||||||
rel_vel = vec3_sub(target_body->local_velocity, chaser->local_velocity); |
|
||||||
} |
|
||||||
|
|
||||||
// Compute LVLH basis for chaser
|
|
||||||
Vec3 r_hat, v_hat, h_hat; |
|
||||||
cartesian_to_lvlh_basis(chaser->local_position, chaser->local_velocity, |
|
||||||
parent->mass, &r_hat, &v_hat, &h_hat); |
|
||||||
|
|
||||||
// Project to LVLH frame
|
|
||||||
LVLHRelativeState lvlh; |
|
||||||
project_to_lvlh_frame(rel_pos, r_hat, v_hat, h_hat, rel_vel, &lvlh); |
|
||||||
|
|
||||||
// Check spatial validity
|
|
||||||
double max_separation = fmax(fabs(lvlh.radial), |
|
||||||
fmax(fabs(lvlh.along_track), fabs(lvlh.cross_track))); |
|
||||||
double spatial_fraction = max_separation / orbital_radius; |
|
||||||
|
|
||||||
bool spatial_ok = spatial_fraction < CW_SPATIAL_LIMIT_FRACTION; |
|
||||||
|
|
||||||
// Check time validity
|
|
||||||
double time_since_linearization = current_time - chaser->rendezvous_target.cw_linearization_time; |
|
||||||
double n_dt = n * time_since_linearization; |
|
||||||
|
|
||||||
bool time_ok = n_dt < CW_TIME_LIMIT_N_DT; |
|
||||||
|
|
||||||
// Compute expected error (empirical estimate)
|
|
||||||
double error_percent = spatial_fraction * 100.0 * 3.0; // ~3x spatial fraction
|
|
||||||
|
|
||||||
result.spatial_valid = spatial_ok; |
|
||||||
result.time_valid = time_ok; |
|
||||||
result.overall_valid = spatial_ok && time_ok; |
|
||||||
result.spatial_fraction = spatial_fraction; |
|
||||||
result.n_dt = n_dt; |
|
||||||
result.expected_error = error_percent; |
|
||||||
|
|
||||||
return result; |
|
||||||
} |
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// CW Guidance Functions
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
CWGuidanceSolution solve_cw_guidance( |
|
||||||
Spacecraft* chaser, |
|
||||||
void* target, |
|
||||||
CelestialBody* parent, |
|
||||||
double time_to_intercept, |
|
||||||
double current_time |
|
||||||
) { |
|
||||||
CWGuidanceSolution solution = {0}; |
|
||||||
|
|
||||||
// Get orbital parameters
|
|
||||||
double orbital_radius = vec3_magnitude(chaser->local_position); |
|
||||||
double n = compute_mean_motion(parent->mass, orbital_radius); |
|
||||||
|
|
||||||
// Get relative state in LVLH frame
|
|
||||||
Vec3 rel_pos; |
|
||||||
Vec3 rel_vel; |
|
||||||
|
|
||||||
bool is_spacecraft_target = false; |
|
||||||
if (target != NULL) { |
|
||||||
is_spacecraft_target = ((Spacecraft*)target)->mass > 0 && |
|
||||||
((Spacecraft*)target)->parent_index >= 0; |
|
||||||
} |
|
||||||
|
|
||||||
if (is_spacecraft_target) { |
|
||||||
Spacecraft* target_craft = (Spacecraft*)target; |
|
||||||
rel_pos = vec3_sub(target_craft->local_position, chaser->local_position); |
|
||||||
rel_vel = vec3_sub(target_craft->local_velocity, chaser->local_velocity); |
|
||||||
} else { |
|
||||||
CelestialBody* target_body = (CelestialBody*)target; |
|
||||||
rel_pos = vec3_sub(target_body->local_position, chaser->local_position); |
|
||||||
rel_vel = vec3_sub(target_body->local_velocity, chaser->local_velocity); |
|
||||||
} |
|
||||||
|
|
||||||
// Compute LVLH basis
|
|
||||||
Vec3 r_hat, v_hat, h_hat; |
|
||||||
cartesian_to_lvlh_basis(chaser->local_position, chaser->local_velocity, |
|
||||||
parent->mass, &r_hat, &v_hat, &h_hat); |
|
||||||
|
|
||||||
// Project to LVLH frame
|
|
||||||
LVLHRelativeState lvlh; |
|
||||||
project_to_lvlh_frame(rel_pos, r_hat, v_hat, h_hat, rel_vel, &lvlh); |
|
||||||
|
|
||||||
// Closed-form CW solutions for required delta-v
|
|
||||||
// For rendezvous at time t:
|
|
||||||
// x(t) = (4 - 3*cos(nt)) * x0 + (1/n) * sin(nt) * vx0 + (2/n) * (1 - cos(nt)) * vy0
|
|
||||||
// y(t) = 6 * (sin(nt) - nt) * x0 + (4 * sin(nt) / n - 3 * t) * vx0 + (2 / n) * (cos(nt) - 1) * vy0
|
|
||||||
// z(t) = (1 / cos(nt)) * z0 + (1 / n) * sin(nt) * vz0
|
|
||||||
//
|
|
||||||
// To reach origin (x=y=z=0), solve for required delta-v
|
|
||||||
// This gives the impulsive burn needed at t=0
|
|
||||||
|
|
||||||
double sin_nt = sin(n * time_to_intercept); |
|
||||||
double cos_nt = cos(n * time_to_intercept); |
|
||||||
double nt = n * time_to_intercept; |
|
||||||
|
|
||||||
// CW transfer matrix elements
|
|
||||||
double A = 4.0 - 3.0 * cos_nt; |
|
||||||
double B = sin_nt / n; |
|
||||||
double C = 2.0 * (1.0 - cos_nt) / n; |
|
||||||
double D = 6.0 * (sin_nt - nt); |
|
||||||
double E = 4.0 * sin_nt / n - 3.0 * time_to_intercept; |
|
||||||
double F = 2.0 * (cos_nt - 1.0) / n; |
|
||||||
double G = 1.0 / cos_nt; // For z-direction (may be unstable)
|
|
||||||
double H = sin_nt / n; |
|
||||||
|
|
||||||
// Solve for required initial velocities to reach origin
|
|
||||||
// x0 = 0, y0 = 0, z0 = 0 at time t
|
|
||||||
// vx0 = -(A * vx0 + B * vy0 + C * vy0) / B ... simplified:
|
|
||||||
//
|
|
||||||
// For x-direction:
|
|
||||||
double vx0_required = -n * (4.0 * sin_nt - 3.0 * nt * sin_nt) * lvlh.radial / (sin_nt * sin_nt + 4.0 * (1.0 - cos_nt) * (1.0 - cos_nt)); |
|
||||||
double vy0_required = -2.0 * n * (1.0 - cos_nt) * lvlh.radial / (sin_nt * sin_nt + 4.0 * (1.0 - cos_nt) * (1.0 - cos_nt)); |
|
||||||
|
|
||||||
// Simplified approach: use standard CW impulsive transfer formulas
|
|
||||||
// Delta-v to cancel current relative velocity and reach target
|
|
||||||
|
|
||||||
// For x (radial): delta_vx = -2*n*(1-cos(nt))*y0 - n*sin(nt)*vx0 / (1-cos(nt))
|
|
||||||
double dx = -lvlh.radial; |
|
||||||
double dy = -lvlh.along_track; |
|
||||||
double dz = -lvlh.cross_track; |
|
||||||
|
|
||||||
// Standard CW impulsive solution for rendezvous
|
|
||||||
// Delta-v = -F(t) * r0 - G(t) * v0
|
|
||||||
// where F(t) and G(t) are state transition matrices
|
|
||||||
|
|
||||||
// Simplified: compute delta-v to cancel current relative motion
|
|
||||||
double dv_radial = -lvlh.v_radial; |
|
||||||
double dv_along = -lvlh.v_along_track; |
|
||||||
double dv_cross = -lvlh.v_cross_track; |
|
||||||
|
|
||||||
// Add correction terms for orbital curvature
|
|
||||||
dv_radial -= 2.0 * n * lvlh.along_track; // Coriolis term
|
|
||||||
dv_along += 4.0 * n * lvlh.radial; // Coriolis term
|
|
||||||
dv_cross -= n * lvlh.cross_track; // Restoring force
|
|
||||||
|
|
||||||
// Compute magnitude
|
|
||||||
double dv_mag = sqrt(dv_radial * dv_radial + |
|
||||||
dv_along * dv_along + |
|
||||||
dv_cross * dv_cross); |
|
||||||
|
|
||||||
// Normalize direction
|
|
||||||
if (dv_mag > 1e-10) { |
|
||||||
solution.valid = true; |
|
||||||
solution.delta_v_magnitude = dv_mag; |
|
||||||
solution.burn_direction_radial = dv_radial / dv_mag; |
|
||||||
solution.burn_direction_along_track = dv_along / dv_mag; |
|
||||||
solution.burn_direction_cross_track = dv_cross / dv_mag; |
|
||||||
solution.time_to_intercept = time_to_intercept; |
|
||||||
} else { |
|
||||||
solution.valid = false; |
|
||||||
} |
|
||||||
|
|
||||||
return solution; |
|
||||||
} |
|
||||||
|
|
||||||
double calculate_optimal_intercept_time( |
|
||||||
LVLHRelativeState* lvlh, |
|
||||||
double mean_motion |
|
||||||
) { |
|
||||||
// For circular coplanar orbits, optimal intercept time depends on separation
|
|
||||||
//
|
|
||||||
// For along-track separation: optimal t = pi / n (half orbit)
|
|
||||||
// For radial separation: optimal t varies based on initial conditions
|
|
||||||
//
|
|
||||||
// Simple heuristic: use half orbital period for along-track,
|
|
||||||
// adjust for radial component
|
|
||||||
|
|
||||||
double T = 2.0 * M_PI / mean_motion; |
|
||||||
double half_orbit = T / 2.0; |
|
||||||
|
|
||||||
// If primarily along-track separation, use half orbit
|
|
||||||
if (fabs(lvlh->along_track) > fabs(lvlh->radial)) { |
|
||||||
return half_orbit; |
|
||||||
} |
|
||||||
|
|
||||||
// If primarily radial, use quarter orbit
|
|
||||||
return T / 4.0; |
|
||||||
} |
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Rendezvous Target Management
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
void initialize_rendezvous_target( |
|
||||||
RendezvousTarget* target, |
|
||||||
int target_index, |
|
||||||
bool is_spacecraft_target, |
|
||||||
double approach_distance, |
|
||||||
double capture_distance, |
|
||||||
double max_relative_velocity |
|
||||||
) { |
|
||||||
target->target_index = target_index; |
|
||||||
target->state = RENDEZVOUS_PLANNING; |
|
||||||
target->approach_distance = approach_distance; |
|
||||||
target->capture_distance = capture_distance; |
|
||||||
target->max_relative_velocity = max_relative_velocity; |
|
||||||
target->cw_linearization_time = 0.0; |
|
||||||
target->is_spacecraft_target = is_spacecraft_target; |
|
||||||
} |
|
||||||
|
|
||||||
void update_rendezvous_state( |
|
||||||
Spacecraft* chaser, |
|
||||||
RendezvousTarget* target, |
|
||||||
CelestialBody* parent, |
|
||||||
double current_time, |
|
||||||
void* target_obj |
|
||||||
) { |
|
||||||
if (target->state == RENDEZVOUS_NONE || target->state == RENDEZVOUS_COMPLETE || |
|
||||||
target->state == RENDEZVOUS_FAILED) { |
|
||||||
return; |
|
||||||
} |
|
||||||
|
|
||||||
// Calculate current distance and relative velocity
|
|
||||||
double distance = calculate_rendezvous_distance(chaser, target_obj); |
|
||||||
double rel_vel_mag = calculate_relative_velocity_magnitude(chaser, target_obj, parent); |
|
||||||
|
|
||||||
// Check CW validity
|
|
||||||
CWValidityResult validity = check_cw_validity(chaser, target_obj, parent, current_time); |
|
||||||
|
|
||||||
// State machine transitions
|
|
||||||
switch (target->state) { |
|
||||||
case RENDEZVOUS_PLANNING: |
|
||||||
// Transition to APPROACHING when within approach distance
|
|
||||||
if (distance <= target->approach_distance && validity.overall_valid) { |
|
||||||
target->cw_linearization_time = current_time; |
|
||||||
target->state = RENDEZVOUS_APPROACHING; |
|
||||||
} |
|
||||||
break; |
|
||||||
|
|
||||||
case RENDEZVOUS_APPROACHING: |
|
||||||
// Transition to MATCHING when relative velocity is low
|
|
||||||
if (rel_vel_mag < target->max_relative_velocity * 0.5) { |
|
||||||
target->state = RENDEZVOUS_MATCHING; |
|
||||||
} |
|
||||||
// Check if we've moved away (failed approach)
|
|
||||||
else if (distance > target->approach_distance * 1.5) { |
|
||||||
target->state = RENDEZVOUS_FAILED; |
|
||||||
} |
|
||||||
// Update CW linearization time periodically
|
|
||||||
else if (current_time - target->cw_linearization_time > 100.0) { |
|
||||||
target->cw_linearization_time = current_time; |
|
||||||
} |
|
||||||
break; |
|
||||||
|
|
||||||
case RENDEZVOUS_MATCHING: |
|
||||||
// Transition to COMPLETE when within capture distance
|
|
||||||
if (distance <= target->capture_distance && rel_vel_mag < target->max_relative_velocity) { |
|
||||||
target->state = RENDEZVOUS_COMPLETE; |
|
||||||
} |
|
||||||
// Check if CW validity is lost
|
|
||||||
else if (!validity.overall_valid) { |
|
||||||
target->state = RENDEZVOUS_FAILED; |
|
||||||
} |
|
||||||
break; |
|
||||||
|
|
||||||
default: |
|
||||||
break; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Burn Application Functions
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
void apply_cw_guidance_burn( |
|
||||||
Spacecraft* chaser, |
|
||||||
CWGuidanceSolution* solution, |
|
||||||
CelestialBody* parent, |
|
||||||
double current_time |
|
||||||
) { |
|
||||||
if (!solution->valid) { |
|
||||||
return; |
|
||||||
} |
|
||||||
|
|
||||||
// Compute LVLH basis
|
|
||||||
Vec3 r_hat, v_hat, h_hat; |
|
||||||
cartesian_to_lvlh_basis(chaser->local_position, chaser->local_velocity, |
|
||||||
parent->mass, &r_hat, &v_hat, &h_hat); |
|
||||||
|
|
||||||
// Construct delta-v vector in Cartesian frame
|
|
||||||
Vec3 dv_cartesian = {0}; |
|
||||||
dv_cartesian = vec3_add(dv_cartesian, vec3_scale(r_hat, solution->burn_direction_radial * solution->delta_v_magnitude)); |
|
||||||
dv_cartesian = vec3_add(dv_cartesian, vec3_scale(v_hat, solution->burn_direction_along_track * solution->delta_v_magnitude)); |
|
||||||
dv_cartesian = vec3_add(dv_cartesian, vec3_scale(h_hat, solution->burn_direction_cross_track * solution->delta_v_magnitude)); |
|
||||||
|
|
||||||
// Apply delta-v to spacecraft velocity
|
|
||||||
chaser->local_velocity = vec3_add(chaser->local_velocity, dv_cartesian); |
|
||||||
chaser->global_velocity = vec3_add(chaser->global_velocity, dv_cartesian); |
|
||||||
|
|
||||||
// Reconstruct orbital elements after burn
|
|
||||||
chaser->orbit = cartesian_to_orbital_elements(chaser->local_position, chaser->local_velocity, parent->mass); |
|
||||||
} |
|
||||||
|
|
||||||
double calculate_relative_velocity_magnitude( |
|
||||||
Spacecraft* chaser, |
|
||||||
void* target, |
|
||||||
CelestialBody* parent |
|
||||||
) { |
|
||||||
Vec3 rel_vel; |
|
||||||
bool is_spacecraft = ((Spacecraft*)target)->mass > 0 && |
|
||||||
((Spacecraft*)target)->parent_index >= 0; |
|
||||||
|
|
||||||
if (is_spacecraft) { |
|
||||||
Spacecraft* target_craft = (Spacecraft*)target; |
|
||||||
rel_vel = vec3_sub(target_craft->local_velocity, chaser->local_velocity); |
|
||||||
} else { |
|
||||||
CelestialBody* target_body = (CelestialBody*)target; |
|
||||||
rel_vel = vec3_sub(target_body->local_velocity, chaser->local_velocity); |
|
||||||
} |
|
||||||
|
|
||||||
return vec3_magnitude(rel_vel); |
|
||||||
} |
|
||||||
|
|
||||||
double calculate_rendezvous_distance( |
|
||||||
Spacecraft* chaser, |
|
||||||
void* target |
|
||||||
) { |
|
||||||
Vec3 rel_pos; |
|
||||||
bool is_spacecraft = ((Spacecraft*)target)->mass > 0 && |
|
||||||
((Spacecraft*)target)->parent_index >= 0; |
|
||||||
|
|
||||||
if (is_spacecraft) { |
|
||||||
Spacecraft* target_craft = (Spacecraft*)target; |
|
||||||
rel_pos = vec3_sub(target_craft->local_position, chaser->local_position); |
|
||||||
} else { |
|
||||||
CelestialBody* target_body = (CelestialBody*)target; |
|
||||||
rel_pos = vec3_sub(target_body->local_position, chaser->local_position); |
|
||||||
} |
|
||||||
|
|
||||||
return vec3_magnitude(rel_pos); |
|
||||||
} |
|
||||||
@ -1,187 +0,0 @@ |
|||||||
#ifndef RENDEZVOUS_H |
|
||||||
#define RENDEZVOUS_H |
|
||||||
|
|
||||||
#include "physics.h" |
|
||||||
#include "orbital_mechanics.h" |
|
||||||
#include "orbital_objects.h" |
|
||||||
|
|
||||||
// Rendezvous Module
|
|
||||||
// Provides Clohessy-Wiltshire (Hill's) equations-based guidance for spacecraft rendezvous.
|
|
||||||
// Supports both spacecraft-to-spacecraft and spacecraft-to-body rendezvous in circular,
|
|
||||||
// coplanar orbits.
|
|
||||||
//
|
|
||||||
// Validity Limits (dimensionless, scale with orbital radius):
|
|
||||||
// - Spatial: 5% of orbital radius (x/r, y/r, z/r < 0.05)
|
|
||||||
// - Time: 2.0 radians of orbital motion (n*dt < 2.0, ~1/3 orbit)
|
|
||||||
|
|
||||||
|
|
||||||
// CW validity thresholds (dimensionless)
|
|
||||||
#define CW_SPATIAL_LIMIT_FRACTION 0.05 // 5% of orbital radius
|
|
||||||
#define CW_TIME_LIMIT_N_DT 2.0 // ~2 radians of orbital motion
|
|
||||||
|
|
||||||
// Relative state in LVLH frame
|
|
||||||
struct LVLHRelativeState { |
|
||||||
double radial; // x: radial separation (positive outward)
|
|
||||||
double along_track; // y: along-track separation (positive in direction of motion)
|
|
||||||
double cross_track; // z: cross-track separation
|
|
||||||
double v_radial; // radial velocity
|
|
||||||
double v_along_track;// along-track velocity
|
|
||||||
double v_cross_track;// cross-track velocity
|
|
||||||
}; |
|
||||||
|
|
||||||
// CW validity result
|
|
||||||
struct CWValidityResult { |
|
||||||
bool spatial_valid; // Within spatial limits
|
|
||||||
bool time_valid; // Within time limits
|
|
||||||
bool overall_valid; // Both spatial and time valid
|
|
||||||
double spatial_fraction; // max(|x|,|y|,|z|) / orbital_radius
|
|
||||||
double n_dt; // n * time_since_linearization
|
|
||||||
double expected_error; // Estimated error percentage
|
|
||||||
}; |
|
||||||
|
|
||||||
// CW guidance solution
|
|
||||||
struct CWGuidanceSolution { |
|
||||||
bool valid; // Whether solution is valid
|
|
||||||
double delta_v_magnitude; // Required delta-v (m/s)
|
|
||||||
double burn_direction_radial; // Radial component (unit vector)
|
|
||||||
double burn_direction_along_track; // Along-track component
|
|
||||||
double burn_direction_cross_track; // Cross-track component
|
|
||||||
double time_to_intercept; // Time to reach target (s)
|
|
||||||
}; |
|
||||||
|
|
||||||
|
|
||||||
// Utility Functions
|
|
||||||
|
|
||||||
// Transform Cartesian position/velocity to LVLH (Local Vertical Local Horizontal) frame
|
|
||||||
// LVLH basis vectors:
|
|
||||||
// - r_hat: Radial direction (from parent to object)
|
|
||||||
// - v_hat: Along-track direction (velocity direction for circular orbit)
|
|
||||||
// - h_hat: Cross-track direction (orbit normal)
|
|
||||||
void cartesian_to_lvlh_basis( |
|
||||||
Vec3 position, |
|
||||||
Vec3 velocity, |
|
||||||
double parent_mass, |
|
||||||
Vec3* out_r_hat, // Output: radial unit vector
|
|
||||||
Vec3* out_v_hat, // Output: along-track unit vector
|
|
||||||
Vec3* out_h_hat // Output: cross-track unit vector
|
|
||||||
); |
|
||||||
|
|
||||||
// Project relative state onto LVLH basis
|
|
||||||
void project_to_lvlh_frame( |
|
||||||
Vec3 rel_pos, // Relative position (target - chaser)
|
|
||||||
Vec3 r_hat, // Radial unit vector
|
|
||||||
Vec3 v_hat, // Along-track unit vector
|
|
||||||
Vec3 h_hat, // Cross-track unit vector
|
|
||||||
Vec3 rel_vel, // Relative velocity
|
|
||||||
LVLHRelativeState* out // Output: Relative state in LVLH frame
|
|
||||||
); |
|
||||||
|
|
||||||
// Transform LVLH relative state back to Cartesian
|
|
||||||
void lvlh_to_cartesian( |
|
||||||
LVLHRelativeState* lvlh, // Relative state in LVLH frame
|
|
||||||
Vec3 r_hat, // Radial unit vector
|
|
||||||
Vec3 v_hat, // Along-track unit vector
|
|
||||||
Vec3 h_hat, // Cross-track unit vector
|
|
||||||
Vec3 chaser_pos, // Chaser position (for absolute position calculation)
|
|
||||||
Vec3* out_r_cart, // Output: relative position in Cartesian
|
|
||||||
Vec3* out_v_cart // Output: relative velocity in Cartesian
|
|
||||||
); |
|
||||||
|
|
||||||
|
|
||||||
// CW Validity Functions
|
|
||||||
|
|
||||||
// Check if CW equations are valid for current relative state
|
|
||||||
// Validity criteria:
|
|
||||||
// - Spatial: max(|x|,|y|,|z|) / orbital_radius < 0.05
|
|
||||||
// - Time: n * dt < 2.0 (where dt is time since last linearization)
|
|
||||||
CWValidityResult check_cw_validity( // Returns: CWValidityResult with validity flags and error estimates
|
|
||||||
Spacecraft* chaser, // Chaser spacecraft
|
|
||||||
void* target, // Target (body or spacecraft)
|
|
||||||
CelestialBody* parent, // Central body
|
|
||||||
double current_time // Current simulation time
|
|
||||||
); |
|
||||||
|
|
||||||
// Compute mean motion for given orbital radius
|
|
||||||
double compute_mean_motion( // Returns: Mean motion n = sqrt(mu / a^3)
|
|
||||||
double parent_mass, // Mass of central body
|
|
||||||
double orbital_radius // Orbital radius
|
|
||||||
); |
|
||||||
|
|
||||||
|
|
||||||
// CW Guidance Functions
|
|
||||||
|
|
||||||
// Solve CW equations for rendezvous guidance
|
|
||||||
// Uses closed-form CW solutions to compute required delta-v for interception
|
|
||||||
// CW Equations (linearized relative motion):
|
|
||||||
// x'' - 2n*y' - 3n^2*x = 0
|
|
||||||
// y'' + 2n*x' = 0
|
|
||||||
// z'' + n^2*z = 0
|
|
||||||
CWGuidanceSolution solve_cw_guidance( // Returns: CWGuidanceSolution with required delta-v
|
|
||||||
Spacecraft* chaser, // Chaser spacecraft
|
|
||||||
void* target, // Target
|
|
||||||
CelestialBody* parent, // Central body
|
|
||||||
double time_to_intercept, // Desired time to intercept
|
|
||||||
double current_time // Current simulation time
|
|
||||||
); |
|
||||||
|
|
||||||
// Calculate optimal time to intercept for minimum delta-v
|
|
||||||
// For circular coplanar orbits, optimal intercept occurs at:
|
|
||||||
// - Half the relative orbital period for along-track separation
|
|
||||||
// - Adjusted for radial separation
|
|
||||||
double calculate_optimal_intercept_time( // Returns: Optimal time to intercept
|
|
||||||
LVLHRelativeState* lvlh, // Relative state in LVLH frame
|
|
||||||
double mean_motion // Mean motion of reference orbit
|
|
||||||
); |
|
||||||
|
|
||||||
|
|
||||||
// Rendezvous Target Management
|
|
||||||
|
|
||||||
// Initialize rendezvous target structure
|
|
||||||
void initialize_rendezvous_target( |
|
||||||
RendezvousTarget* target, // Target structure to initialize
|
|
||||||
int target_index, // Index of target object
|
|
||||||
bool is_spacecraft_target, // True if target is spacecraft, false if body
|
|
||||||
double approach_distance, // Distance to start approach phase
|
|
||||||
double capture_distance, // Distance for capture
|
|
||||||
double max_relative_velocity // Max closing speed for capture
|
|
||||||
); |
|
||||||
|
|
||||||
// Update rendezvous state machine based on current relative state
|
|
||||||
// State transitions:
|
|
||||||
// - PLANNING -> APPROACHING: when within approach_distance
|
|
||||||
// - APPROACHING -> MATCHING: when relative velocity < threshold
|
|
||||||
// - MATCHING -> COMPLETE: when within capture_distance AND relative velocity < max
|
|
||||||
// - Any -> FAILED: if CW validity is lost or distance increases
|
|
||||||
void update_rendezvous_state( |
|
||||||
Spacecraft* chaser, // Chaser spacecraft
|
|
||||||
RendezvousTarget* target, // Rendezvous target
|
|
||||||
CelestialBody* parent, // Central body
|
|
||||||
double current_time, // Current simulation time
|
|
||||||
void* target_obj // Target object (Spacecraft* or CelestialBody*)
|
|
||||||
); |
|
||||||
|
|
||||||
|
|
||||||
// Burn Application Functions
|
|
||||||
|
|
||||||
// Apply CW guidance burn to chaser spacecraft
|
|
||||||
void apply_cw_guidance_burn( |
|
||||||
Spacecraft* chaser, // Chaser spacecraft
|
|
||||||
CWGuidanceSolution* solution, // CW guidance solution
|
|
||||||
CelestialBody* parent, // Central body
|
|
||||||
double current_time // Current simulation time
|
|
||||||
); |
|
||||||
|
|
||||||
// Calculate relative velocity magnitude between chaser and target
|
|
||||||
double calculate_relative_velocity_magnitude( // Returns: Relative velocity magnitude (m/s)
|
|
||||||
Spacecraft* chaser, // Chaser spacecraft
|
|
||||||
void* target, // Target
|
|
||||||
CelestialBody* parent // Central body
|
|
||||||
); |
|
||||||
|
|
||||||
// Calculate distance between chaser and target
|
|
||||||
double calculate_rendezvous_distance( // Returns: Distance (m)
|
|
||||||
Spacecraft* chaser, // Chaser spacecraft
|
|
||||||
void* target // Target
|
|
||||||
); |
|
||||||
|
|
||||||
#endif // RENDEZVOUS_H
|
|
||||||
@ -1,611 +0,0 @@ |
|||||||
#include <catch2/catch_test_macros.hpp> |
|
||||||
#include <catch2/matchers/catch_matchers_floating_point.hpp> |
|
||||||
#include "../src/physics.h" |
|
||||||
#include "../src/orbital_mechanics.h" |
|
||||||
#include "../src/simulation.h" |
|
||||||
#include "../src/orbital_objects.h" |
|
||||||
#include "../src/rendezvous.h" |
|
||||||
#include "../src/config_loader.h" |
|
||||||
#include <cmath> |
|
||||||
#include <cstring> |
|
||||||
|
|
||||||
// Tolerances for rendezvous testing
|
|
||||||
const double POSITION_TOLERANCE = 100.0; // 100 m position tolerance for encounter
|
|
||||||
const double VELOCITY_TOLERANCE = 0.1; // 0.1 m/s velocity tolerance
|
|
||||||
const double CW_SPATIAL_TOLERANCE = 0.001; // 0.1% for CW validity checks
|
|
||||||
const double TIME_TOLERANCE = 1.0; // 1 second time tolerance
|
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Helper Functions
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
int find_spacecraft_by_name(SimulationState* sim, const char* name) { |
|
||||||
for (int i = 0; i < sim->craft_count; i++) { |
|
||||||
if (strcmp(sim->spacecraft[i].name, name) == 0) { |
|
||||||
return i; |
|
||||||
} |
|
||||||
} |
|
||||||
return -1; |
|
||||||
} |
|
||||||
|
|
||||||
void initialize_rendezvous_for_spacecraft( |
|
||||||
SimulationState* sim, |
|
||||||
const char* chaser_name, |
|
||||||
const char* target_name, |
|
||||||
double approach_distance, |
|
||||||
double capture_distance, |
|
||||||
double max_relative_velocity |
|
||||||
) { |
|
||||||
int chaser_index = find_spacecraft_by_name(sim, chaser_name); |
|
||||||
int target_index = find_spacecraft_by_name(sim, target_name); |
|
||||||
|
|
||||||
REQUIRE(chaser_index >= 0); |
|
||||||
REQUIRE(target_index >= 0); |
|
||||||
|
|
||||||
Spacecraft* chaser = &sim->spacecraft[chaser_index]; |
|
||||||
Spacecraft* target = &sim->spacecraft[target_index]; |
|
||||||
|
|
||||||
// Initialize rendezvous target on chaser
|
|
||||||
initialize_rendezvous_target( |
|
||||||
&chaser->rendezvous_target, |
|
||||||
target_index, |
|
||||||
true, // is spacecraft target
|
|
||||||
approach_distance, |
|
||||||
capture_distance, |
|
||||||
max_relative_velocity |
|
||||||
); |
|
||||||
|
|
||||||
// Initialize CW linearization time
|
|
||||||
chaser->rendezvous_target.cw_linearization_time = sim->time; |
|
||||||
} |
|
||||||
|
|
||||||
double calculate_relative_distance(Spacecraft* chaser, Spacecraft* target) { |
|
||||||
Vec3 rel_pos = vec3_sub(target->local_position, chaser->local_position); |
|
||||||
return vec3_magnitude(rel_pos); |
|
||||||
} |
|
||||||
|
|
||||||
double calculate_relative_velocity_magnitude(Spacecraft* chaser, Spacecraft* target) { |
|
||||||
Vec3 rel_vel = vec3_sub(target->local_velocity, chaser->local_velocity); |
|
||||||
return vec3_magnitude(rel_vel); |
|
||||||
} |
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Test Cases
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
TEST_CASE("Config loading for rendezvous", "[rendezvous][config]") { |
|
||||||
const double TIME_STEP = 30.0; |
|
||||||
|
|
||||||
SimulationState* sim = create_simulation(2, 5, 10, TIME_STEP); |
|
||||||
|
|
||||||
REQUIRE(load_system_config(sim, "tests/test_rendezvous.toml")); |
|
||||||
|
|
||||||
REQUIRE(sim->body_count == 1); |
|
||||||
REQUIRE(std::string(sim->bodies[0].name) == "Earth"); |
|
||||||
|
|
||||||
REQUIRE(sim->craft_count == 2); |
|
||||||
REQUIRE(std::string(sim->spacecraft[0].name) == "Target_Satellite"); |
|
||||||
REQUIRE(std::string(sim->spacecraft[1].name) == "Chaser_Satellite"); |
|
||||||
|
|
||||||
REQUIRE(sim->spacecraft[0].parent_index == 0); |
|
||||||
REQUIRE(sim->spacecraft[1].parent_index == 0); |
|
||||||
|
|
||||||
// Verify initial orbits
|
|
||||||
REQUIRE_THAT(sim->spacecraft[0].orbit.semi_major_axis, |
|
||||||
Catch::Matchers::WithinAbs(6.771e6, 1.0)); |
|
||||||
REQUIRE_THAT(sim->spacecraft[1].orbit.semi_major_axis, |
|
||||||
Catch::Matchers::WithinAbs(6.821e6, 1.0)); |
|
||||||
|
|
||||||
REQUIRE(sim->spacecraft[0].orbit.eccentricity == 0.0); |
|
||||||
REQUIRE(sim->spacecraft[1].orbit.eccentricity == 0.0); |
|
||||||
|
|
||||||
destroy_simulation(sim); |
|
||||||
} |
|
||||||
|
|
||||||
SCENARIO("CW validity check for close spacecraft", "[rendezvous][cw][validity]") { |
|
||||||
const double TIME_STEP = 30.0; |
|
||||||
|
|
||||||
SimulationState* sim = create_simulation(2, 5, 10, TIME_STEP); |
|
||||||
|
|
||||||
REQUIRE(load_system_config(sim, "tests/test_rendezvous.toml")); |
|
||||||
|
|
||||||
Spacecraft* chaser = &sim->spacecraft[1]; |
|
||||||
Spacecraft* target = &sim->spacecraft[0]; |
|
||||||
CelestialBody* earth = &sim->bodies[0]; |
|
||||||
|
|
||||||
// Initialize orbital positions
|
|
||||||
initialize_orbital_objects(sim); |
|
||||||
|
|
||||||
Vec3 initial_chaser_pos = chaser->local_position; |
|
||||||
Vec3 initial_target_pos = target->local_position; |
|
||||||
|
|
||||||
SECTION("Valid when within 5% of orbital radius") { |
|
||||||
// Initial separation is small (different semi-major axes)
|
|
||||||
CWValidityResult validity = check_cw_validity(chaser, target, earth, sim->time); |
|
||||||
|
|
||||||
INFO("Spatial fraction: " << validity.spatial_fraction); |
|
||||||
INFO("n*dt: " << validity.n_dt); |
|
||||||
INFO("Overall valid: " << validity.overall_valid); |
|
||||||
|
|
||||||
REQUIRE(validity.spatial_fraction < CW_SPATIAL_TOLERANCE * 10); // Should be well within 5%
|
|
||||||
REQUIRE(validity.overall_valid == true); |
|
||||||
} |
|
||||||
|
|
||||||
SECTION("Invalid when CW linearization is too old") { |
|
||||||
// Artificially set old linearization time
|
|
||||||
double old_time = sim->time - 2000.0; // 2000 seconds ago
|
|
||||||
chaser->rendezvous_target.cw_linearization_time = old_time; |
|
||||||
|
|
||||||
CWValidityResult validity = check_cw_validity(chaser, target, earth, sim->time); |
|
||||||
|
|
||||||
INFO("Time since linearization: " << (sim->time - chaser->rendezvous_target.cw_linearization_time)); |
|
||||||
INFO("n*dt: " << validity.n_dt); |
|
||||||
INFO("Overall valid: " << validity.overall_valid); |
|
||||||
|
|
||||||
// Should be invalid due to time limit (n*dt > 2.0)
|
|
||||||
REQUIRE(validity.time_valid == false); |
|
||||||
REQUIRE(validity.overall_valid == false); |
|
||||||
} |
|
||||||
|
|
||||||
SECTION("Spatial fraction scales with orbital radius") { |
|
||||||
double orbital_radius = vec3_magnitude(chaser->local_position); |
|
||||||
double separation = calculate_relative_distance(chaser, target); |
|
||||||
double expected_fraction = separation / orbital_radius; |
|
||||||
|
|
||||||
INFO("Orbital radius: " << orbital_radius); |
|
||||||
INFO("Separation: " << separation); |
|
||||||
INFO("Expected fraction: " << expected_fraction); |
|
||||||
INFO("CW limit: " << CW_SPATIAL_LIMIT_FRACTION); |
|
||||||
|
|
||||||
REQUIRE(expected_fraction < CW_SPATIAL_LIMIT_FRACTION); |
|
||||||
} |
|
||||||
|
|
||||||
destroy_simulation(sim); |
|
||||||
} |
|
||||||
|
|
||||||
SCENARIO("CW guidance calculation for rendezvous", "[rendezvous][cw][guidance]") { |
|
||||||
const double TIME_STEP = 30.0; |
|
||||||
|
|
||||||
SimulationState* sim = create_simulation(2, 5, 10, TIME_STEP); |
|
||||||
|
|
||||||
REQUIRE(load_system_config(sim, "tests/test_rendezvous.toml")); |
|
||||||
|
|
||||||
Spacecraft* chaser = &sim->spacecraft[1]; |
|
||||||
Spacecraft* target = &sim->spacecraft[0]; |
|
||||||
CelestialBody* earth = &sim->bodies[0]; |
|
||||||
|
|
||||||
initialize_orbital_objects(sim); |
|
||||||
|
|
||||||
SECTION("Calculate guidance for quarter-orbit intercept") { |
|
||||||
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
|
||||||
double intercept_time = orbital_period / 4.0; // Quarter orbit (valid: n*dt < 2.0)
|
|
||||||
|
|
||||||
// Initialize rendezvous target so cw_linearization_time is set
|
|
||||||
initialize_rendezvous_for_spacecraft( |
|
||||||
sim, "Chaser_Satellite", "Target_Satellite", |
|
||||||
5000.0, 100.0, 0.5 |
|
||||||
); |
|
||||||
|
|
||||||
// Check CW validity first
|
|
||||||
CWValidityResult validity = check_cw_validity(chaser, target, earth, sim->time); |
|
||||||
INFO("Spatial fraction: " << validity.spatial_fraction); |
|
||||||
INFO("n*dt: " << validity.n_dt); |
|
||||||
INFO("Overall valid: " << validity.overall_valid); |
|
||||||
|
|
||||||
REQUIRE(validity.overall_valid == true); |
|
||||||
|
|
||||||
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, intercept_time, sim->time); |
|
||||||
|
|
||||||
INFO("Intercept time: " << intercept_time << " s"); |
|
||||||
INFO("Solution valid: " << solution.valid); |
|
||||||
INFO("Delta-v magnitude: " << solution.delta_v_magnitude << " m/s"); |
|
||||||
INFO("Burn direction radial: " << solution.burn_direction_radial); |
|
||||||
INFO("Burn direction along-track: " << solution.burn_direction_along_track); |
|
||||||
|
|
||||||
REQUIRE(solution.valid == true); |
|
||||||
REQUIRE(solution.delta_v_magnitude > 0.0); |
|
||||||
// Simplified CW approach gives ~255 m/s for 50km separation, quarter-orbit
|
|
||||||
// (cancels relative velocity + orbital curvature correction)
|
|
||||||
// Acceptable range: [250, 260] m/s
|
|
||||||
REQUIRE(solution.delta_v_magnitude > 250.0); |
|
||||||
REQUIRE(solution.delta_v_magnitude < 260.0); |
|
||||||
} |
|
||||||
|
|
||||||
SECTION("Calculate guidance for quarter-orbit, 1km separation") { |
|
||||||
// Initialize rendezvous target so cw_linearization_time is set
|
|
||||||
initialize_rendezvous_for_spacecraft( |
|
||||||
sim, "Chaser_Satellite", "Target_Satellite", |
|
||||||
5000.0, 100.0, 0.5 |
|
||||||
); |
|
||||||
|
|
||||||
// Create smaller separation (1 km instead of 50 km)
|
|
||||||
// Move chaser from 50 km higher to 1 km higher (reduce by 49 km)
|
|
||||||
Vec3 r_hat = vec3_normalize(chaser->local_position); |
|
||||||
Vec3 initial_chaser_pos = chaser->local_position; |
|
||||||
chaser->local_position = vec3_sub(chaser->local_position, vec3_scale(r_hat, 49000.0)); |
|
||||||
chaser->orbit = cartesian_to_orbital_elements(chaser->local_position, |
|
||||||
chaser->local_velocity, |
|
||||||
earth->mass); |
|
||||||
compute_spacecraft_globals(sim); |
|
||||||
|
|
||||||
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
|
||||||
double intercept_time = orbital_period / 4.0; // Quarter orbit (valid: n*dt < 2.0)
|
|
||||||
|
|
||||||
// Check CW validity first
|
|
||||||
CWValidityResult validity = check_cw_validity(chaser, target, earth, sim->time); |
|
||||||
INFO("Spatial fraction: " << validity.spatial_fraction); |
|
||||||
INFO("n*dt: " << validity.n_dt); |
|
||||||
INFO("Overall valid: " << validity.overall_valid); |
|
||||||
|
|
||||||
REQUIRE(validity.overall_valid == true); |
|
||||||
|
|
||||||
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, intercept_time, sim->time); |
|
||||||
|
|
||||||
INFO("Intercept time: " << intercept_time << " s"); |
|
||||||
INFO("Delta-v magnitude: " << solution.delta_v_magnitude << " m/s"); |
|
||||||
|
|
||||||
REQUIRE(solution.valid == true); |
|
||||||
REQUIRE(solution.delta_v_magnitude > 0.0); |
|
||||||
// Simplified CW approach gives ~5-35 m/s for 1km separation
|
|
||||||
// (varies based on exact orbital configuration)
|
|
||||||
// Acceptable range: [30, 40] m/s
|
|
||||||
REQUIRE(solution.delta_v_magnitude > 30.0); |
|
||||||
REQUIRE(solution.delta_v_magnitude < 40.0); |
|
||||||
} |
|
||||||
|
|
||||||
SECTION("Optimal intercept time calculation") { |
|
||||||
// Get relative state in LVLH frame
|
|
||||||
Vec3 r_hat, v_hat, h_hat; |
|
||||||
cartesian_to_lvlh_basis(chaser->local_position, chaser->local_velocity, |
|
||||||
earth->mass, &r_hat, &v_hat, &h_hat); |
|
||||||
|
|
||||||
Vec3 rel_pos = vec3_sub(target->local_position, chaser->local_position); |
|
||||||
Vec3 rel_vel = vec3_sub(target->local_velocity, chaser->local_velocity); |
|
||||||
|
|
||||||
LVLHRelativeState lvlh; |
|
||||||
project_to_lvlh_frame(rel_pos, r_hat, v_hat, h_hat, rel_vel, &lvlh); |
|
||||||
|
|
||||||
double mean_motion = compute_mean_motion(earth->mass, |
|
||||||
vec3_magnitude(chaser->local_position)); |
|
||||||
|
|
||||||
double optimal_time = calculate_optimal_intercept_time(&lvlh, mean_motion); |
|
||||||
|
|
||||||
INFO("LVLH radial: " << lvlh.radial); |
|
||||||
INFO("LVLH along-track: " << lvlh.along_track); |
|
||||||
INFO("Mean motion: " << mean_motion); |
|
||||||
INFO("Optimal intercept time: " << optimal_time << " s"); |
|
||||||
|
|
||||||
// Should be within CW validity limits: n*dt < 2.0
|
|
||||||
// i.e., optimal_time < 2.0 / mean_motion
|
|
||||||
double orbital_period = 2.0 * M_PI / mean_motion; |
|
||||||
double max_valid_time = CW_TIME_LIMIT_N_DT / mean_motion; |
|
||||||
|
|
||||||
INFO("Orbital period: " << orbital_period << " s"); |
|
||||||
INFO("Max valid time (n*dt=2.0): " << max_valid_time << " s"); |
|
||||||
|
|
||||||
REQUIRE(optimal_time > 0.0); |
|
||||||
REQUIRE(optimal_time < max_valid_time); // Must be within CW validity
|
|
||||||
} |
|
||||||
|
|
||||||
destroy_simulation(sim); |
|
||||||
} |
|
||||||
|
|
||||||
SCENARIO("Rendezvous execution with CW guidance", "[rendezvous][execution]") { |
|
||||||
const double TIME_STEP = 10.0; |
|
||||||
|
|
||||||
SimulationState* sim = create_simulation(2, 5, 10, TIME_STEP); |
|
||||||
|
|
||||||
REQUIRE(load_system_config(sim, "tests/test_rendezvous.toml")); |
|
||||||
|
|
||||||
Spacecraft* chaser = &sim->spacecraft[1]; |
|
||||||
Spacecraft* target = &sim->spacecraft[0]; |
|
||||||
CelestialBody* earth = &sim->bodies[0]; |
|
||||||
|
|
||||||
initialize_orbital_objects(sim); |
|
||||||
|
|
||||||
// Store initial positions
|
|
||||||
Vec3 initial_chaser_pos = chaser->local_position; |
|
||||||
Vec3 initial_target_pos = target->local_position; |
|
||||||
|
|
||||||
SECTION("Execute single CW burn with quarter-orbit intercept") { |
|
||||||
// Move chaser to 1 km radial separation for valid CW scenario
|
|
||||||
Vec3 r_hat = vec3_normalize(chaser->local_position); |
|
||||||
Vec3 v_hat = vec3_normalize(chaser->local_velocity); |
|
||||||
chaser->local_position = vec3_add(target->local_position, vec3_scale(r_hat, 1000.0)); |
|
||||||
|
|
||||||
// Update velocity to match new orbit (circular orbit velocity)
|
|
||||||
double mu = G * earth->mass; |
|
||||||
double r_new = vec3_magnitude(chaser->local_position); |
|
||||||
double v_new = sqrt(mu / r_new); |
|
||||||
chaser->local_velocity = vec3_scale(v_hat, v_new); |
|
||||||
|
|
||||||
chaser->orbit = cartesian_to_orbital_elements(chaser->local_position, |
|
||||||
chaser->local_velocity, |
|
||||||
earth->mass); |
|
||||||
compute_spacecraft_globals(sim); |
|
||||||
|
|
||||||
// Initialize rendezvous
|
|
||||||
initialize_rendezvous_for_spacecraft( |
|
||||||
sim, "Chaser_Satellite", "Target_Satellite", |
|
||||||
5000.0, // approach_distance: 5 km
|
|
||||||
100.0, // capture_distance: 100 m
|
|
||||||
0.5 // max_relative_velocity: 0.5 m/s
|
|
||||||
); |
|
||||||
|
|
||||||
double initial_distance = calculate_relative_distance(chaser, target); |
|
||||||
INFO("Initial distance: " << initial_distance << " m"); |
|
||||||
|
|
||||||
// Check CW validity before guidance
|
|
||||||
CWValidityResult validity = check_cw_validity(chaser, target, earth, sim->time); |
|
||||||
INFO("Spatial fraction: " << validity.spatial_fraction); |
|
||||||
INFO("n*dt: " << validity.n_dt); |
|
||||||
INFO("Overall valid: " << validity.overall_valid); |
|
||||||
REQUIRE(validity.overall_valid == true); |
|
||||||
|
|
||||||
// Calculate and execute CW guidance burn (quarter-orbit, valid time)
|
|
||||||
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
|
||||||
double intercept_time = orbital_period / 4.0; // Quarter orbit
|
|
||||||
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, intercept_time, sim->time); |
|
||||||
|
|
||||||
INFO("Intercept time: " << intercept_time << " s"); |
|
||||||
INFO("Calculated delta-v: " << solution.delta_v_magnitude << " m/s"); |
|
||||||
REQUIRE(solution.valid == true); |
|
||||||
|
|
||||||
apply_cw_guidance_burn(chaser, &solution, earth, sim->time); |
|
||||||
|
|
||||||
// Propagate for quarter orbit
|
|
||||||
double propagation_time = intercept_time; |
|
||||||
int num_steps = (int)(propagation_time / TIME_STEP); |
|
||||||
|
|
||||||
for (int i = 0; i < num_steps; i++) { |
|
||||||
update_spacecraft_physics(sim); |
|
||||||
compute_spacecraft_globals(sim); |
|
||||||
sim->time += TIME_STEP; |
|
||||||
} |
|
||||||
|
|
||||||
double final_distance = calculate_relative_distance(chaser, target); |
|
||||||
double final_rel_vel = calculate_relative_velocity_magnitude(chaser, target); |
|
||||||
|
|
||||||
INFO("Final distance: " << final_distance << " m"); |
|
||||||
INFO("Final relative velocity: " << final_rel_vel << " m/s"); |
|
||||||
|
|
||||||
// Verify that we got closer (CW guidance should reduce separation)
|
|
||||||
// Note: Exact rendezvous may not be achieved due to linearization errors
|
|
||||||
REQUIRE(final_distance < initial_distance * 1.5); // At least 33% improvement
|
|
||||||
REQUIRE(solution.delta_v_magnitude < 200.0); // Reasonable delta-v
|
|
||||||
} |
|
||||||
|
|
||||||
SECTION("Update rendezvous state machine") { |
|
||||||
initialize_rendezvous_for_spacecraft( |
|
||||||
sim, "Chaser_Satellite", "Target_Satellite", |
|
||||||
5000.0, 100.0, 0.5 |
|
||||||
); |
|
||||||
|
|
||||||
// Initially should be in PLANNING state
|
|
||||||
REQUIRE(sim->spacecraft[1].rendezvous_target.state == RENDEZVOUS_PLANNING); |
|
||||||
|
|
||||||
// Execute burn to get into approach phase (quarter-orbit)
|
|
||||||
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
|
||||||
double intercept_time = orbital_period / 4.0; // Quarter orbit
|
|
||||||
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, intercept_time, sim->time); |
|
||||||
REQUIRE(solution.valid == true); |
|
||||||
apply_cw_guidance_burn(chaser, &solution, earth, sim->time); |
|
||||||
|
|
||||||
// Propagate for quarter orbit
|
|
||||||
int num_steps = (int)(intercept_time / TIME_STEP); |
|
||||||
for (int i = 0; i < num_steps; i++) { |
|
||||||
update_spacecraft_physics(sim); |
|
||||||
compute_spacecraft_globals(sim); |
|
||||||
sim->time += TIME_STEP; |
|
||||||
} |
|
||||||
|
|
||||||
// Update state machine
|
|
||||||
update_rendezvous_state(chaser, &chaser->rendezvous_target, earth, sim->time, target); |
|
||||||
|
|
||||||
INFO("Final rendezvous state: " << sim->spacecraft[1].rendezvous_target.state); |
|
||||||
|
|
||||||
// Should have progressed to APPROACHING or MATCHING
|
|
||||||
REQUIRE(sim->spacecraft[1].rendezvous_target.state != RENDEZVOUS_NONE); |
|
||||||
REQUIRE(sim->spacecraft[1].rendezvous_target.state != RENDEZVOUS_FAILED); |
|
||||||
} |
|
||||||
|
|
||||||
destroy_simulation(sim); |
|
||||||
} |
|
||||||
|
|
||||||
SCENARIO("Rendezvous with different initial separations", "[rendezvous][separation]") { |
|
||||||
const double TIME_STEP = 30.0; |
|
||||||
|
|
||||||
SimulationState* sim = create_simulation(2, 5, 10, TIME_STEP); |
|
||||||
|
|
||||||
REQUIRE(load_system_config(sim, "tests/test_rendezvous.toml")); |
|
||||||
|
|
||||||
Spacecraft* chaser = &sim->spacecraft[1]; |
|
||||||
Spacecraft* target = &sim->spacecraft[0]; |
|
||||||
CelestialBody* earth = &sim->bodies[0]; |
|
||||||
|
|
||||||
initialize_orbital_objects(sim); |
|
||||||
|
|
||||||
SECTION("Small separation (1 km along-track)") { |
|
||||||
// Manually adjust chaser to be 1 km behind target
|
|
||||||
// Move chaser from 50 km radial separation to 1 km along-track separation
|
|
||||||
Vec3 r_hat = vec3_normalize(chaser->local_position); |
|
||||||
Vec3 v_hat = vec3_normalize(chaser->local_velocity); |
|
||||||
|
|
||||||
// First, move chaser to target's orbital radius (remove 50 km radial separation)
|
|
||||||
Vec3 chaser_to_target = vec3_sub(target->local_position, chaser->local_position); |
|
||||||
double current_separation = vec3_magnitude(chaser_to_target); |
|
||||||
|
|
||||||
// Move chaser to be 1 km behind target along-track
|
|
||||||
chaser->local_position = vec3_add(target->local_position, vec3_scale(v_hat, -1000.0)); |
|
||||||
|
|
||||||
// Reconstruct orbital elements
|
|
||||||
chaser->orbit = cartesian_to_orbital_elements(chaser->local_position, |
|
||||||
chaser->local_velocity, |
|
||||||
earth->mass); |
|
||||||
compute_spacecraft_globals(sim); |
|
||||||
|
|
||||||
initialize_rendezvous_for_spacecraft( |
|
||||||
sim, "Chaser_Satellite", "Target_Satellite", |
|
||||||
5000.0, 100.0, 0.5 |
|
||||||
); |
|
||||||
|
|
||||||
double initial_distance = calculate_relative_distance(chaser, target); |
|
||||||
INFO("Initial distance: " << initial_distance << " m"); |
|
||||||
|
|
||||||
REQUIRE(initial_distance < 10000.0); // Should be ~1 km
|
|
||||||
|
|
||||||
// Check CW validity
|
|
||||||
CWValidityResult validity = check_cw_validity(chaser, target, earth, sim->time); |
|
||||||
INFO("Spatial fraction: " << validity.spatial_fraction); |
|
||||||
INFO("n*dt: " << validity.n_dt); |
|
||||||
REQUIRE(validity.overall_valid == true); |
|
||||||
|
|
||||||
// Execute rendezvous with quarter-orbit intercept
|
|
||||||
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
|
||||||
double intercept_time = orbital_period / 4.0; // Quarter orbit
|
|
||||||
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, intercept_time, sim->time); |
|
||||||
REQUIRE(solution.valid == true); |
|
||||||
apply_cw_guidance_burn(chaser, &solution, earth, sim->time); |
|
||||||
|
|
||||||
// Propagate for quarter orbit
|
|
||||||
int num_steps = (int)(intercept_time / TIME_STEP); |
|
||||||
for (int i = 0; i < num_steps; i++) { |
|
||||||
update_spacecraft_physics(sim); |
|
||||||
compute_spacecraft_globals(sim); |
|
||||||
sim->time += TIME_STEP; |
|
||||||
} |
|
||||||
|
|
||||||
double final_distance = calculate_relative_distance(chaser, target); |
|
||||||
INFO("Final distance: " << final_distance << " m"); |
|
||||||
|
|
||||||
// Verify improvement (CW guidance should reduce separation)
|
|
||||||
REQUIRE(final_distance < initial_distance); |
|
||||||
REQUIRE(solution.delta_v_magnitude < 10.0); // Small delta-v for 1 km separation
|
|
||||||
} |
|
||||||
|
|
||||||
SECTION("Medium separation (10 km radial)") { |
|
||||||
// Manually adjust chaser to be 10 km above target
|
|
||||||
// Move chaser from 50 km radial separation to 10 km radial separation
|
|
||||||
Vec3 r_hat = vec3_normalize(chaser->local_position); |
|
||||||
|
|
||||||
// Move chaser to be 10 km above target (radial separation)
|
|
||||||
chaser->local_position = vec3_add(target->local_position, vec3_scale(r_hat, 10000.0)); |
|
||||||
|
|
||||||
chaser->orbit = cartesian_to_orbital_elements(chaser->local_position, |
|
||||||
chaser->local_velocity, |
|
||||||
earth->mass); |
|
||||||
compute_spacecraft_globals(sim); |
|
||||||
|
|
||||||
initialize_rendezvous_for_spacecraft( |
|
||||||
sim, "Chaser_Satellite", "Target_Satellite", |
|
||||||
50000.0, 100.0, 0.5 |
|
||||||
); |
|
||||||
|
|
||||||
double initial_distance = calculate_relative_distance(chaser, target); |
|
||||||
INFO("Initial distance: " << initial_distance << " m"); |
|
||||||
|
|
||||||
REQUIRE(initial_distance < 20000.0); // Should be ~10 km
|
|
||||||
|
|
||||||
// Check CW validity
|
|
||||||
CWValidityResult validity = check_cw_validity(chaser, target, earth, sim->time); |
|
||||||
INFO("Spatial fraction: " << validity.spatial_fraction); |
|
||||||
INFO("n*dt: " << validity.n_dt); |
|
||||||
REQUIRE(validity.overall_valid == true); |
|
||||||
|
|
||||||
// Execute rendezvous with quarter-orbit intercept
|
|
||||||
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
|
||||||
double intercept_time = orbital_period / 4.0; // Quarter orbit
|
|
||||||
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, intercept_time, sim->time); |
|
||||||
REQUIRE(solution.valid == true); |
|
||||||
apply_cw_guidance_burn(chaser, &solution, earth, sim->time); |
|
||||||
|
|
||||||
// Propagate for quarter orbit
|
|
||||||
int num_steps = (int)(intercept_time / TIME_STEP); |
|
||||||
for (int i = 0; i < num_steps; i++) { |
|
||||||
update_spacecraft_physics(sim); |
|
||||||
compute_spacecraft_globals(sim); |
|
||||||
sim->time += TIME_STEP; |
|
||||||
} |
|
||||||
|
|
||||||
double final_distance = calculate_relative_distance(chaser, target); |
|
||||||
INFO("Final distance: " << final_distance << " m"); |
|
||||||
|
|
||||||
// Verify improvement
|
|
||||||
REQUIRE(final_distance < initial_distance); |
|
||||||
REQUIRE(solution.delta_v_magnitude < 50.0); // Reasonable delta-v for 10 km
|
|
||||||
} |
|
||||||
|
|
||||||
destroy_simulation(sim); |
|
||||||
} |
|
||||||
|
|
||||||
SCENARIO("Rendezvous with CW linearization updates", "[rendezvous][linearization]") { |
|
||||||
const double TIME_STEP = 30.0; |
|
||||||
|
|
||||||
SimulationState* sim = create_simulation(2, 5, 10, TIME_STEP); |
|
||||||
|
|
||||||
REQUIRE(load_system_config(sim, "tests/test_rendezvous.toml")); |
|
||||||
|
|
||||||
Spacecraft* chaser = &sim->spacecraft[1]; |
|
||||||
Spacecraft* target = &sim->spacecraft[0]; |
|
||||||
CelestialBody* earth = &sim->bodies[0]; |
|
||||||
|
|
||||||
initialize_orbital_objects(sim); |
|
||||||
|
|
||||||
SECTION("CW validity maintained with periodic linearization") { |
|
||||||
initialize_rendezvous_for_spacecraft( |
|
||||||
sim, "Chaser_Satellite", "Target_Satellite", |
|
||||||
5000.0, 100.0, 0.5 |
|
||||||
); |
|
||||||
|
|
||||||
// Execute burn with quarter-orbit intercept
|
|
||||||
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
|
||||||
double intercept_time = orbital_period / 4.0; // Quarter orbit
|
|
||||||
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, intercept_time, sim->time); |
|
||||||
REQUIRE(solution.valid == true); |
|
||||||
apply_cw_guidance_burn(chaser, &solution, earth, sim->time); |
|
||||||
|
|
||||||
// Propagate for quarter orbit with periodic linearization updates
|
|
||||||
int num_steps = (int)(intercept_time / TIME_STEP); |
|
||||||
double update_interval = 200.0; // Update every 200 seconds
|
|
||||||
double last_update_time = sim->time; |
|
||||||
|
|
||||||
for (int i = 0; i < num_steps; i++) { |
|
||||||
// Update CW linearization time periodically
|
|
||||||
if (sim->time - last_update_time >= update_interval) { |
|
||||||
chaser->rendezvous_target.cw_linearization_time = sim->time; |
|
||||||
last_update_time = sim->time; |
|
||||||
} |
|
||||||
|
|
||||||
update_spacecraft_physics(sim); |
|
||||||
compute_spacecraft_globals(sim); |
|
||||||
sim->time += TIME_STEP; |
|
||||||
} |
|
||||||
|
|
||||||
double final_distance = calculate_relative_distance(chaser, target); |
|
||||||
INFO("Final distance: " << final_distance << " m"); |
|
||||||
|
|
||||||
// Verify improvement
|
|
||||||
REQUIRE(final_distance < orbital_period * 1000.0); // Reasonable bound
|
|
||||||
} |
|
||||||
|
|
||||||
SECTION("CW validity lost without updates") { |
|
||||||
// Don't update linearization time
|
|
||||||
initialize_rendezvous_for_spacecraft( |
|
||||||
sim, "Chaser_Satellite", "Target_Satellite", |
|
||||||
5000.0, 100.0, 0.5 |
|
||||||
); |
|
||||||
|
|
||||||
// Artificially delay linearization
|
|
||||||
chaser->rendezvous_target.cw_linearization_time = sim->time - 3000.0; // 3000 seconds ago
|
|
||||||
|
|
||||||
CWValidityResult validity = check_cw_validity(chaser, target, earth, sim->time); |
|
||||||
INFO("n*dt: " << validity.n_dt); |
|
||||||
INFO("Overall valid: " << validity.overall_valid); |
|
||||||
|
|
||||||
// Should be invalid due to old linearization
|
|
||||||
REQUIRE(validity.overall_valid == false); |
|
||||||
REQUIRE(validity.time_valid == false); |
|
||||||
} |
|
||||||
|
|
||||||
destroy_simulation(sim); |
|
||||||
} |
|
||||||
@ -1,48 +0,0 @@ |
|||||||
# Test Configuration: Spacecraft-to-Spacecraft Rendezvous |
|
||||||
# Two spacecraft in circular, coplanar LEO orbits |
|
||||||
# Chaser starts in higher orbit, performs CW-based rendezvous with target |
|
||||||
# Tests the complete rendezvous workflow: CW validity, guidance calculation, execution |
|
||||||
|
|
||||||
[[bodies]] |
|
||||||
name = "Earth" |
|
||||||
mass = 5.972e24 |
|
||||||
radius = 6.371e6 |
|
||||||
parent_index = -1 |
|
||||||
color = { r = 0.0, g = 0.5, b = 1.0 } |
|
||||||
orbit = { |
|
||||||
semi_major_axis = 0.0, |
|
||||||
eccentricity = 0.0, |
|
||||||
true_anomaly = 0.0 |
|
||||||
} |
|
||||||
|
|
||||||
# ========== TARGET SPACECRAFT ========== |
|
||||||
# Circular LEO orbit at 400 km altitude |
|
||||||
# This is the spacecraft being rendezvoused with |
|
||||||
[[spacecraft]] |
|
||||||
name = "Target_Satellite" |
|
||||||
mass = 500.0 |
|
||||||
parent_index = 0 |
|
||||||
orbit = { |
|
||||||
semi_major_axis = 6.771e6, |
|
||||||
eccentricity = 0.0, |
|
||||||
true_anomaly = 0.0, |
|
||||||
inclination = 0.0, |
|
||||||
longitude_of_ascending_node = 0.0, |
|
||||||
argument_of_periapsis = 0.0 |
|
||||||
} |
|
||||||
|
|
||||||
# ========== CHASER SPACECRAFT ========== |
|
||||||
# Circular LEO orbit at 450 km altitude (slightly higher) |
|
||||||
# Will perform rendezvous with Target_Satellite |
|
||||||
[[spacecraft]] |
|
||||||
name = "Chaser_Satellite" |
|
||||||
mass = 500.0 |
|
||||||
parent_index = 0 |
|
||||||
orbit = { |
|
||||||
semi_major_axis = 6.821e6, |
|
||||||
eccentricity = 0.0, |
|
||||||
true_anomaly = 0.0, |
|
||||||
inclination = 0.0, |
|
||||||
longitude_of_ascending_node = 0.0, |
|
||||||
argument_of_periapsis = 0.0 |
|
||||||
} |
|
||||||
Loading…
Reference in new issue