mirror of
https://github.com/k3ng/k3ng_rotator_controller.git
synced 2024-12-20 13:43:12 +00:00
9cdc072cc3
After pulling my hair out for two days, I rewrote the satellite tracking to use the P13 library from Mark VandeWettering
\^ command to activate and deactive satellite tracking
\~ command to view satellite tracking status
457 lines
10 KiB
C++
Executable File
457 lines
10 KiB
C++
Executable File
//
|
|
// P13.cpp
|
|
//
|
|
// An implementation of Plan13 in C++ by Mark VandeWettering
|
|
//
|
|
// Plan13 is an algorithm for satellite orbit prediction first formulated
|
|
// by James Miller G3RUH. I learned about it when I saw it was the basis
|
|
// of the PIC based antenna rotator project designed by G6LVB.
|
|
//
|
|
// http://www.g6lvb.com/Articles/LVBTracker2/index.htm
|
|
//
|
|
// I ported the algorithm to Python, and it was my primary means of orbit
|
|
// prediction for a couple of years while I operated the "Easy Sats" with
|
|
// a dual band hand held and an Arrow antenna.
|
|
//
|
|
// I've long wanted to redo the work in C++ so that I could port the code
|
|
// to smaller processors including the Atmel AVR chips. Bruce Robertson,
|
|
// VE9QRP started the qrpTracker project to fufill many of the same goals,
|
|
// but I thought that the code could be made more compact and more modular,
|
|
// and could serve not just the embedded targets but could be of more
|
|
// use for more general applications. And, I like the BSD License a bit
|
|
// better too.
|
|
//
|
|
// So, here it is!
|
|
//
|
|
|
|
#include "P13.h"
|
|
|
|
double
|
|
RADIANS(double deg)
|
|
{
|
|
return deg * M_PI / 180. ;
|
|
}
|
|
|
|
double
|
|
DEGREES(double rad)
|
|
{
|
|
return rad * 180. / M_PI ;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------
|
|
// _ ___ _ _____ _
|
|
// __| |__ _ ______ | \ __ _| |_ __|_ _(_)_ __ ___
|
|
// / _| / _` (_-<_-< | |) / _` | _/ -_)| | | | ' \/ -_)
|
|
// \__|_\__,_/__/__/ |___/\__,_|\__\___||_| |_|_|_|_\___|
|
|
//
|
|
//----------------------------------------------------------------------
|
|
|
|
static long
|
|
fnday(int y, int m, int d)
|
|
{
|
|
if (m < 3) {
|
|
m += 12 ;
|
|
y -- ;
|
|
}
|
|
return (long) (y * YM) + (long) ((m+1)*30.6f) + (long)d - 428L ;
|
|
}
|
|
|
|
static void
|
|
fndate(int &y, int &m, int &d, long dt)
|
|
{
|
|
dt += 428L ;
|
|
y = (int) ((dt-122.1)/365.25) ;
|
|
dt -= (long) (y*365.25) ;
|
|
m = (int) (dt / 30.61) ;
|
|
dt -= (long) (m*30.6) ;
|
|
m -- ;
|
|
if (m > 12) {
|
|
m -= 12 ;
|
|
y++ ;
|
|
}
|
|
d = dt ;
|
|
}
|
|
|
|
SatDateTime::SatDateTime(int year, int month, int day, int h, int m, int s)
|
|
{
|
|
settime(year, month, day, h, m, s) ;
|
|
}
|
|
|
|
|
|
SatDateTime::SatDateTime(const SatDateTime &dt)
|
|
{
|
|
DN = dt.DN ;
|
|
TN = dt.TN ;
|
|
}
|
|
|
|
SatDateTime::SatDateTime()
|
|
{
|
|
DN = 0L ;
|
|
TN = 0. ;
|
|
}
|
|
|
|
void
|
|
SatDateTime::gettime(int &year, int &month, int &day, int &h, int &m, int &s)
|
|
{
|
|
fndate(year, month, day, DN) ;
|
|
double t = TN ;
|
|
t *= 24. ;
|
|
h = (int) t ;
|
|
t -= h ;
|
|
t *= 60 ;
|
|
m = (int) t ;
|
|
t -= m ;
|
|
t *= 60 ;
|
|
s = (int) t ;
|
|
}
|
|
|
|
void
|
|
SatDateTime::settime(int year, int month, int day, int h, int m, int s)
|
|
{
|
|
DN = fnday(year, month, day) ;
|
|
TN = ((double) h + m / 60. + s / 3600.) / 24. ;
|
|
}
|
|
|
|
void
|
|
SatDateTime::ascii(char *buf)
|
|
{
|
|
int year, mon, day ;
|
|
int h, m, s ;
|
|
gettime(year, mon, day, h, m, s) ;
|
|
sprintf(buf, "%02d/%02d/%4d %02d:%02d:%02d", mon, day, year, h, m, s) ;
|
|
}
|
|
|
|
|
|
void
|
|
SatDateTime::add(double days)
|
|
{
|
|
TN += days ;
|
|
DN += (long) TN ;
|
|
TN -= (long) TN ;
|
|
}
|
|
|
|
void
|
|
SatDateTime::roundup(double t)
|
|
{
|
|
double inc = t-fmod(TN, t) ;
|
|
TN += inc ;
|
|
DN += (long) TN ;
|
|
TN -= (long) TN ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------
|
|
// _ ___ _
|
|
// __| |__ _ ______ / _ \| |__ ___ ___ _ ___ _____ _ _
|
|
// / _| / _` (_-<_-< | (_) | '_ (_-</ -_) '_\ V / -_) '_|
|
|
// \__|_\__,_/__/__/ \___/|_.__/__/\___|_| \_/\___|_|
|
|
//
|
|
//----------------------------------------------------------------------
|
|
|
|
Observer::Observer(const char *nm, double lat, double lng, double hgt)
|
|
{
|
|
this->name = nm ;
|
|
LA = RADIANS(lat) ;
|
|
LO = RADIANS(lng) ;
|
|
HT = hgt / 1000 ;
|
|
|
|
U[0] = cos(LA)*cos(LO) ;
|
|
U[1] = cos(LA)*sin(LO) ;
|
|
U[2] = sin(LA) ;
|
|
|
|
E[0] = -sin(LO) ;
|
|
E[1] = cos(LO) ;
|
|
E[2] = 0. ;
|
|
|
|
N[0] = -sin(LA)*cos(LO) ;
|
|
N[1] = -sin(LA)*sin(LO) ;
|
|
N[2] = cos(LA) ;
|
|
|
|
double RP = RE * (1 - FL) ;
|
|
double XX = RE * RE ;
|
|
double ZZ = RP * RP ;
|
|
double D = sqrt(XX*cos(LA)*cos(LA) +
|
|
ZZ*sin(LA)*sin(LA)) ;
|
|
double Rx = XX / D + HT ;
|
|
double Rz = ZZ / D + HT ;
|
|
|
|
O[0] = Rx * U[0] ;
|
|
O[1] = Rx * U[1] ;
|
|
O[2] = Rz * U[2] ;
|
|
|
|
V[0] = -O[1] * W0 ;
|
|
V[1] = O[0] * W0 ;
|
|
V[2] = 0 ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------
|
|
// _ ___ _ _ _ _ _
|
|
// __| |__ _ ______ / __| __ _| |_ ___| | (_) |_ ___
|
|
// / _| / _` (_-<_-< \__ \/ _` | _/ -_) | | | _/ -_)
|
|
// \__|_\__,_/__/__/ |___/\__,_|\__\___|_|_|_|\__\___|
|
|
//
|
|
//----------------------------------------------------------------------
|
|
|
|
static double
|
|
getdouble(const char *c, int i0, int i1)
|
|
{
|
|
char buf[20] ;
|
|
int i ;
|
|
for (i=0; i0+i<i1; i++)
|
|
buf[i] = c[i0+i] ;
|
|
buf[i] = '\0' ;
|
|
return strtod(buf, NULL) ;
|
|
}
|
|
|
|
static long
|
|
getlong(const char *c, int i0, int i1)
|
|
{
|
|
char buf[20] ;
|
|
int i ;
|
|
for (i=0; i0+i<i1; i++)
|
|
buf[i] = c[i0+i] ;
|
|
buf[i] = '\0' ;
|
|
return atol(buf) ;
|
|
}
|
|
|
|
Satellite::Satellite(const char *nm, const char *l1, const char *l2)
|
|
{
|
|
tle(nm, l1, l2) ;
|
|
}
|
|
|
|
Satellite::~Satellite()
|
|
{
|
|
}
|
|
|
|
void
|
|
Satellite::tle(const char *nm, const char *l1, const char *l2)
|
|
{
|
|
name = nm ;
|
|
|
|
// direct quantities from the orbital elements
|
|
|
|
N = getlong(l2, 2, 7) ;
|
|
YE = getlong(l1, 18, 20) ;
|
|
if (YE < 58)
|
|
YE += 2000 ;
|
|
else
|
|
YE += 1900 ;
|
|
|
|
TE = getdouble(l1, 20, 32) ;
|
|
M2 = RADIANS(getdouble(l1, 33, 43)) ;
|
|
|
|
IN = RADIANS(getdouble(l2, 8, 16)) ;
|
|
RA = RADIANS(getdouble(l2, 17, 25)) ;
|
|
EC = getdouble(l2, 26, 33)/1e7f ;
|
|
WP = RADIANS(getdouble(l2, 34, 42)) ;
|
|
MA = RADIANS(getdouble(l2, 43, 51)) ;
|
|
MM = 2.0f * M_PI * getdouble(l2, 52, 63) ;
|
|
RV = getlong(l2, 63, 68) ;
|
|
|
|
// derived quantities from the orbital elements
|
|
|
|
// convert TE to DE and TE
|
|
DE = fnday(YE, 1, 0) + (long) TE ;
|
|
TE -= (long) TE ;
|
|
N0 = MM/86400 ;
|
|
A_0 = pow(GM/(N0*N0), 1./3.) ;
|
|
B_0 = A_0*sqrt(1.-EC*EC) ;
|
|
PC = RE*A_0/(B_0*B_0) ;
|
|
PC = 1.5f*J2*PC*PC*MM ;
|
|
double CI = cos(IN) ;
|
|
QD = -PC*CI ;
|
|
WD = PC*(5*CI*CI-1)/2 ;
|
|
DC = -2*M2/(3*MM) ;
|
|
}
|
|
void
|
|
Satellite::predict(const SatDateTime &dt)
|
|
{
|
|
long DN = dt.DN ;
|
|
double TN = dt.TN ;
|
|
|
|
float TEG = DE - fnday(YG, 1, 0) + TE ;
|
|
|
|
float GHAE = radians(G0) + TEG * WE ;
|
|
float MRSE = radians(G0) + TEG * WW + M_PI ;
|
|
float MASE = radians(MAS0 + TEG * MASD) ;
|
|
|
|
double T = (double) (DN - DE) + (TN-TE) ;
|
|
double DT = DC * T / 2. ;
|
|
double KD = 1. + 4. * DT ;
|
|
double KDP = 1. - 7. * DT ;
|
|
|
|
double M = MA + MM * T * (1. - 3. * DT) ;
|
|
double DR = (long) (M / (2. * M_PI)) ;
|
|
M -= DR * 2. * M_PI ;
|
|
double RN = RV + DR ;
|
|
double EA = M ;
|
|
|
|
double DNOM, C_EA, S_EA ;
|
|
|
|
for (;;) {
|
|
C_EA = cos(EA) ;
|
|
S_EA = sin(EA) ;
|
|
DNOM = 1. - EC * C_EA ;
|
|
double D = (EA-EC*S_EA-M)/DNOM ;
|
|
EA -= D ;
|
|
if (fabs(D) < 1e-5)
|
|
break ;
|
|
}
|
|
|
|
double A = A_0 * KD ;
|
|
double B = B_0 * KD ;
|
|
RS = A * DNOM ;
|
|
|
|
double Vx, Vy ;
|
|
double Sx, Sy ;
|
|
Sx = A * (C_EA - EC) ;
|
|
Sy = B * S_EA ;
|
|
|
|
Vx = -A * S_EA / DNOM * N0 ;
|
|
Vy = B * C_EA / DNOM * N0 ;
|
|
|
|
double AP = WP + WD * T * KDP ;
|
|
double CW = cos(AP) ;
|
|
double SW = sin(AP) ;
|
|
|
|
double RAAN = RA + QD * T * KDP ;
|
|
|
|
double CQ = cos(RAAN) ;
|
|
double SQ = sin(RAAN) ;
|
|
|
|
double CI = cos(IN) ;
|
|
double SI = sin(IN) ;
|
|
|
|
// CX, CY, and CZ form a 3x3 matrix
|
|
// that converts between orbit coordinates,
|
|
// and celestial coordinates.
|
|
|
|
Vec3 CX, CY, CZ ;
|
|
|
|
CX[0] = CW * CQ - SW * CI * SQ ;
|
|
CX[1] = -SW * CQ - CW * CI * SQ ;
|
|
CX[2] = SI * SQ ;
|
|
|
|
CY[0] = CW * SQ + SW * CI * CQ ;
|
|
CY[1] = -SW * SQ + CW * CI * CQ ;
|
|
CY[2] = -SI * CQ ;
|
|
|
|
CZ[0] = SW * SI ;
|
|
CZ[1] = CW * SI ;
|
|
CZ[2] = CI ;
|
|
|
|
// satellite in celestial coords
|
|
|
|
SAT[0] = Sx * CX[0] + Sy * CX[1] ;
|
|
SAT[1] = Sx * CY[0] + Sy * CY[1] ;
|
|
SAT[2] = Sx * CZ[0] + Sy * CZ[1] ;
|
|
|
|
VEL[0] = Vx * CX[0] + Vy * CX[1] ;
|
|
VEL[1] = Vx * CY[0] + Vy * CY[1] ;
|
|
VEL[2] = Vx * CZ[0] + Vy * CZ[1] ;
|
|
|
|
// and in geocentric coordinates
|
|
|
|
double GHAA = (GHAE + WE * T) ;
|
|
double CG = cos(-GHAA) ;
|
|
double SG = sin(-GHAA) ;
|
|
|
|
S[0] = SAT[0] * CG - SAT[1] * SG ;
|
|
S[1] = SAT[0] * SG + SAT[1] * CG ;
|
|
S[2] = SAT[2] ;
|
|
|
|
V[0] = VEL[0] * CG - VEL[1]* SG ;
|
|
V[1] = VEL[0] * SG + VEL[1]* CG ;
|
|
V[2] = VEL[2] ;
|
|
}
|
|
|
|
void
|
|
Satellite::LL(double &lat, double &lng)
|
|
{
|
|
lat = DEGREES(asin(S[2]/RS)) ;
|
|
lng = DEGREES(atan2(S[1], S[0])) ;
|
|
}
|
|
|
|
void
|
|
Satellite::altaz(const Observer &obs, double &alt, double &az)
|
|
{
|
|
Vec3 R ;
|
|
R[0] = S[0] - obs.O[0] ;
|
|
R[1] = S[1] - obs.O[1] ;
|
|
R[2] = S[2] - obs.O[2] ;
|
|
double r = sqrt(R[0]*R[0]+R[1]*R[1]+R[2]*R[2]) ;
|
|
R[0] /= r ;
|
|
R[1] /= r ;
|
|
R[2] /= r ;
|
|
|
|
double u = R[0] * obs.U[0] + R[1] * obs.U[1] + R[2] * obs.U[2] ;
|
|
double e = R[0] * obs.E[0] + R[1] * obs.E[1] + R[2] * obs.E[2] ;
|
|
double n = R[0] * obs.N[0] + R[1] * obs.N[1] + R[2] * obs.N[2] ;
|
|
|
|
az = DEGREES(atan2(e, n)) ;
|
|
if (az < 0.) az += 360. ;
|
|
alt = DEGREES(asin(u)) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------
|
|
|
|
Sun::Sun()
|
|
{
|
|
}
|
|
|
|
void
|
|
Sun::predict(const SatDateTime &dt)
|
|
{
|
|
long DN = dt.DN ;
|
|
double TN = dt.TN ;
|
|
|
|
double T = (double) (DN - fnday(YG, 1, 0)) + TN ;
|
|
double GHAE = RADIANS(G0) + T * WE ;
|
|
double MRSE = RADIANS(G0) + T * WW + M_PI ;
|
|
double MASE = RADIANS(MAS0 + T * MASD) ;
|
|
double TAS = MRSE + EQC1*sin(MASE) + EQC2*sin(2.*MASE) ;
|
|
double C, S ;
|
|
|
|
C = cos(TAS) ;
|
|
S = sin(TAS) ;
|
|
SUN[0]=C ;
|
|
SUN[1]=S*CNS ;
|
|
SUN[2]=S*SNS ;
|
|
C = cos(-GHAE) ;
|
|
S = sin(-GHAE) ;
|
|
H[0]=SUN[0]*C - SUN[1]*S ;
|
|
H[1]=SUN[0]*S + SUN[1]*C ;
|
|
H[2]=SUN[2] ;
|
|
}
|
|
|
|
void
|
|
Sun::LL(double &lat, double &lng)
|
|
{
|
|
lat = DEGREES(asin(H[2])) ;
|
|
lng = DEGREES(atan2(H[1], H[0])) ;
|
|
}
|
|
|
|
// oops.... this is broken.
|
|
|
|
void
|
|
Sun::altaz(const Observer &obs, double &alt, double &az)
|
|
{
|
|
Vec3 R ;
|
|
|
|
R[0] = H[0] - obs.O[0] ;
|
|
R[1] = H[1] - obs.O[1] ;
|
|
R[2] = H[2] - obs.O[2] ;
|
|
double r = sqrt(R[0]*R[0]+R[1]*R[1]+R[2]*R[2]) ;
|
|
R[0] /= r ;
|
|
R[1] /= r ;
|
|
R[2] /= r ;
|
|
|
|
double u = R[0] * obs.U[0] + R[1] * obs.U[1] + R[2] * obs.U[2] ;
|
|
double e = R[0] * obs.E[0] + R[1] * obs.E[1] + R[2] * obs.E[2] ;
|
|
double n = R[0] * obs.N[0] + R[1] * obs.N[1] + R[2] * obs.N[2] ;
|
|
|
|
az = DEGREES(atan2(e, n)) ;
|
|
if (az < 0.) az += 360. ;
|
|
alt = DEGREES(asin(u)) ;
|
|
}
|