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!
Plan13.cpp
- Committer:
- SamClarke
- Date:
- 2014-01-20
- Revision:
- 5:8af1b8452986
- Parent:
- 1:ab43ae956815
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; }