Plan13.cpp

00001 // Code ported from qrptracker
00002 // https://code.google.com/p/qrptracker/
00003 
00004 #include "mbed.h"
00005 #include "Plan13.h"
00006 #define DEBUG false
00007 #define TEST false
00008 
00009 
00010 //Plan13::Plan13() {
00011 //
00012 //}
00013 
00014 double Plan13::rad(double deg)
00015 {
00016   return (M_PI / 180.0 * deg);
00017 }
00018 
00019 double Plan13::deg(double rad)
00020 {
00021   return (rad * 180.0 / M_PI);
00022 }
00023 
00024 double Plan13::FNatn(double y, double x)
00025 {
00026   float a;
00027   if (x != 0)
00028   {
00029      a = atan(y / x);
00030   }
00031   else
00032   {
00033      a = M_PI / 2.0 * sin(y);
00034   }
00035   if (x < 0)
00036   {
00037      a = a + M_PI;
00038   }
00039   if (a < 0)
00040   {
00041      a = a + 2.0 * M_PI;
00042   }
00043   return (a);
00044 }
00045 
00046 /* Convert date to day number
00047  *
00048  * Function returns a general day number from year, month, and day.
00049  * Value is (JulianDay - 1721409.5) or (AmsatDay + 722100)
00050  *
00051  */
00052 double Plan13::FNday(int year, int month, int day)
00053 {
00054   double JulianDate;
00055   if (month <= 2)
00056   {
00057      year -= 1;
00058      month += 12;
00059   }
00060 
00061 
00062  JulianDate = (long)(year * YM) + (int)((month + 1) * 30.6) + (day - 428);
00063    return (JulianDate);
00064 }
00065 
00066 
00067 void Plan13::initSat(void)
00068 {
00069   //readElements(SAT); //now done beforehand
00070 
00071    /* Observer's location */
00072   LA = rad( observer_lat );
00073   LO = rad( observer_lon );
00074   HT = ((float) observer_height)/1000.0; // this needs to be in km
00075   CL = cos(LA);
00076   SL = sin(LA);
00077   CO = cos(LO);
00078   SO = sin(LO);
00079   /* WGS-84 Earth Ellipsoid */
00080   RE = 6378.137;
00081   FL = 1.0 / 298.257224;
00082 
00083  /* IAU-76 Earth Ellipsoid */
00084  // RE = 6378.140;
00085  // FL = 1.0 / 298.257;
00086 
00087   RP = RE * (1.0 - FL);
00088   XX = RE * RE;
00089   ZZ = RP * RP;
00090 
00091   D = sqrt( XX * CL * CL + ZZ * SL * SL );
00092 
00093   Rx = XX / D + HT;
00094   Rz = ZZ / D + HT;
00095 
00096    /* Observer's unit vectors Up EAST and NORTH in geocentric coordinates */
00097   Ux = CL * CO;
00098   Ex = -SO;
00099   Nx = -SL * CO;
00100 
00101   Uy = CL * SO;
00102   Ey = CO;
00103   Ny = -SL * SO;
00104 
00105   Uz = SL;
00106   Ez = 0;
00107   Nz = CL;
00108 
00109    /* Observer's XYZ coordinates at earth's surface */
00110   Ox = Rx * Ux;
00111   Oy = Rx * Uy;
00112   Oz = Rz * Uz;
00113 
00114    /* Convert angles to radians, etc. */
00115   RA = rad(RA);
00116   IN = rad(IN);
00117   WP = rad(WP);
00118   MA = rad(MA);
00119   MM = MM * 2.0 * M_PI;
00120   M2 = M2 * 2.0 * M_PI;
00121 
00122 //  YM = 365.25;         /* Mean year, days                           */
00123 //  YT = 365.2421970;            /* Tropical year, days                       */
00124   WW = 2.0 * M_PI / YT;                /* Earth's rotation rate, rads/whole day     */
00125   WE = 2.0 * M_PI + WW;                /* Earth's rotation rate, rads/day           */
00126   W0 = WE / 86400;             /* Earth's rotation rate, rads/sec           */
00127 
00128    /* Observer's velocity, geocentric coordinates */
00129   VOx = -Oy * W0;
00130   VOy = Ox * W0;
00131 
00132    /* Convert satellite epoch to Day No. and fraction of a day */
00133   DE = FNday(YE, 1, 0) + (int)TE;
00134 
00135   TE = TE - (int)TE;
00136    /* Average Precession rates */
00137   GM = 3.986E5;               /* Earth's gravitational constant km^3/s^2   */
00138   J2 = 1.08263E-3;            /* 2nd Zonal coeff, Earth's gravity Field    */
00139   N0 = MM / 86400.0;                    /* Mean motion rads/s                */
00140   AU = pow((float)GM / N0 / N0, (float)1.0 / 3.0);  /* Semi major axis km                */
00141   b0 = AU * sqrt(1.0 - EC * EC);      /* Semi minor axis km                */
00142   SI = sin(IN);
00143   CI = cos(IN);
00144   PC = RE * AU / (b0 * b0);
00145   PC = 1.5 * J2 * PC * PC * MM;       /* Precession const, rad/day         */
00146   QD = -PC * CI;                      /* Node Precession rate, rad/day     */
00147   WD = PC *(5.0 * CI*CI - 1.0) / 2.0; /* Perigee Precession rate, rad/day  */
00148   DC = -2.0 * M2 / MM / 3.0;          /* Drag coeff                        */
00149 
00150    /* Sideral and solar data. Never needs changing. Valid to year 2000+ */
00151 
00152 
00153                /* GHAA, Year YG, Jan 0.0 */
00154  YG = 2014;
00155  G0 = 99.5578;
00156   /* MA Sun and rate, deg, deg/day */
00157  MAS0 = 356.4485;
00158  MASD = 0.98560028;
00159   /* Sun's inclination */
00160  INS = rad(23.4380);
00161  CNS = cos(INS);
00162  SNS = sin(INS);
00163   /* Sun's equation of center terms */
00164  EQC1 = 0.03341;
00165  EQC2 = 0.00035;
00166 
00167 
00168    /* Bring Sun data to satellite epoch */
00169   TEG = (DE - FNday(YG, 1, 0)) + TE;  /* Elapsed Time: Epoch - YG          */
00170   GHAE = rad(G0) + TEG * WE;          /* GHA Aries, epoch                  */
00171   MRSE = rad(G0) + (TEG * WW) + M_PI;   /* Mean RA Sun at Sat Epoch          */
00172   MASE = rad(MAS0 + MASD * TEG);      /* Mean MA Sun                       */
00173 
00174    /* Antenna unit vector in orbit plane coordinates */
00175   CO = cos(rad(ALON));
00176   SO = sin(rad(ALON));
00177   CL = cos(rad(ALAT));
00178   SL = sin(rad(ALAT));
00179   ax = -CL * CO;
00180   ay = -CL * SO;
00181   az = -SL;
00182 
00183    /* Miscellaneous */
00184   OLDRN = -99999;
00185 }
00186 /*
00187  * Calculate satellite position at DN, TN
00188  */
00189 void Plan13::satvec()
00190 {
00191   T = (DN - DE) + (TN - TE);//83.848 ;          /* Elapsed T since epoch             */
00192   DT = DC * T / 2.0;                  /* Linear drag terms                 */
00193   KD = 1.0 + 4.0 * DT;
00194   KDP = 1.0 - 7.0 * DT;
00195   M = MA + MM * T * (1.0 - 3.0 * DT); /* Mean anomaly at YR,/ TN           */
00196   DR = (int)(M / (2.0 * M_PI));         /* Strip out whole no of revs        */
00197   M = M - DR * 2.0 * M_PI;              /* M now in range 0 - 2PI            */
00198   RN = RV + DR + 1;                   /* Current orbit number              */
00199 
00200    /* Solve M = EA - EC * sin(EA) for EA given M, by Newton's method        */
00201   EA = M;                             /* Initail solution                  */
00202   do
00203   {
00204      C = cos(EA);
00205      S = sin(EA);
00206      DNOM = 1.0 - EC * C;
00207      D = (EA - EC * S - M) / DNOM; /* Change EA to better resolution    */
00208      EA = EA - D;                    /* by this amount until converged    */
00209   }
00210   while (fabs(D) > 1.0E-5);
00211 
00212    /* Distances */
00213   A = AU * KD;
00214   B = b0 * KD;
00215   RS = A * DNOM;
00216 
00217    /* Calculate satellite position and velocity in plane of ellipse */
00218   Sx = A * (C - EC);
00219   Vx = -A * S / DNOM * N0;
00220   Sy = B * S;
00221   Vy = B * C / DNOM * N0;
00222 
00223   AP = WP + WD * T * KDP;
00224   CW = cos(AP);
00225   SW = sin(AP);
00226   RAAN = RA + QD * T * KDP;
00227   CQ = cos(RAAN);
00228   SQ = sin(RAAN);
00229 
00230    /* Plane -> celestial coordinate transformation, [C] = [RAAN]*[IN]*[AP] */
00231   CXx = CW * CQ - SW * CI * SQ;
00232   CXy = -SW * CQ - CW * CI * SQ;
00233   CXz = SI * SQ;
00234   CYx = CW * SQ + SW * CI * CQ;
00235   CYy = -SW * SQ + CW * CI * CQ;
00236   CYz = -SI * CQ;
00237   CZx = SW * SI;
00238   CZy = CW * SI;
00239   CZz = CI;
00240 
00241    /* Compute satellite's position vector, ANTenna axis unit vector    */
00242    /*   and velocity  in celestial coordinates. (Note: Sz = 0, Vz = 0) */
00243   SATx = Sx * CXx + Sy * CXy;
00244   ANTx = ax * CXx + ay * CXy + az * CXz;
00245   VELx = Vx * CXx + Vy * CXy;
00246   SATy = Sx * CYx + Sy * CYy;
00247   ANTy = ax * CYx + ay * CYy + az * CYz;
00248   VELy = Vx * CYx + Vy * CYy;
00249   SATz = Sx * CZx + Sy * CZy;
00250   ANTz = ax * CZx + ay * CZy + az * CZz;
00251   VELz = Vx * CZx + Vy * CZy;
00252 
00253    /* Also express SAT, ANT, and VEL in geocentric coordinates */
00254   GHAA = GHAE + WE * T;       /* GHA Aries at elaprsed time T */
00255   C = cos(-GHAA);
00256   S = sin(-GHAA);
00257   Sx = SATx * C - SATy * S;
00258   Ax = ANTx * C - ANTy * S;
00259   Vx = VELx * C - VELy * S;
00260   Sy = SATx * S + SATy * C;
00261   Ay = ANTx * S + ANTy * C;
00262   Vy = VELx * S + VELy * C;
00263   Sz = SATz;
00264   Az = ANTz;
00265   Vz = VELz;
00266 }
00267 
00268 /*
00269  * Compute and manipulate range/velocity/antenna vectors
00270  */
00271 void Plan13::rangevec(void)
00272 {
00273    /* Range vector = sat vector - observer vector */
00274   Rx = Sx - Ox;
00275   Ry = Sy - Oy;
00276   Rz = Sz - Oz;
00277 
00278   R = sqrt(Rx * Rx + Ry * Ry + Rz * Rz);    /* Range Magnitute */
00279 
00280    /* Normalize range vector */
00281   Rx = Rx / R;
00282   Ry = Ry / R;
00283   Rz = Rz / R;
00284   U = Rx * Ux + Ry * Uy + Rz * Uz;
00285   E = Rx * Ex + Ry * Ey;
00286   N = Rx * Nx + Ry * Ny + Rz * Nz;
00287 
00288   AZ = deg(FNatn(E, N));
00289   EL = deg(asin(U));
00290 
00291    /* Solve antenna vector along unit range vector, -r.a = cos(SQ) */
00292 /*    SQ = deg(acos(-(Ax * Rx + Ay * Ry + Az * Rz)));
00293 */
00294    /* Calculate sub-satellite Lat/Lon */
00295   SLON = deg(FNatn(Sy, Sx));   /* Lon, + East  */
00296   SLAT = deg(asin(Sz / RS));   /* Lat, + North */
00297 
00298    /* Resolve Sat-Obs velocity vector along unit range vector. (VOz = 0) */
00299   RR = (Vx - VOx) * Rx + (Vy - VOy) * Ry + Vz * Rz; /* Range rate, km/sec */
00300   //FR = rxFrequency * (1 - RR / 299792);
00301 dopplerFactor = RR / 299792.0;
00302 int rxDoppler = getDoppler(rxFrequencyLong);
00303 int txDoppler = getDoppler(txFrequencyLong);
00304 rxOutLong = rxFrequencyLong - rxDoppler;
00305 txOutLong = txFrequencyLong + txDoppler;
00306 }
00307 
00308 void Plan13::sunvec(void)
00309 {
00310     //TODO
00311 }
00312 
00313 
00314 int Plan13::getDoppler(unsigned long freq) {
00315   freq = (freq + 50000L) / 100000L;
00316   long factor = dopplerFactor * 1E11;
00317   int digit;
00318   float tally = 0;
00319   for (int x = 4; x > -1; x--) {
00320     digit = freq/pow((float)10,(float)x);
00321     long bare = digit * pow((float)10,(float)x);
00322     freq = freq - bare;
00323     float inBetween =  factor * (float(bare) / 1E6);
00324     tally += inBetween;
00325 }
00326 return int( tally + .5); //round
00327 }
00328 
00329 int Plan13::getDoppler64(unsigned long freq) {
00330     //long factor = dopplerFactor * 1E11;
00331     uint64_t doppler_sixfour = freq * dopplerFactor;
00332     return (int) doppler_sixfour/1E11;
00333 }
00334 
00335 
00336 void Plan13::setFrequency(unsigned long rxFrequency_in, unsigned long txFrequency_in) {
00337     rxFrequencyLong = rxFrequency_in;
00338     txFrequencyLong = txFrequency_in;
00339     if (TEST) {
00340         rxFrequencyLong = 435300000L;
00341         txFrequencyLong = 45920000L;
00342     }
00343 }
00344 
00345 void Plan13::setLocation(double observer_lon_in, double observer_lat_in, int height) {
00346     observer_lon = observer_lon_in;//-64.375; //0.06; // lon east is positive, west is negative
00347     observer_lat = observer_lat_in;//45.8958; //52.21; //Cambridge UK
00348     observer_height = height; //60m height in meters
00349 }
00350 
00351 void Plan13::setTime(int yearIn, int monthIn, int mDayIn, int hourIn, int minIn, int secIn) {
00352      int aYear = yearIn;
00353      int aMonth = monthIn;
00354      int aMday = mDayIn;
00355      int aHour = hourIn;
00356      int aMin  = minIn;
00357      int aSec  = secIn;
00358      DN = FNday(aYear, aMonth, aMday);
00359      TN = ((float)aHour + ((float)aMin + ((float)aSec/60.0)) /60.0)/24.0;
00360      DN = (long)DN;
00361 }
00362  void   Plan13::setElements(double YE_in, double TE_in, double IN_in, double
00363          RA_in, double EC_in, double WP_in, double MA_in, double MM_in,
00364          double M2_in, double RV_in, double ALON_in ) {
00365 
00366             YE = YE_in;
00367             TE = TE_in;
00368             IN = IN_in;
00369             RA = RA_in;
00370             EC = EC_in;
00371             WP = WP_in;
00372             MA = MA_in;
00373             MM = MM_in;
00374             M2 = M2_in;
00375             RV = RV_in;
00376             ALON = ALON_in;
00377 }
00378 
00379   void  Plan13::calculate() {
00380         initSat();
00381         satvec();
00382         rangevec();
00383    }
00384 
00385 float  *Plan13::footprintOctagon(float slat, float slon) {
00386     static float points[16];
00387     float srad = acos(RE/RS);
00388     float cla= cos(slat);
00389     float sla = sin(slat);
00390     float clo = cos(slon);
00391     float slo = sin(slon);
00392     float sra = sin(srad);
00393     float cra = cos(srad);
00394     for (int i = 0; i < 16; i = i +2) {
00395         float a = 2 * M_PI * i / 8;
00396         float X = cra;
00397         float Y = sra*sin(a);
00398         float Z = sra*cos(a);
00399         float x = X*cla - Z*sla;
00400         float y = Y;
00401         float z = X*sla + Z*cla;
00402         X = x*clo - y*slo;
00403         Y = x*slo + y*clo;
00404         Z = z;
00405         points[i] = FNatn(Y,X);
00406         points[i+1] = asin(Z);
00407     }
00408     return points;
00409 }