7 changed files with 1370 additions and 0 deletions
@ -0,0 +1,493 @@ |
|||||||
|
#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, |
||||||
|
SimulationState* sim |
||||||
|
) { |
||||||
|
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 = sim->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, |
||||||
|
SimulationState* sim |
||||||
|
) { |
||||||
|
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* get_rendezvous_target( |
||||||
|
SimulationState* sim, |
||||||
|
int target_index, |
||||||
|
bool* is_spacecraft_target |
||||||
|
) { |
||||||
|
if (target_index < 0 || target_index >= sim->craft_count) { |
||||||
|
*is_spacecraft_target = true; |
||||||
|
return NULL; |
||||||
|
} |
||||||
|
|
||||||
|
// Default to spacecraft
|
||||||
|
*is_spacecraft_target = true; |
||||||
|
return &sim->spacecraft[target_index]; |
||||||
|
} |
||||||
|
|
||||||
|
void update_rendezvous_state( |
||||||
|
Spacecraft* chaser, |
||||||
|
SimulationState* sim |
||||||
|
) { |
||||||
|
RendezvousTarget* target = &chaser->rendezvous_target; |
||||||
|
|
||||||
|
if (target->state == RENDEZVOUS_NONE || target->state == RENDEZVOUS_COMPLETE || |
||||||
|
target->state == RENDEZVOUS_FAILED) { |
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
// Get target object
|
||||||
|
void* target_obj = get_rendezvous_target(sim, target->target_index, &target->is_spacecraft_target); |
||||||
|
if (target_obj == NULL) { |
||||||
|
target->state = RENDEZVOUS_FAILED; |
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
// Get parent body (assumed same for both)
|
||||||
|
CelestialBody* parent = NULL; |
||||||
|
if (chaser->parent_index >= 0 && chaser->parent_index < sim->body_count) { |
||||||
|
parent = &sim->bodies[chaser->parent_index]; |
||||||
|
} else { |
||||||
|
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, sim); |
||||||
|
|
||||||
|
// State machine transitions
|
||||||
|
switch (chaser->rendezvous_target.state) { |
||||||
|
case RENDEZVOUS_PLANNING: |
||||||
|
// Transition to APPROACHING when within approach distance
|
||||||
|
if (distance <= chaser->rendezvous_target.approach_distance && validity.overall_valid) { |
||||||
|
chaser->rendezvous_target.cw_linearization_time = sim->time; |
||||||
|
chaser->rendezvous_target.state = RENDEZVOUS_APPROACHING; |
||||||
|
} |
||||||
|
break; |
||||||
|
|
||||||
|
case RENDEZVOUS_APPROACHING: |
||||||
|
// Transition to MATCHING when relative velocity is low
|
||||||
|
if (rel_vel_mag < chaser->rendezvous_target.max_relative_velocity * 0.5) { |
||||||
|
chaser->rendezvous_target.state = RENDEZVOUS_MATCHING; |
||||||
|
} |
||||||
|
// Check if we've moved away (failed approach)
|
||||||
|
else if (distance > chaser->rendezvous_target.approach_distance * 1.5) { |
||||||
|
chaser->rendezvous_target.state = RENDEZVOUS_FAILED; |
||||||
|
} |
||||||
|
// Update CW linearization time periodically
|
||||||
|
else if (sim->time - chaser->rendezvous_target.cw_linearization_time > 100.0) { |
||||||
|
chaser->rendezvous_target.cw_linearization_time = sim->time; |
||||||
|
} |
||||||
|
break; |
||||||
|
|
||||||
|
case RENDEZVOUS_MATCHING: |
||||||
|
// Transition to COMPLETE when within capture distance
|
||||||
|
if (distance <= chaser->rendezvous_target.capture_distance && rel_vel_mag < chaser->rendezvous_target.max_relative_velocity) { |
||||||
|
chaser->rendezvous_target.state = RENDEZVOUS_COMPLETE; |
||||||
|
} |
||||||
|
// Check if CW validity is lost
|
||||||
|
else if (!validity.overall_valid) { |
||||||
|
chaser->rendezvous_target.state = RENDEZVOUS_FAILED; |
||||||
|
} |
||||||
|
break; |
||||||
|
|
||||||
|
default: |
||||||
|
break; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// ============================================================================
|
||||||
|
// Burn Application Functions
|
||||||
|
// ============================================================================
|
||||||
|
|
||||||
|
void apply_cw_guidance_burn( |
||||||
|
Spacecraft* chaser, |
||||||
|
CWGuidanceSolution* solution, |
||||||
|
SimulationState* sim |
||||||
|
) { |
||||||
|
if (!solution->valid) { |
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
// Get parent body
|
||||||
|
if (chaser->parent_index < 0 || chaser->parent_index >= sim->body_count) { |
||||||
|
return; |
||||||
|
} |
||||||
|
CelestialBody* parent = &sim->bodies[chaser->parent_index]; |
||||||
|
|
||||||
|
// 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); |
||||||
|
} |
||||||
@ -0,0 +1,299 @@ |
|||||||
|
#ifndef RENDEZVOUS_H |
||||||
|
#define RENDEZVOUS_H |
||||||
|
|
||||||
|
#include "simulation.h" |
||||||
|
#include "spacecraft.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
|
||||||
|
typedef struct { |
||||||
|
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
|
||||||
|
} LVLHRelativeState; |
||||||
|
|
||||||
|
// CW validity result
|
||||||
|
typedef struct { |
||||||
|
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
|
||||||
|
} CWValidityResult; |
||||||
|
|
||||||
|
// CW guidance solution
|
||||||
|
typedef struct { |
||||||
|
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)
|
||||||
|
} CWGuidanceSolution; |
||||||
|
|
||||||
|
// ============================================================================
|
||||||
|
// 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) |
||||||
|
* |
||||||
|
* @param position Position vector in inertial frame |
||||||
|
* @param velocity Velocity vector in inertial frame |
||||||
|
* @param parent_mass Mass of central body |
||||||
|
* @param out_r_hat Output: radial unit vector |
||||||
|
* @param out_v_hat Output: along-track unit vector |
||||||
|
* @param out_h_hat Output: cross-track unit vector |
||||||
|
*/ |
||||||
|
void cartesian_to_lvlh_basis( |
||||||
|
Vec3 position, |
||||||
|
Vec3 velocity, |
||||||
|
double parent_mass, |
||||||
|
Vec3* out_r_hat, |
||||||
|
Vec3* out_v_hat, |
||||||
|
Vec3* out_h_hat |
||||||
|
); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Project relative state onto LVLH basis |
||||||
|
* |
||||||
|
* @param rel_pos Relative position (target - chaser) |
||||||
|
* @param r_hat Radial unit vector |
||||||
|
* @param v_hat Along-track unit vector |
||||||
|
* @param h_hat Cross-track unit vector |
||||||
|
* @param rel_vel Relative velocity |
||||||
|
* @param out Relative state in LVLH frame |
||||||
|
*/ |
||||||
|
void project_to_lvlh_frame( |
||||||
|
Vec3 rel_pos, |
||||||
|
Vec3 r_hat, |
||||||
|
Vec3 v_hat, |
||||||
|
Vec3 h_hat, |
||||||
|
Vec3 rel_vel, |
||||||
|
LVLHRelativeState* out |
||||||
|
); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Transform LVLH relative state back to Cartesian |
||||||
|
* |
||||||
|
* @param lvlh Relative state in LVLH frame |
||||||
|
* @param r_hat Radial unit vector |
||||||
|
* @param v_hat Along-track unit vector |
||||||
|
* @param h_hat Cross-track unit vector |
||||||
|
* @param chaser_pos Chaser position (for absolute position calculation) |
||||||
|
* @param out_r_cart Output: relative position in Cartesian |
||||||
|
* @param out_v_cart Output: relative velocity in Cartesian |
||||||
|
*/ |
||||||
|
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 |
||||||
|
); |
||||||
|
|
||||||
|
// ============================================================================
|
||||||
|
// 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) |
||||||
|
* |
||||||
|
* @param chaser Chaser spacecraft |
||||||
|
* @param target Target (body or spacecraft) |
||||||
|
* @param parent Central body |
||||||
|
* @param sim Simulation state |
||||||
|
* @return CWValidityResult with validity flags and error estimates |
||||||
|
*/ |
||||||
|
CWValidityResult check_cw_validity( |
||||||
|
Spacecraft* chaser, |
||||||
|
void* target, // Can be Spacecraft* or CelestialBody*
|
||||||
|
CelestialBody* parent, |
||||||
|
SimulationState* sim |
||||||
|
); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Compute mean motion for given orbital radius |
||||||
|
* |
||||||
|
* @param parent_mass Mass of central body |
||||||
|
* @param orbital_radius Orbital radius |
||||||
|
* @return Mean motion n = sqrt(mu / a^3) |
||||||
|
*/ |
||||||
|
double compute_mean_motion( |
||||||
|
double parent_mass, |
||||||
|
double 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 |
||||||
|
* |
||||||
|
* @param chaser Chaser spacecraft |
||||||
|
* @param target Target |
||||||
|
* @param parent Central body |
||||||
|
* @param time_to_intercept Desired time to intercept |
||||||
|
* @return CWGuidanceSolution with required delta-v |
||||||
|
*/ |
||||||
|
CWGuidanceSolution solve_cw_guidance( |
||||||
|
Spacecraft* chaser, |
||||||
|
void* target, |
||||||
|
CelestialBody* parent, |
||||||
|
double time_to_intercept, |
||||||
|
SimulationState* sim |
||||||
|
); |
||||||
|
|
||||||
|
/**
|
||||||
|
* 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 |
||||||
|
* |
||||||
|
* @param lvlh Relative state in LVLH frame |
||||||
|
* @param mean_motion Mean motion of reference orbit |
||||||
|
* @return Optimal time to intercept |
||||||
|
*/ |
||||||
|
double calculate_optimal_intercept_time( |
||||||
|
LVLHRelativeState* lvlh, |
||||||
|
double mean_motion |
||||||
|
); |
||||||
|
|
||||||
|
// ============================================================================
|
||||||
|
// Rendezvous Target Management
|
||||||
|
// ============================================================================
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Initialize rendezvous target structure |
||||||
|
* |
||||||
|
* @param target Target structure to initialize |
||||||
|
* @param target_index Index of target object |
||||||
|
* @param is_spacecraft_target True if target is spacecraft, false if body |
||||||
|
* @param approach_distance Distance to start approach phase |
||||||
|
* @param capture_distance Distance for capture |
||||||
|
* @param max_relative_velocity Max closing speed for capture |
||||||
|
*/ |
||||||
|
void initialize_rendezvous_target( |
||||||
|
RendezvousTarget* target, |
||||||
|
int target_index, |
||||||
|
bool is_spacecraft_target, |
||||||
|
double approach_distance, |
||||||
|
double capture_distance, |
||||||
|
double max_relative_velocity |
||||||
|
); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Get target object (spacecraft or body) from index |
||||||
|
* |
||||||
|
* @param sim Simulation state |
||||||
|
* @param target_index Index of target |
||||||
|
* @param is_spacecraft_target Output: whether target is spacecraft |
||||||
|
* @return Pointer to target (Spacecraft* or CelestialBody*) |
||||||
|
*/ |
||||||
|
void* get_rendezvous_target( |
||||||
|
SimulationState* sim, |
||||||
|
int target_index, |
||||||
|
bool* is_spacecraft_target |
||||||
|
); |
||||||
|
|
||||||
|
/**
|
||||||
|
* 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 |
||||||
|
* |
||||||
|
* @param chaser Chaser spacecraft |
||||||
|
* @param target Rendezvous target |
||||||
|
* @param sim Simulation state |
||||||
|
*/ |
||||||
|
void update_rendezvous_state( |
||||||
|
Spacecraft* chaser, |
||||||
|
SimulationState* sim |
||||||
|
); |
||||||
|
|
||||||
|
// ============================================================================
|
||||||
|
// Burn Application Functions
|
||||||
|
// ============================================================================
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Apply CW guidance burn to chaser spacecraft |
||||||
|
* |
||||||
|
* @param chaser Chaser spacecraft |
||||||
|
* @param solution CW guidance solution |
||||||
|
* @param sim Simulation state |
||||||
|
*/ |
||||||
|
void apply_cw_guidance_burn( |
||||||
|
Spacecraft* chaser, |
||||||
|
CWGuidanceSolution* solution, |
||||||
|
SimulationState* sim |
||||||
|
); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Calculate relative velocity magnitude between chaser and target |
||||||
|
* |
||||||
|
* @param chaser Chaser spacecraft |
||||||
|
* @param target Target |
||||||
|
* @param parent Central body |
||||||
|
* @return Relative velocity magnitude (m/s) |
||||||
|
*/ |
||||||
|
double calculate_relative_velocity_magnitude( |
||||||
|
Spacecraft* chaser, |
||||||
|
void* target, |
||||||
|
CelestialBody* parent |
||||||
|
); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Calculate distance between chaser and target |
||||||
|
* |
||||||
|
* @param chaser Chaser spacecraft |
||||||
|
* @param target Target |
||||||
|
* @return Distance (m) |
||||||
|
*/ |
||||||
|
double calculate_rendezvous_distance( |
||||||
|
Spacecraft* chaser, |
||||||
|
void* target |
||||||
|
); |
||||||
|
|
||||||
|
#endif // RENDEZVOUS_H
|
||||||
@ -0,0 +1,503 @@ |
|||||||
|
#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/spacecraft.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); |
||||||
|
|
||||||
|
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); |
||||||
|
|
||||||
|
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 1-orbit intercept") { |
||||||
|
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
||||||
|
double intercept_time = orbital_period; // 1 orbit
|
||||||
|
|
||||||
|
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, intercept_time, sim); |
||||||
|
|
||||||
|
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); |
||||||
|
REQUIRE(solution.delta_v_magnitude < 100.0); // Should be small for close orbits
|
||||||
|
} |
||||||
|
|
||||||
|
SECTION("Calculate guidance for half-orbit intercept") { |
||||||
|
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
||||||
|
double intercept_time = orbital_period / 2.0; // Half orbit
|
||||||
|
|
||||||
|
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, intercept_time, sim); |
||||||
|
|
||||||
|
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); |
||||||
|
} |
||||||
|
|
||||||
|
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 around quarter to half orbit
|
||||||
|
double orbital_period = 2.0 * M_PI / mean_motion; |
||||||
|
REQUIRE(optimal_time > orbital_period * 0.2); |
||||||
|
REQUIRE(optimal_time < orbital_period * 0.6); |
||||||
|
} |
||||||
|
|
||||||
|
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 and verify encounter") { |
||||||
|
// 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"); |
||||||
|
|
||||||
|
// Calculate and execute CW guidance burn
|
||||||
|
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
||||||
|
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, orbital_period, sim); |
||||||
|
|
||||||
|
INFO("Calculated delta-v: " << solution.delta_v_magnitude << " m/s"); |
||||||
|
|
||||||
|
apply_cw_guidance_burn(chaser, &solution, sim); |
||||||
|
|
||||||
|
// Propagate for one orbital period
|
||||||
|
double propagation_time = orbital_period; |
||||||
|
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"); |
||||||
|
INFO("Distance reduction: " << (initial_distance - final_distance) << " m"); |
||||||
|
|
||||||
|
// Verify rendezvous success (within 100 m)
|
||||||
|
REQUIRE(final_distance < POSITION_TOLERANCE); |
||||||
|
REQUIRE(final_rel_vel < VELOCITY_TOLERANCE); |
||||||
|
} |
||||||
|
|
||||||
|
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
|
||||||
|
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
||||||
|
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, orbital_period, sim); |
||||||
|
apply_cw_guidance_burn(chaser, &solution, sim); |
||||||
|
|
||||||
|
// Propagate for half an orbit
|
||||||
|
int num_steps = (int)(orbital_period / 2.0 / 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, sim); |
||||||
|
|
||||||
|
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
|
||||||
|
Vec3 r_hat = vec3_normalize(chaser->local_position); |
||||||
|
Vec3 v_hat = vec3_normalize(chaser->local_velocity); |
||||||
|
|
||||||
|
Vec3 desired_pos = vec3_sub(chaser->local_position, vec3_scale(v_hat, 1000.0)); |
||||||
|
chaser->local_position = desired_pos; |
||||||
|
|
||||||
|
// Reconstruct orbital elements
|
||||||
|
chaser->orbit = cartesian_to_orbital_elements(chaser->local_position, |
||||||
|
chaser->local_velocity, |
||||||
|
earth->mass); |
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
// Execute rendezvous
|
||||||
|
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
||||||
|
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, orbital_period, sim); |
||||||
|
apply_cw_guidance_burn(chaser, &solution, sim); |
||||||
|
|
||||||
|
// Propagate
|
||||||
|
int num_steps = (int)(orbital_period / 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); |
||||||
|
REQUIRE(final_distance < POSITION_TOLERANCE); |
||||||
|
} |
||||||
|
|
||||||
|
SECTION("Medium separation (10 km radial)") { |
||||||
|
// Manually adjust chaser to be 10 km above target
|
||||||
|
Vec3 r_hat = vec3_normalize(chaser->local_position); |
||||||
|
|
||||||
|
Vec3 desired_pos = vec3_add(chaser->local_position, vec3_scale(r_hat, 10000.0)); |
||||||
|
chaser->local_position = desired_pos; |
||||||
|
|
||||||
|
chaser->orbit = cartesian_to_orbital_elements(chaser->local_position, |
||||||
|
chaser->local_velocity, |
||||||
|
earth->mass); |
||||||
|
|
||||||
|
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 (should still be valid at 10 km)
|
||||||
|
CWValidityResult validity = check_cw_validity(chaser, target, earth, sim); |
||||||
|
INFO("Spatial fraction: " << validity.spatial_fraction); |
||||||
|
|
||||||
|
REQUIRE(validity.overall_valid == true); |
||||||
|
|
||||||
|
// Execute rendezvous
|
||||||
|
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
||||||
|
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, orbital_period, sim); |
||||||
|
apply_cw_guidance_burn(chaser, &solution, sim); |
||||||
|
|
||||||
|
// Propagate
|
||||||
|
int num_steps = (int)(orbital_period / 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"); |
||||||
|
|
||||||
|
REQUIRE(final_distance < POSITION_TOLERANCE); |
||||||
|
} |
||||||
|
|
||||||
|
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
|
||||||
|
double orbital_period = 2.0 * M_PI * sqrt(pow(6.771e6, 3) / (G * earth->mass)); |
||||||
|
CWGuidanceSolution solution = solve_cw_guidance(chaser, target, earth, orbital_period, sim); |
||||||
|
apply_cw_guidance_burn(chaser, &solution, sim); |
||||||
|
|
||||||
|
// Propagate for 2 orbits with periodic linearization updates
|
||||||
|
int num_steps = (int)(2.0 * orbital_period / TIME_STEP); |
||||||
|
double update_interval = 500.0; // Update every 500 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"); |
||||||
|
|
||||||
|
REQUIRE(final_distance < POSITION_TOLERANCE); |
||||||
|
} |
||||||
|
|
||||||
|
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); |
||||||
|
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); |
||||||
|
} |
||||||
@ -0,0 +1,48 @@ |
|||||||
|
# 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