warehack.ing/pg_orrery
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
pg_orrery/src/coord_funcs.c
Ryan Malloy 5552bf3280 Add convenience functions for Craft integration 
Implements 5 new C functions requested by the Craft (Astrolock) API team:
- tle_from_lines(text, text): two-argument TLE constructor
- observer_from_geodetic(float8, float8, float8): numeric observer constructor
- observe(tle, observer, timestamptz): single-call propagate + topocentric
- sgp4_propagate_safe(tle, timestamptz): returns NULL on propagation error
- observe_safe(tle, observer, timestamptz): returns NULL on propagation error

Refactors do_propagate() into safe/unsafe variants to support NULL returns.
Adds regression test (convenience.sql) covering all new functions including
an equivalence test verifying observe() matches the manual two-step pipeline.
All 6 regression tests pass.
2026-02-15 17:44:41 -07:00

984 lines
28 KiB
C
Raw Blame History

/*
* coord_funcs.c -- Coordinate transform functions for pg_orbit
*
* TEME -> WGS-84 geodetic and TEME -> topocentric transforms.
*
* SGP4 outputs in the TEME (True Equator, Mean Equinox) frame.
* Converting to Earth-fixed coordinates requires rotating by GMST.
* We use only the 4 IAU-80 nutation terms that SGP4 itself uses --
* applying the full 106-term model would "correct" what SGP4
* already accounts for.
*
* The TEME->ITRF rotation is a single Z-axis rotation by -GMST.
* After that, standard geodetic conversions on the WGS-84 ellipsoid.
*/
#include "postgres.h"
#include "fmgr.h"
#include "funcapi.h"
#include "utils/timestamp.h"
#include "libpq/pqformat.h"
#include "norad.h"
#include "types.h"
#include <math.h>
#include <string.h>
#define DEG_TO_RAD (M_PI / 180.0)
#define RAD_TO_DEG (180.0 / M_PI)
PG_FUNCTION_INFO_V1(geodetic_in);
PG_FUNCTION_INFO_V1(geodetic_out);
PG_FUNCTION_INFO_V1(geodetic_recv);
PG_FUNCTION_INFO_V1(geodetic_send);
PG_FUNCTION_INFO_V1(geodetic_lat);
PG_FUNCTION_INFO_V1(geodetic_lon);
PG_FUNCTION_INFO_V1(geodetic_alt);
PG_FUNCTION_INFO_V1(topocentric_in);
PG_FUNCTION_INFO_V1(topocentric_out);
PG_FUNCTION_INFO_V1(topocentric_recv);
PG_FUNCTION_INFO_V1(topocentric_send);
PG_FUNCTION_INFO_V1(topo_azimuth);
PG_FUNCTION_INFO_V1(topo_elevation);
PG_FUNCTION_INFO_V1(topo_range);
PG_FUNCTION_INFO_V1(topo_range_rate);
PG_FUNCTION_INFO_V1(eci_to_geodetic);
PG_FUNCTION_INFO_V1(eci_to_topocentric);
PG_FUNCTION_INFO_V1(subsatellite_point);
PG_FUNCTION_INFO_V1(ground_track);
PG_FUNCTION_INFO_V1(observe);
PG_FUNCTION_INFO_V1(observe_safe);
/* ================================================================
* Internal helpers -- GMST, frame rotation, geodetic conversion
* ================================================================
*/
/*
* Greenwich Mean Sidereal Time from Julian date.
* Vallado, "Fundamentals of Astrodynamics", Eq. 3-47.
* Returns angle in radians.
*/
static double
gmst_from_jd(double jd)
{
double t_ut1 = (jd - 2451545.0) / 36525.0;
double gmst = 67310.54841
+ (876600.0 * 3600.0 + 8640184.812866) * t_ut1
+ 0.093104 * t_ut1 * t_ut1
- 6.2e-6 * t_ut1 * t_ut1 * t_ut1;
/* Convert seconds of time to radians, then normalize to [0, 2*pi) */
gmst = fmod(gmst * M_PI / 43200.0, 2.0 * M_PI);
if (gmst < 0.0)
gmst += 2.0 * M_PI;
return gmst;
}
/*
* Rotate TEME position/velocity to ECEF (Earth-Centered Earth-Fixed).
* Single rotation around Z by -GMST angle.
* omega_e = Earth rotation rate = 7.2921158553e-5 rad/s
*/
#define OMEGA_EARTH 7.2921158553e-5 /* rad/s */
static void
teme_to_ecef(const double *pos_teme, const double *vel_teme,
double gmst, double *pos_ecef, double *vel_ecef)
{
double cos_g = cos(gmst);
double sin_g = sin(gmst);
/* Position rotation */
pos_ecef[0] = cos_g * pos_teme[0] + sin_g * pos_teme[1];
pos_ecef[1] = -sin_g * pos_teme[0] + cos_g * pos_teme[1];
pos_ecef[2] = pos_teme[2];
if (vel_ecef && vel_teme)
{
/* Velocity includes both rotation and Earth angular velocity cross product */
vel_ecef[0] = cos_g * vel_teme[0] + sin_g * vel_teme[1]
+ OMEGA_EARTH * pos_ecef[1];
vel_ecef[1] = -sin_g * vel_teme[0] + cos_g * vel_teme[1]
- OMEGA_EARTH * pos_ecef[0];
vel_ecef[2] = vel_teme[2];
}
}
/*
* ECEF (km) to WGS-84 geodetic (lat, lon in radians; alt in km).
* Bowring's iterative method -- converges in 2-3 iterations for LEO.
*/
static void
ecef_to_geodetic(const double *pos_ecef,
double *lat, double *lon, double *alt_km)
{
double x = pos_ecef[0];
double y = pos_ecef[1];
double z = pos_ecef[2];
double a = WGS84_A;
double e2 = WGS84_E2;
double p = sqrt(x * x + y * y);
double phi, N, phi_prev;
int i;
*lon = atan2(y, x);
/* Bowring's iterative method */
phi = atan2(z, p * (1.0 - e2)); /* initial estimate */
for (i = 0; i < 10; i++)
{
phi_prev = phi;
N = a / sqrt(1.0 - e2 * sin(phi) * sin(phi));
phi = atan2(z + e2 * N * sin(phi), p);
if (fabs(phi - phi_prev) < 1.0e-12)
break;
}
*lat = phi;
N = a / sqrt(1.0 - e2 * sin(phi) * sin(phi));
*alt_km = p / cos(phi) - N;
}
/*
* Observer (WGS-84 lat/lon radians, alt meters) to ECEF vector in km.
*/
static void
observer_to_ecef(const pg_observer *obs, double *pos_ecef)
{
double a = WGS84_A;
double e2 = WGS84_E2;
double sin_lat = sin(obs->lat);
double cos_lat = cos(obs->lat);
double N = a / sqrt(1.0 - e2 * sin_lat * sin_lat);
double alt_km = obs->alt_m / 1000.0;
pos_ecef[0] = (N + alt_km) * cos_lat * cos(obs->lon);
pos_ecef[1] = (N + alt_km) * cos_lat * sin(obs->lon);
pos_ecef[2] = (N * (1.0 - e2) + alt_km) * sin_lat;
}
/*
* ECEF range vector to topocentric (azimuth, elevation, range).
* Azimuth: 0=N, 90=E, 180=S, 270=W.
* Elevation: 0=horizon, +90=zenith.
*/
static void
ecef_to_topocentric(const double *sat_ecef, const double *obs_ecef,
double obs_lat, double obs_lon,
double *az, double *el, double *range_km)
{
/* Range vector in ECEF */
double dx = sat_ecef[0] - obs_ecef[0];
double dy = sat_ecef[1] - obs_ecef[1];
double dz = sat_ecef[2] - obs_ecef[2];
/* Rotate to topocentric (South, East, Up) */
double sin_lat = sin(obs_lat);
double cos_lat = cos(obs_lat);
double sin_lon = sin(obs_lon);
double cos_lon = cos(obs_lon);
double south = sin_lat * cos_lon * dx + sin_lat * sin_lon * dy - cos_lat * dz;
double east = -sin_lon * dx + cos_lon * dy;
double up = cos_lat * cos_lon * dx + cos_lat * sin_lon * dy + sin_lat * dz;
*range_km = sqrt(dx * dx + dy * dy + dz * dz);
*el = asin(up / *range_km);
*az = atan2(east, -south); /* negative south = north */
if (*az < 0.0)
*az += 2.0 * M_PI;
}
/* ================================================================
* TLE struct conversion + propagation (local copies)
*
* These replicate helpers from sgp4_funcs.c. All .c files link into
* a single .so, so we could call those directly, but keeping them
* static avoids symbol coupling between translation units.
* ================================================================
*/
static void
pg_tle_to_sat_code(const pg_tle *src, tle_t *dst)
{
memset(dst, 0, sizeof(tle_t));
dst->epoch = src->epoch;
dst->xincl = src->inclination;
dst->xnodeo = src->raan;
dst->eo = src->eccentricity;
dst->omegao = src->arg_perigee;
dst->xmo = src->mean_anomaly;
dst->xno = src->mean_motion;
dst->xndt2o = src->mean_motion_dot;
dst->xndd6o = src->mean_motion_ddot;
dst->bstar = src->bstar;
dst->norad_number = src->norad_id;
dst->bulletin_number = src->elset_num;
dst->revolution_number = src->rev_num;
dst->classification = src->classification;
dst->ephemeris_type = src->ephemeris_type;
memcpy(dst->intl_desig, src->intl_desig, 9);
}
/*
* do_propagate_internal -- core propagation, returns error code.
* Does NOT call ereport on failure. Caller decides policy.
* Returns 0 on success, negative on error, -999 on bad TLE.
*/
static int
do_propagate_internal(const pg_tle *tle, double jd,
double *pos, double *vel)
{
tle_t sat;
double *params;
int is_deep;
double tsince;
int err;
pg_tle_to_sat_code(tle, &sat);
is_deep = select_ephemeris(&sat);
if (is_deep < 0)
return -999;
params = palloc(sizeof(double) * N_SAT_PARAMS);
if (is_deep)
SDP4_init(params, &sat);
else
SGP4_init(params, &sat);
tsince = (jd - sat.epoch) * 1440.0;
if (is_deep)
err = SDP4(tsince, &sat, params, pos, vel);
else
err = SGP4(tsince, &sat, params, pos, vel);
pfree(params);
return err;
}
/*
* do_propagate -- propagate with error reporting (original behavior).
*/
static int
do_propagate(const pg_tle *tle, double jd, double *pos, double *vel)
{
int err = do_propagate_internal(tle, jd, pos, vel);
if (err == -999)
ereport(ERROR,
(errcode(ERRCODE_DATA_EXCEPTION),
errmsg("invalid TLE for NORAD %d: "
"mean motion or eccentricity out of range",
tle->norad_id)));
if (err != 0 &&
err != SXPX_WARN_ORBIT_WITHIN_EARTH &&
err != SXPX_WARN_PERIGEE_WITHIN_EARTH)
ereport(ERROR,
(errcode(ERRCODE_DATA_EXCEPTION),
errmsg("SGP4/SDP4 propagation failed for NORAD %d "
"at t+%.1f min",
tle->norad_id,
(jd - tle->epoch) * 1440.0)));
return err;
}
/*
* is_propagation_error -- true if the error code is a hard failure
* (not a warning, not success).
*/
static bool
is_propagation_error(int err)
{
if (err == 0)
return false;
if (err == SXPX_WARN_ORBIT_WITHIN_EARTH ||
err == SXPX_WARN_PERIGEE_WITHIN_EARTH)
return false;
return true;
}
/* ================================================================
* Geodetic type I/O
* ================================================================
*/
/*
* geodetic_in -- parse text to pg_geodetic
*
* Accepts: (lat_deg,lon_deg,alt_km)
*/
Datum
geodetic_in(PG_FUNCTION_ARGS)
{
char *str = PG_GETARG_CSTRING(0);
pg_geodetic *result;
double lat_deg, lon_deg, alt_km;
int nfields;
result = (pg_geodetic *) palloc(sizeof(pg_geodetic));
nfields = sscanf(str, " ( %lf , %lf , %lf )",
&lat_deg, &lon_deg, &alt_km);
if (nfields != 3)
ereport(ERROR,
(errcode(ERRCODE_INVALID_TEXT_REPRESENTATION),
errmsg("invalid input syntax for type geodetic: \"%s\"", str),
errhint("Expected (lat_deg,lon_deg,alt_km).")));
if (lat_deg < -90.0 || lat_deg > 90.0)
ereport(ERROR,
(errcode(ERRCODE_NUMERIC_VALUE_OUT_OF_RANGE),
errmsg("latitude out of range: %.6f", lat_deg),
errhint("Latitude must be between -90 and +90 degrees.")));
if (lon_deg < -180.0 || lon_deg > 360.0)
ereport(ERROR,
(errcode(ERRCODE_NUMERIC_VALUE_OUT_OF_RANGE),
errmsg("longitude out of range: %.6f", lon_deg),
errhint("Longitude must be between -180 and +360 degrees.")));
result->lat = lat_deg * DEG_TO_RAD;
result->lon = lon_deg * DEG_TO_RAD;
result->alt = alt_km;
PG_RETURN_POINTER(result);
}
/*
* geodetic_out -- pg_geodetic to text
*/
Datum
geodetic_out(PG_FUNCTION_ARGS)
{
pg_geodetic *geo = (pg_geodetic *) PG_GETARG_POINTER(0);
PG_RETURN_CSTRING(psprintf("(%.6f,%.6f,%.3f)",
geo->lat * RAD_TO_DEG,
geo->lon * RAD_TO_DEG,
geo->alt));
}
/*
* geodetic_recv -- binary input
*/
Datum
geodetic_recv(PG_FUNCTION_ARGS)
{
StringInfo buf = (StringInfo) PG_GETARG_POINTER(0);
pg_geodetic *result;
result = (pg_geodetic *) palloc(sizeof(pg_geodetic));
result->lat = pq_getmsgfloat8(buf);
result->lon = pq_getmsgfloat8(buf);
result->alt = pq_getmsgfloat8(buf);
PG_RETURN_POINTER(result);
}
/*
* geodetic_send -- binary output
*/
Datum
geodetic_send(PG_FUNCTION_ARGS)
{
pg_geodetic *geo = (pg_geodetic *) PG_GETARG_POINTER(0);
StringInfoData buf;
pq_begintypsend(&buf);
pq_sendfloat8(&buf, geo->lat);
pq_sendfloat8(&buf, geo->lon);
pq_sendfloat8(&buf, geo->alt);
PG_RETURN_BYTEA_P(pq_endtypsend(&buf));
}
/* --- geodetic accessor functions --- */
Datum
geodetic_lat(PG_FUNCTION_ARGS)
{
pg_geodetic *geo = (pg_geodetic *) PG_GETARG_POINTER(0);
PG_RETURN_FLOAT8(geo->lat * RAD_TO_DEG);
}
Datum
geodetic_lon(PG_FUNCTION_ARGS)
{
pg_geodetic *geo = (pg_geodetic *) PG_GETARG_POINTER(0);
PG_RETURN_FLOAT8(geo->lon * RAD_TO_DEG);
}
Datum
geodetic_alt(PG_FUNCTION_ARGS)
{
pg_geodetic *geo = (pg_geodetic *) PG_GETARG_POINTER(0);
PG_RETURN_FLOAT8(geo->alt);
}
/* ================================================================
* Topocentric type I/O
* ================================================================
*/
/*
* topocentric_in -- parse text to pg_topocentric
*
* Accepts: (az_deg,el_deg,range_km,range_rate_kms)
*/
Datum
topocentric_in(PG_FUNCTION_ARGS)
{
char *str = PG_GETARG_CSTRING(0);
pg_topocentric *result;
double az_deg, el_deg, range_km, range_rate;
int nfields;
result = (pg_topocentric *) palloc(sizeof(pg_topocentric));
nfields = sscanf(str, " ( %lf , %lf , %lf , %lf )",
&az_deg, &el_deg, &range_km, &range_rate);
if (nfields != 4)
ereport(ERROR,
(errcode(ERRCODE_INVALID_TEXT_REPRESENTATION),
errmsg("invalid input syntax for type topocentric: \"%s\"", str),
errhint("Expected (az_deg,el_deg,range_km,range_rate_kms).")));
if (az_deg < 0.0 || az_deg > 360.0)
ereport(ERROR,
(errcode(ERRCODE_NUMERIC_VALUE_OUT_OF_RANGE),
errmsg("azimuth out of range: %.6f", az_deg),
errhint("Azimuth must be between 0 and 360 degrees.")));
if (el_deg < -90.0 || el_deg > 90.0)
ereport(ERROR,
(errcode(ERRCODE_NUMERIC_VALUE_OUT_OF_RANGE),
errmsg("elevation out of range: %.6f", el_deg),
errhint("Elevation must be between -90 and +90 degrees.")));
result->azimuth = az_deg * DEG_TO_RAD;
result->elevation = el_deg * DEG_TO_RAD;
result->range_km = range_km;
result->range_rate = range_rate;
PG_RETURN_POINTER(result);
}
/*
* topocentric_out -- pg_topocentric to text
*/
Datum
topocentric_out(PG_FUNCTION_ARGS)
{
pg_topocentric *topo = (pg_topocentric *) PG_GETARG_POINTER(0);
PG_RETURN_CSTRING(psprintf("(%.4f,%.4f,%.3f,%.6f)",
topo->azimuth * RAD_TO_DEG,
topo->elevation * RAD_TO_DEG,
topo->range_km,
topo->range_rate));
}
/*
* topocentric_recv -- binary input
*/
Datum
topocentric_recv(PG_FUNCTION_ARGS)
{
StringInfo buf = (StringInfo) PG_GETARG_POINTER(0);
pg_topocentric *result;
result = (pg_topocentric *) palloc(sizeof(pg_topocentric));
result->azimuth = pq_getmsgfloat8(buf);
result->elevation = pq_getmsgfloat8(buf);
result->range_km = pq_getmsgfloat8(buf);
result->range_rate = pq_getmsgfloat8(buf);
PG_RETURN_POINTER(result);
}
/*
* topocentric_send -- binary output
*/
Datum
topocentric_send(PG_FUNCTION_ARGS)
{
pg_topocentric *topo = (pg_topocentric *) PG_GETARG_POINTER(0);
StringInfoData buf;
pq_begintypsend(&buf);
pq_sendfloat8(&buf, topo->azimuth);
pq_sendfloat8(&buf, topo->elevation);
pq_sendfloat8(&buf, topo->range_km);
pq_sendfloat8(&buf, topo->range_rate);
PG_RETURN_BYTEA_P(pq_endtypsend(&buf));
}
/* --- topocentric accessor functions --- */
Datum
topo_azimuth(PG_FUNCTION_ARGS)
{
pg_topocentric *topo = (pg_topocentric *) PG_GETARG_POINTER(0);
PG_RETURN_FLOAT8(topo->azimuth * RAD_TO_DEG);
}
Datum
topo_elevation(PG_FUNCTION_ARGS)
{
pg_topocentric *topo = (pg_topocentric *) PG_GETARG_POINTER(0);
PG_RETURN_FLOAT8(topo->elevation * RAD_TO_DEG);
}
Datum
topo_range(PG_FUNCTION_ARGS)
{
pg_topocentric *topo = (pg_topocentric *) PG_GETARG_POINTER(0);
PG_RETURN_FLOAT8(topo->range_km);
}
Datum
topo_range_rate(PG_FUNCTION_ARGS)
{
pg_topocentric *topo = (pg_topocentric *) PG_GETARG_POINTER(0);
PG_RETURN_FLOAT8(topo->range_rate);
}
/* ================================================================
* Coordinate transform functions
* ================================================================
*/
/*
* eci_to_geodetic(eci_position, timestamptz) -> geodetic
*
* Convert TEME state vector to WGS-84 geodetic coordinates.
* The timestamptz provides the time for GMST calculation.
*/
Datum
eci_to_geodetic(PG_FUNCTION_ARGS)
{
pg_eci *eci = (pg_eci *) PG_GETARG_POINTER(0);
int64 ts = PG_GETARG_INT64(1);
double jd;
double gmst;
double pos_teme[3];
double pos_ecef[3];
double lat, lon, alt_km;
pg_geodetic *result;
jd = timestamptz_to_jd(ts);
gmst = gmst_from_jd(jd);
pos_teme[0] = eci->x;
pos_teme[1] = eci->y;
pos_teme[2] = eci->z;
teme_to_ecef(pos_teme, NULL, gmst, pos_ecef, NULL);
ecef_to_geodetic(pos_ecef, &lat, &lon, &alt_km);
result = (pg_geodetic *) palloc(sizeof(pg_geodetic));
result->lat = lat;
result->lon = lon;
result->alt = alt_km;
PG_RETURN_POINTER(result);
}
/*
* eci_to_topocentric(eci_position, observer, timestamptz) -> topocentric
*
* Convert TEME state vector to observer-relative look angles.
* Range rate is computed by projecting the ECEF-relative velocity
* onto the line-of-sight unit vector.
*/
Datum
eci_to_topocentric(PG_FUNCTION_ARGS)
{
pg_eci *eci = (pg_eci *) PG_GETARG_POINTER(0);
pg_observer *obs = (pg_observer *) PG_GETARG_POINTER(1);
int64 ts = PG_GETARG_INT64(2);
double jd;
double gmst;
double pos_teme[3], vel_teme[3];
double pos_ecef[3], vel_ecef[3];
double obs_ecef[3];
double az, el, range_km;
double range_rate;
double dx, dy, dz;
double dvx, dvy, dvz;
pg_topocentric *result;
jd = timestamptz_to_jd(ts);
gmst = gmst_from_jd(jd);
pos_teme[0] = eci->x;
pos_teme[1] = eci->y;
pos_teme[2] = eci->z;
/* ECI velocity is in km/s; teme_to_ecef expects km/s */
vel_teme[0] = eci->vx;
vel_teme[1] = eci->vy;
vel_teme[2] = eci->vz;
teme_to_ecef(pos_teme, vel_teme, gmst, pos_ecef, vel_ecef);
observer_to_ecef(obs, obs_ecef);
ecef_to_topocentric(pos_ecef, obs_ecef, obs->lat, obs->lon,
&az, &el, &range_km);
/*
* Range rate: project relative velocity onto line-of-sight.
* Positive = satellite receding from observer.
* Observer is stationary in ECEF, so relative velocity = sat ECEF velocity.
*/
dx = pos_ecef[0] - obs_ecef[0];
dy = pos_ecef[1] - obs_ecef[1];
dz = pos_ecef[2] - obs_ecef[2];
dvx = vel_ecef[0];
dvy = vel_ecef[1];
dvz = vel_ecef[2];
range_rate = (dx * dvx + dy * dvy + dz * dvz) / range_km;
result = (pg_topocentric *) palloc(sizeof(pg_topocentric));
result->azimuth = az;
result->elevation = el;
result->range_km = range_km;
result->range_rate = range_rate;
PG_RETURN_POINTER(result);
}
/*
* subsatellite_point(tle, timestamptz) -> geodetic
*
* Propagate TLE to the given time and return the nadir point
* on the WGS-84 ellipsoid. The altitude field contains the
* satellite's altitude above the ellipsoid, not zero.
*/
Datum
subsatellite_point(PG_FUNCTION_ARGS)
{
pg_tle *tle = (pg_tle *) PG_GETARG_POINTER(0);
int64 ts = PG_GETARG_INT64(1);
double jd;
double gmst;
double pos[3], vel[3];
double pos_ecef[3];
double lat, lon, alt_km;
pg_geodetic *result;
jd = timestamptz_to_jd(ts);
do_propagate(tle, jd, pos, vel);
gmst = gmst_from_jd(jd);
teme_to_ecef(pos, NULL, gmst, pos_ecef, NULL);
ecef_to_geodetic(pos_ecef, &lat, &lon, &alt_km);
result = (pg_geodetic *) palloc(sizeof(pg_geodetic));
result->lat = lat;
result->lon = lon;
result->alt = alt_km;
PG_RETURN_POINTER(result);
}
/* ================================================================
* ground_track -- SRF returning subsatellite points over time
*
* ground_track(tle, start_ts, stop_ts, step_interval)
* -> SETOF (t timestamptz, point geodetic)
*
* The model is initialized once and reused for every step,
* same pattern as sgp4_propagate_series in sgp4_funcs.c.
* ================================================================
*/
typedef struct
{
tle_t sat;
double params[N_SAT_PARAMS];
int is_deep;
double epoch_jd;
int64 start_ts;
int64 step_usec;
} ground_track_ctx;
Datum
ground_track(PG_FUNCTION_ARGS)
{
FuncCallContext *funcctx;
ground_track_ctx *ctx;
if (SRF_IS_FIRSTCALL())
{
MemoryContext oldctx;
pg_tle *tle;
int64 start_ts;
int64 stop_ts;
Interval *step;
int64 step_usec;
int64 span;
uint64 nsteps;
TupleDesc tupdesc;
funcctx = SRF_FIRSTCALL_INIT();
oldctx = MemoryContextSwitchTo(funcctx->multi_call_memory_ctx);
tle = (pg_tle *) PG_GETARG_POINTER(0);
start_ts = PG_GETARG_INT64(1);
stop_ts = PG_GETARG_INT64(2);
step = PG_GETARG_INTERVAL_P(3);
/* Convert interval to microseconds */
step_usec = step->time
+ (int64) step->day * USECS_PER_DAY
+ (int64) step->month * (30 * USECS_PER_DAY);
if (step_usec <= 0)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("step interval must be positive")));
if (stop_ts < start_ts)
ereport(ERROR,
(errcode(ERRCODE_INVALID_PARAMETER_VALUE),
errmsg("stop time must be >= start time")));
span = stop_ts - start_ts;
nsteps = (uint64)(span / step_usec) + 1;
ctx = (ground_track_ctx *)
palloc0(sizeof(ground_track_ctx));
pg_tle_to_sat_code(tle, &ctx->sat);
ctx->is_deep = select_ephemeris(&ctx->sat);
if (ctx->is_deep < 0)
ereport(ERROR,
(errcode(ERRCODE_DATA_EXCEPTION),
errmsg("invalid TLE for NORAD %d: "
"mean motion or eccentricity out of range",
ctx->sat.norad_number)));
/* Initialize the model once */
if (ctx->is_deep)
SDP4_init(ctx->params, &ctx->sat);
else
SGP4_init(ctx->params, &ctx->sat);
ctx->epoch_jd = ctx->sat.epoch;
ctx->start_ts = start_ts;
ctx->step_usec = step_usec;
funcctx->max_calls = nsteps;
funcctx->user_fctx = ctx;
/* Output tuple: (timestamptz, lat_deg, lon_deg, alt_km) */
tupdesc = CreateTemplateTupleDesc(4);
TupleDescInitEntry(tupdesc, 1, "t", TIMESTAMPTZOID, -1, 0);
TupleDescInitEntry(tupdesc, 2, "lat_deg", FLOAT8OID, -1, 0);
TupleDescInitEntry(tupdesc, 3, "lon_deg", FLOAT8OID, -1, 0);
TupleDescInitEntry(tupdesc, 4, "alt_km", FLOAT8OID, -1, 0);
funcctx->tuple_desc = BlessTupleDesc(tupdesc);
MemoryContextSwitchTo(oldctx);
}
funcctx = SRF_PERCALL_SETUP();
ctx = (ground_track_ctx *) funcctx->user_fctx;
if (funcctx->call_cntr < funcctx->max_calls)
{
int64 ts;
double jd;
double tsince;
double pos[3], vel[3];
double gmst;
double pos_ecef[3];
double lat, lon, alt_km;
int err;
Datum values[4];
bool nulls[4];
HeapTuple tuple;
ts = ctx->start_ts + (int64) funcctx->call_cntr * ctx->step_usec;
jd = timestamptz_to_jd(ts);
tsince = jd_to_minutes_since_epoch(jd, ctx->epoch_jd);
if (ctx->is_deep)
err = SDP4(tsince, &ctx->sat, ctx->params, pos, vel);
else
err = SGP4(tsince, &ctx->sat, ctx->params, pos, vel);
if (err != 0 &&
err != SXPX_WARN_ORBIT_WITHIN_EARTH &&
err != SXPX_WARN_PERIGEE_WITHIN_EARTH)
ereport(ERROR,
(errcode(ERRCODE_DATA_EXCEPTION),
errmsg("SGP4/SDP4 propagation failed for NORAD %d "
"at t+%.1f min",
ctx->sat.norad_number, tsince)));
gmst = gmst_from_jd(jd);
teme_to_ecef(pos, NULL, gmst, pos_ecef, NULL);
ecef_to_geodetic(pos_ecef, &lat, &lon, &alt_km);
memset(nulls, 0, sizeof(nulls));
values[0] = Int64GetDatum(ts);
values[1] = Float8GetDatum(lat * RAD_TO_DEG);
values[2] = Float8GetDatum(lon * RAD_TO_DEG);
values[3] = Float8GetDatum(alt_km);
tuple = heap_form_tuple(funcctx->tuple_desc, values, nulls);
SRF_RETURN_NEXT(funcctx, HeapTupleGetDatum(tuple));
}
SRF_RETURN_DONE(funcctx);
}
/* ================================================================
* observe(tle, observer, timestamptz) -> topocentric
*
* Single-call propagate + coordinate transform. Avoids the
* intermediate eci_position allocation and three nested function
* calls. Velocity is converted from km/min to km/s internally
* for the range rate calculation.
* ================================================================
*/
Datum
observe(PG_FUNCTION_ARGS)
{
pg_tle *tle = (pg_tle *) PG_GETARG_POINTER(0);
pg_observer *obs = (pg_observer *) PG_GETARG_POINTER(1);
int64 ts = PG_GETARG_INT64(2);
double jd;
double gmst;
double pos[3], vel[3];
double vel_kms[3];
double pos_ecef[3], vel_ecef[3];
double obs_ecef[3];
double az, el, range_km;
double dx, dy, dz, dvx, dvy, dvz;
double range_rate;
pg_topocentric *result;
jd = timestamptz_to_jd(ts);
do_propagate(tle, jd, pos, vel);
/* SGP4 outputs velocity in km/min; convert to km/s */
vel_kms[0] = vel[0] / 60.0;
vel_kms[1] = vel[1] / 60.0;
vel_kms[2] = vel[2] / 60.0;
gmst = gmst_from_jd(jd);
teme_to_ecef(pos, vel_kms, gmst, pos_ecef, vel_ecef);
observer_to_ecef(obs, obs_ecef);
ecef_to_topocentric(pos_ecef, obs_ecef, obs->lat, obs->lon,
&az, &el, &range_km);
/* Range rate: project relative velocity onto line-of-sight */
dx = pos_ecef[0] - obs_ecef[0];
dy = pos_ecef[1] - obs_ecef[1];
dz = pos_ecef[2] - obs_ecef[2];
dvx = vel_ecef[0];
dvy = vel_ecef[1];
dvz = vel_ecef[2];
range_rate = (dx * dvx + dy * dvy + dz * dvz) / range_km;
result = (pg_topocentric *) palloc(sizeof(pg_topocentric));
result->azimuth = az;
result->elevation = el;
result->range_km = range_km;
result->range_rate = range_rate;
PG_RETURN_POINTER(result);
}
/* ================================================================
* observe_safe(tle, observer, timestamptz) -> topocentric or NULL
*
* Same as observe() but returns NULL instead of raising an error
* when propagation fails. Designed for batch queries where one
* bad TLE should not abort the entire result set.
* ================================================================
*/
Datum
observe_safe(PG_FUNCTION_ARGS)
{
pg_tle *tle = (pg_tle *) PG_GETARG_POINTER(0);
pg_observer *obs = (pg_observer *) PG_GETARG_POINTER(1);
int64 ts = PG_GETARG_INT64(2);
double jd;
double gmst;
double pos[3], vel[3];
double vel_kms[3];
double pos_ecef[3], vel_ecef[3];
double obs_ecef[3];
double az, el, range_km;
double dx, dy, dz, dvx, dvy, dvz;
double range_rate;
int err;
pg_topocentric *result;
jd = timestamptz_to_jd(ts);
err = do_propagate_internal(tle, jd, pos, vel);
if (err == -999 || is_propagation_error(err))
PG_RETURN_NULL();
/* SGP4 outputs velocity in km/min; convert to km/s */
vel_kms[0] = vel[0] / 60.0;
vel_kms[1] = vel[1] / 60.0;
vel_kms[2] = vel[2] / 60.0;
gmst = gmst_from_jd(jd);
teme_to_ecef(pos, vel_kms, gmst, pos_ecef, vel_ecef);
observer_to_ecef(obs, obs_ecef);
ecef_to_topocentric(pos_ecef, obs_ecef, obs->lat, obs->lon,
&az, &el, &range_km);
dx = pos_ecef[0] - obs_ecef[0];
dy = pos_ecef[1] - obs_ecef[1];
dz = pos_ecef[2] - obs_ecef[2];
dvx = vel_ecef[0];
dvy = vel_ecef[1];
dvz = vel_ecef[2];
range_rate = (dx * dvx + dy * dvy + dz * dvz) / range_km;
result = (pg_topocentric *) palloc(sizeof(pg_topocentric));
result->azimuth = az;
result->elevation = el;
result->range_km = range_km;
result->range_rate = range_rate;
PG_RETURN_POINTER(result);
}
Reference in New Issue View Git Blame Copy Permalink