Satellite tracking for the mbed platform(s). Note: This software is beerware.
Satellite Tracking with mbed!
This is a quick example of tracking satellites with the mbed platform. Ensure you have the latest TLE information and map it correctly. I have commented in the original TLEs used for the example to help you do this.
This code is not restricted to geostationary/geosynchronous satellites, you can track any satellite for which you can get a TLE. (Which is almost all non military satellites).
See http://www.celestrak.com/NORAD/elements/ http://en.wikipedia.org/wiki/Two-line_element_set
If you use this code, please send me a beer (see main.cpp header). Enjoy!
Diff: Plan13.cpp
- Revision:
- 0:78d4704bd4e7
- Child:
- 1:ab43ae956815
diff -r 000000000000 -r 78d4704bd4e7 Plan13.cpp --- /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