File content as of revision 5:8af1b8452986:

// 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;
}