Sam Clarke / Mbed 2 deprecated Plan13

Satellite tracking for the mbed platform(s). Note: This software is beerware.

Dependencies:   GPS mbed

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@5:8af1b8452986, 2014-01-20 (annotated)

Committer:
SamClarke
Date:
Mon Jan 20 21:25:27 2014 +0000
Revision:
5:8af1b8452986
Parent:
1:ab43ae956815 
tidy

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SamClarke 0:78d4704bd4e7 1 // Code ported from qrptracker
SamClarke 0:78d4704bd4e7 2 // https://code.google.com/p/qrptracker/
SamClarke 0:78d4704bd4e7 3
SamClarke 0:78d4704bd4e7 4 #include "mbed.h"
SamClarke 0:78d4704bd4e7 5 #include "Plan13.h"
SamClarke 0:78d4704bd4e7 6 #define DEBUG false
SamClarke 0:78d4704bd4e7 7 #define TEST false
SamClarke 0:78d4704bd4e7 8
SamClarke 0:78d4704bd4e7 9
SamClarke 0:78d4704bd4e7 10 //Plan13::Plan13() {
SamClarke 0:78d4704bd4e7 11 //
SamClarke 0:78d4704bd4e7 12 //}
SamClarke 0:78d4704bd4e7 13
SamClarke 0:78d4704bd4e7 14 double Plan13::rad(double deg)
SamClarke 0:78d4704bd4e7 15 {
SamClarke 0:78d4704bd4e7 16 return (M_PI / 180.0 * deg);
SamClarke 0:78d4704bd4e7 17 }
SamClarke 0:78d4704bd4e7 18
SamClarke 0:78d4704bd4e7 19 double Plan13::deg(double rad)
SamClarke 0:78d4704bd4e7 20 {
SamClarke 0:78d4704bd4e7 21 return (rad * 180.0 / M_PI);
SamClarke 0:78d4704bd4e7 22 }
SamClarke 0:78d4704bd4e7 23
SamClarke 0:78d4704bd4e7 24 double Plan13::FNatn(double y, double x)
SamClarke 0:78d4704bd4e7 25 {
SamClarke 0:78d4704bd4e7 26 float a;
SamClarke 0:78d4704bd4e7 27 if (x != 0)
SamClarke 0:78d4704bd4e7 28 {
SamClarke 0:78d4704bd4e7 29 a = atan(y / x);
SamClarke 0:78d4704bd4e7 30 }
SamClarke 0:78d4704bd4e7 31 else
SamClarke 0:78d4704bd4e7 32 {
SamClarke 0:78d4704bd4e7 33 a = M_PI / 2.0 * sin(y);
SamClarke 0:78d4704bd4e7 34 }
SamClarke 0:78d4704bd4e7 35 if (x < 0)
SamClarke 0:78d4704bd4e7 36 {
SamClarke 0:78d4704bd4e7 37 a = a + M_PI;
SamClarke 0:78d4704bd4e7 38 }
SamClarke 0:78d4704bd4e7 39 if (a < 0)
SamClarke 0:78d4704bd4e7 40 {
SamClarke 0:78d4704bd4e7 41 a = a + 2.0 * M_PI;
SamClarke 0:78d4704bd4e7 42 }
SamClarke 0:78d4704bd4e7 43 return (a);
SamClarke 0:78d4704bd4e7 44 }
SamClarke 0:78d4704bd4e7 45
SamClarke 0:78d4704bd4e7 46 /* Convert date to day number
SamClarke 0:78d4704bd4e7 47 *
SamClarke 0:78d4704bd4e7 48 * Function returns a general day number from year, month, and day.
SamClarke 0:78d4704bd4e7 49 * Value is (JulianDay - 1721409.5) or (AmsatDay + 722100)
SamClarke 0:78d4704bd4e7 50 *
SamClarke 0:78d4704bd4e7 51 */
SamClarke 0:78d4704bd4e7 52 double Plan13::FNday(int year, int month, int day)
SamClarke 0:78d4704bd4e7 53 {
SamClarke 0:78d4704bd4e7 54 double JulianDate;
SamClarke 0:78d4704bd4e7 55 if (month <= 2)
SamClarke 0:78d4704bd4e7 56 {
SamClarke 0:78d4704bd4e7 57 year -= 1;
SamClarke 0:78d4704bd4e7 58 month += 12;
SamClarke 0:78d4704bd4e7 59 }
SamClarke 0:78d4704bd4e7 60
SamClarke 0:78d4704bd4e7 61
SamClarke 0:78d4704bd4e7 62 JulianDate = (long)(year * YM) + (int)((month + 1) * 30.6) + (day - 428);
SamClarke 0:78d4704bd4e7 63 return (JulianDate);
SamClarke 0:78d4704bd4e7 64 }
SamClarke 0:78d4704bd4e7 65
SamClarke 0:78d4704bd4e7 66
SamClarke 0:78d4704bd4e7 67 void Plan13::initSat(void)
SamClarke 0:78d4704bd4e7 68 {
SamClarke 0:78d4704bd4e7 69 //readElements(SAT); //now done beforehand
SamClarke 0:78d4704bd4e7 70
SamClarke 0:78d4704bd4e7 71 /* Observer's location */
SamClarke 0:78d4704bd4e7 72 LA = rad( observer_lat );
SamClarke 0:78d4704bd4e7 73 LO = rad( observer_lon );
SamClarke 0:78d4704bd4e7 74 HT = ((float) observer_height)/1000.0; // this needs to be in km
SamClarke 0:78d4704bd4e7 75 CL = cos(LA);
SamClarke 0:78d4704bd4e7 76 SL = sin(LA);
SamClarke 0:78d4704bd4e7 77 CO = cos(LO);
SamClarke 0:78d4704bd4e7 78 SO = sin(LO);
SamClarke 0:78d4704bd4e7 79 /* WGS-84 Earth Ellipsoid */
SamClarke 0:78d4704bd4e7 80 RE = 6378.137;
SamClarke 0:78d4704bd4e7 81 FL = 1.0 / 298.257224;
SamClarke 0:78d4704bd4e7 82
SamClarke 0:78d4704bd4e7 83 /* IAU-76 Earth Ellipsoid */
SamClarke 0:78d4704bd4e7 84 // RE = 6378.140;
SamClarke 0:78d4704bd4e7 85 // FL = 1.0 / 298.257;
SamClarke 0:78d4704bd4e7 86
SamClarke 0:78d4704bd4e7 87 RP = RE * (1.0 - FL);
SamClarke 0:78d4704bd4e7 88 XX = RE * RE;
SamClarke 0:78d4704bd4e7 89 ZZ = RP * RP;
SamClarke 0:78d4704bd4e7 90
SamClarke 0:78d4704bd4e7 91 D = sqrt( XX * CL * CL + ZZ * SL * SL );
SamClarke 0:78d4704bd4e7 92
SamClarke 0:78d4704bd4e7 93 Rx = XX / D + HT;
SamClarke 0:78d4704bd4e7 94 Rz = ZZ / D + HT;
SamClarke 0:78d4704bd4e7 95
SamClarke 0:78d4704bd4e7 96 /* Observer's unit vectors Up EAST and NORTH in geocentric coordinates */
SamClarke 0:78d4704bd4e7 97 Ux = CL * CO;
SamClarke 0:78d4704bd4e7 98 Ex = -SO;
SamClarke 0:78d4704bd4e7 99 Nx = -SL * CO;
SamClarke 0:78d4704bd4e7 100
SamClarke 0:78d4704bd4e7 101 Uy = CL * SO;
SamClarke 0:78d4704bd4e7 102 Ey = CO;
SamClarke 0:78d4704bd4e7 103 Ny = -SL * SO;
SamClarke 0:78d4704bd4e7 104
SamClarke 0:78d4704bd4e7 105 Uz = SL;
SamClarke 0:78d4704bd4e7 106 Ez = 0;
SamClarke 0:78d4704bd4e7 107 Nz = CL;
SamClarke 0:78d4704bd4e7 108
SamClarke 0:78d4704bd4e7 109 /* Observer's XYZ coordinates at earth's surface */
SamClarke 0:78d4704bd4e7 110 Ox = Rx * Ux;
SamClarke 0:78d4704bd4e7 111 Oy = Rx * Uy;
SamClarke 0:78d4704bd4e7 112 Oz = Rz * Uz;
SamClarke 0:78d4704bd4e7 113
SamClarke 0:78d4704bd4e7 114 /* Convert angles to radians, etc. */
SamClarke 0:78d4704bd4e7 115 RA = rad(RA);
SamClarke 0:78d4704bd4e7 116 IN = rad(IN);
SamClarke 0:78d4704bd4e7 117 WP = rad(WP);
SamClarke 0:78d4704bd4e7 118 MA = rad(MA);
SamClarke 0:78d4704bd4e7 119 MM = MM * 2.0 * M_PI;
SamClarke 0:78d4704bd4e7 120 M2 = M2 * 2.0 * M_PI;
SamClarke 0:78d4704bd4e7 121
SamClarke 0:78d4704bd4e7 122 // YM = 365.25; /* Mean year, days */
SamClarke 0:78d4704bd4e7 123 // YT = 365.2421970; /* Tropical year, days */
SamClarke 0:78d4704bd4e7 124 WW = 2.0 * M_PI / YT; /* Earth's rotation rate, rads/whole day */
SamClarke 0:78d4704bd4e7 125 WE = 2.0 * M_PI + WW; /* Earth's rotation rate, rads/day */
SamClarke 0:78d4704bd4e7 126 W0 = WE / 86400; /* Earth's rotation rate, rads/sec */
SamClarke 0:78d4704bd4e7 127
SamClarke 0:78d4704bd4e7 128 /* Observer's velocity, geocentric coordinates */
SamClarke 0:78d4704bd4e7 129 VOx = -Oy * W0;
SamClarke 0:78d4704bd4e7 130 VOy = Ox * W0;
SamClarke 0:78d4704bd4e7 131
SamClarke 0:78d4704bd4e7 132 /* Convert satellite epoch to Day No. and fraction of a day */
SamClarke 0:78d4704bd4e7 133 DE = FNday(YE, 1, 0) + (int)TE;
SamClarke 0:78d4704bd4e7 134
SamClarke 0:78d4704bd4e7 135 TE = TE - (int)TE;
SamClarke 0:78d4704bd4e7 136 /* Average Precession rates */
SamClarke 0:78d4704bd4e7 137 GM = 3.986E5; /* Earth's gravitational constant km^3/s^2 */
SamClarke 0:78d4704bd4e7 138 J2 = 1.08263E-3; /* 2nd Zonal coeff, Earth's gravity Field */
SamClarke 0:78d4704bd4e7 139 N0 = MM / 86400.0; /* Mean motion rads/s */
SamClarke 0:78d4704bd4e7 140 AU = pow((float)GM / N0 / N0, (float)1.0 / 3.0); /* Semi major axis km */
SamClarke 0:78d4704bd4e7 141 b0 = AU * sqrt(1.0 - EC * EC); /* Semi minor axis km */
SamClarke 0:78d4704bd4e7 142 SI = sin(IN);
SamClarke 0:78d4704bd4e7 143 CI = cos(IN);
SamClarke 0:78d4704bd4e7 144 PC = RE * AU / (b0 * b0);
SamClarke 0:78d4704bd4e7 145 PC = 1.5 * J2 * PC * PC * MM; /* Precession const, rad/day */
SamClarke 0:78d4704bd4e7 146 QD = -PC * CI; /* Node Precession rate, rad/day */
SamClarke 0:78d4704bd4e7 147 WD = PC *(5.0 * CI*CI - 1.0) / 2.0; /* Perigee Precession rate, rad/day */
SamClarke 0:78d4704bd4e7 148 DC = -2.0 * M2 / MM / 3.0; /* Drag coeff */
SamClarke 0:78d4704bd4e7 149
SamClarke 0:78d4704bd4e7 150 /* Sideral and solar data. Never needs changing. Valid to year 2000+ */
SamClarke 0:78d4704bd4e7 151
SamClarke 0:78d4704bd4e7 152
SamClarke 0:78d4704bd4e7 153 /* GHAA, Year YG, Jan 0.0 */
SamClarke 0:78d4704bd4e7 154 YG = 2014;
SamClarke 0:78d4704bd4e7 155 G0 = 99.5578;
SamClarke 0:78d4704bd4e7 156 /* MA Sun and rate, deg, deg/day */
SamClarke 0:78d4704bd4e7 157 MAS0 = 356.4485;
SamClarke 0:78d4704bd4e7 158 MASD = 0.98560028;
SamClarke 0:78d4704bd4e7 159 /* Sun's inclination */
SamClarke 0:78d4704bd4e7 160 INS = rad(23.4380);
SamClarke 0:78d4704bd4e7 161 CNS = cos(INS);
SamClarke 0:78d4704bd4e7 162 SNS = sin(INS);
SamClarke 0:78d4704bd4e7 163 /* Sun's equation of center terms */
SamClarke 0:78d4704bd4e7 164 EQC1 = 0.03341;
SamClarke 0:78d4704bd4e7 165 EQC2 = 0.00035;
SamClarke 0:78d4704bd4e7 166
SamClarke 0:78d4704bd4e7 167
SamClarke 0:78d4704bd4e7 168 /* Bring Sun data to satellite epoch */
SamClarke 0:78d4704bd4e7 169 TEG = (DE - FNday(YG, 1, 0)) + TE; /* Elapsed Time: Epoch - YG */
SamClarke 0:78d4704bd4e7 170 GHAE = rad(G0) + TEG * WE; /* GHA Aries, epoch */
SamClarke 0:78d4704bd4e7 171 MRSE = rad(G0) + (TEG * WW) + M_PI; /* Mean RA Sun at Sat Epoch */
SamClarke 0:78d4704bd4e7 172 MASE = rad(MAS0 + MASD * TEG); /* Mean MA Sun */
SamClarke 0:78d4704bd4e7 173
SamClarke 0:78d4704bd4e7 174 /* Antenna unit vector in orbit plane coordinates */
SamClarke 0:78d4704bd4e7 175 CO = cos(rad(ALON));
SamClarke 0:78d4704bd4e7 176 SO = sin(rad(ALON));
SamClarke 0:78d4704bd4e7 177 CL = cos(rad(ALAT));
SamClarke 0:78d4704bd4e7 178 SL = sin(rad(ALAT));
SamClarke 0:78d4704bd4e7 179 ax = -CL * CO;
SamClarke 0:78d4704bd4e7 180 ay = -CL * SO;
SamClarke 0:78d4704bd4e7 181 az = -SL;
SamClarke 0:78d4704bd4e7 182
SamClarke 0:78d4704bd4e7 183 /* Miscellaneous */
SamClarke 0:78d4704bd4e7 184 OLDRN = -99999;
SamClarke 0:78d4704bd4e7 185 }
SamClarke 0:78d4704bd4e7 186 /*
SamClarke 0:78d4704bd4e7 187 * Calculate satellite position at DN, TN
SamClarke 0:78d4704bd4e7 188 */
SamClarke 0:78d4704bd4e7 189 void Plan13::satvec()
SamClarke 0:78d4704bd4e7 190 {
SamClarke 0:78d4704bd4e7 191 T = (DN - DE) + (TN - TE);//83.848 ; /* Elapsed T since epoch */
SamClarke 0:78d4704bd4e7 192 DT = DC * T / 2.0; /* Linear drag terms */
SamClarke 0:78d4704bd4e7 193 KD = 1.0 + 4.0 * DT;
SamClarke 0:78d4704bd4e7 194 KDP = 1.0 - 7.0 * DT;
SamClarke 0:78d4704bd4e7 195 M = MA + MM * T * (1.0 - 3.0 * DT); /* Mean anomaly at YR,/ TN */
SamClarke 0:78d4704bd4e7 196 DR = (int)(M / (2.0 * M_PI)); /* Strip out whole no of revs */
SamClarke 0:78d4704bd4e7 197 M = M - DR * 2.0 * M_PI; /* M now in range 0 - 2PI */
SamClarke 0:78d4704bd4e7 198 RN = RV + DR + 1; /* Current orbit number */
SamClarke 0:78d4704bd4e7 199
SamClarke 0:78d4704bd4e7 200 /* Solve M = EA - EC * sin(EA) for EA given M, by Newton's method */
SamClarke 0:78d4704bd4e7 201 EA = M; /* Initail solution */
SamClarke 0:78d4704bd4e7 202 do
SamClarke 0:78d4704bd4e7 203 {
SamClarke 0:78d4704bd4e7 204 C = cos(EA);
SamClarke 0:78d4704bd4e7 205 S = sin(EA);
SamClarke 0:78d4704bd4e7 206 DNOM = 1.0 - EC * C;
SamClarke 0:78d4704bd4e7 207 D = (EA - EC * S - M) / DNOM; /* Change EA to better resolution */
SamClarke 0:78d4704bd4e7 208 EA = EA - D; /* by this amount until converged */
SamClarke 0:78d4704bd4e7 209 }
SamClarke 0:78d4704bd4e7 210 while (fabs(D) > 1.0E-5);
SamClarke 0:78d4704bd4e7 211
SamClarke 0:78d4704bd4e7 212 /* Distances */
SamClarke 0:78d4704bd4e7 213 A = AU * KD;
SamClarke 0:78d4704bd4e7 214 B = b0 * KD;
SamClarke 0:78d4704bd4e7 215 RS = A * DNOM;
SamClarke 0:78d4704bd4e7 216
SamClarke 0:78d4704bd4e7 217 /* Calculate satellite position and velocity in plane of ellipse */
SamClarke 0:78d4704bd4e7 218 Sx = A * (C - EC);
SamClarke 0:78d4704bd4e7 219 Vx = -A * S / DNOM * N0;
SamClarke 0:78d4704bd4e7 220 Sy = B * S;
SamClarke 0:78d4704bd4e7 221 Vy = B * C / DNOM * N0;
SamClarke 0:78d4704bd4e7 222
SamClarke 0:78d4704bd4e7 223 AP = WP + WD * T * KDP;
SamClarke 0:78d4704bd4e7 224 CW = cos(AP);
SamClarke 0:78d4704bd4e7 225 SW = sin(AP);
SamClarke 0:78d4704bd4e7 226 RAAN = RA + QD * T * KDP;
SamClarke 0:78d4704bd4e7 227 CQ = cos(RAAN);
SamClarke 0:78d4704bd4e7 228 SQ = sin(RAAN);
SamClarke 0:78d4704bd4e7 229
SamClarke 0:78d4704bd4e7 230 /* Plane -> celestial coordinate transformation, [C] = [RAAN]*[IN]*[AP] */
SamClarke 0:78d4704bd4e7 231 CXx = CW * CQ - SW * CI * SQ;
SamClarke 0:78d4704bd4e7 232 CXy = -SW * CQ - CW * CI * SQ;
SamClarke 0:78d4704bd4e7 233 CXz = SI * SQ;
SamClarke 0:78d4704bd4e7 234 CYx = CW * SQ + SW * CI * CQ;
SamClarke 0:78d4704bd4e7 235 CYy = -SW * SQ + CW * CI * CQ;
SamClarke 0:78d4704bd4e7 236 CYz = -SI * CQ;
SamClarke 0:78d4704bd4e7 237 CZx = SW * SI;
SamClarke 0:78d4704bd4e7 238 CZy = CW * SI;
SamClarke 0:78d4704bd4e7 239 CZz = CI;
SamClarke 0:78d4704bd4e7 240
SamClarke 0:78d4704bd4e7 241 /* Compute satellite's position vector, ANTenna axis unit vector */
SamClarke 0:78d4704bd4e7 242 /* and velocity in celestial coordinates. (Note: Sz = 0, Vz = 0) */
SamClarke 0:78d4704bd4e7 243 SATx = Sx * CXx + Sy * CXy;
SamClarke 0:78d4704bd4e7 244 ANTx = ax * CXx + ay * CXy + az * CXz;
SamClarke 0:78d4704bd4e7 245 VELx = Vx * CXx + Vy * CXy;
SamClarke 0:78d4704bd4e7 246 SATy = Sx * CYx + Sy * CYy;
SamClarke 0:78d4704bd4e7 247 ANTy = ax * CYx + ay * CYy + az * CYz;
SamClarke 0:78d4704bd4e7 248 VELy = Vx * CYx + Vy * CYy;
SamClarke 0:78d4704bd4e7 249 SATz = Sx * CZx + Sy * CZy;
SamClarke 0:78d4704bd4e7 250 ANTz = ax * CZx + ay * CZy + az * CZz;
SamClarke 0:78d4704bd4e7 251 VELz = Vx * CZx + Vy * CZy;
SamClarke 0:78d4704bd4e7 252
SamClarke 0:78d4704bd4e7 253 /* Also express SAT, ANT, and VEL in geocentric coordinates */
SamClarke 0:78d4704bd4e7 254 GHAA = GHAE + WE * T; /* GHA Aries at elaprsed time T */
SamClarke 0:78d4704bd4e7 255 C = cos(-GHAA);
SamClarke 0:78d4704bd4e7 256 S = sin(-GHAA);
SamClarke 0:78d4704bd4e7 257 Sx = SATx * C - SATy * S;
SamClarke 0:78d4704bd4e7 258 Ax = ANTx * C - ANTy * S;
SamClarke 0:78d4704bd4e7 259 Vx = VELx * C - VELy * S;
SamClarke 0:78d4704bd4e7 260 Sy = SATx * S + SATy * C;
SamClarke 0:78d4704bd4e7 261 Ay = ANTx * S + ANTy * C;
SamClarke 0:78d4704bd4e7 262 Vy = VELx * S + VELy * C;
SamClarke 0:78d4704bd4e7 263 Sz = SATz;
SamClarke 0:78d4704bd4e7 264 Az = ANTz;
SamClarke 0:78d4704bd4e7 265 Vz = VELz;
SamClarke 0:78d4704bd4e7 266 }
SamClarke 0:78d4704bd4e7 267
SamClarke 0:78d4704bd4e7 268 /*
SamClarke 0:78d4704bd4e7 269 * Compute and manipulate range/velocity/antenna vectors
SamClarke 0:78d4704bd4e7 270 */
SamClarke 0:78d4704bd4e7 271 void Plan13::rangevec(void)
SamClarke 0:78d4704bd4e7 272 {
SamClarke 0:78d4704bd4e7 273 /* Range vector = sat vector - observer vector */
SamClarke 0:78d4704bd4e7 274 Rx = Sx - Ox;
SamClarke 0:78d4704bd4e7 275 Ry = Sy - Oy;
SamClarke 0:78d4704bd4e7 276 Rz = Sz - Oz;
SamClarke 0:78d4704bd4e7 277
SamClarke 0:78d4704bd4e7 278 R = sqrt(Rx * Rx + Ry * Ry + Rz * Rz); /* Range Magnitute */
SamClarke 0:78d4704bd4e7 279
SamClarke 0:78d4704bd4e7 280 /* Normalize range vector */
SamClarke 0:78d4704bd4e7 281 Rx = Rx / R;
SamClarke 0:78d4704bd4e7 282 Ry = Ry / R;
SamClarke 0:78d4704bd4e7 283 Rz = Rz / R;
SamClarke 0:78d4704bd4e7 284 U = Rx * Ux + Ry * Uy + Rz * Uz;
SamClarke 0:78d4704bd4e7 285 E = Rx * Ex + Ry * Ey;
SamClarke 0:78d4704bd4e7 286 N = Rx * Nx + Ry * Ny + Rz * Nz;
SamClarke 0:78d4704bd4e7 287
SamClarke 0:78d4704bd4e7 288 AZ = deg(FNatn(E, N));
SamClarke 0:78d4704bd4e7 289 EL = deg(asin(U));
SamClarke 0:78d4704bd4e7 290
SamClarke 0:78d4704bd4e7 291 /* Solve antenna vector along unit range vector, -r.a = cos(SQ) */
SamClarke 0:78d4704bd4e7 292 /* SQ = deg(acos(-(Ax * Rx + Ay * Ry + Az * Rz)));
SamClarke 0:78d4704bd4e7 293 */
SamClarke 0:78d4704bd4e7 294 /* Calculate sub-satellite Lat/Lon */
SamClarke 0:78d4704bd4e7 295 SLON = deg(FNatn(Sy, Sx)); /* Lon, + East */
SamClarke 0:78d4704bd4e7 296 SLAT = deg(asin(Sz / RS)); /* Lat, + North */
SamClarke 0:78d4704bd4e7 297
SamClarke 0:78d4704bd4e7 298 /* Resolve Sat-Obs velocity vector along unit range vector. (VOz = 0) */
SamClarke 0:78d4704bd4e7 299 RR = (Vx - VOx) * Rx + (Vy - VOy) * Ry + Vz * Rz; /* Range rate, km/sec */
SamClarke 0:78d4704bd4e7 300 //FR = rxFrequency * (1 - RR / 299792);
SamClarke 0:78d4704bd4e7 301 dopplerFactor = RR / 299792.0;
SamClarke 0:78d4704bd4e7 302 int rxDoppler = getDoppler(rxFrequencyLong);
SamClarke 0:78d4704bd4e7 303 int txDoppler = getDoppler(txFrequencyLong);
SamClarke 0:78d4704bd4e7 304 rxOutLong = rxFrequencyLong - rxDoppler;
SamClarke 0:78d4704bd4e7 305 txOutLong = txFrequencyLong + txDoppler;
SamClarke 0:78d4704bd4e7 306 }
SamClarke 0:78d4704bd4e7 307
SamClarke 0:78d4704bd4e7 308 void Plan13::sunvec(void)
SamClarke 0:78d4704bd4e7 309 {
SamClarke 0:78d4704bd4e7 310 //TODO
SamClarke 0:78d4704bd4e7 311 }
SamClarke 0:78d4704bd4e7 312
SamClarke 0:78d4704bd4e7 313
SamClarke 0:78d4704bd4e7 314 int Plan13::getDoppler(unsigned long freq) {
SamClarke 0:78d4704bd4e7 315 freq = (freq + 50000L) / 100000L;
SamClarke 0:78d4704bd4e7 316 long factor = dopplerFactor * 1E11;
SamClarke 0:78d4704bd4e7 317 int digit;
SamClarke 0:78d4704bd4e7 318 float tally = 0;
SamClarke 0:78d4704bd4e7 319 for (int x = 4; x > -1; x--) {
SamClarke 0:78d4704bd4e7 320 digit = freq/pow((float)10,(float)x);
SamClarke 0:78d4704bd4e7 321 long bare = digit * pow((float)10,(float)x);
SamClarke 0:78d4704bd4e7 322 freq = freq - bare;
SamClarke 0:78d4704bd4e7 323 float inBetween = factor * (float(bare) / 1E6);
SamClarke 0:78d4704bd4e7 324 tally += inBetween;
SamClarke 0:78d4704bd4e7 325 }
SamClarke 0:78d4704bd4e7 326 return int( tally + .5); //round
SamClarke 0:78d4704bd4e7 327 }
SamClarke 0:78d4704bd4e7 328
SamClarke 0:78d4704bd4e7 329 int Plan13::getDoppler64(unsigned long freq) {
SamClarke 1:ab43ae956815 330 //long factor = dopplerFactor * 1E11;
SamClarke 0:78d4704bd4e7 331 uint64_t doppler_sixfour = freq * dopplerFactor;
SamClarke 0:78d4704bd4e7 332 return (int) doppler_sixfour/1E11;
SamClarke 0:78d4704bd4e7 333 }
SamClarke 0:78d4704bd4e7 334
SamClarke 0:78d4704bd4e7 335
SamClarke 0:78d4704bd4e7 336 void Plan13::setFrequency(unsigned long rxFrequency_in, unsigned long txFrequency_in) {
SamClarke 0:78d4704bd4e7 337 rxFrequencyLong = rxFrequency_in;
SamClarke 0:78d4704bd4e7 338 txFrequencyLong = txFrequency_in;
SamClarke 0:78d4704bd4e7 339 if (TEST) {
SamClarke 0:78d4704bd4e7 340 rxFrequencyLong = 435300000L;
SamClarke 0:78d4704bd4e7 341 txFrequencyLong = 45920000L;
SamClarke 0:78d4704bd4e7 342 }
SamClarke 0:78d4704bd4e7 343 }
SamClarke 0:78d4704bd4e7 344
SamClarke 0:78d4704bd4e7 345 void Plan13::setLocation(double observer_lon_in, double observer_lat_in, int height) {
SamClarke 0:78d4704bd4e7 346 observer_lon = observer_lon_in;//-64.375; //0.06; // lon east is positive, west is negative
SamClarke 0:78d4704bd4e7 347 observer_lat = observer_lat_in;//45.8958; //52.21; //Cambridge UK
SamClarke 0:78d4704bd4e7 348 observer_height = height; //60m height in meters
SamClarke 0:78d4704bd4e7 349 }
SamClarke 0:78d4704bd4e7 350
SamClarke 0:78d4704bd4e7 351 void Plan13::setTime(int yearIn, int monthIn, int mDayIn, int hourIn, int minIn, int secIn) {
SamClarke 0:78d4704bd4e7 352 int aYear = yearIn;
SamClarke 0:78d4704bd4e7 353 int aMonth = monthIn;
SamClarke 0:78d4704bd4e7 354 int aMday = mDayIn;
SamClarke 0:78d4704bd4e7 355 int aHour = hourIn;
SamClarke 0:78d4704bd4e7 356 int aMin = minIn;
SamClarke 0:78d4704bd4e7 357 int aSec = secIn;
SamClarke 0:78d4704bd4e7 358 DN = FNday(aYear, aMonth, aMday);
SamClarke 0:78d4704bd4e7 359 TN = ((float)aHour + ((float)aMin + ((float)aSec/60.0)) /60.0)/24.0;
SamClarke 0:78d4704bd4e7 360 DN = (long)DN;
SamClarke 0:78d4704bd4e7 361 }
SamClarke 0:78d4704bd4e7 362 void Plan13::setElements(double YE_in, double TE_in, double IN_in, double
SamClarke 0:78d4704bd4e7 363 RA_in, double EC_in, double WP_in, double MA_in, double MM_in,
SamClarke 0:78d4704bd4e7 364 double M2_in, double RV_in, double ALON_in ) {
SamClarke 0:78d4704bd4e7 365
SamClarke 0:78d4704bd4e7 366 YE = YE_in;
SamClarke 0:78d4704bd4e7 367 TE = TE_in;
SamClarke 0:78d4704bd4e7 368 IN = IN_in;
SamClarke 0:78d4704bd4e7 369 RA = RA_in;
SamClarke 0:78d4704bd4e7 370 EC = EC_in;
SamClarke 0:78d4704bd4e7 371 WP = WP_in;
SamClarke 0:78d4704bd4e7 372 MA = MA_in;
SamClarke 0:78d4704bd4e7 373 MM = MM_in;
SamClarke 0:78d4704bd4e7 374 M2 = M2_in;
SamClarke 0:78d4704bd4e7 375 RV = RV_in;
SamClarke 0:78d4704bd4e7 376 ALON = ALON_in;
SamClarke 0:78d4704bd4e7 377 }
SamClarke 0:78d4704bd4e7 378
SamClarke 0:78d4704bd4e7 379 void Plan13::calculate() {
SamClarke 0:78d4704bd4e7 380 initSat();
SamClarke 0:78d4704bd4e7 381 satvec();
SamClarke 0:78d4704bd4e7 382 rangevec();
SamClarke 0:78d4704bd4e7 383 }
SamClarke 0:78d4704bd4e7 384
SamClarke 0:78d4704bd4e7 385 float *Plan13::footprintOctagon(float slat, float slon) {
SamClarke 0:78d4704bd4e7 386 static float points[16];
SamClarke 0:78d4704bd4e7 387 float srad = acos(RE/RS);
SamClarke 0:78d4704bd4e7 388 float cla= cos(slat);
SamClarke 0:78d4704bd4e7 389 float sla = sin(slat);
SamClarke 0:78d4704bd4e7 390 float clo = cos(slon);
SamClarke 0:78d4704bd4e7 391 float slo = sin(slon);
SamClarke 0:78d4704bd4e7 392 float sra = sin(srad);
SamClarke 0:78d4704bd4e7 393 float cra = cos(srad);
SamClarke 0:78d4704bd4e7 394 for (int i = 0; i < 16; i = i +2) {
SamClarke 0:78d4704bd4e7 395 float a = 2 * M_PI * i / 8;
SamClarke 0:78d4704bd4e7 396 float X = cra;
SamClarke 0:78d4704bd4e7 397 float Y = sra*sin(a);
SamClarke 0:78d4704bd4e7 398 float Z = sra*cos(a);
SamClarke 0:78d4704bd4e7 399 float x = X*cla - Z*sla;
SamClarke 0:78d4704bd4e7 400 float y = Y;
SamClarke 0:78d4704bd4e7 401 float z = X*sla + Z*cla;
SamClarke 0:78d4704bd4e7 402 X = x*clo - y*slo;
SamClarke 0:78d4704bd4e7 403 Y = x*slo + y*clo;
SamClarke 0:78d4704bd4e7 404 Z = z;
SamClarke 0:78d4704bd4e7 405 points[i] = FNatn(Y,X);
SamClarke 0:78d4704bd4e7 406 points[i+1] = asin(Z);
SamClarke 0:78d4704bd4e7 407 }
SamClarke 0:78d4704bd4e7 408 return points;
SamClarke 0:78d4704bd4e7 409 }