/* internal ana subroutines and functions, this is file fun7.c, it contains functions for testing Solar B flight s/w. There is an associated fun7.h with lookup tables used. */ #include "defs.h" #include #include #include #include #include #include "ana_structures.h" #include #include #include #include #include #include #include "fun7.h" #define ABS(x) ((x)>=0?(x):-(x)) #define MIN(a,b) (((a)<(b))?(a):(b)) #define MAX(a,b) (((a)>(b))?(a):(b)) #if __APPLE__ /* the G5's have 64 bit file stuff by default */ #define off64_t off_t #define stat64 stat #define lstat64 lstat #define lseek64 lseek #endif #if LINUX /* just use standards for Linux */ #define off64_t off_t #define stat64 stat #define lstat64 lstat #define lseek64 lseek #endif extern int scrat[NSCRAT]; extern struct sym_desc sym[]; extern struct sym_list *subr_sym_list[]; extern int temp_base; extern int ana_float(); extern char *find_sym_name(int iq); extern int ana_type_size[]; extern int vfix_top, num_ana_subr, next_user_subr_num; extern int lastmin_sym, lastmax_sym; extern int num_ana_func, next_user_func_num; extern float float_arg(); extern double double_arg(); static int result_sym; extern int get_p_c(), get_p_c_r(); extern char *strsave(); union types_ptr { byte *b; short *w; int *l; float *f; double *d;}; int dim[8]; struct ahead *h; /* 3/8/2002 - set to high values for testing */ INT32 Minimum_sum = 0x0fffffff; INT32 MAD_threshold = 0x0fffffff; INT32 signx = 0, signy = 0; INT32 switchxy = 0; UINT32 N_CTME_Error_Max = 2; UINT32 N_Hard_Limit_Max = 5; UINT32 N_Out_of_Range_Max = 20; int DN_per_pixel = 440; int DN_saturate = 880; UINT32 DN_sat2 = 774400; // DN_saturate^2 int DN_ring = 293; // = DN_saturate / 3 UINT32 Bigword1 = 4096; UINT32 Bigword2 = 1024; UINT32 N_CTME_Error = 0; UINT32 N_Hard_Limit = 0; UINT32 N_out_of_range = 0; UINT32 N_Bad_CTStatus = 0; UINT32 N_MAD_threshold = 0; UINT32 N_Minimum_sum = 0; UINT16 CTImageHdr[32]; UINT16 CTImage[50][50]; UINT32 DDpointer = 0; UINT32 DDmax = 60900 - 7; UINT16 DD_Hdr[32]; UINT16 Diag_Data[60900]; UINT32 jittermode = 1; UINT16 jitterdata[7]; UINT16 FPP_CTM_Hard_Limit_Count = 0; UINT16 FPP_CT_OvrFlow_Count = 0; UINT16 FPP_CT_Range_Err_Count = 0; UINT16 FPP_CT_Saturation_Err_Count = 0; UINT32 ring_index[13]; int hiTableCnt = 2648, loTableCnt = 2800; int hiTableOffset = 1448, loTableOffset = 1012; int highTlimit = 40000, lowTlimit = 0, forceHighRange = 1; /* table used for a crude atan function */ int tableoftans[43] = {0,2,7,12,17,22,27,32,37,42,47,53,59,65,71,77, 84,91,98,106,114,123,133,143,154,167,180,195,212,232,254,279, 309,345,389,444,516,613,753,972,1365,2279,6844}; /* the trig masks, these let us do Fourier components for a single frequency; used for finding peaks */ int icostab42[42] = {4096,4050,3914,3690,3384,3002,2553,2047,1496,911, 306,-306,-911, -1496,-2048,-2553,-3002,-3384,-3690,-3914,-4050,-4096, -4050,-3914, -3690,-3384,-3002,-2553,-2047,-1496,-911,-306,306,911,1496, 2048,2553, 3002,3384,3690,3914,4050}; int isintab42[42] = {0,610,1207,1777,2307,2785,3202,3547,3812,3993, 4084,4084,3993,3812,3547,3202,2785,2307,1777,1207,610,0,-610,-1207,-1777, -2307,-2785,-3202,-3547,-3812,-3993,-4084,-4084,-3993,-3812,-3547,-3202, -2785,-2307,-1777,-1207,-610}; int icostab14[14] = {4096,3690,2553,911,-911,-2553,-3690,-4096,-3690, -2553,-911,911,2553,3690}; int isintab14[14] = {0,1777,3202,3993,3993,3202,1777,0,-1777,-3202, -3993,-3993,-3202,-1777}; int icostab7[7] = {4096,2553,-911,-3690,-3690,-911,2553}; int isintab7[7] = {0,3202,3993,1777,-1777,-3993,-3202}; int icostab6[6] = {4096,2047,-2048,-4096,-2047,2048}; int isintab6[6] = {0,3547,3547,0,-3547,-3547}; /*-------------------------------------------------------------------------*/ int ana_thermAvg(narg,ps) int narg, ps[]; { /* test thermistor averaging code, input is a set of thermistor readouts in a 6 * nt array, outputs are a time stream of averaged thermistor values and the buffer, may add more for heater ops if we have any problems there */ #define MAX_TSAMP_AVG 64 struct ahead *h; int nt, ntout, ntherm, nd, dim[8], i, iq, jq, class; short *ptherm, *avgBuffer, *thermResults; int navg = 32, navg2p = 5, navgd2 = 16, tindex; int inext[4] = {0,0,0,0}; /* for the individual structure heaters */ int inextCCD = 0; /* just one for both CCD's */ int fillCnt[4] = {0,0,0,0}; /* for the structure heaters */ int fillCntCCD = 0; /* for both CCD's */ int thermAverages[6]; /* for our thermal averages */ unsigned int TempRaw; unsigned short HiChanLmt = 0x6d4; float accTime; iq = ps[0]; /* cast it to a I*2 array */ iq = ana_word(1,&iq); class = sym[iq].class; if (class != ARRAY) return execute_error(66); h = (struct ahead *) sym[iq].spec.array.ptr; nd = h->ndim; nt = h->dims[1]; ntherm = h->dims[0]; if (nd != 2 || ntherm != 6) { fprintf(stderr,"thermAvg: illegal input, nd = %d, ntherm = %d, must be (2,6)\n", nd, ntherm); return -1; } ptherm = (short *) ((char *)h + sizeof(struct ahead)); /* define the buffer array, this a fixed size */ iq = ps[1]; jq = ps[2]; nd = 2; dim[0] = MAX_TSAMP_AVG; dim[1] = 6; if (redef_array(iq, 1, nd, dim) != 1) return -1; h = (struct ahead *) sym[iq].spec.array.ptr; avgBuffer = (short *) ((char *)h + sizeof(struct ahead)); /* assume a measurement taken every 50*6*4/580 seconds, we produce a result from the current average every 10s so the number of results is: */ ntout = nt * 120/580; fprintf(stderr,"ntout = %d\n", ntout); nd = 2; dim[0] = 6; dim[1] = ntout; if (redef_array(jq, 1, nd, dim) != 1) return -1; h = (struct ahead *) sym[jq].spec.array.ptr; thermResults = (short *) ((char *)h + sizeof(struct ahead)); bzero( (char *) avgBuffer, MAX_TSAMP_AVG * 6 *sizeof(short)); bzero( (char *) thermAverages, 6 *sizeof(int)); bzero( (char *) thermResults, 6 * ntout * sizeof(short)); accTime = 0.0; while (nt--) { unsigned short LowRangeFlag; accTime += 1200./580.; tindex = (int) (accTime/10.); if (tindex >= ntout) break; for( i = 0; i < 4; i++ ) { int iq; /* the 4 structure heaters and thermistors, we now always read these thermistors here even if we are not controlling the heaters. The values are still inserted into the telemetry fields in alogMon.cpp however. The averaging technique depends on the summation always being the sum of the navg entries in the averaging buffer for each thermistor. */ //if ( (nrpts % StrtReadInterval) == 0) { /* time to read a temperature */ LowRangeFlag = 0x0000; //TempChan = HtrStVec->TempChan; /* note that getAnalog now does double/triple read as needed */ //TempRaw = ((int) mechif.getAnalog( TempChan )) & 0xfff; TempRaw = *ptherm++; /* check if in hi range (usually is) and read low range if not */ //if( TempRaw > HtrStVec->HiChanLmt ) if( TempRaw > HiChanLmt ) { /* for low range temperatures, we disable averaging and just use the last value. We also 0 the entry count so when we do get back into high range, we have to fill the buffer again. */ LowRangeFlag = 0x8000; //TempChan -= 0x10; //TempRaw = (int) mechif.getAnalog( TempChan ); /* we don't have a second one to read in the simulation, so use a special value of 0xf */ TempRaw = 0xf; inext[i] = fillCnt[i] = 0; /* don't need to zero the buffer, as fillCnt goes up it will purge itself of the old values */ } else { /* in high range, we do an average after we have enough entries, until then we just use the last value */ fillCnt[i]++; iq = i*MAX_TSAMP_AVG + inext[i]; thermAverages[i] += (TempRaw - (int) avgBuffer[iq]); avgBuffer[iq] = TempRaw; inext[i] = (inext[i] + 1) % navg; if (fillCnt[i] >= navg) { fillCnt[i] = navg; TempRaw = (thermAverages[i] + navgd2) >> navg2p; } } //HtrStVec->TempVal = TempRaw | LowRangeFlag; thermResults[i + tindex*6] = TempRaw | LowRangeFlag; /* note that HtrStVec->TempVal is used in htrOp method and in alogMon */ } /* get the CCD thermistor readings, because there are 2 heaters for each CCD, we load each HtrStVec with the single reading. Also note that CCD thermistors do not have a low/high channel. */ //if ( (nrpts % CCDtReadInterval) == 0) { { int iq; //HtrStVec = CCD_Heater[0].getState(); //TempChan = HtrStVec->TempChan; //TempRaw = ((int) mechif.getAnalog( TempChan )) & 0xfff; TempRaw = *ptherm++; /* the CCD's don't have thge low/high range problem so we can use the same fill counter for both and keep them synched, we should always have a full average buffer except after a re-boot */ fillCntCCD++; iq = 4*MAX_TSAMP_AVG + inextCCD; thermAverages[4] += (TempRaw - (int) avgBuffer[iq]); avgBuffer[iq] = TempRaw; if (fillCntCCD >= navg) { fillCntCCD = navg; TempRaw = (thermAverages[4] + navgd2) >> navg2p; } //HtrStVec->TempVal = TempRaw; thermResults[4 + tindex*6] = TempRaw; /* now use same value for other htr for this CCD */ //HtrStVec = CCD_Heater[1].getState(); //HtrStVec->TempVal = TempRaw; /* and the second CCD, also 2 heater structures to set */ //HtrStVec = CCD_Heater[2].getState(); //TempChan = HtrStVec->TempChan; //TempRaw = ((int) mechif.getAnalog( TempChan )) & 0xfff; TempRaw = *ptherm++; iq = 5*MAX_TSAMP_AVG + inextCCD; thermAverages[5] += (TempRaw - (int) avgBuffer[iq]); avgBuffer[iq] = TempRaw; inextCCD = (inextCCD + 1) % navg; if (fillCntCCD >= navg) { TempRaw = (thermAverages[5] + navgd2) >> navg2p; } //HtrStVec->TempVal = TempRaw; thermResults[5 + tindex*6] = TempRaw; /* now use same value for other htr for this CCD */ //HtrStVec = CCD_Heater[3].getState(); //HtrStVec->TempVal = TempRaw; } } return 1; } /*-------------------------------------------------------------------------*/ int ana_cterror_set(narg,ps) int narg, ps[]; { if (int_arg_stat(ps[0], &Minimum_sum) != 1) return -1; if (int_arg_stat(ps[1], &MAD_threshold) != 1) return -1; return 1; } /*-------------------------------------------------------------------------*/ int ana_cterror_wtset(narg,ps) int narg, ps[]; { int iq; if (int_arg_stat(ps[0], &iq) != 1) return -1; if (iq == 0) fit_matrix = fit_matrix_0; if (iq == 1) fit_matrix = fit_matrix_1; if (iq == 2) fit_matrix = fit_matrix_2; if (iq == 3) fit_matrix = fit_matrix_3; if (iq == 4) fit_matrix = fit_matrix_4; if (iq == 5) fit_matrix = fit_matrix_5; if (iq == 6) fit_matrix = fit_matrix_6; if (iq == 7) fit_matrix = fit_matrix_7; if (iq == 8) fit_matrix = fit_matrix_8; return 1; } /*-------------------------------------------------------------------------*/ int ana_cterror(narg,ps) int narg, ps[]; { /* a C port of Tarbell's CTerror code, taken from D. Mathur's port with an ANA boilerplate added */ /* returns the CTME set of 3 I*2's, the first is the command and the next are the x and y offsets */ #define CT_VALID 1 extern double systime(); short int *ctme_out; int minsum_index, minsum; int maxsum; int data_val; int ct_scaled, N_saturate, ct_passed6, normal_ct; int i, j, k, ctme_dim = 3; int a[5]; int det; int rad2; int imin; int root; int Linecount; int count = 10000, ncount, n, iq, class; double t1, t2; /* here we have some fake CTdata and CTMEstatus defined locally, would normally be passed as arguments */ unsigned short int CTdata[70]; short int CTMEstatus[3]; register int s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13, smin, smax; unsigned short *ctptr = CTdata + 1; /* note start at index 1 */ unsigned short *pin; /*12/14/2001 - more tests of the algorithm, let CTdata be passed here now as a short array */ iq = ps[0]; // CTdata /* cast it to a I*2 array */ iq = ana_word(1,&iq); n = get_p_c(&iq, &pin); class = sym[iq].class; if (class != ARRAY) return execute_error(66); /* change to only require 14, the first is still the MAD and not used here */ if (n < 14) { printf("CTerror - difference array too small, n = %d\n", n); return -1; } for( i = 0; i < 14; i++ ) CTdata[i] = *pin++; CTMEstatus[0] = 12; CTMEstatus[1] = 20; CTMEstatus[2] = 30; result_sym = array_scratch(1, 1, &ctme_dim ); h = (struct ahead *) sym[result_sym].spec.array.ptr; ctme_out = (short *) ((char *)h + sizeof(struct ahead)); //if (narg > 0) if (int_arg_stat(ps[0], &count) != 1) return -1; //ncount = count; //t1 = systime(); //while (count--) { /* Check status of CTME, take action after error computation if necessary */ if (CTMEstatus[0] & 0x08) N_CTME_Error += 1; else N_CTME_Error = 0; if (CTMEstatus[0] & 0x700) { N_Hard_Limit += 1; FPP_CTM_Hard_Limit_Count += 1; } else N_Hard_Limit = 0; // Start error computation ctme_out[0] = ctme_out[1] = ctme_out[2] = 0; /* need index of minimum of CTdata(1:13) and also an array of the values squared, the maximum is also needed to check if right shifts are necessary */ /* each of the 13 results goes into a 32 bit register */ s1 = (int) ctptr[0]; s1 = smin = smax = s1*s1; minsum_index =0; s2 = (int) ctptr[1]; s2 = s2*s2; s3 = (int) ctptr[2]; s3 = s3*s3; if (s2 > smax) smax = s2; if (s2 < smin) { smin = s2; minsum_index =1; } s4 = (int) ctptr[3]; s4 = s4*s4; if (s3 > smax) smax = s3; if (s3 < smin) { smin = s3; minsum_index =2; } s5 = (int) ctptr[4]; s5 = s5*s5; if (s4 > smax) smax = s4; if (s4 < smin) { smin = s4; minsum_index =3; } s6 = (int) ctptr[5]; s6 = s6*s6; if (s5 > smax) smax = s5; if (s5 < smin) { smin = s5; minsum_index =4; } s7 = (int) ctptr[6]; s7 = s7*s7; if (s6 > smax) smax = s6; if (s6 < smin) { smin = s6; minsum_index =5; } s8 = (int) ctptr[7]; s8 = s8*s8; if (s7 > smax) smax = s7; if (s7 < smin) { smin = s7; minsum_index =6; } s9 = (int) ctptr[8]; s9 = s9*s9; if (s8 > smax) smax = s8; if (s8 < smin) { smin = s8; minsum_index =7; } s10 = (int) ctptr[9]; s10 = s10*s10; if (s9 > smax) smax = s9; if (s9 < smin) { smin = s9; minsum_index =8; } s11 = (int) ctptr[10]; s11 = s11*s11; if (s10 > smax) smax = s10; if (s10 < smin) { smin = s10; minsum_index =9; } s12 = (int) ctptr[11]; s12 = s12*s12; if (s11 > smax) smax = s11; if (s11 < smin) { smin = s11; minsum_index =10; } s13 = (int) ctptr[12]; s13 = s13*s13; if (s12 > smax) smax = s12; if (s12 < smin) { smin = s12; minsum_index =11; } if (s13 > smax) smax = s13; if (s13 < smin) { smin = s13; minsum_index =12; } /* Check CT status word for overflows or other errors; (CT status word is not defined yet - this is a place holder for a proper test) */ /* disabled here */ // if( CTdata[15] == 999 ) // { /* Set error signal to "out of range" */ // ctme_out[0] = CT_OUT_OF_RANGE; // FPP_CT_OvrFlow_Count += 1; // printf("error 1\n"); // return result_sym; // } else // // /* Check mean absolute deviation of the LIVE frame and minimum residual */ // if(( CTdata[14] > MAD_threshold ) || ( smin > Minimum_sum )) // { // ctme_out[0] = CT_OUT_OF_RANGE; // printf("error 2\n"); // printf("smin = %d, CTdata[14] = %d\n", smin, CTdata[14]); // return result_sym; // } // else { /* the usual case, compute a fit for the minimum */ /* first check max and right shift if appropiate */ if( smax > Bigword1 ) { //register int temp; int temp; temp = smax / Bigword1; i = 0; while( temp > 0 ) { temp >>= 1; i++; } //printf("need to scale!, i = %d\n", i); ct_scaled++; s1 = s1 >> i; s2 = s2 >> i; s3 = s3 >> i; s4 = s4 >> i; s5 = s5 >> i; s6 = s6 >> i; s7 = s7 >> i; s8 = s8 >> i; s9 = s9 >> i; s10 = s10 >> i; s11 = s11 >> i; s12 = s12 >> i; s13 = s13 >> i; } printf("(scaled) sums\n%d %d %d %d %d %d %d %d %d %d %d %d %d\n", s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13); /* minsum_index = 0; */ printf("minsum_index = %d\n", minsum_index); /* Is minimum in the central cross, ie imin = 0,3,6,9, or 12? */ if( minsum_index%3 == 0 ) { register int acc, a1, a2; register INT32 *fit_matrix_ptr; INT32 *a_ptr; /* printf("doing the matrix multiply\n"); */ imin = minsum_index / 3; printf("imin = %d\n", imin); fit_matrix_ptr = fit_matrix + imin*65; //printf("fit_matrix, fit_matrix_ptr = %d %d\n", fit_matrix, fit_matrix_ptr); //printf("delta = %d\n", (int) fit_matrix_ptr - (int) fit_matrix); a_ptr = a; j = 5; /* each of the 5 results is an inner product of one of the rows of the selected design matrix and the 13 signals */ while (j--) { acc = s1 * *fit_matrix_ptr++; acc += s2 * *fit_matrix_ptr++; acc += s3 * *fit_matrix_ptr++; acc += s4 * *fit_matrix_ptr++; acc += s5 * *fit_matrix_ptr++; acc += s6 * *fit_matrix_ptr++; acc += s7 * *fit_matrix_ptr++; acc += s8 * *fit_matrix_ptr++; acc += s9 * *fit_matrix_ptr++; acc += s10 * *fit_matrix_ptr++; acc += s11 * *fit_matrix_ptr++; acc += s12 * *fit_matrix_ptr++; acc += s13 * *fit_matrix_ptr++; /* load the appropiate a array element */ *a_ptr++ = acc; } /* the totally unwrapped multiply is below, this doesn't help, probably because the compiler doesn't use registers for everybody. But this will be useful when explicitly allowing for the zeros and symmetries. The total operation count can be reduced with a saving in time. */ /* a1 = s1 * fit_matrix_ptr[0]; a2 = s4 * fit_matrix_ptr[13]; a3 = s7 * fit_matrix_ptr[26]; a4 = s10 * fit_matrix_ptr[39]; a5 = s13 * fit_matrix_ptr[52]; a1 += s2 * fit_matrix_ptr[1]; a2 += s5 * fit_matrix_ptr[14]; a3 += s9 * fit_matrix_ptr[27]; a4 += s11 * fit_matrix_ptr[40]; a5 += s1 * fit_matrix_ptr[53]; a1 += s3 * fit_matrix_ptr[2]; a2 += s6 * fit_matrix_ptr[15]; a3 += s10 * fit_matrix_ptr[28]; a4 += s12 * fit_matrix_ptr[41]; a5 += s2 * fit_matrix_ptr[54]; a1 += s4 * fit_matrix_ptr[3]; a2 += s7 * fit_matrix_ptr[16]; a3 += s11 * fit_matrix_ptr[29]; a4 += s13 * fit_matrix_ptr[42]; a5 += s3 * fit_matrix_ptr[55]; a1 += s5 * fit_matrix_ptr[4]; a2 += s8 * fit_matrix_ptr[17]; a3 += s12 * fit_matrix_ptr[30]; a4 += s1 * fit_matrix_ptr[43]; a5 += s4 * fit_matrix_ptr[56]; a1 += s6 * fit_matrix_ptr[5]; a2 += s9 * fit_matrix_ptr[18]; a3 += s13 * fit_matrix_ptr[31]; a4 += s2 * fit_matrix_ptr[44]; a5 += s5 * fit_matrix_ptr[57]; a1 += s7 * fit_matrix_ptr[6]; a2 += s10 * fit_matrix_ptr[19]; a3 += s1 * fit_matrix_ptr[32]; a4 += s3 * fit_matrix_ptr[45]; a5 += s6 * fit_matrix_ptr[58]; a1 += s8 * fit_matrix_ptr[7]; a2 += s11 * fit_matrix_ptr[20]; a3 += s2 * fit_matrix_ptr[33]; a4 += s4 * fit_matrix_ptr[46]; a5 += s7 * fit_matrix_ptr[59]; a1 += s9 * fit_matrix_ptr[8]; a2 += s12 * fit_matrix_ptr[21]; a3 += s3 * fit_matrix_ptr[34]; a4 += s5 * fit_matrix_ptr[47]; a5 += s8 * fit_matrix_ptr[60]; a1 += s10 * fit_matrix_ptr[9]; a2 += s13 * fit_matrix_ptr[22]; a3 += s4 * fit_matrix_ptr[35]; a4 += s6 * fit_matrix_ptr[48]; a5 += s9 * fit_matrix_ptr[61]; a1 += s11 * fit_matrix_ptr[10]; a2 += s1 * fit_matrix_ptr[23]; a3 += s5 * fit_matrix_ptr[36]; a4 += s7 * fit_matrix_ptr[49]; a5 += s10 * fit_matrix_ptr[62]; a1 += s12 * fit_matrix_ptr[11]; a2 += s2 * fit_matrix_ptr[24]; a3 += s6 * fit_matrix_ptr[37]; a4 += s9 * fit_matrix_ptr[51]; a5 += s11 * fit_matrix_ptr[63]; a1 += s13 * fit_matrix_ptr[12]; a2 += s3 * fit_matrix_ptr[25]; a3 += s7 * fit_matrix_ptr[38]; a4 += s8 * fit_matrix_ptr[50]; a5 += s12 * fit_matrix_ptr[64]; a[0] = a1; a[1] = a2; a[2] = a3; a[3] = a4; a[4] = a5; */ /* now check if we need to rescale */ printf("the a vector:\n"); for (i=0;i<5;i++) printf("%2d %6d\n",i, a[i]); /* since the rescaling seems to be usually necessary here, a conversion to float may be more efficient for the minimum calculation */ a_ptr = a; j = 4; a1 = *a_ptr++; if (a1 < 0) a1 = -a1; while (j--) { a2 = *a_ptr++; if (a2 < 0) a2 = -a2; if (a2 > a1) a1 = a2; } if (a1 > Bigword2 ) { //register int temp, i; int temp, i; temp = a1 / Bigword2; i = 0; while( temp > 0 ) { temp >>= 1; i++; } j = 5; a_ptr = a; while (j--) *a_ptr++ = *a_ptr >> i; } printf("the shifted a vector:\n"); for (i=0;i<5;i++) printf("%2d %6d\n",i, a[i]); /* the surface equation we fit is f = c + a(0)*x + a(1)*y + a(2)*x*x + a(3)*x*y + a(4)*y*y where c is not needed here. To find the minimum in this surface, we find the point where the partial derivatives df/dx and df/dy are 0. This is 2 equations in 2 unknowns (x and y) given the derived values of a. */ det = ( 4 * a[2] * a[4] ) - ( a[3] * a[3] ); printf("det = %d\n", det); if( det ) { int dx, dy; /* be careful here, some optimizers will notice that DN_per_pixel/det is common to both offsets and make that calculation first. This is OK for fp but the result is generally 0 for I*4 and not (to understate) desirable */ dx = (DN_per_pixel * ( a[1]*a[3] - 2*a[0]*a[4] )); dy = (DN_per_pixel * ( a[0]*a[3] - 2*a[1]*a[2] )); dx = dx / det; dy = dy / det; ctme_out[0] = CT_VALID; /* 8/17/2004 - swap x/y and change sign of x, (this is equivalent to setting the swapxy flag and the change y sign flag since the swap was done last) */ //ctme_out[1] = (short int) dx; //ctme_out[2] = (short int) dy; ctme_out[2] = (short int) dx; ctme_out[1] = -(short int) dy; // the following zero was omitted for testing and is now back in 2/28/2002 N_out_of_range = 0; /* Is the error saturated (beyond the linear range)? */ rad2 = ctme_out[1]*ctme_out[1] + ctme_out[2]*ctme_out[2]; if( rad2 > DN_sat2 ) { int dx, dy; fprintf(stderr,"beyond linear range, original %d, %d", ctme_out[1], ctme_out[2]); root = rad2/DN_saturate; for( j=0; j < 4; j++ ) root = (root + rad2 / root) / 2; dx = ( ctme_out[1] * DN_saturate); dy = ( ctme_out[2] * DN_saturate); /* already swapped, etc above */ ctme_out[1] = (short int) (dx / root); ctme_out[2] = (short int) (dy / root); ctme_out[0] = CT_SAT_RANGE; fprintf(stderr,", modified %d, %d\n", ctme_out[1], ctme_out[2]); N_saturate++; // the following zero of N_saturate was omitted for testing and is now back in 2/28/2002 } else { normal_ct++; N_saturate = 0; } } else /* Determinant 0 (this should not happen) */ { ctme_out[0] = CT_OUT_OF_RANGE; ct_passed6++; N_out_of_range++; } } else // No, minimum sum is out on the periphery, saturation of error signal { // register int eps, dq; int eps, dq; int ilo, ihi; ctme_out[0] = CT_SAT_RANGE; // the following zero was omitted for testing N_out_of_range = 0; N_saturate++; // Quadratic fit to direction only, this should not be common ilo = ring_lo[minsum_index]; ihi = ring_hi[minsum_index]; s1 = (int) ctptr[ilo]; s2 = (int) ctptr[minsum_index]; s3 = (int) ctptr[ihi]; /* note that the squaring of the sums is not done here, fit a parabola to the 3 points assuming they are equally spaced along a line. s = a * x * x + b * x + c and ds/dx = 0, let x be [-1,0,1], then the x at minimum is (s1-s3)/(2*(s1+s3-2*s2) */ eps = ((DN_ring * (s1-s3)) / (s3+s1-2*s2))/2; if( eps > 0) // closer to s3 or ihi { dq = (DN_ring - eps); /* 8/17/2004 - swap x/y and change sign of x, (this is equivalent to setting the swapxy flag and the change y sign flag since the swap was done last) */ //ctme_out[1] = dq * X_coord[minsum_index] + eps * X_coord[ihi]; //ctme_out[2] = dq * Y_coord[minsum_index] + eps * Y_coord[ihi]; ctme_out[2] = dq * X_coord[minsum_index] + eps * X_coord[ihi]; ctme_out[1] = -dq * Y_coord[minsum_index] - eps * Y_coord[ihi]; } else // closer to s1 or ilo { dq = (DN_ring + eps); /* 8/17/2004 - swap x/y and change sign of x, (this is equivalent to setting the swapxy flag and the change y sign flag since the swap was done last) */ //ctme_out[1] = dq * X_coord[minsum_index] - eps * X_coord[ilo]; //ctme_out[2] = dq * Y_coord[minsum_index] - eps * Y_coord[ilo]; ctme_out[2] = dq * X_coord[minsum_index] - eps * X_coord[ilo]; ctme_out[1] = -dq * Y_coord[minsum_index] + eps * Y_coord[ilo]; } } /* // All elements of ctme_out have valid entries in proper units now // OK to send ctme_out to CTME, or do we bail out? if( ctme_out[0] == 8192 ) { FPP_CT_Range_Err_Count += 1; N_out_of_range += 1; } if( N_CTME_Error >= N_CTME_Error_Max ) { // set status to reflect CTME Error, ie don't use CTME until reset printf( "CTME Error\n" ); } else if( N_Hard_Limit >= N_Hard_Limit_Max ) { ctme_out[0] = 2; // Command servo off // set status to reflect PZT Hard Limit, ie open loop, reset PZTs, start again printf( "PZT Hard Limit\n" ); } else if( N_out_of_range >= N_Out_of_Range_Max ) { ctme_out[0] = 2; // Command servo off // set status to reflect out of range, ie open loop, reset PZTs, start again printf( "Out of Range\n" ); } else { // normal operation, send error signals to CTME //printf( "Sending ctme_out to CTME: %d %d %d\n", ctme_out[0], ctme_out[1], ctme_out[2] ); if( ctme_out[0] == 4096 ) FPP_CT_Saturation_Err_Count += 1; } */ //} //t2 = systime(); //printf("total time = %12.4f\n", t2-t1); //printf("time per cycle = %12.4f\n", 1.E6*(t2-t1)/(float) ncount); printf("ctme_out = %d, %d, %d\n", ctme_out[0], ctme_out[1], ctme_out[2]); return result_sym; } /*-------------------------------------------------------------------------*/ int ana_cterrorfp(narg,ps) int narg, ps[]; { /* a C port of Tarbell's CTerror code, taken from D. Mathur's port with an ANA boilerplate added */ /* this is the FP version */ extern double systime(); #define INT32 int #define UINT32 unsigned int #define UINT16 unsigned short int INT32 signx = 0, signy = 0; INT32 switchxy = 0; UINT32 N_CTME_Error_Max = 2; UINT32 N_Hard_Limit_Max = 5; UINT32 N_Out_of_Range_Max = 20; UINT32 DN_per_pixel = 440; UINT32 DN_saturate = 880; UINT32 DN_sat2 = 774400; // DN_saturate^2 UINT32 DN_ring = 293; // = DN_saturate / 3 UINT32 Bigword1 = 4096; UINT32 Bigword2 = 1024; UINT32 N_CTME_Error = 0; UINT32 N_Hard_Limit = 0; UINT32 N_out_of_range = 0; UINT32 N_Bad_CTStatus = 0; UINT32 N_MAD_threshold = 0; UINT32 N_Minimum_sum = 0; INT32 Minimum_sum = 1000; INT32 MAD_threshold = 5000; UINT16 CTImageHdr[32]; UINT16 CTImage[50][50]; UINT32 DDpointer = 0; UINT32 DDmax = 60900 - 7; UINT16 DD_Hdr[32]; UINT16 Diag_Data[60900]; UINT16 jitterdata[7]; UINT16 FPP_CTM_Hard_Limit_Count = 0; UINT16 FPP_CT_OvrFlow_Count = 0; UINT16 FPP_CT_Range_Err_Count = 0; UINT16 FPP_CT_Saturation_Err_Count = 0; UINT32 ring_index[13]; INT32 ring_lo[13] = {9, 11, 1, 0, 2, 4, 3, 5, 7, 6, 8, 10, 12}; INT32 ring_hi[13] = {3, 2, 4, 6, 5, 7, 9, 8, 10, 0, 11, 1, 12}; INT32 X_coord[13] = {-1, -3, -2, 0, 0, 2, 1, 3, 2, 0, 0, -2, 0}; INT32 Y_coord[13] = {0, 0, -2, -1, -3, -2, 0, 0, 2, 1, 3, 2, 0}; float fit_matrix[5*5*13] = { -8675, -2015, -2644, -217, 479, 1619, 9820, 2799, 1619, -217, 479, -2644, -403, 0, 0, 1197, 8083, 4041, 1197, 0, 0, -1197, -8083, -4041, -1197, 0, -3581, 4015, -32, -1860, -46, 601, 2454, 1159, 601, -1860, -46, -32, -1372, 0, 0, -5089, 3143, 1571, 1160, 0, 0, -1160, -3143, -1571, 5089, 0, -2494, -1123, 1444, -650, 2099, 245, -1147, 26, 245, -650, 2099, 1444, -1537, -8083, -4041, -1197, 0, 0, 1197, 8083, 4041, 1197, 0, 0, -1197, 0, 217, -479, 2644, 8675, 2015, 2644, 217, -479, -1619, -9820, -2799, -1619, 403, -650, 2099, 1444, -2494, -1123, 1444, -650, 2099, 245, -1147, 26, 245, -1537, 3143, 1571, -5089, 0, 0, 5089, -3143, -1571, -1160, 0, 0, 1160, 0, -1860, -46, -32, -3581, 4015, -32, -1860, -46, 601, 2454, 1159, 601, -1372, -9820, -2799, -1619, 217, -479, 2644, 8675, 2015, 2644, 217, -479, -1619, 403, 0, 0, 1197, 8083, 4041, 1197, 0, 0, -1197, -8083, -4041, -1197, 0, 2454, 1159, 601, -1860, -46, -32, -3581, 4015, -32, -1860, -46, 601, -1372, 0, 0, -1160, -3143, -1571, 5089, 0, 0, -5089, 3143, 1571, 1160, 0, -1147, 26, 245, -650, 2099, 1444, -2494, -1123, 1444, -650, 2099, 245, -1537, -8083, -4041, -1197, 0, 0, 1197, 8083, 4041, 1197, 0, 0, -1197, 0, -217, 479, 1619, 9820, 2799, 1619, -217, 479, -2644, -8675, -2015, -2644, -403, -650, 2099, 245, -1147, 26, 245, -650, 2099, 1444, -2494, -1123, 1444, -1537, -3143, -1571, -1160, 0, 0, 1160, 3143, 1571, -5089, 0, 0, 5089, 0, -1860, -46, 601, 2454, 1159, 601, -1860, -46, -32, -3581, 4015, -32, -1372, -6521, -3260, -2173, 0, 0, 2173, 6521, 3260, 2173, 0, 0, -2173, 0, 0, 0, 2173, 6521, 3260, 2173, 0, 0, -2173, -6521, -3260, -2173, 0, -579, 2179, 745, -2303, -406, 745, -579, 2179, 745, -2303, -406, 745, -762, 0, 0, -3125, 0, 0, 3125, 0, 0, -3125, 0, 0, 3125, 0, -2303, -406, 745, -579, 2179, 745, -2303, -406, 745, -579, 2179, 745, -762 }; short int CTerror[3] = {0, 0, 0}; int minsum_index, minsum; UINT32 maxsum; float sums[13]; INT32 data_val; int i, j, k; float det; float rad2; int imin; float root; int ilo, ihi; float eps; int Linecount; int count = 10000, ncount; double t1, t2; /* here we have some fake CTdata and CTMEstatus defined locally, would normally be passed as arguments */ short int CTdata[70]; short int CTMEstatus[3]; for( i = 0; i < 70; i++ ) CTdata[i] = i; CTMEstatus[0] = 12; CTMEstatus[1] = 20; CTMEstatus[2] = 30; if (narg > 0) if (int_arg_stat(ps[0], &count) != 1) return -1; ncount = count; printf(" sizeof (float) = %d\n", sizeof (float)); t1 = systime(); while (count--) { /* Check status of CTME, take action after error computation if necessary */ if (CTMEstatus[0] & 0x08) N_CTME_Error += 1; else N_CTME_Error = 0; if (CTMEstatus[0] & 0x700) { N_Hard_Limit += 1; FPP_CTM_Hard_Limit_Count += 1; } else N_Hard_Limit = 0; // Start error computation CTerror[0] = CTerror[1] = CTerror[2] = 0; /* need index of minimum of CTdata(1:13) and also an array of the values squared in FP, still using minsum also */ { short *ctptr = CTdata + 1; /* note start at index 1 */ int n = 12; register float *sumptr = sums; register float xq, yq; xq = yq = (float) *ctptr++; /* set to first value */ minsum_index = 0; *sumptr++ = xq * xq; for (i=1; i<13; i++) { xq = (float) *ctptr++; if (xq < yq) { minsum_index = i; yq = xq; } *sumptr++ = xq * xq; } minsum = (int) yq; } // Check CT status word for overflows or other errors; // (CT status word is not defined yet - this is a place holder for // a proper test) if( CTdata[15] == 999 ) { /* Set error signal to "out of range" */ CTerror[0] = 8192; FPP_CT_OvrFlow_Count += 1; printf("error 1\n"); } else // Check mean absolute deviation of the LIVE frame and minimum residual if(( CTdata[14] > MAD_threshold ) || ( minsum > Minimum_sum )) { CTerror[2] = 8192; printf("error 2\n"); } else { /* the usual case, compute a fit for the minimum */ minsum_index = 0; /* for test, just to be sure we go through the matrix multiply */ /* Is minimum in the central cross, ie imin = 0,3,6,9, or 12? */ if( minsum_index%3 == 0 ) { register float acc, acc2, *sums_ptr; register int j=5; register float *fit_matrix_ptr; float a[5], *a_ptr; imin = minsum_index / 3; fit_matrix_ptr = fit_matrix + imin*25; a_ptr = a; while (j--) // for (j=0; j<5; j++) { sums_ptr = sums; acc = *sums_ptr++ * *fit_matrix_ptr++; //while (n--) acc += *sums_ptr++ * *fit_matrix_ptr++; acc2 = *sums_ptr++ * *fit_matrix_ptr++; acc += *sums_ptr++ * *fit_matrix_ptr++; acc2 += *sums_ptr++ * *fit_matrix_ptr++; acc += *sums_ptr++ * *fit_matrix_ptr++; acc2 += *sums_ptr++ * *fit_matrix_ptr++; acc += *sums_ptr++ * *fit_matrix_ptr++; acc2 += *sums_ptr++ * *fit_matrix_ptr++; acc += *sums_ptr++ * *fit_matrix_ptr++; acc2 += *sums_ptr++ * *fit_matrix_ptr++; acc += *sums_ptr++ * *fit_matrix_ptr++; acc2 += *sums_ptr++ * *fit_matrix_ptr++; acc += *sums_ptr++ * *fit_matrix_ptr++; *a_ptr++ = acc+ acc2; // a[j] = acc; } det = ( 4.0 * a[2] * a[4] ) - ( a[3] * a[3] ); if( det ) { CTerror[1] = DN_per_pixel * ( a[1]*a[3] - 2*a[0]*a[4] ) / det; CTerror[2] = DN_per_pixel * ( a[0]*a[3] - 2*a[1]*a[2] ) / det; CTerror[0] = 0; N_out_of_range = 0; /* Is the error saturated (beyond the linear range)? */ rad2 = CTerror[1]*CTerror[1] + CTerror[2]*CTerror[2]; if( rad2 > DN_sat2 ) { root = rad2/DN_saturate; for( j=0; j < 4; j++ ) root = (root + rad2 / root) / 2; CTerror[1] = ( CTerror[0] * DN_saturate) / root; CTerror[2] = ( CTerror[1] * DN_saturate) / root; CTerror[0] = CT_SAT_RANGE; } } else /* Determinant 0 (this should not happen) */ { printf("zero determinant\n"); CTerror[0] = CT_OUT_OF_RANGE; } } } } t2 = systime(); printf("total time = %12.4f\n", t2-t1); printf("time per cycle = %12.4f\n", 1.E6*(t2-t1)/(float) ncount); printf("CTerror = %d %d %d\n", CTerror[0], CTerror[1], CTerror[2]); return 1; } /*------------------------------------------------------------------------- */ int therm2temp(th) int th; { int t, val; /* the upper bit is a flag and the lower 12 are the value */ val = th & 0xfff; /* check for high or low range and then use the appropiate lookup table, note that the thermistor values decrease as the temperature increases */ if (0x8000 & th | forceHighRange ) { val = val - hiTableOffset; if (val < 0 ) return highTlimit; if (val > hiTableCnt ) return lowTlimit; t = ((int) highTable[val]) & 0xffff; } else { val = val - loTableOffset; if (val < 0 ) return highTlimit; if (val > loTableCnt ) return lowTlimit; t = ((int) lowTable[val]) & 0xffff; } return t; } /*----------------------------------------------------------------------*/ int patan(x) int x; { /* a very crude atan function for Solar B flight s/w, uses an integer lookup to return one of 42 possible values. Input is the tangent * 128, this covers a 0-90 degree range, the other 4 quadrants can be determinied by the original signs of the sin and cos inner products */ int low, high, mid, n = 43; low = 0; high = n-1; while (1) { mid = (low+high)/2; if (x < tableoftans[mid]) high = mid; else if (x > tableoftans[mid]) low = mid+1; else break; if (low >= high) { if (x <= tableoftans[low]) mid = MAX((low - 1), 0); else mid = low; break; } if (low >= n) { mid = n; break; } if (high <= 0) { mid = 0; break; } } return mid; } /*----------------------------------------------------------------------*/ #define NELEMS 8 /* number of elements in this TF */ #define NTEMPS 8 /* number of thermistors used for elements */ #define NFI_5172 8 LineList lnlst[10] = { /* v reference motor positions */ /* 2/19/2005 - we now have a real 5172 filter again */ { 0,{32,28,30, 1,29, 5, 8,70}, 1842,1000, 6379, 1725, /* 5172.700 */ /* set to 6302 for laser scans with glass slug in 5172 position */ //{ 0,{38,60,20,81,20,65,16,69}, 1426,1000, 4679, 1967, {20000,20000,20000,20000,20000,20000,20000,20000}}, { 1,{38,23,10,31, 2, 8,36,74}, 1805,1000, 6152, 1751, /* 5250.200 */ {20000,20000,20000,20000,20000,20000,20000,20000}}, { 2,{33,44,33, 3,32, 4,22,63}, 1668,1000, 5329, 1860, /* 5576.100 */ {20000,20000,20000,20000,20000,20000,20000,20000}}, { 3,{38,60,20,81,20,65,16,69}, 1426,1000, 4016, 2102, /* 6302.500 */ {20000,20000,20000,20000,20000,20000,20000,20000}}, //{ 6562800,{32, 6, 7,33, 3,33,28, 9}, 1356,1000}, /* 5172.700 */ /* new H alpha reference, 9/13/2003 */ { 4,{32, 4, 8,37, 0,54,28,13}, 1356,1000, 3665, 2189, /* 6562.800 */ {20000,20000,20000,20000,20000,20000,20000,20000}}, { 5,{ 8,37, 8,62,27,22,30,43}, 1552,1000, 4680, 1967, /* 5895.940 * /* set to 6302 for laser scans with glass slug in 5896 position */ //{ 5,{38,60,20,81,20,65,16,69}, 1426,1000, 4679, 1967, {20000,20000,20000,20000,20000,20000,20000,20000}}, /* the laser lines */ /* change to use blocker 0 (5172) for all the laser lines */ //{ 6328000,{31,7,24,8,0,40,26,11}, 1419,1000}, { 0,{2,78,33,56,24,58,37,14}, 1419,1000, 3981, 2111, /* 6328.000 */ {20000,20000,20000,20000,20000,20000,20000,20000}}, { 0,{18,10,21,23,15,60, 4,10}, 1726,1000, 5673, 1892, /* 5430.000 */ {20000,20000,20000,20000,20000,20000,20000,20000}}, { 0,{14,21,41,36,29,15, 0,83}, 1537,1000, 4599, 1981, /* 5940.000 */ {20000,20000,20000,20000,20000,20000,20000,20000}}, { 0,{38,23,10, 7, 4,55, 5,43}, 1481,1000, 4300, 2040, /* 6117.000 */ {20000,20000,20000,20000,20000,20000,20000,20000}} }; int lgths[8] = {881, 1057, 1402, 11190, 5604, 5600, 2795, 692}; int map2ratioOrder[8] = {7, 6, 4, 0, 1, 2, 3, 5}; int tuneType[8] = { 1, 0, 1, 0, 1, 0, 1, 0 }; int cs[8] = { 1, 1, 1,-1, -1,-1, -1,-1}; int dels[8], mdels[8]; int trepos[8]; /* for new computed positions */ int currentMotorPositions[8] = {0, 0, 0, 0, 0, 0, 0, 0}; /*----------------------------------------------------------------------*/ int ana_tftune(narg,ps) int narg, ps[]; { /* test the tfTune.cpp algorithm for motor positions, this is a function that returns a vector of 8 motor positions. Input is wave index, v, offset, and the therms (raw format) as an array of 8, if the therms are omitted, we assume a nominal 20 C temperature */ int v, offset, winx, n, TFwaveIndex, dim[8]; int eltmp[NELEMS], temparr[NTEMPS], *p, *q; short *ptherm; int ma, delta, slp1, slp2, xq, yq, i, tc, lgthi, sac, mtrq, woff; int dmotor[NELEMS]; /* for the difference to move */ int absd[NELEMS]; /* the absolute values of the differences */ if (int_arg_stat(ps[0], &TFwaveIndex) != 1) return -1; if (int_arg_stat(ps[1], &v) != 1) return -1; if (int_arg_stat(ps[2], &offset) != 1) return -1; //printf("TFwaveIndex %d, v %d, offset %d\n", TFwaveIndex, v, offset); if (narg > 3) { if (sym[ps[3]].class != 4|| sym[ps[3]].type != 1) return execute_error(111); n = get_p_c(&ps[3], &ptherm); if (n != 8) { fprintf(stderr, "tftune needs 8 thermistor value, not %d\n", n); return -1; } /* first convert to temperatures */ p = temparr; while (n--) *p++ = therm2temp(*ptherm++); } else { n = 8; p = temparr; while (n--) *p++ = 20000; } /* check them while debugging */ //n = 8; p = temparr; printf("converted thermistors"); //while (n--) printf(" %d", *p++); //printf("\n"); /* this is a first guess on how the element T's are related to the thermistors, note that they are backwards relative to my starting end */ eltmp[0] = temparr[7]; eltmp[1] = temparr[6]; eltmp[2] = temparr[6]; eltmp[3] = (temparr[4]+temparr[5])/2; eltmp[4] = (temparr[4] + 3*temparr[3])/4; /* 11/20/2003 - changed, R. Shine */ eltmp[5] = temparr[2]; eltmp[6] = temparr[1]; eltmp[7] = temparr[0]; //n = 8; p = eltmp; printf("element temperatures"); //while (n--) printf(" %d", *p++); //printf("\n"); winx = TFwaveIndex - NFI_5172; /* winx is TF line #, starts with 0 */ /* get the velocity shift in mA for this wavelength, note rounding */ ma = (v * lnlst[winx].meters2ma + 50000)/100000; woff = ma + offset; /* this is total offset from line center */ sac = lnlst[winx].sacfi; //printf("sac = %d\n", sac); slp1 = lnlst[winx].slope1; slp2 = lnlst[winx].slope2; /* get positions relative to reference position at reference T */ /* there are 2 parts to this, the wavelength offset and the T correction, the order of operations is intended to avoid loss of any accurancy from the integer operations, it is awkward */ for (i = 0; i < NELEMS; i++){ lgthi = lgths[i]; /* its length */ /* we are using 32 bit ints, so do in parts to avoid overflow */ xq = sac*lgthi; /* range for this is 2.74E6 to 7.1E7 */ xq = (xq + 500)/1000; /* now 2.74E3 to 7.1E4 */ //if (tfdebug) printf("i, xq = %d %6d", i, xq); tc = eltmp[i] - lnlst[winx].tRef[i]; /* temperature difference from ref */ //if (tfdebug) printf(" tc %6d", tc); delta = woff * xq; /* should be within range of I*4 for woff < 30,000 */ /* note - rounding for positive or negative values */ if (delta > 0 ) delta += 5000; if (delta < 0 ) delta -= 5000; //if (tfdebug) printf(" *woff %d ", delta); delta = delta/10000; /* this is still 10 times the offset steps */ yq = tc * lgthi; /* up to 1E8 if dt is 10C */ yq = yq/slp2; yq = yq * slp1; if (yq > 0 ) yq += 5000; if (yq < 0 ) yq -= 5000; yq = yq/10000; /* this is still 10 times the offset steps */ delta = delta + yq; if (delta > 0 ) delta += 5; if (delta < 0 ) delta -= 5; delta = delta/10; //if (tfdebug) printf(" delta steps %d\n", delta); /* apply the orientation (aka coupling) sign */ delta = cs[i] * delta; /* delta is the tuning steps from line center, the actual motor steps are done below */ dels[i] = delta % 84; } /* The following is somewhat hardwired to our situation rather than a general formulation like that in the SOUP case (which was also very confusing). The 4 P motors are done first since they are not affected by the H motors. */ for (i = 1; i < NELEMS; i=i+2) { // note start at 1, assumes first is an H xq = -dels[i]; // apply sign /* note that tunedirection is +1 here */ if (xq < 0) xq = 84 + xq; mdels[i] = xq; } /* the H motors use their delta's combined with the neighboring P and then rotate by half the combined value (with a range of 90 degrees) */ for (i = 0; i < (NELEMS-1); i=i+2) { // note end, assumes last is a P /* each H is coupled to the next P (these are in physical order) */ xq = - (dels[i] + mdels[i+1])/2; /* note that tunedirection is -1 here */ if (xq < 0) xq = 42 + xq; mdels[i] = xq; } /* the mdels now contain the motor offsets from the line center position, combine with known position for line center to get the desired motor positions. */ for (i = 0; i < NELEMS; i++){ mtrq = lnlst[winx].tfrefpos[i]; xq= (mtrq + mdels[i]); /* the range we use depends on type, half a rotation for P (84 steps) and a quarter for H (42 steps) */ if (tuneType[i]) { xq = xq % 42; if (xq < 0) xq = 42 + xq; } else { xq = xq % 84; if (xq < 0) xq = 84 + xq; } trepos[i]= xq; } //n = 8; p = trepos; printf("motors"); //while (n--) printf(" %d", *p++); //printf("\n"); dim[0] = 8; result_sym = array_scratch(2, 1, dim); h = (struct ahead *) sym[result_sym].spec.array.ptr; p = (int *) ((char *)h + sizeof(struct ahead)); q = trepos; n = 8; while (n--) *p++ = *q++; return result_sym; } /*----------------------------------------------------------------------*/ int ana_imaxtune(narg,ps) int narg, ps[]; { /* uses a trig mask to determine the position of maximum intensity for a P/H or H rotation, all with integer arithmetic to test Solar B flight s/w, the call is imaxtune(vector) */ int iq, pmax, n; union types_ptr q1; iq = ana_long(narg,ps); if ( sym[iq].class != 4 ) return execute_error(66); n = get_p_c(&iq, &q1); if (n <= 0) return -1; result_sym =scalar_scratch(2); pmax = imaxtune(q1.l, n); if (pmax < 0) return 2; sym[result_sym].spec.scalar.l = pmax; return result_sym; } /*----------------------------------------------------------------------*/ int imaxtune(yin, npts) unsigned int *yin, npts; /* returns a motor position for the maximum based on a trig fit, this works only for a limited set of n's and scales the integer input if it exceeds 4095. None of this is very efficient but doesn't take long because of the limited # of data points. */ { unsigned int n, max, *p, nshift, ns; int *sv, *cv, sdot, cdot, sflag, ratio, pq, xq; /* check the max and scale down if necessary */ max = 0; n = npts; p = yin; while (n--) { max = MAX(max, *p); p++; } nshift = 0; while (max > 4097) { nshift++; max = max >> 1; } printf("nshift = %d\n", nshift); /* compute the inner products with the appropiate sized trig array */ switch (npts) { case 42: sv = isintab42; cv = icostab42; break; case 14: sv = isintab14; cv = icostab14; break; case 7: sv = isintab7; cv = icostab7; break; case 6: sv = isintab6; cv = icostab6; break; default: printf("bad count in imaxtune = %d\n", npts); return -1; } n = npts; p = yin; sdot = cdot = 0; while (n--) { xq = (int) *p++; xq = xq >> nshift; sdot = sdot + xq * *sv++; cdot = cdot + xq * *cv++; } /* note that sdot and cdot should be within I*4 range */ printf("sdot:cdot = %10d:%10d\n", sdot,cdot); /* the signs determine the quadrant */ if (sdot < 0) { sdot = - sdot; if (cdot < 0) { cdot = -cdot; sflag = 2; } else sflag = 3; } else if (cdot < 0) { cdot = -cdot; sflag = 1; } else sflag = 0; /* compute the ratio 128 * sdot/cdot */ ns = 7; while (1) { if (ns > 0 && sdot < 1073741824) { sdot = sdot *2; ns--; } else break; } /* apply any leftovers to cdot */ if (ns > 0) cdot = cdot >> ns; if (cdot <= 0) ratio = 10000; else ratio = sdot/cdot; printf("normalized sdot:cdot = %10d:%10d, ratio %10d\n", sdot,cdot, ratio); pq = patan(ratio); switch (sflag) { case 1: pq = 84 - pq; break; case 2: pq = 84 + pq; break; case 3: pq = 168 - pq; break; } /* round to a 0-83 range */ pq = (pq+1)/2; return pq; } /*----------------------------------------------------------------------*/ int ana_fppratio(int narg,int ps[]) { /* this is a function */ /* test FPP ratio algorithms, call is result = fppratio(smem1, smem2, ratio_type) */ int iq, n, n2, result_sym, ratio_type, class; struct ahead *h; /* the FPP parameters */ int nc; int rat, denom; short *pout, *p1, *p2; /* input is passed as 2 short arrays, returns a ratio in a short array, also a type parameter for testing various */ iq = ps[0]; /* cast it to a I*2 array */ iq = ana_word(1,&iq); n = get_p_c(&iq, &p1); class = sym[iq].class; if (class != ARRAY) return execute_error(66); iq = ps[1]; iq = ana_word(1,&iq); n2 = get_p_c(&iq, &p2); class = sym[iq].class; if (class != ARRAY) return execute_error(66); /* check if arrays the same size */ if (n2 != n) return execute_error(103); result_sym = array_clone(iq, sym[iq].type); h = (struct ahead *) sym[result_sym].spec.array.ptr; pout = (short *) ((char *)h + sizeof(struct ahead)); if (narg > 2) { if (int_arg_stat(ps[2], &ratio_type) != 1) return -1; } else ratio_type = 0; nc = n; switch (ratio_type) { case 0: while (nc--) { if ( (denom = (int) *p1++ ) <= 0) { rat = 0; p2++; } else rat = (((int) (*p2++)) << 16) / denom; *pout++ = (short) (rat >> 5); /* for a 12 bit signed ratio where 2048 is 1 */ } break; case 1: while (nc--) { if ( (denom = (int) *p1++ ) == 0) { rat = 0; p2++; } else rat = (((int) (*p2++)) << 11) / denom; *pout++ = (short) (rat); /* for a 12 bit signed ratio where 2048 is 1 */ } break; default: printf("only cases 0 and 1 supported at present\n"); } return result_sym; } /*----------------------------------------------------------------------*/ typedef struct { unsigned short obs_id; unsigned short frame_def_id; unsigned short wave; short wave_off; /* this is signed */ unsigned short exposure; unsigned short dark_flag; unsigned short ix0; unsigned short ix1; unsigned short iy0; unsigned short iy1; unsigned short nx; unsigned short ny; unsigned short nxCCD; unsigned short nyCCD; unsigned short ny1; unsigned short ny2; unsigned short nyp; unsigned short npack1; unsigned short npack2; unsigned short nframes; unsigned short wscan_npos; unsigned short wscan_step; unsigned short roi_start; unsigned short roi_stop; unsigned short sumx; unsigned short sumy; unsigned short binx; unsigned short biny; unsigned short ccd_mode; unsigned short mask; unsigned short shutter_mode; } fg_expanded_parameters; int *FGsmemBuf, *FGsmemBuf2; /*------------------------------------------------------------------------- */ /* this function extracted from packets.cpp */ int ratioCopy(char *pin, char *p, int nc, int bias) { /* 4/27/2008 - mod to create a 13 bit signed result rather than a 12 bit one */ /* this assumes that the values we want to ratio are in the 2 smart memory read buffers (already read from smart memory sub buffers). The I signal is usually in pin and the difference signal in FGsmemBuf2 */ short *ps, *p1, *p2; int rat, denom; ps = (short *) p; /* note that p1 is signed for the V/I case and the DG's */ p1 = (short *) pin; /* use the same offset applied to FGsmemBuf2 */ p2 = (short *) ( (char *) FGsmemBuf2 + ( (char *) p1 - (char *) FGsmemBuf ) ); /* count shorts rather than bytes */ nc = nc/2; while (nc--) { /* 5/14/2008 - subtract passed pedestal from I signal in *p1 */ if ( (denom = ((int) *p1++ ) - bias) <= 0) { rat = 0; p2++; } else // rat = (((int) (*p2++)) << 11) / denom; rat = (((int) (*p2++)) << 12) / denom; *ps++ = (short) (rat); /* for a 13 bit signed ratio where 4096 is 1 */ } return 0; } /*----------------------------------------------------------------------*/ int ana_fppbinratio(int narg,int ps[]) { /* to test code in packets.cpp that handles the combined bin and ratio scenario and the more ordinary cases for comparison input 4 short arrays for the 4 smart memory buffers, also a bin and ratio flag and bias for each side so result = fppbinratio(smem1LHS, smem2LHS, smem1RHS, smem2RHS, bin, ratio, biasLHS, biasRHF) */ int iq, n, n2, result_sym, ratio_type, binvalue, class, rhsOffset, nexpimagecount; struct ahead *h; /* the FPP parameters */ int nc, nd, nx, ny, biasLHS, biasRHS, dim[3], nylhs, nyrhs; int rat, denom, smem_read; short *pout, *pout2, *p1lhs, *p2lhs, *p1rhs, *p2rhs; fg_expanded_parameters *fg_obs_p; /* input is passed as 2 short arrays, returns a ratio in a short array */ iq = ps[0]; /* cast it to a I*2 array */ iq = ana_word(1,&iq); n = get_p_c(&iq, &p1lhs); class = sym[iq].class; if (class != ARRAY) return execute_error(66); iq = ps[1]; iq = ana_word(1,&iq); n2 = get_p_c(&iq, &p2lhs); class = sym[iq].class; if (class != ARRAY) return execute_error(66); /* check if arrays the same size */ if (n2 != n) return execute_error(103); h = (struct ahead *) sym[iq].spec.array.ptr; nd = h->ndim; nylhs = h->dims[1]; nx = h->dims[0]; if (nd != 2 || nx != 2048) { fprintf(stderr,"fppbinratio: illegal input, nd = %d (must be 2), nx = %d (must be 2048)\n", nd); return -1; } iq = ps[2]; /* cast it to a I*2 array */ iq = ana_word(1,&iq); n = get_p_c(&iq, &p1rhs); class = sym[iq].class; if (class != ARRAY) return execute_error(66); iq = ps[3]; iq = ana_word(1,&iq); n2 = get_p_c(&iq, &p2rhs); class = sym[iq].class; if (class != ARRAY) return execute_error(66); /* check if arrays the same size */ if (n2 != n) return execute_error(103); h = (struct ahead *) sym[iq].spec.array.ptr; nd = h->ndim; nyrhs = h->dims[1]; nx = h->dims[0]; if (nd != 2 || nx != 2048) { fprintf(stderr,"fppbinratio: illegal input, nd = %d (must be 2), nx = %d (must be 2048)\n", nd); return -1; } if (nylhs != nyrhs) { fprintf(stderr,"fppbinratio: ny must match but LHS = %d, and RHS = %d\n", nylhs, nyrhs); return -1; } ny = nylhs; if (narg > 4) { if (int_arg_stat(ps[4], &binvalue) != 1) return -1; } else binvalue = 1; if (narg > 5) { if (int_arg_stat(ps[5], &ratio_type) != 1) return -1; } else ratio_type = 0; if (narg > 6) { if (int_arg_stat(ps[6], &biasLHS) != 1) return -1; } else biasLHS = 0; if (narg > 7) { if (int_arg_stat(ps[7], &biasRHS) != 1) return -1; } else biasRHS = 0; if (narg > 8) { if (int_arg_stat(ps[8], &nexpimagecount) != 1) return -1; } else nexpimagecount = 1; /* the output array will be smaller if binned */ dim[0] = nx >> binvalue; dim[1] = ny >> binvalue; dim[2] = 2; rhsOffset = nx*ny; result_sym = array_scratch(1, 3, dim); h = (struct ahead *) sym[result_sym].spec.array.ptr; pout = (short *) ((char *)h + sizeof(struct ahead)); pout2 = pout + ((nx*ny) >> (2*binvalue)); { short *pq; pq = pout; int n = nx*ny*2; n = n >> (2*binvalue); while (n--) *pq++ = 0xdead; } printf("mark 1, nx, ny = %d, %d\n", nx, ny); /* use variables from packets.cpp, some unneccesary looking equates are done to match variables in packets.cpp so we can drop in code from there later */ fg_obs_p = (void *) malloc( sizeof(fg_expanded_parameters)); fg_obs_p->nxCCD = nx; fg_obs_p->nyCCD = ny; fg_obs_p->ix0 = 0; fg_obs_p->iy0 = 0; fg_obs_p->ix1 = fg_obs_p->ix0 + fg_obs_p->nxCCD - 1; fg_obs_p->iy1 = fg_obs_p->iy0 + fg_obs_p->nyCCD - 1; fg_obs_p->binx = fg_obs_p->biny = binvalue; fg_obs_p->sumx = fg_obs_p->sumy = 0; fg_obs_p->nx = (fg_obs_p->nxCCD >> (fg_obs_p->sumx + fg_obs_p->binx)); fg_obs_p->ny = (fg_obs_p->nyCCD >> (fg_obs_p->sumy + fg_obs_p->biny)); fg_obs_p->ny1 = fg_obs_p->nyp = fg_obs_p->ny; FGsmemBuf = (int *) p1lhs; FGsmemBuf2 = (int *) p2lhs; smem_read = nx * ny; /* note that inout nx must be 2048 to correspond to the full height of CCD */ { char *pdata; int bin_flag, nybin, nxbin, nysmem; int ratioFlag, totbin, roundbin; int pixpercolumn, ny_left, need_data_flag; int ix0, iy0, iy1, npack1, npack2, nx, ny, ny1, nyp, nxp; unsigned short *pq, *pbase; unsigned short *pqq, *pd; pbase = FGsmemBuf; bin_flag = (fg_obs_p->biny || fg_obs_p->binx); ratioFlag = ratio_type; pdata = pout; pd = (unsigned short *) pdata; pixpercolumn = (2048 >> fg_obs_p->sumx); nysmem = smem_read/pixpercolumn; /* some setups extracted from packets.cpp */ ix0 = fg_obs_p->ix0; iy0 = fg_obs_p->iy0; iy1 = fg_obs_p->iy1; npack1 = fg_obs_p->npack1; npack2 = fg_obs_p->npack2; nx = fg_obs_p->nx; ny = fg_obs_p->ny; ny1 = fg_obs_p->ny1; nyp = fg_obs_p->nyp; nxp = nx; /* all partials have full nx */ ny_left = ny; /************* start of a section of new code extracted from packets.cpp ****************/ /* some binning setups (if we are binning) */ bin_flag = (fg_obs_p->biny || fg_obs_p->binx); if (bin_flag) { nybin = 0x1 << fg_obs_p->biny; nxbin = 0x1 << fg_obs_p->binx; totbin = fg_obs_p->biny + fg_obs_p->binx; roundbin = (0x1 << (totbin - 1)); /* 5/14/2008 - to avoid extra shifts when binning with ratios, we need to multiply the biases by the bin factor */ biasRHS = biasRHS * nybin * nxbin; biasLHS = biasLHS * nybin * nxbin; } /* we also have to account for multiple images in the I accumulation, this means we have to multiply our bias by the # of actual exposures, but note that nexpimagecount is actually twice the exposure count (in order to count shifted accumulations) so also divide by 2 */ biasRHS = biasRHS * nexpimagecount/2; biasLHS = biasLHS * nexpimagecount/2; printf("biasLHS, biasRHS = %d, %d\n", biasLHS, biasRHS); printf("bin_flag, ratioFlag, roundbin = %d, %d, %d\n", bin_flag, ratioFlag, roundbin); /************* end of a section of new code extracted from packets.cpp ****************/ /* the LHS, uses p1lhs and p2lhs for smart memory, set above */ printf("start LHS, pd = %#x, ny_left = %d\n", pd, ny_left); /************* start of a section of new code extracted from packets.cpp ****************/ if (bin_flag) { /* general but not very efficient case */ /* also does not do ratio yet in bin mode */ int nyout = nysmem >> fg_obs_p->biny; int n, mm, nn, stride; int acc; //printf("binning LHS\n"); /* note that nyout could be > or < what we need for the packet, if too much we escape with the break below, if not enough we do another round of fgsm.copyIncr */ stride = pixpercolumn - nxbin; /* 4/30/2008 - if we are doing a ratio and binning, we need a different loop, lots of duplication */ if (ratioFlag) { short *p2, *pq2, *pqq2; int rat, denom; int acc2; /* note that pq is signed for the V/I case and the DG's */ /* use the same offset applied to FGsmemBuf2 */ p2 = (short *) ( (char *) FGsmemBuf2 + ( (char *) pbase - (char *) FGsmemBuf ) ); /* pbase is generally I signal, p2 the difference signal */ while (nyout--) { n = nx; pq = pbase; pq2 = p2; while (n--) { mm = nybin; acc = roundbin; acc2 = 0; /* acc2 will be signed so proper rounding is more difficult */ pqq = pq; pqq2 = pq2; while (mm--) { nn = nxbin; while (nn--) { acc += *pqq++; acc2 += *pqq2++; } pqq += stride; pqq2 += stride; } /* we have numerator (top) in acc2 and denominator in acc */ //*pd++ = acc >> totbin; /* subtract the pedestal bias for the LHS */ acc = acc - biasLHS; if (acc <= 0) { rat = 0; } else { rat = (acc2 << 12) / acc; } *pd++ = (short) (rat); /* for a 13 bit signed ratio where 4096 is 1 */ pq = pq + nxbin; pq2 = pq2 + nxbin; } pbase = pbase + nybin * pixpercolumn; p2 = p2 + nybin * pixpercolumn; nysmem = nysmem - nybin; ny_left--; if (ny_left <= 0) { need_data_flag = 0; break; } } } else { /* not a ratio */ while (nyout--) { n = nx; pq = pbase; while (n--) { mm = nybin; acc = roundbin; pqq = pq; while (mm--) { nn = nxbin; while (nn--) { acc += *pqq++; } pqq += stride; } *pd++ = acc >> totbin; pq = pq + nxbin; } pbase = pbase + nybin * pixpercolumn; nysmem = nysmem - nybin; ny_left--; if (ny_left <= 0) { need_data_flag = 0; break; } } } } else { /* we are not binning */ while (nysmem) { /* note that w/o binning, nxp is same for data and smart memory */ int nbytes = nxp * 2; if (ratioFlag) { ratioCopy( (char *) pbase, pdata, nbytes, biasLHS); } else { bcopy( (char *) pbase, pdata, nbytes); } pdata = pdata + nbytes; pbase = pbase + pixpercolumn; ny_left--; nysmem--; if (ny_left <= 0) { need_data_flag = 0; break; } } } /************* end of a section of new code extracted from packets.cpp ****************/ /* the RHS, uses p1lhs and p2lhs for smart memory, set here */ FGsmemBuf = (int *) p1rhs; FGsmemBuf2 = (int *) p2rhs; ny_left = ny; pdata = pout2; pd = (unsigned short *) pdata; printf("start RHS, pd = %#x, ny_left = %d\n", pd, ny_left); pbase = FGsmemBuf; nysmem = smem_read/pixpercolumn; /************* start of a section of new code extracted from packets.cpp ****************/ if (bin_flag) { /* general but not very efficient case */ /* also does not do ratio yet in bin mode */ int nyout = nysmem >> fg_obs_p->biny; int n, mm, nn, stride; int acc; //printf("binning RHS\n"); stride = pixpercolumn - nxbin; /* 4/30/2008 - if we are doing a ratio and binning, we need a different loop, lots of duplication */ if (ratioFlag) { short *p2, *pq2, *pqq2; int rat, denom; int acc2; /* note that pq is signed for the V/I case and the DG's */ /* use the same offset applied to FGsmemBuf2 */ p2 = (short *) ( (char *) FGsmemBuf2 + ( (char *) pbase - (char *) FGsmemBuf ) ); /* pbase is generally I signal, p2 the difference signal */ while (nyout--) { n = nx; pq = pbase; pq2 = p2; while (n--) { mm = nybin; acc = roundbin; acc2 = 0; /* acc2 will be signed so proper rounding is more difficult */ pqq = pq; /* should be I signal */ pqq2 = pq2; while (mm--) { nn = nxbin; while (nn--) { acc += *pqq++; acc2 += *pqq2++; } pqq += stride; pqq2 += stride; } /* we have numerator (top) in acc2 and denominator in acc */ //*pd++ = acc >> totbin; /* subtract the pedestal bias for the RHS */ acc = acc - biasRHS; if (acc <= 0) { rat = 0; } else { rat = (acc2 << 12) / acc; } *pd++ = (short) (rat); /* for a 13 bit signed ratio where 4096 is 1 */ pq = pq + nxbin; pq2 = pq2 + nxbin; } pbase = pbase + nybin * pixpercolumn; p2 = p2 + nybin * pixpercolumn; nysmem = nysmem - nybin; ny_left--; if (ny_left <= 0) { need_data_flag = 0; break; } } } else { /* not a ratio */ while (nyout--) { n = nx; pq = pbase; while (n--) { mm = nybin; acc = roundbin; pqq = pq; while (mm--) { nn = nxbin; while (nn--) { acc += *pqq++; } pqq += stride; } *pd++ = acc >> totbin; pq = pq + nxbin; } pbase = pbase + nybin * pixpercolumn; nysmem = nysmem - nybin; ny_left--; if (ny_left <= 0) { need_data_flag = 0; break; } } } } else { /* we are not binning */ while (nysmem) { /* note that w/o binning, nxp is same for data and smart memory */ int nbytes = nxp * 2; if (ratioFlag) { ratioCopy( (char *) pbase, pdata, nbytes, biasRHS); } else { bcopy( (char *) pbase, pdata, nbytes); } pdata = pdata + nbytes; pbase = pbase + pixpercolumn; ny_left--; nysmem--; if (ny_left <= 0) { need_data_flag = 0; break; } } } /************* end of a section of new code extracted from packets.cpp ****************/ } return result_sym; } /*----------------------------------------------------------------------*/ /* related to FPP status packets and CCSDS files */ FILE *status_fin, *ccsds_fin; #if LINUX | __FreeBSD__ int fsize, filesize(char *name); int ipos = 0; #else off64_t fsize64, filesize64(char *name); off64_t ipos = 0; #endif int status_file, ccsds_file; static char *var1 = {"$PCKTIME"}, *var2 = {"$PCKTYPE"}; int pcktime_sym, pcktype_sym; int pcksize, psize, event_type; /* structures and definitions for status requests and friends */ #define STATUS1_REQUEST 0x01 #define STATUS2_REQUEST 0x02 #define STATUS3_REQUEST 0x03 #define STATUS4_REQUEST 0x04 #define STATUS1_PACKET_ID 0x48 #define STATUS2_PACKET_ID 0x4a #define STATUS3_PACKET_ID 0x4c #define STATUS4_PACKET_ID 0x4e typedef struct { unsigned fg_ready : 1; unsigned fg_id : 7; unsigned sp_ready : 1; unsigned sp_id : 7; unsigned char modulator_position_1; unsigned char modulator_delay_1; unsigned char modulator_position_2; unsigned char modulator_delay_2; unsigned char modulator_position_3; unsigned char modulator_delay_3; } Stat_Block; typedef struct { unsigned char type; unsigned char size[3]; /* packet length in bytes */ } StatusHead; /* status block from status_stuff.c */ struct { Stat_Block s1; /* the 8 bytes in every packet */ char margin[1024]; /* rest of packet, varies. Here we just want to make sure there is enough space */ } spack; /*------------------------------------------------------------------------- */ int get_status_packet() { /* this version is designed to read files, not sockets */ /* gulp down a single status packet, synch first if necessary */ /* this will block until we get one, uses msgsock */ /* the status is put into spack */ int eflag, i, n, ntry, ntrypack, cnt; unsigned char ubuf[4], *p; extern int verify_macro_synch; ntrypack = 100; while (ntrypack--) { /* loop until a good status packet */ ntry = 100000; eflag = 0; i = ntry; while (i--) { /* read should block, so 0 => EOF, probably want to put in a timeout */ cnt = read(status_file, ubuf, 1); if (cnt <= 0) { printf("could not get first byte in file\n"); return 1; } /* check for any one of 8 status packets (4 types and the data monitor bit which can be on or off) */ switch (ubuf[0] & 0xfe ) { case 72: { eflag = 1; break; } case 74: { eflag = 2; break; } case 76: { eflag = 3; break; } case 78: { eflag = 4; break; } default: break; } if (eflag) break; } /* if we never got one, give up */ if ( !eflag) { printf("could not find a valid packet ID after scanning %d bytes\n", ntry); return 1; } printf("eflag = %d\n", eflag); /* read next 3 bytes for size of packet */ //if (dribble(msgsock, ubuf, 3)) return 1; if (read(status_file, ubuf, 3) != 3) { printf("could not read bytes 2-4\n"); return 1; } //printf("bytes 0,1,2 = %d %d %d\n", ubuf[0], ubuf[1], ubuf[2]); psize = ((unsigned int) ubuf[0]) * 65536 + ((unsigned int) ubuf[1]) * 256 + (unsigned int) ubuf[2]; psize = psize & 0x00ffffff; /* make sure psize is reasonable */ /* normal status packets (not mutated) */ /* 6/14/2001 check size against the type for a better filter */ switch (eflag) { case 1: if (psize != 12) psize = 0; break; /* no go if not 12 */ /* note that type 2 and 3 sizes might change! */ /* 8/9/2003 - type 2 is 184 and type 3 is 384 */ case 2: if (psize != 184) psize = 0; break; /* and so forth */ case 3: if (psize != 384) psize = 0; break; /* and so forth */ /* type 4 can be a wide range of sizes */ } if (psize < 12 || psize > 513) { printf("invalid packet size = %d, type was %d\n", psize, eflag); psize = 0; } /* only attempt to read if psize was valid */ pcksize = psize; printf("size = %d\n", psize); if (psize) { psize = psize - 4; /* remaining */ // if (dribble(msgsock, &spack, psize)) return 1; if (read(status_file, &spack, psize) != psize) { printf("could not read file\n"); return 1; } event_type = eflag; return 0; } } /* only get here if we read ntrypack packets with bad sizes */ return 1; } /*----------------------------------------------------------------------*/ int ana_fppclosestatusfile(narg,ps) int narg, ps[]; /* close the current FPP status packet file */ /* a subroutine with no arguments */ { if (status_file) close(status_file); return 1; } /*----------------------------------------------------------------------*/ int ana_fppopenstatusfile(narg,ps) int narg, ps[]; { static char *name; int returnSym, val; /* a function with one argument */ /* opens an FPP status file and does various setups for reading packets */ if ( sym[ ps[0] ].class != 2 ) return execute_error(70); name = (char *) sym[ps[0] ].spec.array.ptr; #if LINUX | __FreeBSD__ fsize = filesize(name); printf("size = %d bytes\n", fsize); #else fsize64 = filesize64(name); printf("size = %lld bytes\n", fsize64); #endif #if __APPLE__ | LINUX if ((status_file = open(name, O_RDONLY)) < 0) { #else if ((status_file = open(name, O_RDONLY|O_LARGEFILE)) < 0) { #endif perror("opening input file"); fprintf(stdout,"can't open input file, name: %s\n", name); } /* define some new globals */ pcktime_sym = find_sym(var1); val = -1; if (redef_scalar( pcktime_sym, 2, &val) != 1) return execute_error(13); pcktype_sym = find_sym(var2); val = -1; if (redef_scalar( pcktype_sym, 2, &val) != 1) return execute_error(13); /* return the size as a double */ returnSym = scalar_scratch(4); #if LINUX | __FreeBSD__ sym[returnSym].spec.scalar.d = (double) fsize; #else sym[returnSym].spec.scalar.d = (double) fsize64; #endif return returnSym; } /*----------------------------------------------------------------------*/ int ana_fppgetstatuspacket(narg,ps) int narg, ps[]; { int result_sym, packet_time; struct ahead *h; char *p; /* reads an FPP status packet from an already open status file */ if ( get_status_packet() ) { printf("problem reading status packet\n"); return -1; } dim[0] = pcksize; printf("pcksize = %d\n", pcksize); result_sym = array_scratch(0, 1, dim); /* for the result */ h = (struct ahead *) sym[result_sym].spec.array.ptr; p = (char *) ((char *)h + sizeof(struct ahead)); bcopy( (char *) &spack, p, pcksize); bcopy( p+8, &packet_time, 4); /* the hard way */ redef_scalar( pcktype_sym, 2, &event_type); redef_scalar( pcktime_sym, 2, &packet_time); return result_sym; } /*----------------------------------------------------------------------*/ /* modules for handling CCSDS packets and combining into FPP packets */ /*----------------------------------------------------------------------*/ static char *ccsdsvar1 = {"$APID"}, *ccsdsvar2 = {"$CCSDSSIZE"}; static char *ccsdsvar3 = {"$CCSDSDATA"}, *ccsdsvar4 = {"$FPPPACKET"}; static char *ccsdsvar5 = {"$CCSDSSCOUNT"}, *ccsdsvar6 = {"$CCSDSSFLAG"}; static char *ccsdsvar7 = {"$FPPSIZE"}, *ccsdsvar8 = {"$FPPCOMP"}; static char *ccsdsvar9 = {"$FPPNX"}, *ccsdsvar10 = {"$FPPNY"}; static char *ccsdsvar11 = {"$FPPNBYTES"}, *ccsdsvar12 = {"$FPPSUBID"}; static char *ccsdsvar13 = {"$FPPCOMPUSED"}, *ccsdsvar14 = {"$FPPHFLAG"}; int apid, ccsdssize, scount, sflag, dataoffset, datasize, ccsdsinit = 1; int fppsize, fppmaxsize, hflag, subid; int apid_sym, ccsdssize_sym, ccsdsdata_sym, fpppacket_sym, fppsize_sym; int scount_sym, sflag_sym, fppcomp_sym, fppnx_sym, fppny_sym, fppnbytes_sym; int fppsubid_sym, fppcompused_sym, fpphflag_sym; char *ccsdsdata, *fpppacket; int ccsdspos = 0, nccsdspacks = 0, ccsdsdebug, uselastFlag = 0; /* the compression_parameters structure is only 2 bytes so it gets expanded to 4 bytes by the compiler. Hence, usage within other structures causes a gap. So beware! */ typedef struct { unsigned reserved_1 : 1; unsigned bit_compression_mode : 4; unsigned image_compression_mode : 3; unsigned reserved_2 : 1; unsigned huffman_ac : 2; unsigned huffman_dc : 2; unsigned quantization : 3; } compression_parameters; compression_parameters comp_used; /*----------------------------------------------------------------------*/ int ana_ccsdsopen(narg,ps) int narg, ps[]; { static char *name; int returnSym, val, dim[8]; /* a function with one argument */ /* opens an CCSDS file and does various setups for reading packets */ if ( sym[ ps[0] ].class != 2 ) return execute_error(70); /* return the size (or some errors) as a double */ returnSym = scalar_scratch(4); name = (char *) sym[ps[0] ].spec.array.ptr; #if LINUX | __FreeBSD__ fsize = filesize(name); printf("size = %d bytes\n", fsize); #else fsize64 = filesize64(name); printf("size = %lld bytes\n", fsize64); #endif /* if we forgot to close a previous one, close it now */ if (ccsds_file) close(ccsds_file); /* reset our counters */ ccsdspos = nccsdspacks = 0; uselastFlag = 0; #if __APPLE__ | LINUX if ((ccsds_file = open(name, O_RDONLY)) < 0) { #else if ((ccsds_file = open(name, O_RDONLY|O_LARGEFILE)) < 0) { #endif perror("opening input file"); fprintf(stdout,"can't open input file, name: %s\n", name); return 2; /* that's a minus 1 */ } /* define a buffer to keep the CCSDS packets and the FPP packet we hope to make from them */ if (ccsdsinit) { apid_sym = find_sym(ccsdsvar1); ccsdssize_sym = find_sym(ccsdsvar2); scount_sym = find_sym(ccsdsvar5); sflag_sym = find_sym(ccsdsvar6); ccsdsdata_sym = find_sym(ccsdsvar3); fpppacket_sym = find_sym(ccsdsvar4); fppsize_sym = find_sym(ccsdsvar7); fppcomp_sym = find_sym(ccsdsvar8); fppnx_sym = find_sym(ccsdsvar9); fppny_sym = find_sym(ccsdsvar10); fppnbytes_sym = find_sym(ccsdsvar11); fppsubid_sym = find_sym(ccsdsvar12); fppcompused_sym = find_sym(ccsdsvar13); fpphflag_sym = find_sym(ccsdsvar14); dim[0] = 2048; if (redef_array( ccsdsdata_sym, 0, 1, dim) != 1) return execute_error(13); dim[0] = fppmaxsize = 131584; if (redef_array( fpppacket_sym, 0, 1, dim) != 1) return execute_error(13); ccsdsinit = 0; } val = -1; apid = ccsdssize = fppsize = scount = sflag = -1; if (redef_scalar( apid_sym, 2, &val) != 1) return execute_error(13); if (redef_scalar( ccsdssize_sym, 2, &val) != 1) return execute_error(13); if (redef_scalar( scount_sym, 2, &val) != 1) return execute_error(13); if (redef_scalar( sflag_sym, 2, &val) != 1) return execute_error(13); if (redef_scalar( fppsize_sym, 2, &val) != 1) return execute_error(13); if (redef_scalar( fppnx_sym, 1, &val) != 1) return execute_error(13); if (redef_scalar( fppny_sym, 1, &val) != 1) return execute_error(13); if (redef_scalar( fppnbytes_sym, 2, &val) != 1) return execute_error(13); if (redef_scalar( fppsubid_sym, 2, &val) != 1) return execute_error(13); val = 0; if (redef_scalar( fpphflag_sym, 2, &val) != 1) return execute_error(13); dim[0] = 8; if (redef_array( fppcomp_sym, 0, 1, dim) != 1) return execute_error(13); dim[0] = 5; if (redef_array( fppcompused_sym, 0, 1, dim) != 1) return execute_error(13); #if LINUX | __FreeBSD__ sym[returnSym].spec.scalar.d = (double) fsize; #else sym[returnSym].spec.scalar.d = (double) fsize64; #endif return returnSym; } /*----------------------------------------------------------------------*/ int ana_ccsdsclose(narg,ps) int narg, ps[]; /* close the current CCSDS packet file */ /* a subroutine with no arguments */ { if (ccsds_file) close(ccsds_file); return 1; } /*----------------------------------------------------------------------*/ int ccsdsread(unsigned char *p) { /* the internal CCSDS reader */ /* get first 6 bytes, this get primary header */ char p0; unsigned int cnt, sheaderflag, plen; cnt = read(ccsds_file, p, 6); if (cnt != 6) { printf("CCSDS read error\n"); return 1; } ccsdspos += cnt; p0 = p[0]; if ( p0 & 0xf0) { printf("top 4 bits not zero, %#x\n", p0); return 1; } /* normally we expect a secondary header for all Solar B packets, but check */ sheaderflag = p0 & 0x08; apid = ((p0 & 0x07) << 8) | (unsigned int) p[1]; plen = ((unsigned int) p[4] << 8) | (unsigned int) p[5]; sflag = ((unsigned int) p[2] & 0xc0) >> 6; scount = ((p[2] & 0x3f) << 8) | (unsigned int) p[3]; if (ccsdsdebug) printf("apid = %#x, plen = %d, sflag = %d, scount = %d, pck #%d, pos %d\n", apid, plen, sflag,scount, nccsdspacks, ccsdspos); /* read the entire packet now */ plen = plen + 1; p = p + 6; cnt = read(ccsds_file, p, plen); if (cnt != plen) { printf("CCSDS read error\n"); return 1; } ccsdspos += cnt; /* the total size */ ccsdssize = plen + 6; /* also want the data position (offset) and the data size */ dataoffset = 6; if (sheaderflag) dataoffset += 4; datasize = ccsdssize - dataoffset; nccsdspacks++; /* another way to consider would be to put the header and the secondary header in their own places and put just the data in ccsdsdata */ return 0; } /*----------------------------------------------------------------------*/ int getCCSDSbuffer() { /* put CCSDS input buffer into ccsdsdata */ /* for the ANA version we use a symbol, we check it in case of tampering and pass the array address */ /* verify the buffer and adjust if it has been changed, we know the symbol */ int class, dim[8], type; int n, nd, i; class = sym[ccsdsdata_sym].class; if (class == ARRAY) { /* we don't really care if the type has been modified by the user, as long as there is enough room */ h = (struct ahead *) sym[ccsdsdata_sym].spec.array.ptr; n = sym[ccsdsdata_sym].spec.array.bstore; //printf("size of $CCSDSDATA is %d\n", n); } else n = 0; if (n < 2048) { /* we have to redefine */ printf("redefining $CCSDSDATA\n"); dim[0] = 2048; if (redef_array( ccsdsdata_sym, 0, 1, dim) != 1) return execute_error(13); } h = (struct ahead *) sym[ccsdsdata_sym].spec.array.ptr; ccsdsdata = (char *) ((char *)h + sizeof(struct ahead)); return 0; } /*----------------------------------------------------------------------*/ int getFPPbuffer() { /* put FPP input buffer into fpppacket */ /* for the ANA version we use a symbol, we check it in case of tampering and pass the array address */ /* verify the buffer and adjust if it has been changed, we know the symbol */ int class, dim[8], type; int n, nd, i; class = sym[fpppacket_sym].class; if (class == ARRAY) { /* we don't really care if the type has been modified by the user, as long as there is enough room */ h = (struct ahead *) sym[fpppacket_sym].spec.array.ptr; n = sym[fpppacket_sym].spec.array.bstore; //nd = h->ndim; //type = sym[fpppacket_sym].type; /* # of elements for nsym */ //n = ana_type_size[type]; for (i=0;idims[i]; //printf("size of $FPPPACKET is %d\n", n); } else n = 0; if (n < 131584) { /* we have to redefine */ printf("redefining $FPPPACKET\n"); dim[0] = fppmaxsize = 131584; if (redef_array( fpppacket_sym, 0, 1, dim) != 1) return execute_error(13); } h = (struct ahead *) sym[fpppacket_sym].spec.array.ptr; fpppacket = (char *) ((char *)h + sizeof(struct ahead)); return 0; } /*----------------------------------------------------------------------*/ int ana_ccsds2fpp(narg,ps) int narg, ps[]; /* a function, no arguments but returns a status, the status is 0 for success */ { int nignored = 0, npack = 0, baseadpid, nc, i, dim[8], nbytes, npad, n; short nx, ny; char *pccsds, *pfpp, *comp, comp_used_expanded[5]; /* uses the internal ccsdsread() */ if (getCCSDSbuffer() ) return 1; if (getFPPbuffer() ) return 1; /* read CCSDS packets, look for a start flag */ /* note that we ignore the single packet case, those are type I's, we may want to pick them up later */ while (1) { if (uselastFlag == 0) if ( ccsdsread(ccsdsdata) ) return 1; if (sflag == 1) { /* only certain AP id's are good here */ if ( (apid & 0x07f0) == 0x140) break; else printf("rejected start for APID %#x", apid); } nignored++; } printf("new start, APID = %#x, # of CCSDS packets skipped = %d\n", apid, nignored); pccsds = ccsdsdata + dataoffset; pfpp = fpppacket; /* first get the 8 byte compression field, this is not part of the original packet */ dim[0] = 8; if (redef_array( fppcomp_sym, 0, 1, dim) != 1) return execute_error(13); h = (struct ahead *) sym[fppcomp_sym].spec.array.ptr; comp = (char *) ((char *)h + sizeof(struct ahead)); bcopy(pccsds, comp, 8); /* the compression field */ pccsds += 8; datasize = datasize - 8; bcopy( pccsds, pfpp, datasize); /* usually the entire packet header is here except for some misunderstanding on the CT packets. But even then, the parts with the size and compression are here */ nbytes = (fpppacket[1] << 16) + (fpppacket[2] << 8) + (fpppacket[3]); bcopy(&fpppacket[26], (char *) &nx, 2); bcopy(&fpppacket[28], (char *) &ny, 2); /* and the compression area is always in bytes 30/31, this is the passed or used */ bcopy(&fpppacket[30], (char *) &comp_used, 2); comp_used_expanded[0] = comp_used.bit_compression_mode; comp_used_expanded[1] = comp_used.image_compression_mode; comp_used_expanded[2] = comp_used.huffman_ac; comp_used_expanded[3] = comp_used.huffman_dc; comp_used_expanded[4] = comp_used.quantization; /* now we can put it into a byte array */ dim[0] = 5; if (redef_array( fppcompused_sym, 0, 1, dim) != 1) return execute_error(13); h = (struct ahead *) sym[fppcompused_sym].spec.array.ptr; comp = (char *) ((char *)h + sizeof(struct ahead)); bcopy(comp_used_expanded, comp, 5); /* the compression field */ if (redef_scalar( fppnx_sym, 1, &nx) != 1) return execute_error(13); if (redef_scalar( fppny_sym, 1, &ny) != 1) return execute_error(13); if (redef_scalar( fppnbytes_sym, 2, &nbytes) != 1) return execute_error(13); if (redef_scalar( apid_sym, 2, &apid) != 1) return execute_error(13); /* also check if a header packet */ hflag = (fpppacket[14] & 0x80) >> 7; hflag = hflag ^ 1; if (hflag) printf("HEADER *******************\n"); subid = fpppacket[15] & 0x1f; if (redef_scalar( fppsubid_sym, 2, &subid) != 1) return execute_error(13); if (redef_scalar( fpphflag_sym, 2, &hflag) != 1) return execute_error(13); npack = 1; fppsize = datasize; pfpp += datasize; baseadpid = apid; nc = scount; uselastFlag = 0; /* and add new packets */ while (1) { /* we always have at least one more since the single packet case is not serviced */ if ( ccsdsread(ccsdsdata) ) return 1; /* we can have others imbedded, reject these and check for ordering. We don't have re-ordering logic yet, not sure if that will be a problem */ if (apid != baseadpid) { /* every now and then, a set is not complete and a new one starts before we see an end packet. Try to handle this, it means that a CCSDS packet read here may be needed for a new start, so we need a way to pass that info */ printf("APID mismatch, apid %#x != %#x, sflag = %d, pck # %d, pos = %d\n", apid, baseadpid, sflag, nccsdspacks, ccsdspos); if ( ((apid & 0x07f0) == 0x140) && sflag == 1) { /* if this is a valid AP ID and a new start, we abandon our incomplete set. The flag tells this code to use this CCSDS packet instead of reading a new one for the next call. */ uselastFlag = 1; printf("truncated a FPP packet\n"); break; } else printf("rejected packet, apid %#x != %#x, sflag = %d, pck # %d, pos = %d\n", apid, baseadpid, sflag, nccsdspacks, ccsdspos); } else { nc = nc + 1; /* new expected scount for a check below */ nc = nc & 0x3fff; if (scount != nc) { printf("CCSDS counter discontinuity, scount %d != %d, pck # %d, pos = %d\n", scount, nc, nccsdspacks, ccsdspos); printf("truncated a FPP packet\n"); /* we can't use this and must truncate, but in case it is a new start, save it */ if (sflag == 1) uselastFlag = 1; break; } else { npack++; pccsds = ccsdsdata + dataoffset; fppsize += datasize; /* the new size */ if (fppsize >= fppmaxsize) { /* this seems to happen with bad compression (?), we just ignore the rest for present so as not to confuse nugu reader */ printf("overflow in ccsds2fpp\n"); bcopy( pccsds, pfpp, fppmaxsize - (fppsize - datasize)); break; } bcopy( pccsds, pfpp, datasize); pfpp += datasize; if (sflag == 2) break; } } } /* must be done, pad up to original packet size with 0's */ npad = nbytes - fppsize; if (npad < 0) { printf("pad size error, nbytes = %d, fppsize = %d, pck # %d, pos = %d\n", nbytes, fppsize, nccsdspacks, ccsdspos); /* for the present, we will assume a bad compression here and truncate the original so we don't confuse the nugu reader */ } if (npad > 0) { bzero(pfpp, npad); } printf("found the end, # of CCSDS packets = %d, FPP size = %d, packet size %d\n", npack, fppsize, nbytes); //printf("compression bytes:"); /* display compression field */ //for (i=0;i<8;i++) printf(" %#x", comp[i]); //printf(", nx = %d, ny = %d, pad = %d\n", nx, ny, nbytes - fppsize); /* set up the user symbols */ h = (struct ahead *) sym[fpppacket_sym].spec.array.ptr; sym[fpppacket_sym].type = BYTE; h->ndim = 1; if (nbytes > fppmaxsize) { printf("too big? %d\n", nbytes); return 1; } h->dims[0] = nbytes; if (redef_scalar( fppsize_sym, 2, &fppsize) != 1) return execute_error(13); return 4; /* 4 is an ana zero (symbol), use for success return */ } /*----------------------------------------------------------------------*/ int ana_ccsdsread(narg,ps) int narg, ps[]; /* a function, no arguments but returns a status, the status is 0 for success */ { int sheaderflag; /* verify the buffer and adjust if it has been changed, we know the symbol */ if (getCCSDSbuffer() ) return 1; if ( ccsdsread((unsigned char *) ccsdsdata) ) return 1; /* set up the user symbols */ if (redef_scalar( apid_sym, 2, &apid) != 1) return execute_error(13); if (redef_scalar( ccsdssize_sym, 2, &ccsdssize) != 1) return execute_error(13); if (redef_scalar( scount_sym, 2, &scount) != 1) return execute_error(13); if (redef_scalar( sflag_sym, 2, &sflag) != 1) return execute_error(13); return 4; /* 4 is an ana zero (symbol), use for success return */ } /*----------------------------------------------------------------------*/