| 1 |
#define ASDSERIES "sdo.lev0_asd_0004" |
| 2 |
|
| 3 |
#define PT_OUT_OF_MEMORY (-1) |
| 4 |
#define PT_DRMS_OPEN_FAILED (-2) |
| 5 |
#define PT_NO_VALID_TOBS (-3) |
| 6 |
|
| 7 |
typedef struct { |
| 8 |
float sat_y0; |
| 9 |
float sat_z0; |
| 10 |
float sat_rot; |
| 11 |
char acs_mode[16]; |
| 12 |
char acs_eclp[16]; |
| 13 |
char acs_sunp[16]; |
| 14 |
char acs_safe[16]; |
| 15 |
char acs_cgt[16]; |
| 16 |
char asd_rec[64]; |
| 17 |
} PTINFO; |
| 18 |
|
| 19 |
static int find_closest(TIME *t, int n, TIME tt) |
| 20 |
{ |
| 21 |
TIME d, dmin = DBL_MAX; |
| 22 |
int i, idx = -1; |
| 23 |
|
| 24 |
for (i=0; i<n; ++i) { |
| 25 |
d = fabs(tt - t[i]); |
| 26 |
if (d < dmin) { |
| 27 |
dmin = d; idx = i; |
| 28 |
} |
| 29 |
} |
| 30 |
|
| 31 |
return idx; |
| 32 |
} |
| 33 |
|
| 34 |
// for a fixed vector x0 in some reference frame |
| 35 |
// find its components x in another reference |
| 36 |
// frame rotated from the first one by q |
| 37 |
// |
| 38 |
// x = inv(q) x0 q |
| 39 |
// |
| 40 |
static void qrot(const double *q, const double *x0, double *x) |
| 41 |
{ |
| 42 |
double q11,q12,q13,q21,q22,q23,q31,q32,q33; |
| 43 |
q11 = 2 * (q[0]*q[0] + q[1]*q[1]) - 1.0; |
| 44 |
q12 = 2 * (q[1]*q[2] + q[0]*q[3]); |
| 45 |
q13 = 2 * (q[1]*q[3] - q[0]*q[2]); |
| 46 |
q21 = 2 * (q[1]*q[2] - q[0]*q[3]); |
| 47 |
q22 = 2 * (q[0]*q[0] + q[2]*q[2]) - 1.0; |
| 48 |
q23 = 2 * (q[2]*q[3] + q[0]*q[1]); |
| 49 |
q31 = 2 * (q[1]*q[3] + q[0]*q[2]); |
| 50 |
q32 = 2 * (q[2]*q[3] - q[0]*q[1]); |
| 51 |
q33 = 2 * (q[0]*q[0] + q[3]*q[3]) - 1.0; |
| 52 |
x[0] = q11*x0[0] + q12*x0[1] + q13*x0[2]; |
| 53 |
x[1] = q21*x0[0] + q22*x0[1] + q23*x0[2]; |
| 54 |
x[2] = q31*x0[0] + q32*x0[1] + q33*x0[2]; |
| 55 |
} |
| 56 |
|
| 57 |
// quaternion product |
| 58 |
// |
| 59 |
// r = p q |
| 60 |
// |
| 61 |
static void qmul(const double *p, const double *q, double *r) |
| 62 |
{ |
| 63 |
r[0] = p[0]*q[0] - p[1]*q[1] - p[2]*q[2] - p[3]*q[3]; |
| 64 |
r[1] = p[0]*q[1] + p[1]*q[0] + p[2]*q[3] - p[3]*q[2]; |
| 65 |
r[2] = p[0]*q[2] + p[2]*q[0] + p[3]*q[1] - p[1]*q[3]; |
| 66 |
r[3] = p[0]*q[3] + p[3]*q[0] + p[1]*q[2] - p[2]*q[1]; |
| 67 |
} |
| 68 |
|
| 69 |
// quaternion inverse |
| 70 |
static void qinv(double *q) |
| 71 |
{ |
| 72 |
q[1] = -q[1]; q[2] = -q[2]; q[3] = -q[3]; |
| 73 |
} |
| 74 |
|
| 75 |
// quaternion norm |
| 76 |
static double qnorm(double *q) |
| 77 |
{ |
| 78 |
return sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]); |
| 79 |
} |
| 80 |
|
| 81 |
int get_pointing_info(DRMS_Env_t *drms_env, TIME *tobs, int nobs, PTINFO **ptinfo) |
| 82 |
{ |
| 83 |
PTINFO *p; |
| 84 |
DRMS_RecordSet_t *rset; |
| 85 |
TIME tstart, tend, t, *tasd; |
| 86 |
int *asdidx; |
| 87 |
char dsname[256]; |
| 88 |
int status, nrec, i, j, k, l; |
| 89 |
|
| 90 |
asdidx = (int *) malloc(nobs*sizeof(int)); |
| 91 |
p = *ptinfo = (PTINFO *) malloc(nobs*sizeof(PTINFO)); |
| 92 |
if (!p) return PT_OUT_OF_MEMORY; |
| 93 |
|
| 94 |
for (i=0; i<nobs; ++i) { |
| 95 |
asdidx[i] = -1; |
| 96 |
p[i].sat_y0 = p[i].sat_z0 = p[i].sat_rot = DRMS_MISSING_FLOAT; |
| 97 |
p[i].acs_eclp[0] = p[i].acs_sunp[0] = p[i].acs_safe[0] = 0; |
| 98 |
p[i].acs_mode[0] = p[i].acs_cgt[0] = p[i].asd_rec[0] = 0; |
| 99 |
} |
| 100 |
|
| 101 |
for (k=0; k<nobs; ++k) { // find first good t_obs |
| 102 |
if (tobs[k] > 0) break; |
| 103 |
} |
| 104 |
if (k == nobs) { // no good t_obs found |
| 105 |
free(asdidx); |
| 106 |
free(p); |
| 107 |
*ptinfo = 0; |
| 108 |
return PT_NO_VALID_TOBS; |
| 109 |
} |
| 110 |
|
| 111 |
// do in chunks spanning no more than 600s in T_OBS |
| 112 |
// to reduce the number of records returned from db |
| 113 |
do { |
| 114 |
for (i=k+1; i<nobs; ++i) |
| 115 |
if ((tobs[i]-tobs[k]) > 600.0) |
| 116 |
break; |
| 117 |
l = i-1; |
| 118 |
tstart = tobs[k] - 30.0; |
| 119 |
tend = tobs[l] + 30.0; |
| 120 |
sprintf(dsname, "%s[? PACKET_TIME > %.0f and PACKET_TIME < %.0f ?]", ASDSERIES, tstart, tend); |
| 121 |
rset = drms_open_records(drms_env, dsname, &status); |
| 122 |
if (!rset || !rset->n || status) { |
| 123 |
free(asdidx); |
| 124 |
free(p); |
| 125 |
*ptinfo = 0; |
| 126 |
if (rset) drms_close_records(rset, DRMS_FREE_RECORD); |
| 127 |
return PT_DRMS_OPEN_FAILED; |
| 128 |
} |
| 129 |
nrec = rset->n; |
| 130 |
tasd = (TIME *) malloc(nrec*sizeof(TIME)); |
| 131 |
if (!tasd) return PT_OUT_OF_MEMORY; |
| 132 |
for (i=0; i<nrec; ++i) |
| 133 |
tasd[i] = drms_getkey_time(rset->records[i], "PACKET_TIME", &status); |
| 134 |
|
| 135 |
// find which ASD packet to use based on closest match between tobs and tasd |
| 136 |
for (i=k; i<=l; ++i) { |
| 137 |
int done = 0; |
| 138 |
int idx; |
| 139 |
char *str; |
| 140 |
double qbcs[4], qsn[4], qq[4]; |
| 141 |
double x[3], x0[3] = {1.0,0.0,0.0}; |
| 142 |
double z[3], z0[3] = {0.0,0.0,1.0}; |
| 143 |
double tmp; |
| 144 |
|
| 145 |
if (drms_ismissing_time(tobs[i])) continue; |
| 146 |
asdidx[i] = idx = find_closest(tasd, nrec, tobs[i]); |
| 147 |
if (idx < 0) continue; // no match for whatever reason; give up |
| 148 |
|
| 149 |
for (j=i-1; j>=k; --j) |
| 150 |
if (asdidx[i] == asdidx[j]) { // have used this ASD packet before |
| 151 |
memcpy(&p[i], &p[j], sizeof(PTINFO)); |
| 152 |
done = 1; |
| 153 |
break; |
| 154 |
} |
| 155 |
if (!done) { // a new ASD packet has been picked |
| 156 |
str = drms_getkey_string(rset->records[idx], "ACS_AN_FLAG_CSS_ECLIPSE", &status); |
| 157 |
if (str) { |
| 158 |
strncpy(p[i].acs_eclp, str, 16); |
| 159 |
free(str); |
| 160 |
} |
| 161 |
str = drms_getkey_string(rset->records[idx], "ACS_AN_FLAG_ACE_INSAFEHOLD", &status); |
| 162 |
if (str) { |
| 163 |
strncpy(p[i].acs_safe, str, 16); |
| 164 |
free(str); |
| 165 |
} |
| 166 |
str = drms_getkey_string(rset->records[idx], "ACS_AN_FLAG_DSS_SUNPRES", &status); |
| 167 |
if (str) { |
| 168 |
strncpy(p[i].acs_sunp, str, 16); |
| 169 |
free(str); |
| 170 |
} |
| 171 |
str = drms_getkey_string(rset->records[idx], "ACS_AN_NUM_CGT", &status); |
| 172 |
if (str) { |
| 173 |
strncpy(p[i].acs_cgt, str, 16); |
| 174 |
free(str); |
| 175 |
} |
| 176 |
str = drms_getkey_string(rset->records[idx], "ACS_AN_ACS_MODE", &status); |
| 177 |
if (str) { |
| 178 |
strncpy(p[i].acs_mode, str, 16); |
| 179 |
free(str); |
| 180 |
} |
| 181 |
snprintf(p[i].asd_rec, 64, "%s[:#%lld]", ASDSERIES, rset->records[idx]->recnum); |
| 182 |
|
| 183 |
status = 0; |
| 184 |
qbcs[0] = drms_getkey_float(rset->records[idx], "ACS_AN_QUAT_GCIFTOBCSF_EST_S", &status); |
| 185 |
qbcs[1] = drms_getkey_float(rset->records[idx], "ACS_AN_QUAT_GCIFTOBCSF_EST_X", &status); |
| 186 |
qbcs[2] = drms_getkey_float(rset->records[idx], "ACS_AN_QUAT_GCIFTOBCSF_EST_Y", &status); |
| 187 |
qbcs[3] = drms_getkey_float(rset->records[idx], "ACS_AN_QUAT_GCIFTOBCSF_EST_Z", &status); |
| 188 |
qsn[0] = drms_getkey_float(rset->records[idx], "ACS_AN_QUAT_GCIFTOSNRF_S", &status); |
| 189 |
qsn[1] = drms_getkey_float(rset->records[idx], "ACS_AN_QUAT_GCIFTOSNRF_X", &status); |
| 190 |
qsn[2] = drms_getkey_float(rset->records[idx], "ACS_AN_QUAT_GCIFTOSNRF_Y", &status); |
| 191 |
qsn[3] = drms_getkey_float(rset->records[idx], "ACS_AN_QUAT_GCIFTOSNRF_Z", &status); |
| 192 |
if (status) continue; |
| 193 |
|
| 194 |
tmp = qnorm(qbcs); |
| 195 |
if (tmp > 1.001 || tmp < 0.999) continue; |
| 196 |
tmp = qnorm(qsn); |
| 197 |
if (tmp > 1.001 || tmp < 0.999) continue; |
| 198 |
|
| 199 |
qinv(qsn); |
| 200 |
qmul(qsn, qbcs, qq); |
| 201 |
|
| 202 |
// x0: sun-pointing vector in SNR frame |
| 203 |
// x: sun-pointing vector in BCS frame |
| 204 |
qrot(qq, x0, x); |
| 205 |
|
| 206 |
// z0: solar north in SNR frame |
| 207 |
// z: solar north in BCS frame |
| 208 |
qrot(qq,z0,z); |
| 209 |
|
| 210 |
p[i].sat_rot = -atan2(z[1],z[2])*180.0/M_PI; |
| 211 |
p[i].sat_y0 = -asin(x[1])*3600.0*180.0/M_PI; |
| 212 |
p[i].sat_z0 = asin(x[2])*3600.0*180.0/M_PI; |
| 213 |
} |
| 214 |
} |
| 215 |
drms_close_records(rset, DRMS_FREE_RECORD); |
| 216 |
free(tasd); |
| 217 |
k = l+1; |
| 218 |
} while (k < nobs); |
| 219 |
|
| 220 |
free(asdidx); |
| 221 |
return 0; |
| 222 |
} |
