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