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@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?
User | Revision | Line number | New 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 | } |