Diff: Plan13.cpp

Revision:

0:78d4704bd4e7

Child:

1:ab43ae956815

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Plan13.cpp	Sun Jan 19 07:44:59 2014 +0000
@@ -0,0 +1,409 @@
+// Code ported from qrptracker
+// https://code.google.com/p/qrptracker/
+
+#include "mbed.h"
+#include "Plan13.h"
+#define DEBUG false
+#define TEST false
+
+
+//Plan13::Plan13() {
+//
+//}
+
+double Plan13::rad(double deg)
+{
+  return (M_PI / 180.0 * deg);
+}
+
+double Plan13::deg(double rad)
+{
+  return (rad * 180.0 / M_PI);
+}
+
+double Plan13::FNatn(double y, double x)
+{
+  float a;
+  if (x != 0)
+  {
+     a = atan(y / x);
+  }
+  else
+  {
+     a = M_PI / 2.0 * sin(y);
+  }
+  if (x < 0)
+  {
+     a = a + M_PI;
+  }
+  if (a < 0)
+  {
+     a = a + 2.0 * M_PI;
+  }
+  return (a);
+}
+
+/* Convert date to day number
+ *
+ * Function returns a general day number from year, month, and day.
+ * Value is (JulianDay - 1721409.5) or (AmsatDay + 722100)
+ *
+ */
+double Plan13::FNday(int year, int month, int day)
+{
+  double JulianDate;
+  if (month <= 2)
+  {
+     year -= 1;
+     month += 12;
+  }
+
+
+ JulianDate = (long)(year * YM) + (int)((month + 1) * 30.6) + (day - 428);
+   return (JulianDate);
+}
+
+
+void Plan13::initSat(void)
+{
+  //readElements(SAT); //now done beforehand
+
+   /* Observer's location */
+  LA = rad( observer_lat );
+  LO = rad( observer_lon );
+  HT = ((float) observer_height)/1000.0; // this needs to be in km
+  CL = cos(LA);
+  SL = sin(LA);
+  CO = cos(LO);
+  SO = sin(LO);
+  /* WGS-84 Earth Ellipsoid */
+  RE = 6378.137;
+  FL = 1.0 / 298.257224;
+
+ /* IAU-76 Earth Ellipsoid */
+ // RE = 6378.140;
+ // FL = 1.0 / 298.257;
+
+  RP = RE * (1.0 - FL);
+  XX = RE * RE;
+  ZZ = RP * RP;
+
+  D = sqrt( XX * CL * CL + ZZ * SL * SL );
+
+  Rx = XX / D + HT;
+  Rz = ZZ / D + HT;
+
+   /* Observer's unit vectors Up EAST and NORTH in geocentric coordinates */
+  Ux = CL * CO;
+  Ex = -SO;
+  Nx = -SL * CO;
+
+  Uy = CL * SO;
+  Ey = CO;
+  Ny = -SL * SO;
+
+  Uz = SL;
+  Ez = 0;
+  Nz = CL;
+
+   /* Observer's XYZ coordinates at earth's surface */
+  Ox = Rx * Ux;
+  Oy = Rx * Uy;
+  Oz = Rz * Uz;
+
+   /* Convert angles to radians, etc. */
+  RA = rad(RA);
+  IN = rad(IN);
+  WP = rad(WP);
+  MA = rad(MA);
+  MM = MM * 2.0 * M_PI;
+  M2 = M2 * 2.0 * M_PI;
+
+//  YM = 365.25;         /* Mean year, days                           */
+//  YT = 365.2421970;            /* Tropical year, days                       */
+  WW = 2.0 * M_PI / YT;                /* Earth's rotation rate, rads/whole day     */
+  WE = 2.0 * M_PI + WW;                /* Earth's rotation rate, rads/day           */
+  W0 = WE / 86400;             /* Earth's rotation rate, rads/sec           */
+
+   /* Observer's velocity, geocentric coordinates */
+  VOx = -Oy * W0;
+  VOy = Ox * W0;
+
+   /* Convert satellite epoch to Day No. and fraction of a day */
+  DE = FNday(YE, 1, 0) + (int)TE;
+
+  TE = TE - (int)TE;
+   /* Average Precession rates */
+  GM = 3.986E5;               /* Earth's gravitational constant km^3/s^2   */
+  J2 = 1.08263E-3;            /* 2nd Zonal coeff, Earth's gravity Field    */
+  N0 = MM / 86400.0;                    /* Mean motion rads/s                */
+  AU = pow((float)GM / N0 / N0, (float)1.0 / 3.0);  /* Semi major axis km                */
+  b0 = AU * sqrt(1.0 - EC * EC);      /* Semi minor axis km                */
+  SI = sin(IN);
+  CI = cos(IN);
+  PC = RE * AU / (b0 * b0);
+  PC = 1.5 * J2 * PC * PC * MM;       /* Precession const, rad/day         */
+  QD = -PC * CI;                      /* Node Precession rate, rad/day     */
+  WD = PC *(5.0 * CI*CI - 1.0) / 2.0; /* Perigee Precession rate, rad/day  */
+  DC = -2.0 * M2 / MM / 3.0;          /* Drag coeff                        */
+
+   /* Sideral and solar data. Never needs changing. Valid to year 2000+ */
+
+
+               /* GHAA, Year YG, Jan 0.0 */
+ YG = 2014;
+ G0 = 99.5578;
+  /* MA Sun and rate, deg, deg/day */
+ MAS0 = 356.4485;
+ MASD = 0.98560028;
+  /* Sun's inclination */
+ INS = rad(23.4380);
+ CNS = cos(INS);
+ SNS = sin(INS);
+  /* Sun's equation of center terms */
+ EQC1 = 0.03341;
+ EQC2 = 0.00035;
+
+
+   /* Bring Sun data to satellite epoch */
+  TEG = (DE - FNday(YG, 1, 0)) + TE;  /* Elapsed Time: Epoch - YG          */
+  GHAE = rad(G0) + TEG * WE;          /* GHA Aries, epoch                  */
+  MRSE = rad(G0) + (TEG * WW) + M_PI;   /* Mean RA Sun at Sat Epoch          */
+  MASE = rad(MAS0 + MASD * TEG);      /* Mean MA Sun                       */
+
+   /* Antenna unit vector in orbit plane coordinates */
+  CO = cos(rad(ALON));
+  SO = sin(rad(ALON));
+  CL = cos(rad(ALAT));
+  SL = sin(rad(ALAT));
+  ax = -CL * CO;
+  ay = -CL * SO;
+  az = -SL;
+
+   /* Miscellaneous */
+  OLDRN = -99999;
+}
+/*
+ * Calculate satellite position at DN, TN
+ */
+void Plan13::satvec()
+{
+  T = (DN - DE) + (TN - TE);//83.848 ;          /* Elapsed T since epoch             */
+  DT = DC * T / 2.0;                  /* Linear drag terms                 */
+  KD = 1.0 + 4.0 * DT;
+  KDP = 1.0 - 7.0 * DT;
+  M = MA + MM * T * (1.0 - 3.0 * DT); /* Mean anomaly at YR,/ TN           */
+  DR = (int)(M / (2.0 * M_PI));         /* Strip out whole no of revs        */
+  M = M - DR * 2.0 * M_PI;              /* M now in range 0 - 2PI            */
+  RN = RV + DR + 1;                   /* Current orbit number              */
+
+   /* Solve M = EA - EC * sin(EA) for EA given M, by Newton's method        */
+  EA = M;                             /* Initail solution                  */
+  do
+  {
+     C = cos(EA);
+     S = sin(EA);
+     DNOM = 1.0 - EC * C;
+     D = (EA - EC * S - M) / DNOM; /* Change EA to better resolution    */
+     EA = EA - D;                    /* by this amount until converged    */
+  }
+  while (fabs(D) > 1.0E-5);
+
+   /* Distances */
+  A = AU * KD;
+  B = b0 * KD;
+  RS = A * DNOM;
+
+   /* Calculate satellite position and velocity in plane of ellipse */
+  Sx = A * (C - EC);
+  Vx = -A * S / DNOM * N0;
+  Sy = B * S;
+  Vy = B * C / DNOM * N0;
+
+  AP = WP + WD * T * KDP;
+  CW = cos(AP);
+  SW = sin(AP);
+  RAAN = RA + QD * T * KDP;
+  CQ = cos(RAAN);
+  SQ = sin(RAAN);
+
+   /* Plane -> celestial coordinate transformation, [C] = [RAAN]*[IN]*[AP] */
+  CXx = CW * CQ - SW * CI * SQ;
+  CXy = -SW * CQ - CW * CI * SQ;
+  CXz = SI * SQ;
+  CYx = CW * SQ + SW * CI * CQ;
+  CYy = -SW * SQ + CW * CI * CQ;
+  CYz = -SI * CQ;
+  CZx = SW * SI;
+  CZy = CW * SI;
+  CZz = CI;
+
+   /* Compute satellite's position vector, ANTenna axis unit vector    */
+   /*   and velocity  in celestial coordinates. (Note: Sz = 0, Vz = 0) */
+  SATx = Sx * CXx + Sy * CXy;
+  ANTx = ax * CXx + ay * CXy + az * CXz;
+  VELx = Vx * CXx + Vy * CXy;
+  SATy = Sx * CYx + Sy * CYy;
+  ANTy = ax * CYx + ay * CYy + az * CYz;
+  VELy = Vx * CYx + Vy * CYy;
+  SATz = Sx * CZx + Sy * CZy;
+  ANTz = ax * CZx + ay * CZy + az * CZz;
+  VELz = Vx * CZx + Vy * CZy;
+
+   /* Also express SAT, ANT, and VEL in geocentric coordinates */
+  GHAA = GHAE + WE * T;       /* GHA Aries at elaprsed time T */
+  C = cos(-GHAA);
+  S = sin(-GHAA);
+  Sx = SATx * C - SATy * S;
+  Ax = ANTx * C - ANTy * S;
+  Vx = VELx * C - VELy * S;
+  Sy = SATx * S + SATy * C;
+  Ay = ANTx * S + ANTy * C;
+  Vy = VELx * S + VELy * C;
+  Sz = SATz;
+  Az = ANTz;
+  Vz = VELz;
+}
+
+/*
+ * Compute and manipulate range/velocity/antenna vectors
+ */
+void Plan13::rangevec(void)
+{
+   /* Range vector = sat vector - observer vector */
+  Rx = Sx - Ox;
+  Ry = Sy - Oy;
+  Rz = Sz - Oz;
+
+  R = sqrt(Rx * Rx + Ry * Ry + Rz * Rz);    /* Range Magnitute */
+
+   /* Normalize range vector */
+  Rx = Rx / R;
+  Ry = Ry / R;
+  Rz = Rz / R;
+  U = Rx * Ux + Ry * Uy + Rz * Uz;
+  E = Rx * Ex + Ry * Ey;
+  N = Rx * Nx + Ry * Ny + Rz * Nz;
+
+  AZ = deg(FNatn(E, N));
+  EL = deg(asin(U));
+
+   /* Solve antenna vector along unit range vector, -r.a = cos(SQ) */
+/*    SQ = deg(acos(-(Ax * Rx + Ay * Ry + Az * Rz)));
+*/
+   /* Calculate sub-satellite Lat/Lon */
+  SLON = deg(FNatn(Sy, Sx));   /* Lon, + East  */
+  SLAT = deg(asin(Sz / RS));   /* Lat, + North */
+
+   /* Resolve Sat-Obs velocity vector along unit range vector. (VOz = 0) */
+  RR = (Vx - VOx) * Rx + (Vy - VOy) * Ry + Vz * Rz; /* Range rate, km/sec */
+  //FR = rxFrequency * (1 - RR / 299792);
+dopplerFactor = RR / 299792.0;
+int rxDoppler = getDoppler(rxFrequencyLong);
+int txDoppler = getDoppler(txFrequencyLong);
+rxOutLong = rxFrequencyLong - rxDoppler;
+txOutLong = txFrequencyLong + txDoppler;
+}
+
+void Plan13::sunvec(void)
+{
+    //TODO
+}
+
+
+int Plan13::getDoppler(unsigned long freq) {
+  freq = (freq + 50000L) / 100000L;
+  long factor = dopplerFactor * 1E11;
+  int digit;
+  float tally = 0;
+  for (int x = 4; x > -1; x--) {
+    digit = freq/pow((float)10,(float)x);
+    long bare = digit * pow((float)10,(float)x);
+    freq = freq - bare;
+    float inBetween =  factor * (float(bare) / 1E6);
+    tally += inBetween;
+}
+return int( tally + .5); //round
+}
+
+int Plan13::getDoppler64(unsigned long freq) {
+    long factor = dopplerFactor * 1E11;
+    uint64_t doppler_sixfour = freq * dopplerFactor;
+    return (int) doppler_sixfour/1E11;
+}
+
+
+void Plan13::setFrequency(unsigned long rxFrequency_in, unsigned long txFrequency_in) {
+    rxFrequencyLong = rxFrequency_in;
+    txFrequencyLong = txFrequency_in;
+    if (TEST) {
+        rxFrequencyLong = 435300000L;
+        txFrequencyLong = 45920000L;
+    }
+}
+
+void Plan13::setLocation(double observer_lon_in, double observer_lat_in, int height) {
+    observer_lon = observer_lon_in;//-64.375; //0.06; // lon east is positive, west is negative
+    observer_lat = observer_lat_in;//45.8958; //52.21; //Cambridge UK
+    observer_height = height; //60m height in meters
+}
+
+void Plan13::setTime(int yearIn, int monthIn, int mDayIn, int hourIn, int minIn, int secIn) {
+     int aYear = yearIn;
+     int aMonth = monthIn;
+     int aMday = mDayIn;
+     int aHour = hourIn;
+     int aMin  = minIn;
+     int aSec  = secIn;
+     DN = FNday(aYear, aMonth, aMday);
+     TN = ((float)aHour + ((float)aMin + ((float)aSec/60.0)) /60.0)/24.0;
+     DN = (long)DN;
+}
+ void   Plan13::setElements(double YE_in, double TE_in, double IN_in, double
+         RA_in, double EC_in, double WP_in, double MA_in, double MM_in,
+         double M2_in, double RV_in, double ALON_in ) {
+
+            YE = YE_in;
+            TE = TE_in;
+            IN = IN_in;
+            RA = RA_in;
+            EC = EC_in;
+            WP = WP_in;
+            MA = MA_in;
+            MM = MM_in;
+            M2 = M2_in;
+            RV = RV_in;
+            ALON = ALON_in;
+}
+
+  void  Plan13::calculate() {
+        initSat();
+        satvec();
+        rangevec();
+   }
+
+float  *Plan13::footprintOctagon(float slat, float slon) {
+    static float points[16];
+    float srad = acos(RE/RS);
+    float cla= cos(slat);
+    float sla = sin(slat);
+    float clo = cos(slon);
+    float slo = sin(slon);
+    float sra = sin(srad);
+    float cra = cos(srad);
+    for (int i = 0; i < 16; i = i +2) {
+        float a = 2 * M_PI * i / 8;
+        float X = cra;
+        float Y = sra*sin(a);
+        float Z = sra*cos(a);
+        float x = X*cla - Z*sla;
+        float y = Y;
+        float z = X*sla + Z*cla;
+        X = x*clo - y*slo;
+        Y = x*slo + y*clo;
+        Z = z;
+        points[i] = FNatn(Y,X);
+        points[i+1] = asin(Z);
+    }
+    return points;
+}
\ No newline at end of file