diff --git a/contrib/sgp4/sgp4unit.cpp b/contrib/sgp4/sgp4unit.cpp index 24d63aa9..b707a7a2 100644 --- a/contrib/sgp4/sgp4unit.cpp +++ b/contrib/sgp4/sgp4unit.cpp @@ -1,2090 +1,2090 @@ -/* ---------------------------------------------------------------- -* -* sgp4unit.cpp -* -* this file contains the sgp4 procedures for analytical propagation -* of a satellite. the code was originally released in the 1980 and 1986 -* spacetrack papers. a detailed discussion of the theory and history -* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, -* and kelso. -* -* companion code for -* fundamentals of astrodynamics and applications -* 2007 -* by david vallado -* -* (w) 719-573-2600, email dvallado@agi.com -* -* current : -* 16 nov 07 david vallado -* misc fixes for better compliance -* changes : -* 20 apr 07 david vallado -* misc fixes for constants -* 11 aug 06 david vallado -* chg lyddane choice back to strn3, constants, misc doc -* 15 dec 05 david vallado -* misc fixes -* 26 jul 05 david vallado -* fixes for paper -* note that each fix is preceded by a -* comment with "sgp4fix" and an explanation of -* what was changed -* 10 aug 04 david vallado -* 2nd printing baseline working -* 14 may 01 david vallado -* 2nd edition baseline -* 80 norad -* original baseline -* ---------------------------------------------------------------- */ - -#include "sgp4unit.h" - -const char help = 'n'; -FILE *dbgfile; - -#define pi 3.14159265358979323846 - - -/* ----------- local functions - only ever used internally by sgp4 ---------- */ -static void dpper - ( - double e3, double ee2, double peo, double pgho, double pho, - double pinco, double plo, double se2, double se3, double sgh2, - double sgh3, double sgh4, double sh2, double sh3, double si2, - double si3, double sl2, double sl3, double sl4, double t, - double xgh2, double xgh3, double xgh4, double xh2, double xh3, - double xi2, double xi3, double xl2, double xl3, double xl4, - double zmol, double zmos, double inclo, - char init, - double& ep, double& inclp, double& nodep, double& argpp, double& mp - ); - -static void dscom - ( - double epoch, double ep, double argpp, double tc, double inclp, - double nodep, double np, - double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, - double& cosomm,double& day, double& e3, double& ee2, double& em, - double& emsq, double& gam, double& peo, double& pgho, double& pho, - double& pinco, double& plo, double& rtemsq, double& se2, double& se3, - double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, - double& si2, double& si3, double& sl2, double& sl3, double& sl4, - double& s1, double& s2, double& s3, double& s4, double& s5, - double& s6, double& s7, double& ss1, double& ss2, double& ss3, - double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, - double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, - double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, - double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, - double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, - double& xl4, double& nm, double& z1, double& z2, double& z3, - double& z11, double& z12, double& z13, double& z21, double& z22, - double& z23, double& z31, double& z32, double& z33, double& zmol, - double& zmos - ); - -static void dsinit - ( - gravconsttype whichconst, - double cosim, double emsq, double argpo, double s1, double s2, - double s3, double s4, double s5, double sinim, double ss1, - double ss2, double ss3, double ss4, double ss5, double sz1, - double sz3, double sz11, double sz13, double sz21, double sz23, - double sz31, double sz33, double t, double tc, double gsto, - double mo, double mdot, double no, double nodeo, double nodedot, - double xpidot, double z1, double z3, double z11, double z13, - double z21, double z23, double z31, double z33, double ecco, - double eccsq, double& em, double& argpm, double& inclm, double& mm, - double& nm, double& nodem, - int& irez, - double& atime, double& d2201, double& d2211, double& d3210, double& d3222, - double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, - double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, - double& dnodt, double& domdt, double& del1, double& del2, double& del3, - double& xfact, double& xlamo, double& xli, double& xni - ); - -static void dspace - ( - int irez, - double d2201, double d2211, double d3210, double d3222, double d4410, - double d4422, double d5220, double d5232, double d5421, double d5433, - double dedt, double del1, double del2, double del3, double didt, - double dmdt, double dnodt, double domdt, double argpo, double argpdot, - double t, double tc, double gsto, double xfact, double xlamo, - double no, - double& atime, double& em, double& argpm, double& inclm, double& xli, - double& mm, double& xni, double& nodem, double& dndt, double& nm - ); - -static void initl - ( - int satn, gravconsttype whichconst, - double ecco, double epoch, double inclo, double& no, - char& method, - double& ainv, double& ao, double& con41, double& con42, double& cosio, - double& cosio2,double& eccsq, double& omeosq, double& posq, - double& rp, double& rteosq,double& sinio , double& gsto - ); - -/* ----------------------------------------------------------------------------- -* -* procedure dpper -* -* this procedure provides deep space long period periodic contributions -* to the mean elements. by design, these periodics are zero at epoch. -* this used to be dscom which included initialization, but it's really a -* recurring function. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* e3 - -* ee2 - -* peo - -* pgho - -* pho - -* pinco - -* plo - -* se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 - -* t - -* xh2, xh3, xi2, xi3, xl2, xl3, xl4 - -* zmol - -* zmos - -* ep - eccentricity 0.0 - 1.0 -* inclo - inclination - needed for lyddane modification -* nodep - right ascension of ascending node -* argpp - argument of perigee -* mp - mean anomaly -* -* outputs : -* ep - eccentricity 0.0 - 1.0 -* inclp - inclination -* nodep - right ascension of ascending node -* argpp - argument of perigee -* mp - mean anomaly -* -* locals : -* alfdp - -* betdp - -* cosip , sinip , cosop , sinop , -* dalf - -* dbet - -* dls - -* f2, f3 - -* pe - -* pgh - -* ph - -* pinc - -* pl - -* sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis , -* sll , sls -* xls - -* xnoh - -* zf - -* zm - -* -* coupling : -* none. -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dpper - ( - double e3, double ee2, double peo, double pgho, double pho, - double pinco, double plo, double se2, double se3, double sgh2, - double sgh3, double sgh4, double sh2, double sh3, double si2, - double si3, double sl2, double sl3, double sl4, double t, - double xgh2, double xgh3, double xgh4, double xh2, double xh3, - double xi2, double xi3, double xl2, double xl3, double xl4, - double zmol, double zmos, double inclo, - char init, - double& ep, double& inclp, double& nodep, double& argpp, double& mp - ) -{ - /* --------------------- local variables ------------------------ */ - const double twopi = 2.0 * pi; - double alfdp, betdp, cosip, cosop, dalf, dbet, dls, - f2, f3, pe, pgh, ph, pinc, pl , - sel, ses, sghl, sghs, shll, shs, sil, - sinip, sinop, sinzf, sis, sll, sls, xls, - xnoh, zf, zm, zel, zes, znl, zns; - - /* ---------------------- constants ----------------------------- */ - zns = 1.19459e-5; - zes = 0.01675; - znl = 1.5835218e-4; - zel = 0.05490; - - /* --------------- calculate time varying periodics ----------- */ - zm = zmos + zns * t; - // be sure that the initial call has time set to zero - if (init == 'y') - zm = zmos; - zf = zm + 2.0 * zes * sin(zm); - sinzf = sin(zf); - f2 = 0.5 * sinzf * sinzf - 0.25; - f3 = -0.5 * sinzf * cos(zf); - ses = se2* f2 + se3 * f3; - sis = si2 * f2 + si3 * f3; - sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; - sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; - shs = sh2 * f2 + sh3 * f3; - zm = zmol + znl * t; - if (init == 'y') - zm = zmol; - zf = zm + 2.0 * zel * sin(zm); - sinzf = sin(zf); - f2 = 0.5 * sinzf * sinzf - 0.25; - f3 = -0.5 * sinzf * cos(zf); - sel = ee2 * f2 + e3 * f3; - sil = xi2 * f2 + xi3 * f3; - sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; - sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; - shll = xh2 * f2 + xh3 * f3; - pe = ses + sel; - pinc = sis + sil; - pl = sls + sll; - pgh = sghs + sghl; - ph = shs + shll; - - if (init == 'n') - { - pe = pe - peo; - pinc = pinc - pinco; - pl = pl - plo; - pgh = pgh - pgho; - ph = ph - pho; - inclp = inclp + pinc; - ep = ep + pe; - sinip = sin(inclp); - cosip = cos(inclp); - - /* ----------------- apply periodics directly ------------ */ - // sgp4fix for lyddane choice - // strn3 used original inclination - this is technically feasible - // gsfc used perturbed inclination - also technically feasible - // probably best to readjust the 0.2 limit value and limit discontinuity - // 0.2 rad = 11.45916 deg - // use next line for original strn3 approach and original inclination - // if (inclo >= 0.2) - // use next line for gsfc version and perturbed inclination - if (inclp >= 0.2) - { - ph = ph / sinip; - pgh = pgh - cosip * ph; - argpp = argpp + pgh; - nodep = nodep + ph; - mp = mp + pl; - } - else - { - /* ---- apply periodics with lyddane modification ---- */ - sinop = sin(nodep); - cosop = cos(nodep); - alfdp = sinip * sinop; - betdp = sinip * cosop; - dalf = ph * cosop + pinc * cosip * sinop; - dbet = -ph * sinop + pinc * cosip * cosop; - alfdp = alfdp + dalf; - betdp = betdp + dbet; - nodep = fmod(nodep, twopi); - // sgp4fix for afspc written intrinsic functions - // nodep used without a trigonometric function ahead - if (nodep < 0.0) - nodep = nodep + twopi; - xls = mp + argpp + cosip * nodep; - dls = pl + pgh - pinc * nodep * sinip; - xls = xls + dls; - xnoh = nodep; - nodep = atan2(alfdp, betdp); - // sgp4fix for afspc written intrinsic functions - // nodep used without a trigonometric function ahead - if (nodep < 0.0) - nodep = nodep + twopi; - if (fabs(xnoh - nodep) > pi){ - if (nodep < xnoh) - nodep = nodep + twopi; - else - nodep = nodep - twopi; - } - mp = mp + pl; - argpp = xls - mp - cosip * nodep; - } - } // if init == 'n' - -//#include "debug1.cpp" -} // end dpper - -/*----------------------------------------------------------------------------- -* -* procedure dscom -* -* this procedure provides deep space common items used by both the secular -* and periodics subroutines. input is provided as shown. this routine -* used to be called dpper, but the functions inside weren't well organized. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* epoch - -* ep - eccentricity -* argpp - argument of perigee -* tc - -* inclp - inclination -* nodep - right ascension of ascending node -* np - mean motion -* -* outputs : -* sinim , cosim , sinomm , cosomm , snodm , cnodm -* day - -* e3 - -* ee2 - -* em - eccentricity -* emsq - eccentricity squared -* gam - -* peo - -* pgho - -* pho - -* pinco - -* plo - -* rtemsq - -* se2, se3 - -* sgh2, sgh3, sgh4 - -* sh2, sh3, si2, si3, sl2, sl3, sl4 - -* s1, s2, s3, s4, s5, s6, s7 - -* ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 - -* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - -* xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 - -* nm - mean motion -* z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 - -* zmol - -* zmos - -* -* locals : -* a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 - -* betasq - -* cc - -* ctem, stem - -* x1, x2, x3, x4, x5, x6, x7, x8 - -* xnodce - -* xnoi - -* zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl , -* zcosi , zsini , zcosil , zsinil , -* zx - -* zy - -* -* coupling : -* none. -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dscom - ( - double epoch, double ep, double argpp, double tc, double inclp, - double nodep, double np, - double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, - double& cosomm,double& day, double& e3, double& ee2, double& em, - double& emsq, double& gam, double& peo, double& pgho, double& pho, - double& pinco, double& plo, double& rtemsq, double& se2, double& se3, - double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, - double& si2, double& si3, double& sl2, double& sl3, double& sl4, - double& s1, double& s2, double& s3, double& s4, double& s5, - double& s6, double& s7, double& ss1, double& ss2, double& ss3, - double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, - double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, - double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, - double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, - double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, - double& xl4, double& nm, double& z1, double& z2, double& z3, - double& z11, double& z12, double& z13, double& z21, double& z22, - double& z23, double& z31, double& z32, double& z33, double& zmol, - double& zmos - ) -{ - /* -------------------------- constants ------------------------- */ - const double zes = 0.01675; - const double zel = 0.05490; - const double c1ss = 2.9864797e-6; - const double c1l = 4.7968065e-7; - const double zsinis = 0.39785416; - const double zcosis = 0.91744867; - const double zcosgs = 0.1945905; - const double zsings = -0.98088458; - const double twopi = 2.0 * pi; - - /* --------------------- local variables ------------------------ */ - int lsflg; - double a1 , a2 , a3 , a4 , a5 , a6 , a7 , - a8 , a9 , a10 , betasq, cc , ctem , stem , - x1 , x2 , x3 , x4 , x5 , x6 , x7 , - x8 , xnodce, xnoi , zcosg , zcosgl, zcosh , zcoshl, - zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini , - zsinil, zx , zy; - - nm = np; - em = ep; - snodm = sin(nodep); - cnodm = cos(nodep); - sinomm = sin(argpp); - cosomm = cos(argpp); - sinim = sin(inclp); - cosim = cos(inclp); - emsq = em * em; - betasq = 1.0 - emsq; - rtemsq = sqrt(betasq); - - /* ----------------- initialize lunar solar terms --------------- */ - peo = 0.0; - pinco = 0.0; - plo = 0.0; - pgho = 0.0; - pho = 0.0; - day = epoch + 18261.5 + tc / 1440.0; - xnodce = fmod(4.5236020 - 9.2422029e-4 * day, twopi); - stem = sin(xnodce); - ctem = cos(xnodce); - zcosil = 0.91375164 - 0.03568096 * ctem; - zsinil = sqrt(1.0 - zcosil * zcosil); - zsinhl = 0.089683511 * stem / zsinil; - zcoshl = sqrt(1.0 - zsinhl * zsinhl); - gam = 5.8351514 + 0.0019443680 * day; - zx = 0.39785416 * stem / zsinil; - zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; - zx = atan2(zx, zy); - zx = gam + zx - xnodce; - zcosgl = cos(zx); - zsingl = sin(zx); - - /* ------------------------- do solar terms --------------------- */ - zcosg = zcosgs; - zsing = zsings; - zcosi = zcosis; - zsini = zsinis; - zcosh = cnodm; - zsinh = snodm; - cc = c1ss; - xnoi = 1.0 / nm; - - for (lsflg = 1; lsflg <= 2; lsflg++) - { - a1 = zcosg * zcosh + zsing * zcosi * zsinh; - a3 = -zsing * zcosh + zcosg * zcosi * zsinh; - a7 = -zcosg * zsinh + zsing * zcosi * zcosh; - a8 = zsing * zsini; - a9 = zsing * zsinh + zcosg * zcosi * zcosh; - a10 = zcosg * zsini; - a2 = cosim * a7 + sinim * a8; - a4 = cosim * a9 + sinim * a10; - a5 = -sinim * a7 + cosim * a8; - a6 = -sinim * a9 + cosim * a10; - - x1 = a1 * cosomm + a2 * sinomm; - x2 = a3 * cosomm + a4 * sinomm; - x3 = -a1 * sinomm + a2 * cosomm; - x4 = -a3 * sinomm + a4 * cosomm; - x5 = a5 * sinomm; - x6 = a6 * sinomm; - x7 = a5 * cosomm; - x8 = a6 * cosomm; - - z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; - z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; - z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; - z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq; - z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq; - z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq; - z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7-6.0 * x3 * x5); - z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq * - (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); - z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); - z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); - z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq * - (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); - z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8); - z1 = z1 + z1 + betasq * z31; - z2 = z2 + z2 + betasq * z32; - z3 = z3 + z3 + betasq * z33; - s3 = cc * xnoi; - s2 = -0.5 * s3 / rtemsq; - s4 = s3 * rtemsq; - s1 = -15.0 * em * s4; - s5 = x1 * x3 + x2 * x4; - s6 = x2 * x3 + x1 * x4; - s7 = x2 * x4 - x1 * x3; - - /* ----------------------- do lunar terms ------------------- */ - if (lsflg == 1) - { - ss1 = s1; - ss2 = s2; - ss3 = s3; - ss4 = s4; - ss5 = s5; - ss6 = s6; - ss7 = s7; - sz1 = z1; - sz2 = z2; - sz3 = z3; - sz11 = z11; - sz12 = z12; - sz13 = z13; - sz21 = z21; - sz22 = z22; - sz23 = z23; - sz31 = z31; - sz32 = z32; - sz33 = z33; - zcosg = zcosgl; - zsing = zsingl; - zcosi = zcosil; - zsini = zsinil; - zcosh = zcoshl * cnodm + zsinhl * snodm; - zsinh = snodm * zcoshl - cnodm * zsinhl; - cc = c1l; - } - } - - zmol = fmod(4.7199672 + 0.22997150 * day - gam, twopi); - zmos = fmod(6.2565837 + 0.017201977 * day, twopi); - - /* ------------------------ do solar terms ---------------------- */ - se2 = 2.0 * ss1 * ss6; - se3 = 2.0 * ss1 * ss7; - si2 = 2.0 * ss2 * sz12; - si3 = 2.0 * ss2 * (sz13 - sz11); - sl2 = -2.0 * ss3 * sz2; - sl3 = -2.0 * ss3 * (sz3 - sz1); - sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes; - sgh2 = 2.0 * ss4 * sz32; - sgh3 = 2.0 * ss4 * (sz33 - sz31); - sgh4 = -18.0 * ss4 * zes; - sh2 = -2.0 * ss2 * sz22; - sh3 = -2.0 * ss2 * (sz23 - sz21); - - /* ------------------------ do lunar terms ---------------------- */ - ee2 = 2.0 * s1 * s6; - e3 = 2.0 * s1 * s7; - xi2 = 2.0 * s2 * z12; - xi3 = 2.0 * s2 * (z13 - z11); - xl2 = -2.0 * s3 * z2; - xl3 = -2.0 * s3 * (z3 - z1); - xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel; - xgh2 = 2.0 * s4 * z32; - xgh3 = 2.0 * s4 * (z33 - z31); - xgh4 = -18.0 * s4 * zel; - xh2 = -2.0 * s2 * z22; - xh3 = -2.0 * s2 * (z23 - z21); - -//#include "debug2.cpp" -} // end dscom - -/*----------------------------------------------------------------------------- -* -* procedure dsinit -* -* this procedure provides deep space contributions to mean motion dot due -* to geopotential resonance with half day and one day orbits. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* cosim, sinim- -* emsq - eccentricity squared -* argpo - argument of perigee -* s1, s2, s3, s4, s5 - -* ss1, ss2, ss3, ss4, ss5 - -* sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 - -* t - time -* tc - -* gsto - greenwich sidereal time rad -* mo - mean anomaly -* mdot - mean anomaly dot (rate) -* no - mean motion -* nodeo - right ascension of ascending node -* nodedot - right ascension of ascending node dot (rate) -* xpidot - -* z1, z3, z11, z13, z21, z23, z31, z33 - -* eccm - eccentricity -* argpm - argument of perigee -* inclm - inclination -* mm - mean anomaly -* xn - mean motion -* nodem - right ascension of ascending node -* -* outputs : -* em - eccentricity -* argpm - argument of perigee -* inclm - inclination -* mm - mean anomaly -* nm - mean motion -* nodem - right ascension of ascending node -* irez - flag for resonance 0-none, 1-one day, 2-half day -* atime - -* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - -* dedt - -* didt - -* dmdt - -* dndt - -* dnodt - -* domdt - -* del1, del2, del3 - -* ses , sghl , sghs , sgs , shl , shs , sis , sls -* theta - -* xfact - -* xlamo - -* xli - -* xni -* -* locals : -* ainv2 - -* aonv - -* cosisq - -* eoc - -* f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 - -* g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 - -* sini2 - -* temp - -* temp1 - -* theta - -* xno2 - -* -* coupling : -* getgravconst -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dsinit - ( - gravconsttype whichconst, - double cosim, double emsq, double argpo, double s1, double s2, - double s3, double s4, double s5, double sinim, double ss1, - double ss2, double ss3, double ss4, double ss5, double sz1, - double sz3, double sz11, double sz13, double sz21, double sz23, - double sz31, double sz33, double t, double tc, double gsto, - double mo, double mdot, double no, double nodeo, double nodedot, - double xpidot, double z1, double z3, double z11, double z13, - double z21, double z23, double z31, double z33, double ecco, - double eccsq, double& em, double& argpm, double& inclm, double& mm, - double& nm, double& nodem, - int& irez, - double& atime, double& d2201, double& d2211, double& d3210, double& d3222, - double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, - double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, - double& dnodt, double& domdt, double& del1, double& del2, double& del3, - double& xfact, double& xlamo, double& xli, double& xni - ) -{ - /* --------------------- local variables ------------------------ */ - const double twopi = 2.0 * pi; - - double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221 , f311 , - f321 , f322 , f330 , f441 , f442 , f522 , f523 , - f542 , f543 , g200 , g201 , g211 , g300 , g310 , - g322 , g410 , g422 , g520 , g521 , g532 , g533 , - ses , sgs , sghl , sghs , shs , shll , sis , - sini2 , sls , temp , temp1 , theta , xno2 , q22 , - q31 , q33 , root22, root44, root54, rptim , root32, - root52, x2o3 , xke , znl , emo , zns , emsqo, - tumin, mu, radiusearthkm, j2, j3, j4, j3oj2; - - q22 = 1.7891679e-6; - q31 = 2.1460748e-6; - q33 = 2.2123015e-7; - root22 = 1.7891679e-6; - root44 = 7.3636953e-9; - root54 = 2.1765803e-9; - rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec - root32 = 3.7393792e-7; - root52 = 1.1428639e-7; - x2o3 = 2.0 / 3.0; - znl = 1.5835218e-4; - zns = 1.19459e-5; - - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - - /* -------------------- deep space initialization ------------ */ - irez = 0; - if ((nm < 0.0052359877) && (nm > 0.0034906585)) - irez = 1; - if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5)) - irez = 2; - - /* ------------------------ do solar terms ------------------- */ - ses = ss1 * zns * ss5; - sis = ss2 * zns * (sz11 + sz13); - sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq); - sghs = ss4 * zns * (sz31 + sz33 - 6.0); - shs = -zns * ss2 * (sz21 + sz23); - // sgp4fix for 180 deg incl - if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) - shs = 0.0; - if (sinim != 0.0) - shs = shs / sinim; - sgs = sghs - cosim * shs; - - /* ------------------------- do lunar terms ------------------ */ - dedt = ses + s1 * znl * s5; - didt = sis + s2 * znl * (z11 + z13); - dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq); - sghl = s4 * znl * (z31 + z33 - 6.0); - shll = -znl * s2 * (z21 + z23); - // sgp4fix for 180 deg incl - if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) - shll = 0.0; - domdt = sgs + sghl; - dnodt = shs; - if (sinim != 0.0) - { - domdt = domdt - cosim / sinim * shll; - dnodt = dnodt + shll / sinim; - } - - /* ----------- calculate deep space resonance effects -------- */ - dndt = 0.0; - theta = fmod(gsto + tc * rptim, twopi); - em = em + dedt * t; - inclm = inclm + didt * t; - argpm = argpm + domdt * t; - nodem = nodem + dnodt * t; - mm = mm + dmdt * t; - // sgp4fix for negative inclinations - // the following if statement should be commented out - //if (inclm < 0.0) - // { - // inclm = -inclm; - // argpm = argpm - pi; - // nodem = nodem + pi; - // } - - /* -------------- initialize the resonance terms ------------- */ - if (irez != 0) - { - aonv = pow(nm / xke, x2o3); - - /* ---------- geopotential resonance for 12 hour orbits ------ */ - if (irez == 2) - { - cosisq = cosim * cosim; - emo = em; - em = ecco; - emsqo = emsq; - emsq = eccsq; - eoc = em * emsq; - g201 = -0.306 - (em - 0.64) * 0.440; - - if (em <= 0.65) - { - g211 = 3.616 - 13.2470 * em + 16.2900 * emsq; - g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc; - g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc; - g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc; - g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc; - g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc; - } - else - { - g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc; - g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc; - g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc; - g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc; - g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc; - if (em > 0.715) - g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc; - else - g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq; - } - if (em < 0.7) - { - g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc; - g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc; - g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc; - } - else - { - g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc; - g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc; - g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc; - } - - sini2= sinim * sinim; - f220 = 0.75 * (1.0 + 2.0 * cosim+cosisq); - f221 = 1.5 * sini2; - f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); - f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); - f441 = 35.0 * sini2 * f220; - f442 = 39.3750 * sini2 * sini2; - f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) + - 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) ); - f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim + - 10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq)); - f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq * - (-12.0 + 8.0 * cosim + 10.0 * cosisq)); - f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq * - (12.0 + 8.0 * cosim - 10.0 * cosisq)); - xno2 = nm * nm; - ainv2 = aonv * aonv; - temp1 = 3.0 * xno2 * ainv2; - temp = temp1 * root22; - d2201 = temp * f220 * g201; - d2211 = temp * f221 * g211; - temp1 = temp1 * aonv; - temp = temp1 * root32; - d3210 = temp * f321 * g310; - d3222 = temp * f322 * g322; - temp1 = temp1 * aonv; - temp = 2.0 * temp1 * root44; - d4410 = temp * f441 * g410; - d4422 = temp * f442 * g422; - temp1 = temp1 * aonv; - temp = temp1 * root52; - d5220 = temp * f522 * g520; - d5232 = temp * f523 * g532; - temp = 2.0 * temp1 * root54; - d5421 = temp * f542 * g521; - d5433 = temp * f543 * g533; - xlamo = fmod(mo + nodeo + nodeo-theta - theta, twopi); - xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no; - em = emo; - emsq = emsqo; - } - - /* ---------------- synchronous resonance terms -------------- */ - if (irez == 1) - { - g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); - g310 = 1.0 + 2.0 * emsq; - g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq); - f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); - f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); - f330 = 1.0 + cosim; - f330 = 1.875 * f330 * f330 * f330; - del1 = 3.0 * nm * nm * aonv * aonv; - del2 = 2.0 * del1 * f220 * g200 * q22; - del3 = 3.0 * del1 * f330 * g300 * q33 * aonv; - del1 = del1 * f311 * g310 * q31 * aonv; - xlamo = fmod(mo + nodeo + argpo - theta, twopi); - xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no; - } - - /* ------------ for sgp4, initialize the integrator ---------- */ - xli = xlamo; - xni = no; - atime = 0.0; - nm = no + dndt; - } - -//#include "debug3.cpp" -} // end dsinit - -/*----------------------------------------------------------------------------- -* -* procedure dspace -* -* this procedure provides deep space contributions to mean elements for -* perturbing third body. these effects have been averaged over one -* revolution of the sun and moon. for earth resonance effects, the -* effects have been averaged over no revolutions of the satellite. -* (mean motion) -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - -* dedt - -* del1, del2, del3 - -* didt - -* dmdt - -* dnodt - -* domdt - -* irez - flag for resonance 0-none, 1-one day, 2-half day -* argpo - argument of perigee -* argpdot - argument of perigee dot (rate) -* t - time -* tc - -* gsto - gst -* xfact - -* xlamo - -* no - mean motion -* atime - -* em - eccentricity -* ft - -* argpm - argument of perigee -* inclm - inclination -* xli - -* mm - mean anomaly -* xni - mean motion -* nodem - right ascension of ascending node -* -* outputs : -* atime - -* em - eccentricity -* argpm - argument of perigee -* inclm - inclination -* xli - -* mm - mean anomaly -* xni - -* nodem - right ascension of ascending node -* dndt - -* nm - mean motion -* -* locals : -* delt - -* ft - -* theta - -* x2li - -* x2omi - -* xl - -* xldot - -* xnddt - -* xndt - -* xomi - -* -* coupling : -* none - -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dspace - ( - int irez, - double d2201, double d2211, double d3210, double d3222, double d4410, - double d4422, double d5220, double d5232, double d5421, double d5433, - double dedt, double del1, double del2, double del3, double didt, - double dmdt, double dnodt, double domdt, double argpo, double argpdot, - double t, double tc, double gsto, double xfact, double xlamo, - double no, - double& atime, double& em, double& argpm, double& inclm, double& xli, - double& mm, double& xni, double& nodem, double& dndt, double& nm - ) -{ - const double twopi = 2.0 * pi; - int iretn , iret; - double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32, - g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp; - - ft = 0.0; - fasx2 = 0.13130908; - fasx4 = 2.8843198; - fasx6 = 0.37448087; - g22 = 5.7686396; - g32 = 0.95240898; - g44 = 1.8014998; - g52 = 1.0508330; - g54 = 4.4108898; - rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec - stepp = 720.0; - stepn = -720.0; - step2 = 259200.0; - - /* ----------- calculate deep space resonance effects ----------- */ - dndt = 0.0; - theta = fmod(gsto + tc * rptim, twopi); - em = em + dedt * t; - - inclm = inclm + didt * t; - argpm = argpm + domdt * t; - nodem = nodem + dnodt * t; - mm = mm + dmdt * t; - - // sgp4fix for negative inclinations - // the following if statement should be commented out - // if (inclm < 0.0) - // { - // inclm = -inclm; - // argpm = argpm - pi; - // nodem = nodem + pi; - // } - - /* - update resonances : numerical (euler-maclaurin) integration - */ - /* ------------------------- epoch restart ---------------------- */ - // sgp4fix for propagator problems - // the following integration works for negative time steps and periods - // the specific changes are unknown because the original code was so convoluted - - ft = 0.0; - atime = 0.0; - if (irez != 0) - { - if ((atime == 0.0) || ((t >= 0.0) && (atime < 0.0)) || - ((t < 0.0) && (atime >= 0.0))) - { - if (t >= 0.0) - delt = stepp; - else - delt = stepn; - atime = 0.0; - xni = no; - xli = xlamo; - } - iretn = 381; // added for do loop - iret = 0; // added for loop - while (iretn == 381) - { - if ((fabs(t) < fabs(atime)) || (iret == 351)) - { - if (t >= 0.0) - delt = stepn; - else - delt = stepp; - iret = 351; - iretn = 381; - } - else - { - if (t > 0.0) // error if prev if has atime:=0.0 and t:=0.0 (ge) - delt = stepp; - else - delt = stepn; - if (fabs(t - atime) >= stepp) - { - iret = 0; - iretn = 381; - } - else - { - ft = t - atime; - iretn = 0; - } - } - - /* ------------------- dot terms calculated ------------- */ - /* ----------- near - synchronous resonance terms ------- */ - if (irez != 2) - { - xndt = del1 * sin(xli - fasx2) + del2 * sin(2.0 * (xli - fasx4)) + - del3 * sin(3.0 * (xli - fasx6)); - xldot = xni + xfact; - xnddt = del1 * cos(xli - fasx2) + - 2.0 * del2 * cos(2.0 * (xli - fasx4)) + - 3.0 * del3 * cos(3.0 * (xli - fasx6)); - xnddt = xnddt * xldot; - } - else - { - /* --------- near - half-day resonance terms -------- */ - xomi = argpo + argpdot * atime; - x2omi = xomi + xomi; - x2li = xli + xli; - xndt = d2201 * sin(x2omi + xli - g22) + d2211 * sin(xli - g22) + - d3210 * sin(xomi + xli - g32) + d3222 * sin(-xomi + xli - g32)+ - d4410 * sin(x2omi + x2li - g44)+ d4422 * sin(x2li - g44) + - d5220 * sin(xomi + xli - g52) + d5232 * sin(-xomi + xli - g52)+ - d5421 * sin(xomi + x2li - g54) + d5433 * sin(-xomi + x2li - g54); - xldot = xni + xfact; - xnddt = d2201 * cos(x2omi + xli - g22) + d2211 * cos(xli - g22) + - d3210 * cos(xomi + xli - g32) + d3222 * cos(-xomi + xli - g32) + - d5220 * cos(xomi + xli - g52) + d5232 * cos(-xomi + xli - g52) + - 2.0 * (d4410 * cos(x2omi + x2li - g44) + - d4422 * cos(x2li - g44) + d5421 * cos(xomi + x2li - g54) + - d5433 * cos(-xomi + x2li - g54)); - xnddt = xnddt * xldot; - } - - /* ----------------------- integrator ------------------- */ - if (iretn == 381) - { - xli = xli + xldot * delt + xndt * step2; - xni = xni + xndt * delt + xnddt * step2; - atime = atime + delt; - } - } // while iretn = 381 - - nm = xni + xndt * ft + xnddt * ft * ft * 0.5; - xl = xli + xldot * ft + xndt * ft * ft * 0.5; - if (irez != 1) - { - mm = xl - 2.0 * nodem + 2.0 * theta; - dndt = nm - no; - } - else - { - mm = xl - nodem - argpm + theta; - dndt = nm - no; - } - nm = no + dndt; - } - -//#include "debug4.cpp" -} // end dsspace - -/*----------------------------------------------------------------------------- -* -* procedure initl -* -* this procedure initializes the spg4 propagator. all the initialization is -* consolidated here instead of having multiple loops inside other routines. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* ecco - eccentricity 0.0 - 1.0 -* epoch - epoch time in days from jan 0, 1950. 0 hr -* inclo - inclination of satellite -* no - mean motion of satellite -* satn - satellite number -* -* outputs : -* ainv - 1.0 / a -* ao - semi major axis -* con41 - -* con42 - 1.0 - 5.0 cos(i) -* cosio - cosine of inclination -* cosio2 - cosio squared -* eccsq - eccentricity squared -* method - flag for deep space 'd', 'n' -* omeosq - 1.0 - ecco * ecco -* posq - semi-parameter squared -* rp - radius of perigee -* rteosq - square root of (1.0 - ecco*ecco) -* sinio - sine of inclination -* gsto - gst at time of observation rad -* no - mean motion of satellite -* -* locals : -* ak - -* d1 - -* del - -* adel - -* po - -* -* coupling : -* getgravconst -* gstime - find greenwich sidereal time from the julian date -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void initl - ( - int satn, gravconsttype whichconst, - double ecco, double epoch, double inclo, double& no, - char& method, - double& ainv, double& ao, double& con41, double& con42, double& cosio, - double& cosio2,double& eccsq, double& omeosq, double& posq, - double& rp, double& rteosq,double& sinio , double& gsto - ) -{ - /* --------------------- local variables ------------------------ */ - double ak, d1, del, adel, po, x2o3, j2, xke, - tumin, mu, radiusearthkm, j3, j4, j3oj2; - - // sgp4fix use old way of finding gst - int ids70; - double ts70, ds70, tfrac, c1, thgr70, fk5r, c1p2p; - const double twopi = 2.0 * pi; - - /* ----------------------- earth constants ---------------------- */ - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - x2o3 = 2.0 / 3.0; - - /* ------------- calculate auxillary epoch quantities ---------- */ - eccsq = ecco * ecco; - omeosq = 1.0 - eccsq; - rteosq = sqrt(omeosq); - cosio = cos(inclo); - cosio2 = cosio * cosio; - - /* ------------------ un-kozai the mean motion ----------------- */ - ak = pow(xke / no, x2o3); - d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq); - del = d1 / (ak * ak); - adel = ak * (1.0 - del * del - del * - (1.0 / 3.0 + 134.0 * del * del / 81.0)); - del = d1/(adel * adel); - no = no / (1.0 + del); - - ao = pow(xke / no, x2o3); - sinio = sin(inclo); - po = ao * omeosq; - con42 = 1.0 - 5.0 * cosio2; - con41 = -con42-cosio2-cosio2; - ainv = 1.0 / ao; - posq = po * po; - rp = ao * (1.0 - ecco); - method = 'n'; - - // sgp4fix modern approach to finding sidereal timew - // gsto = gstime(epoch + 2433281.5); - - // sgp4fix use old way of finding gst - // count integer number of days from 0 jan 1970 - ts70 = epoch - 7305.0; - ids70 = floor(ts70 + 1.0e-8); - ds70 = ids70; - tfrac = ts70 - ds70; - // find greenwich location at epoch - c1 = 1.72027916940703639e-2; - thgr70= 1.7321343856509374; - fk5r = 5.07551419432269442e-15; - c1p2p = c1 + twopi; - gsto = fmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi); - if ( gsto < 0.0 ) - gsto = gsto + twopi; - -//#include "debug5.cpp" -} // end initl - -/*----------------------------------------------------------------------------- -* -* procedure sgp4init -* -* this procedure initializes variables for sgp4. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* satn - satellite number -* bstar - sgp4 type drag coefficient kg/m2er -* ecco - eccentricity -* epoch - epoch time in days from jan 0, 1950. 0 hr -* argpo - argument of perigee (output if ds) -* inclo - inclination -* mo - mean anomaly (output if ds) -* no - mean motion -* nodeo - right ascension of ascending node -* -* outputs : -* satrec - common values for subsequent calls -* return code - non-zero on error. -* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er -* 2 - mean motion less than 0.0 -* 3 - pert elements, ecc < 0.0 or ecc > 1.0 -* 4 - semi-latus rectum < 0.0 -* 5 - epoch elements are sub-orbital -* 6 - satellite has decayed -* -* locals : -* cnodm , snodm , cosim , sinim , cosomm , sinomm -* cc1sq , cc2 , cc3 -* coef , coef1 -* cosio4 - -* day - -* dndt - -* em - eccentricity -* emsq - eccentricity squared -* eeta - -* etasq - -* gam - -* argpm - argument of perigee -* nodem - -* inclm - inclination -* mm - mean anomaly -* nm - mean motion -* perige - perigee -* pinvsq - -* psisq - -* qzms24 - -* rtemsq - -* s1, s2, s3, s4, s5, s6, s7 - -* sfour - -* ss1, ss2, ss3, ss4, ss5, ss6, ss7 - -* sz1, sz2, sz3 -* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - -* tc - -* temp - -* temp1, temp2, temp3 - -* tsi - -* xpidot - -* xhdot1 - -* z1, z2, z3 - -* z11, z12, z13, z21, z22, z23, z31, z32, z33 - -* -* coupling : -* getgravconst- -* initl - -* dscom - -* dpper - -* dsinit - -* sgp4 - -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -int sgp4init - ( - gravconsttype whichconst, const int satn, const double epoch, - const double xbstar, const double xecco, const double xargpo, - const double xinclo, const double xmo, const double xno, - const double xnodeo, elsetrec& satrec - ) -{ - /* --------------------- local variables ------------------------ */ - double ao, ainv, con42, cosio, sinio, cosio2, eccsq, - omeosq, posq, rp, rteosq, - cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq , - cc2 , cc3 , coef , coef1 , cosio4, day , dndt , - em , emsq , eeta , etasq , gam , argpm , nodem , - inclm , mm , nm , perige, pinvsq, psisq , qzms24, - rtemsq, s1 , s2 , s3 , s4 , s5 , s6 , - s7 , sfour , ss1 = 0 , ss2 = 0 , ss3 = 0 , ss4 = 0 , ss5 = 0 , - ss6 = 0 , ss7 = 0 , sz1 = 0 , sz2 = 0 , sz3 = 0 , sz11 = 0 , sz12 = 0 , - sz13 = 0 , sz21 = 0 , sz22 = 0 , sz23 = 0 , sz31 = 0 , sz32 = 0 , sz33 = 0 , - tc , temp , temp1 , temp2 , temp3 , tsi , xpidot, - xhdot1, z1 , z2 , z3 , z11 , z12 , z13 , - z21 , z22 , z23 , z31 , z32 , z33, - qzms2t, ss, j2, j3oj2, j4, x2o3, r[3], v[3], - tumin, mu, radiusearthkm, xke, j3; - - /* ------------------------ initialization --------------------- */ - // sgp4fix divisor for divide by zero check on inclination - const double temp4 = 1.0 + cos(pi-1.0e-9); - - /* ----------- set all near earth variables to zero ------------ */ - satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0; - satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0; - satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0; - satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0; - satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0; - satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0; - satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0; - satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0; - satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0; - - /* ----------- set all deep space variables to zero ------------ */ - satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0; - satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0; - satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0; - satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0; - satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0; - satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0; - satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0; - satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0; - satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0; - satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0; - satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0; - satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0; - satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0; - satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0; - satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0; - satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0; - satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0; - satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0; - satrec.xli = 0.0; satrec.xni = 0.0; - - // sgp4fix - note the following variables are also passed directly via satrec. - // it is possible to streamline the sgp4init call by deleting the "x" - // variables, but the user would need to set the satrec.* values first. we - // include the additional assignments in case twoline2rv is not used. - satrec.bstar = xbstar; - satrec.ecco = xecco; - satrec.argpo = xargpo; - satrec.inclo = xinclo; - satrec.mo = xmo; - satrec.no = xno; - satrec.nodeo = xnodeo; - - /* ------------------------ earth constants ----------------------- */ - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - ss = 78.0 / radiusearthkm + 1.0; - qzms2t = pow(((120.0 - 78.0) / radiusearthkm), 4); - x2o3 = 2.0 / 3.0; - - satrec.init = 'y'; - satrec.t = 0.0; - - initl - ( - satn, whichconst, satrec.ecco, epoch, satrec.inclo, satrec.no, satrec.method, - ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq, - posq, rp, rteosq, sinio, satrec.gsto - ); - satrec.error = 0; - - if (rp < 1.0) - { -// printf("# *** satn%d epoch elts sub-orbital ***\n", satn); - satrec.error = 5; - } - - if ((omeosq >= 0.0 ) || ( satrec.no >= 0.0)) - { - satrec.isimp = 0; - if (rp < (220.0 / radiusearthkm + 1.0)) - satrec.isimp = 1; - sfour = ss; - qzms24 = qzms2t; - perige = (rp - 1.0) * radiusearthkm; - - /* - for perigees below 156 km, s and qoms2t are altered - */ - if (perige < 156.0) - { - sfour = perige - 78.0; - if (perige < 98.0) - sfour = 20.0; - qzms24 = pow(((120.0 - sfour) / radiusearthkm), 4.0); - sfour = sfour / radiusearthkm + 1.0; - } - pinvsq = 1.0 / posq; - - tsi = 1.0 / (ao - sfour); - satrec.eta = ao * satrec.ecco * tsi; - etasq = satrec.eta * satrec.eta; - eeta = satrec.ecco * satrec.eta; - psisq = fabs(1.0 - etasq); - coef = qzms24 * pow(tsi, 4.0); - coef1 = coef / pow(psisq, 3.5); - cc2 = coef1 * satrec.no * (ao * (1.0 + 1.5 * etasq + eeta * - (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec.con41 * - (8.0 + 3.0 * etasq * (8.0 + etasq))); - satrec.cc1 = satrec.bstar * cc2; - cc3 = 0.0; - if (satrec.ecco > 1.0e-4) - cc3 = -2.0 * coef * tsi * j3oj2 * satrec.no * sinio / satrec.ecco; - satrec.x1mth2 = 1.0 - cosio2; - satrec.cc4 = 2.0* satrec.no * coef1 * ao * omeosq * - (satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco * - (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) * - (-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq * - (1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 * - (2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec.argpo))); - satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * - (etasq + eeta) + eeta * etasq); - cosio4 = cosio2 * cosio2; - temp1 = 1.5 * j2 * pinvsq * satrec.no; - temp2 = 0.5 * temp1 * j2 * pinvsq; - temp3 = -0.46875 * j4 * pinvsq * pinvsq * satrec.no; - satrec.mdot = satrec.no + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 * - temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4); - satrec.argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 * - (7.0 - 114.0 * cosio2 + 395.0 * cosio4) + - temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4); - xhdot1 = -temp1 * cosio; - satrec.nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + - 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio; - xpidot = satrec.argpdot+ satrec.nodedot; - satrec.omgcof = satrec.bstar * cc3 * cos(satrec.argpo); - satrec.xmcof = 0.0; - if (satrec.ecco > 1.0e-4) - satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta; - satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1; - satrec.t2cof = 1.5 * satrec.cc1; - // sgp4fix for divide by zero with xinco = 180 deg - if (fabs(cosio+1.0) > 1.5e-12) - satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio); - else - satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4; - satrec.aycof = -0.5 * j3oj2 * sinio; - satrec.delmo = pow((1.0 + satrec.eta * cos(satrec.mo)), 3); - satrec.sinmao = sin(satrec.mo); - satrec.x7thm1 = 7.0 * cosio2 - 1.0; - - /* --------------- deep space initialization ------------- */ - if ((2*pi / satrec.no) >= 225.0) - { - satrec.method = 'd'; - satrec.isimp = 1; - tc = 0.0; - inclm = satrec.inclo; - - dscom - ( - epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo, - satrec.no, snodm, cnodm, sinim, cosim,sinomm, cosomm, - day, satrec.e3, satrec.ee2, em, emsq, gam, - satrec.peo, satrec.pgho, satrec.pho, satrec.pinco, - satrec.plo, rtemsq, satrec.se2, satrec.se3, - satrec.sgh2, satrec.sgh3, satrec.sgh4, - satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, - satrec.sl2, satrec.sl3, satrec.sl4, s1, s2, s3, s4, s5, - s6, s7, ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3, - sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33, - satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2, - satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, - satrec.xl3, satrec.xl4, nm, z1, z2, z3, z11, - z12, z13, z21, z22, z23, z31, z32, z33, - satrec.zmol, satrec.zmos - ); - dpper - ( - satrec.e3, satrec.ee2, satrec.peo, satrec.pgho, - satrec.pho, satrec.pinco, satrec.plo, satrec.se2, - satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4, - satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, - satrec.sl2, satrec.sl3, satrec.sl4, satrec.t, - satrec.xgh2,satrec.xgh3,satrec.xgh4, satrec.xh2, - satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, - satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init, - satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo - ); - - argpm = 0.0; - nodem = 0.0; - mm = 0.0; - - dsinit - ( - whichconst, - cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4, - ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc, - satrec.gsto, satrec.mo, satrec.mdot, satrec.no, satrec.nodeo, - satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33, - satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem, - satrec.irez, satrec.atime, - satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222 , - satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232, - satrec.d5421, satrec.d5433, satrec.dedt, satrec.didt, - satrec.dmdt, dndt, satrec.dnodt, satrec.domdt , - satrec.del1, satrec.del2, satrec.del3, satrec.xfact, - satrec.xlamo, satrec.xli, satrec.xni - ); - } - - /* ----------- set variables if not deep space ----------- */ - if (satrec.isimp != 1) - { - cc1sq = satrec.cc1 * satrec.cc1; - satrec.d2 = 4.0 * ao * tsi * cc1sq; - temp = satrec.d2 * tsi * satrec.cc1 / 3.0; - satrec.d3 = (17.0 * ao + sfour) * temp; - satrec.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * - satrec.cc1; - satrec.t3cof = satrec.d2 + 2.0 * cc1sq; - satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 * - (12.0 * satrec.d2 + 10.0 * cc1sq)); - satrec.t5cof = 0.2 * (3.0 * satrec.d4 + - 12.0 * satrec.cc1 * satrec.d3 + - 6.0 * satrec.d2 * satrec.d2 + - 15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq)); - } - } // if omeosq = 0 ... - - /* finally propogate to zero epoch to initialise all others. */ - if(satrec.error == 0) - sgp4(whichconst, satrec, 0.0, r, v); - - satrec.init = 'n'; - -//#include "debug6.cpp" - return satrec.error; -} // end sgp4init - -/*----------------------------------------------------------------------------- -* -* procedure sgp4 -* -* this procedure is the sgp4 prediction model from space command. this is an -* updated and combined version of sgp4 and sdp4, which were originally -* published separately in spacetrack report #3. this version follows the -* methodology from the aiaa paper (2006) describing the history and -* development of the code. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* satrec - initialised structure from sgp4init() call. -* tsince - time eince epoch (minutes) -* -* outputs : -* r - position vector km -* v - velocity km/sec -* return code - non-zero on error. -* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er -* 2 - mean motion less than 0.0 -* 3 - pert elements, ecc < 0.0 or ecc > 1.0 -* 4 - semi-latus rectum < 0.0 -* 5 - epoch elements are sub-orbital -* 6 - satellite has decayed -* -* locals : -* am - -* axnl, aynl - -* betal - -* cosim , sinim , cosomm , sinomm , cnod , snod , cos2u , -* sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip , -* cosisq , cossu , sinsu , cosu , sinu -* delm - -* delomg - -* dndt - -* eccm - -* emsq - -* ecose - -* el2 - -* eo1 - -* eccp - -* esine - -* argpm - -* argpp - -* omgadf - -* pl - -* r - -* rtemsq - -* rdotl - -* rl - -* rvdot - -* rvdotl - -* su - -* t2 , t3 , t4 , tc -* tem5, temp , temp1 , temp2 , tempa , tempe , templ -* u , ux , uy , uz , vx , vy , vz -* inclm - inclination -* mm - mean anomaly -* nm - mean motion -* nodem - right asc of ascending node -* xinc - -* xincp - -* xl - -* xlm - -* mp - -* xmdf - -* xmx - -* xmy - -* nodedf - -* xnode - -* nodep - -* np - -* -* coupling : -* getgravconst- -* dpper -* dpspace -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -int sgp4 - ( - gravconsttype whichconst, elsetrec& satrec, double tsince, - double r[3], double v[3] - ) -{ - double am , axnl , aynl , betal , cosim , cnod , - cos2u, coseo1, cosi , cosip , cosisq, cossu , cosu, - delm , delomg, em , emsq , ecose , el2 , eo1 , - ep , esine , argpm, argpp , argpdf, pl, mrt = 0.0, - mvt , rdotl , rl , rvdot , rvdotl, sinim , - sin2u, sineo1, sini , sinip , sinsu , sinu , - snod , su , t2 , t3 , t4 , tem5 , temp, - temp1, temp2 , tempa, tempe , templ , u , ux , - uy , uz , vx , vy , vz , inclm , mm , - nm , nodem, xinc , xincp , xl , xlm , mp , - xmdf , xmx , xmy , nodedf, xnode , nodep, tc , dndt, - twopi, x2o3 , j2 , j3 , tumin, j4 , xke , j3oj2, radiusearthkm, - mu, vkmpersec; - int ktr; - - /* ------------------ set mathematical constants --------------- */ - // sgp4fix divisor for divide by zero check on inclination - const double temp4 = 1.0 + cos(pi-1.0e-9); - twopi = 2.0 * pi; - x2o3 = 2.0 / 3.0; - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - vkmpersec = radiusearthkm * xke/60.0; - - /* --------------------- clear sgp4 error flag ----------------- */ - satrec.t = tsince; - satrec.error = 0; - - /* ------- update for secular gravity and atmospheric drag ----- */ - xmdf = satrec.mo + satrec.mdot * satrec.t; - argpdf = satrec.argpo + satrec.argpdot * satrec.t; - nodedf = satrec.nodeo + satrec.nodedot * satrec.t; - argpm = argpdf; - mm = xmdf; - t2 = satrec.t * satrec.t; - nodem = nodedf + satrec.nodecf * t2; - tempa = 1.0 - satrec.cc1 * satrec.t; - tempe = satrec.bstar * satrec.cc4 * satrec.t; - templ = satrec.t2cof * t2; - - if (satrec.isimp != 1) - { - delomg = satrec.omgcof * satrec.t; - delm = satrec.xmcof * - (pow((1.0 + satrec.eta * cos(xmdf)), 3) - - satrec.delmo); - temp = delomg + delm; - mm = xmdf + temp; - argpm = argpdf - temp; - t3 = t2 * satrec.t; - t4 = t3 * satrec.t; - tempa = tempa - satrec.d2 * t2 - satrec.d3 * t3 - - satrec.d4 * t4; - tempe = tempe + satrec.bstar * satrec.cc5 * (sin(mm) - - satrec.sinmao); - templ = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof + - satrec.t * satrec.t5cof); - } - - nm = satrec.no; - em = satrec.ecco; - inclm = satrec.inclo; - if (satrec.method == 'd') - { - tc = satrec.t; - dspace - ( - satrec.irez, - satrec.d2201, satrec.d2211, satrec.d3210, - satrec.d3222, satrec.d4410, satrec.d4422, - satrec.d5220, satrec.d5232, satrec.d5421, - satrec.d5433, satrec.dedt, satrec.del1, - satrec.del2, satrec.del3, satrec.didt, - satrec.dmdt, satrec.dnodt, satrec.domdt, - satrec.argpo, satrec.argpdot, satrec.t, tc, - satrec.gsto, satrec.xfact, satrec.xlamo, - satrec.no, satrec.atime, - em, argpm, inclm, satrec.xli, mm, satrec.xni, - nodem, dndt, nm - ); - } // if method = d - - if (nm <= 0.0) - { -// printf("# error nm %f\n", nm); - satrec.error = 2; - } - am = pow((xke / nm),x2o3) * tempa * tempa; - nm = xke / pow(am, 1.5); - em = em - tempe; - - // fix tolerance for error recognition - if ((em >= 1.0) || (em < -0.001) || (am < 0.95)) - { -// printf("# error em %f\n", em); - satrec.error = 1; - } - if (em < 0.0) - em = 1.0e-6; - mm = mm + satrec.no * templ; - xlm = mm + argpm + nodem; - emsq = em * em; - temp = 1.0 - emsq; - - nodem = fmod(nodem, twopi); - argpm = fmod(argpm, twopi); - xlm = fmod(xlm, twopi); - mm = fmod(xlm - argpm - nodem, twopi); - - /* ----------------- compute extra mean quantities ------------- */ - sinim = sin(inclm); - cosim = cos(inclm); - - /* -------------------- add lunar-solar periodics -------------- */ - ep = em; - xincp = inclm; - argpp = argpm; - nodep = nodem; - mp = mm; - sinip = sinim; - cosip = cosim; - if (satrec.method == 'd') - { - dpper - ( - satrec.e3, satrec.ee2, satrec.peo, - satrec.pgho, satrec.pho, satrec.pinco, - satrec.plo, satrec.se2, satrec.se3, - satrec.sgh2, satrec.sgh3, satrec.sgh4, - satrec.sh2, satrec.sh3, satrec.si2, - satrec.si3, satrec.sl2, satrec.sl3, - satrec.sl4, satrec.t, satrec.xgh2, - satrec.xgh3, satrec.xgh4, satrec.xh2, - satrec.xh3, satrec.xi2, satrec.xi3, - satrec.xl2, satrec.xl3, satrec.xl4, - satrec.zmol, satrec.zmos, satrec.inclo, - 'n', ep, xincp, nodep, argpp, mp - ); - if (xincp < 0.0) - { - xincp = -xincp; - nodep = nodep + pi; - argpp = argpp - pi; - } - if ((ep < 0.0 ) || ( ep > 1.0)) - { - // printf("# error ep %f\n", ep); - satrec.error = 3; - } - } // if method = d - - /* -------------------- long period periodics ------------------ */ - if (satrec.method == 'd') - { - sinip = sin(xincp); - cosip = cos(xincp); - satrec.aycof = -0.5*j3oj2*sinip; - // sgp4fix for divide by zero for xincp = 180 deg - if (fabs(cosip+1.0) > 1.5e-12) - satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip); - else - satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4; - } - axnl = ep * cos(argpp); - temp = 1.0 / (am * (1.0 - ep * ep)); - aynl = ep* sin(argpp) + temp * satrec.aycof; - xl = mp + argpp + nodep + temp * satrec.xlcof * axnl; - - /* --------------------- solve kepler's equation --------------- */ - u = fmod(xl - nodep, twopi); - eo1 = u; - tem5 = 9999.9; - ktr = 1; - // sgp4fix for kepler iteration - // the following iteration needs better limits on corrections - while (( fabs(tem5) >= 1.0e-12) && (ktr <= 10) ) - { - sineo1 = sin(eo1); - coseo1 = cos(eo1); - tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl; - tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5; - if(fabs(tem5) >= 0.95) - tem5 = tem5 > 0.0 ? 0.95 : -0.95; - eo1 = eo1 + tem5; - ktr = ktr + 1; - } - - /* ------------- short period preliminary quantities ----------- */ - ecose = axnl*coseo1 + aynl*sineo1; - esine = axnl*sineo1 - aynl*coseo1; - el2 = axnl*axnl + aynl*aynl; - pl = am*(1.0-el2); - if (pl < 0.0) - { -// printf("# error pl %f\n", pl); - satrec.error = 4; - } - else - { - rl = am * (1.0 - ecose); - rdotl = sqrt(am) * esine/rl; - rvdotl = sqrt(pl) / rl; - betal = sqrt(1.0 - el2); - temp = esine / (1.0 + betal); - sinu = am / rl * (sineo1 - aynl - axnl * temp); - cosu = am / rl * (coseo1 - axnl + aynl * temp); - su = atan2(sinu, cosu); - sin2u = (cosu + cosu) * sinu; - cos2u = 1.0 - 2.0 * sinu * sinu; - temp = 1.0 / pl; - temp1 = 0.5 * j2 * temp; - temp2 = temp1 * temp; - - /* -------------- update for short period periodics ------------ */ - if (satrec.method == 'd') - { - cosisq = cosip * cosip; - satrec.con41 = 3.0*cosisq - 1.0; - satrec.x1mth2 = 1.0 - cosisq; - satrec.x7thm1 = 7.0*cosisq - 1.0; - } - mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) + - 0.5 * temp1 * satrec.x1mth2 * cos2u; - su = su - 0.25 * temp2 * satrec.x7thm1 * sin2u; - xnode = nodep + 1.5 * temp2 * cosip * sin2u; - xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u; - mvt = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / xke; - rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u + - 1.5 * satrec.con41) / xke; - - /* --------------------- orientation vectors ------------------- */ - sinsu = sin(su); - cossu = cos(su); - snod = sin(xnode); - cnod = cos(xnode); - sini = sin(xinc); - cosi = cos(xinc); - xmx = -snod * cosi; - xmy = cnod * cosi; - ux = xmx * sinsu + cnod * cossu; - uy = xmy * sinsu + snod * cossu; - uz = sini * sinsu; - vx = xmx * cossu - cnod * sinsu; - vy = xmy * cossu - snod * sinsu; - vz = sini * cossu; - - /* --------- position and velocity (in km and km/sec) ---------- */ - r[0] = (mrt * ux)* radiusearthkm; - r[1] = (mrt * uy)* radiusearthkm; - r[2] = (mrt * uz)* radiusearthkm; - v[0] = (mvt * ux + rvdot * vx) * vkmpersec; - v[1] = (mvt * uy + rvdot * vy) * vkmpersec; - v[2] = (mvt * uz + rvdot * vz) * vkmpersec; - } // if pl > 0 - - // sgp4fix for decaying satellites - if (mrt < 1.0) - { -// printf("# decay condition %11.6f \n",mrt); - satrec.error = 6; - } - - -//#include "debug7.cpp" - return satrec.error; -} // end sgp4 - - -/* ----------------------------------------------------------------------------- -* -* function gstime -* -* this function finds the greenwich sidereal time. -* -* author : david vallado 719-573-2600 1 mar 2001 -* -* inputs description range / units -* jdut1 - julian date in ut1 days from 4713 bc -* -* outputs : -* gstime - greenwich sidereal time 0 to 2pi rad -* -* locals : -* temp - temporary variable for doubles rad -* tut1 - julian centuries from the -* jan 1, 2000 12 h epoch (ut1) -* -* coupling : -* none -* -* references : -* vallado 2004, 191, eq 3-45 -* --------------------------------------------------------------------------- */ - -double gstime - ( - double jdut1 - ) - { - const double twopi = 2.0 * pi; - const double deg2rad = pi / 180.0; - double temp, tut1; - - tut1 = (jdut1 - 2451545.0) / 36525.0; - temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 + - (876600.0*3600 + 8640184.812866) * tut1 + 67310.54841; // sec - temp = fmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad - - // ------------------------ check quadrants --------------------- - if (temp < 0.0) - temp += twopi; - - return temp; - } // end gstime - -/* ----------------------------------------------------------------------------- -* -* function getgravconst -* -* this function gets constants for the propagator. note that mu is identified to -* facilitiate comparisons with newer models. the common useage is wgs72. -* -* author : david vallado 719-573-2600 21 jul 2006 -* -* inputs : -* whichconst - which set of constants to use wgs72old, wgs72, wgs84 -* -* outputs : -* tumin - minutes in one time unit -* mu - earth gravitational parameter -* radiusearthkm - radius of the earth in km -* xke - reciprocal of tumin -* j2, j3, j4 - un-normalized zonal harmonic values -* j3oj2 - j3 divided by j2 -* -* locals : -* -* coupling : -* none -* -* references : -* norad spacetrack report #3 -* vallado, crawford, hujsak, kelso 2006 - --------------------------------------------------------------------------- */ - -void getgravconst - ( - gravconsttype whichconst, - double& tumin, - double& mu, - double& radiusearthkm, - double& xke, - double& j2, - double& j3, - double& j4, - double& j3oj2 - ) - { - - switch (whichconst) - { - // -- wgs-72 low precision str#3 constants -- - case wgs72old: - mu = 398600.79964; // in km3 / s2 - radiusearthkm = 6378.135; // km - xke = 0.0743669161; - tumin = 1.0 / xke; - j2 = 0.001082616; - j3 = -0.00000253881; - j4 = -0.00000165597; - j3oj2 = j3 / j2; - break; - // ------------ wgs-72 constants ------------ - case wgs72: - mu = 398600.8; // in km3 / s2 - radiusearthkm = 6378.135; // km - xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); - tumin = 1.0 / xke; - j2 = 0.001082616; - j3 = -0.00000253881; - j4 = -0.00000165597; - j3oj2 = j3 / j2; - break; - case wgs84: - // ------------ wgs-84 constants ------------ - mu = 398600.5; // in km3 / s2 - radiusearthkm = 6378.137; // km - xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); - tumin = 1.0 / xke; - j2 = 0.00108262998905; - j3 = -0.00000253215306; - j4 = -0.00000161098761; - j3oj2 = j3 / j2; - break; - default: - fprintf(stderr,"unknown gravity option (%d)\n",whichconst); - break; - } - - } // end getgravconst - - - - - +/* ---------------------------------------------------------------- +* +* sgp4unit.cpp +* +* this file contains the sgp4 procedures for analytical propagation +* of a satellite. the code was originally released in the 1980 and 1986 +* spacetrack papers. a detailed discussion of the theory and history +* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, +* and kelso. +* +* companion code for +* fundamentals of astrodynamics and applications +* 2007 +* by david vallado +* +* (w) 719-573-2600, email dvallado@agi.com +* +* current : +* 16 nov 07 david vallado +* misc fixes for better compliance +* changes : +* 20 apr 07 david vallado +* misc fixes for constants +* 11 aug 06 david vallado +* chg lyddane choice back to strn3, constants, misc doc +* 15 dec 05 david vallado +* misc fixes +* 26 jul 05 david vallado +* fixes for paper +* note that each fix is preceded by a +* comment with "sgp4fix" and an explanation of +* what was changed +* 10 aug 04 david vallado +* 2nd printing baseline working +* 14 may 01 david vallado +* 2nd edition baseline +* 80 norad +* original baseline +* ---------------------------------------------------------------- */ + +#include "sgp4unit.h" + +const char help = 'n'; +FILE *dbgfile; + +#define pi 3.14159265358979323846 + + +/* ----------- local functions - only ever used internally by sgp4 ---------- */ +static void dpper + ( + double e3, double ee2, double peo, double pgho, double pho, + double pinco, double plo, double se2, double se3, double sgh2, + double sgh3, double sgh4, double sh2, double sh3, double si2, + double si3, double sl2, double sl3, double sl4, double t, + double xgh2, double xgh3, double xgh4, double xh2, double xh3, + double xi2, double xi3, double xl2, double xl3, double xl4, + double zmol, double zmos, double inclo, + char init, + double& ep, double& inclp, double& nodep, double& argpp, double& mp + ); + +static void dscom + ( + double epoch, double ep, double argpp, double tc, double inclp, + double nodep, double np, + double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, + double& cosomm,double& day, double& e3, double& ee2, double& em, + double& emsq, double& gam, double& peo, double& pgho, double& pho, + double& pinco, double& plo, double& rtemsq, double& se2, double& se3, + double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, + double& si2, double& si3, double& sl2, double& sl3, double& sl4, + double& s1, double& s2, double& s3, double& s4, double& s5, + double& s6, double& s7, double& ss1, double& ss2, double& ss3, + double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, + double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, + double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, + double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, + double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, + double& xl4, double& nm, double& z1, double& z2, double& z3, + double& z11, double& z12, double& z13, double& z21, double& z22, + double& z23, double& z31, double& z32, double& z33, double& zmol, + double& zmos + ); + +static void dsinit + ( + gravconsttype whichconst, + double cosim, double emsq, double argpo, double s1, double s2, + double s3, double s4, double s5, double sinim, double ss1, + double ss2, double ss3, double ss4, double ss5, double sz1, + double sz3, double sz11, double sz13, double sz21, double sz23, + double sz31, double sz33, double t, double tc, double gsto, + double mo, double mdot, double no, double nodeo, double nodedot, + double xpidot, double z1, double z3, double z11, double z13, + double z21, double z23, double z31, double z33, double ecco, + double eccsq, double& em, double& argpm, double& inclm, double& mm, + double& nm, double& nodem, + int& irez, + double& atime, double& d2201, double& d2211, double& d3210, double& d3222, + double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, + double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, + double& dnodt, double& domdt, double& del1, double& del2, double& del3, + double& xfact, double& xlamo, double& xli, double& xni + ); + +static void dspace + ( + int irez, + double d2201, double d2211, double d3210, double d3222, double d4410, + double d4422, double d5220, double d5232, double d5421, double d5433, + double dedt, double del1, double del2, double del3, double didt, + double dmdt, double dnodt, double domdt, double argpo, double argpdot, + double t, double tc, double gsto, double xfact, double xlamo, + double no, + double& atime, double& em, double& argpm, double& inclm, double& xli, + double& mm, double& xni, double& nodem, double& dndt, double& nm + ); + +static void initl + ( + int satn, gravconsttype whichconst, + double ecco, double epoch, double inclo, double& no, + char& method, + double& ainv, double& ao, double& con41, double& con42, double& cosio, + double& cosio2,double& eccsq, double& omeosq, double& posq, + double& rp, double& rteosq,double& sinio , double& gsto + ); + +/* ----------------------------------------------------------------------------- +* +* procedure dpper +* +* this procedure provides deep space long period periodic contributions +* to the mean elements. by design, these periodics are zero at epoch. +* this used to be dscom which included initialization, but it's really a +* recurring function. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* e3 - +* ee2 - +* peo - +* pgho - +* pho - +* pinco - +* plo - +* se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 - +* t - +* xh2, xh3, xi2, xi3, xl2, xl3, xl4 - +* zmol - +* zmos - +* ep - eccentricity 0.0 - 1.0 +* inclo - inclination - needed for lyddane modification +* nodep - right ascension of ascending node +* argpp - argument of perigee +* mp - mean anomaly +* +* outputs : +* ep - eccentricity 0.0 - 1.0 +* inclp - inclination +* nodep - right ascension of ascending node +* argpp - argument of perigee +* mp - mean anomaly +* +* locals : +* alfdp - +* betdp - +* cosip , sinip , cosop , sinop , +* dalf - +* dbet - +* dls - +* f2, f3 - +* pe - +* pgh - +* ph - +* pinc - +* pl - +* sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis , +* sll , sls +* xls - +* xnoh - +* zf - +* zm - +* +* coupling : +* none. +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dpper + ( + double e3, double ee2, double peo, double pgho, double pho, + double pinco, double plo, double se2, double se3, double sgh2, + double sgh3, double sgh4, double sh2, double sh3, double si2, + double si3, double sl2, double sl3, double sl4, double t, + double xgh2, double xgh3, double xgh4, double xh2, double xh3, + double xi2, double xi3, double xl2, double xl3, double xl4, + double zmol, double zmos, double inclo, + char init, + double& ep, double& inclp, double& nodep, double& argpp, double& mp + ) +{ + /* --------------------- local variables ------------------------ */ + const double twopi = 2.0 * pi; + double alfdp, betdp, cosip, cosop, dalf, dbet, dls, + f2, f3, pe, pgh, ph, pinc, pl , + sel, ses, sghl, sghs, shll, shs, sil, + sinip, sinop, sinzf, sis, sll, sls, xls, + xnoh, zf, zm, zel, zes, znl, zns; + + /* ---------------------- constants ----------------------------- */ + zns = 1.19459e-5; + zes = 0.01675; + znl = 1.5835218e-4; + zel = 0.05490; + + /* --------------- calculate time varying periodics ----------- */ + zm = zmos + zns * t; + // be sure that the initial call has time set to zero + if (init == 'y') + zm = zmos; + zf = zm + 2.0 * zes * sin(zm); + sinzf = sin(zf); + f2 = 0.5 * sinzf * sinzf - 0.25; + f3 = -0.5 * sinzf * cos(zf); + ses = se2* f2 + se3 * f3; + sis = si2 * f2 + si3 * f3; + sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; + sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; + shs = sh2 * f2 + sh3 * f3; + zm = zmol + znl * t; + if (init == 'y') + zm = zmol; + zf = zm + 2.0 * zel * sin(zm); + sinzf = sin(zf); + f2 = 0.5 * sinzf * sinzf - 0.25; + f3 = -0.5 * sinzf * cos(zf); + sel = ee2 * f2 + e3 * f3; + sil = xi2 * f2 + xi3 * f3; + sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; + sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; + shll = xh2 * f2 + xh3 * f3; + pe = ses + sel; + pinc = sis + sil; + pl = sls + sll; + pgh = sghs + sghl; + ph = shs + shll; + + if (init == 'n') + { + pe = pe - peo; + pinc = pinc - pinco; + pl = pl - plo; + pgh = pgh - pgho; + ph = ph - pho; + inclp = inclp + pinc; + ep = ep + pe; + sinip = sin(inclp); + cosip = cos(inclp); + + /* ----------------- apply periodics directly ------------ */ + // sgp4fix for lyddane choice + // strn3 used original inclination - this is technically feasible + // gsfc used perturbed inclination - also technically feasible + // probably best to readjust the 0.2 limit value and limit discontinuity + // 0.2 rad = 11.45916 deg + // use next line for original strn3 approach and original inclination + // if (inclo >= 0.2) + // use next line for gsfc version and perturbed inclination + if (inclp >= 0.2) + { + ph = ph / sinip; + pgh = pgh - cosip * ph; + argpp = argpp + pgh; + nodep = nodep + ph; + mp = mp + pl; + } + else + { + /* ---- apply periodics with lyddane modification ---- */ + sinop = sin(nodep); + cosop = cos(nodep); + alfdp = sinip * sinop; + betdp = sinip * cosop; + dalf = ph * cosop + pinc * cosip * sinop; + dbet = -ph * sinop + pinc * cosip * cosop; + alfdp = alfdp + dalf; + betdp = betdp + dbet; + nodep = fmod(nodep, twopi); + // sgp4fix for afspc written intrinsic functions + // nodep used without a trigonometric function ahead + if (nodep < 0.0) + nodep = nodep + twopi; + xls = mp + argpp + cosip * nodep; + dls = pl + pgh - pinc * nodep * sinip; + xls = xls + dls; + xnoh = nodep; + nodep = atan2(alfdp, betdp); + // sgp4fix for afspc written intrinsic functions + // nodep used without a trigonometric function ahead + if (nodep < 0.0) + nodep = nodep + twopi; + if (fabs(xnoh - nodep) > pi){ + if (nodep < xnoh) + nodep = nodep + twopi; + else + nodep = nodep - twopi; + } + mp = mp + pl; + argpp = xls - mp - cosip * nodep; + } + } // if init == 'n' + +//#include "debug1.cpp" +} // end dpper + +/*----------------------------------------------------------------------------- +* +* procedure dscom +* +* this procedure provides deep space common items used by both the secular +* and periodics subroutines. input is provided as shown. this routine +* used to be called dpper, but the functions inside weren't well organized. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* epoch - +* ep - eccentricity +* argpp - argument of perigee +* tc - +* inclp - inclination +* nodep - right ascension of ascending node +* np - mean motion +* +* outputs : +* sinim , cosim , sinomm , cosomm , snodm , cnodm +* day - +* e3 - +* ee2 - +* em - eccentricity +* emsq - eccentricity squared +* gam - +* peo - +* pgho - +* pho - +* pinco - +* plo - +* rtemsq - +* se2, se3 - +* sgh2, sgh3, sgh4 - +* sh2, sh3, si2, si3, sl2, sl3, sl4 - +* s1, s2, s3, s4, s5, s6, s7 - +* ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 - +* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - +* xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 - +* nm - mean motion +* z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 - +* zmol - +* zmos - +* +* locals : +* a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 - +* betasq - +* cc - +* ctem, stem - +* x1, x2, x3, x4, x5, x6, x7, x8 - +* xnodce - +* xnoi - +* zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl , +* zcosi , zsini , zcosil , zsinil , +* zx - +* zy - +* +* coupling : +* none. +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dscom + ( + double epoch, double ep, double argpp, double tc, double inclp, + double nodep, double np, + double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, + double& cosomm,double& day, double& e3, double& ee2, double& em, + double& emsq, double& gam, double& peo, double& pgho, double& pho, + double& pinco, double& plo, double& rtemsq, double& se2, double& se3, + double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, + double& si2, double& si3, double& sl2, double& sl3, double& sl4, + double& s1, double& s2, double& s3, double& s4, double& s5, + double& s6, double& s7, double& ss1, double& ss2, double& ss3, + double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, + double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, + double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, + double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, + double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, + double& xl4, double& nm, double& z1, double& z2, double& z3, + double& z11, double& z12, double& z13, double& z21, double& z22, + double& z23, double& z31, double& z32, double& z33, double& zmol, + double& zmos + ) +{ + /* -------------------------- constants ------------------------- */ + const double zes = 0.01675; + const double zel = 0.05490; + const double c1ss = 2.9864797e-6; + const double c1l = 4.7968065e-7; + const double zsinis = 0.39785416; + const double zcosis = 0.91744867; + const double zcosgs = 0.1945905; + const double zsings = -0.98088458; + const double twopi = 2.0 * pi; + + /* --------------------- local variables ------------------------ */ + int lsflg; + double a1 , a2 , a3 , a4 , a5 , a6 , a7 , + a8 , a9 , a10 , betasq, cc , ctem , stem , + x1 , x2 , x3 , x4 , x5 , x6 , x7 , + x8 , xnodce, xnoi , zcosg , zcosgl, zcosh , zcoshl, + zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini , + zsinil, zx , zy; + + nm = np; + em = ep; + snodm = sin(nodep); + cnodm = cos(nodep); + sinomm = sin(argpp); + cosomm = cos(argpp); + sinim = sin(inclp); + cosim = cos(inclp); + emsq = em * em; + betasq = 1.0 - emsq; + rtemsq = sqrt(betasq); + + /* ----------------- initialize lunar solar terms --------------- */ + peo = 0.0; + pinco = 0.0; + plo = 0.0; + pgho = 0.0; + pho = 0.0; + day = epoch + 18261.5 + tc / 1440.0; + xnodce = fmod(4.5236020 - 9.2422029e-4 * day, twopi); + stem = sin(xnodce); + ctem = cos(xnodce); + zcosil = 0.91375164 - 0.03568096 * ctem; + zsinil = sqrt(1.0 - zcosil * zcosil); + zsinhl = 0.089683511 * stem / zsinil; + zcoshl = sqrt(1.0 - zsinhl * zsinhl); + gam = 5.8351514 + 0.0019443680 * day; + zx = 0.39785416 * stem / zsinil; + zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; + zx = atan2(zx, zy); + zx = gam + zx - xnodce; + zcosgl = cos(zx); + zsingl = sin(zx); + + /* ------------------------- do solar terms --------------------- */ + zcosg = zcosgs; + zsing = zsings; + zcosi = zcosis; + zsini = zsinis; + zcosh = cnodm; + zsinh = snodm; + cc = c1ss; + xnoi = 1.0 / nm; + + for (lsflg = 1; lsflg <= 2; lsflg++) + { + a1 = zcosg * zcosh + zsing * zcosi * zsinh; + a3 = -zsing * zcosh + zcosg * zcosi * zsinh; + a7 = -zcosg * zsinh + zsing * zcosi * zcosh; + a8 = zsing * zsini; + a9 = zsing * zsinh + zcosg * zcosi * zcosh; + a10 = zcosg * zsini; + a2 = cosim * a7 + sinim * a8; + a4 = cosim * a9 + sinim * a10; + a5 = -sinim * a7 + cosim * a8; + a6 = -sinim * a9 + cosim * a10; + + x1 = a1 * cosomm + a2 * sinomm; + x2 = a3 * cosomm + a4 * sinomm; + x3 = -a1 * sinomm + a2 * cosomm; + x4 = -a3 * sinomm + a4 * cosomm; + x5 = a5 * sinomm; + x6 = a6 * sinomm; + x7 = a5 * cosomm; + x8 = a6 * cosomm; + + z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; + z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; + z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; + z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq; + z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq; + z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq; + z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7-6.0 * x3 * x5); + z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq * + (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); + z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); + z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); + z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq * + (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); + z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8); + z1 = z1 + z1 + betasq * z31; + z2 = z2 + z2 + betasq * z32; + z3 = z3 + z3 + betasq * z33; + s3 = cc * xnoi; + s2 = -0.5 * s3 / rtemsq; + s4 = s3 * rtemsq; + s1 = -15.0 * em * s4; + s5 = x1 * x3 + x2 * x4; + s6 = x2 * x3 + x1 * x4; + s7 = x2 * x4 - x1 * x3; + + /* ----------------------- do lunar terms ------------------- */ + if (lsflg == 1) + { + ss1 = s1; + ss2 = s2; + ss3 = s3; + ss4 = s4; + ss5 = s5; + ss6 = s6; + ss7 = s7; + sz1 = z1; + sz2 = z2; + sz3 = z3; + sz11 = z11; + sz12 = z12; + sz13 = z13; + sz21 = z21; + sz22 = z22; + sz23 = z23; + sz31 = z31; + sz32 = z32; + sz33 = z33; + zcosg = zcosgl; + zsing = zsingl; + zcosi = zcosil; + zsini = zsinil; + zcosh = zcoshl * cnodm + zsinhl * snodm; + zsinh = snodm * zcoshl - cnodm * zsinhl; + cc = c1l; + } + } + + zmol = fmod(4.7199672 + 0.22997150 * day - gam, twopi); + zmos = fmod(6.2565837 + 0.017201977 * day, twopi); + + /* ------------------------ do solar terms ---------------------- */ + se2 = 2.0 * ss1 * ss6; + se3 = 2.0 * ss1 * ss7; + si2 = 2.0 * ss2 * sz12; + si3 = 2.0 * ss2 * (sz13 - sz11); + sl2 = -2.0 * ss3 * sz2; + sl3 = -2.0 * ss3 * (sz3 - sz1); + sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes; + sgh2 = 2.0 * ss4 * sz32; + sgh3 = 2.0 * ss4 * (sz33 - sz31); + sgh4 = -18.0 * ss4 * zes; + sh2 = -2.0 * ss2 * sz22; + sh3 = -2.0 * ss2 * (sz23 - sz21); + + /* ------------------------ do lunar terms ---------------------- */ + ee2 = 2.0 * s1 * s6; + e3 = 2.0 * s1 * s7; + xi2 = 2.0 * s2 * z12; + xi3 = 2.0 * s2 * (z13 - z11); + xl2 = -2.0 * s3 * z2; + xl3 = -2.0 * s3 * (z3 - z1); + xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel; + xgh2 = 2.0 * s4 * z32; + xgh3 = 2.0 * s4 * (z33 - z31); + xgh4 = -18.0 * s4 * zel; + xh2 = -2.0 * s2 * z22; + xh3 = -2.0 * s2 * (z23 - z21); + +//#include "debug2.cpp" +} // end dscom + +/*----------------------------------------------------------------------------- +* +* procedure dsinit +* +* this procedure provides deep space contributions to mean motion dot due +* to geopotential resonance with half day and one day orbits. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* cosim, sinim- +* emsq - eccentricity squared +* argpo - argument of perigee +* s1, s2, s3, s4, s5 - +* ss1, ss2, ss3, ss4, ss5 - +* sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 - +* t - time +* tc - +* gsto - greenwich sidereal time rad +* mo - mean anomaly +* mdot - mean anomaly dot (rate) +* no - mean motion +* nodeo - right ascension of ascending node +* nodedot - right ascension of ascending node dot (rate) +* xpidot - +* z1, z3, z11, z13, z21, z23, z31, z33 - +* eccm - eccentricity +* argpm - argument of perigee +* inclm - inclination +* mm - mean anomaly +* xn - mean motion +* nodem - right ascension of ascending node +* +* outputs : +* em - eccentricity +* argpm - argument of perigee +* inclm - inclination +* mm - mean anomaly +* nm - mean motion +* nodem - right ascension of ascending node +* irez - flag for resonance 0-none, 1-one day, 2-half day +* atime - +* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - +* dedt - +* didt - +* dmdt - +* dndt - +* dnodt - +* domdt - +* del1, del2, del3 - +* ses , sghl , sghs , sgs , shl , shs , sis , sls +* theta - +* xfact - +* xlamo - +* xli - +* xni +* +* locals : +* ainv2 - +* aonv - +* cosisq - +* eoc - +* f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 - +* g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 - +* sini2 - +* temp - +* temp1 - +* theta - +* xno2 - +* +* coupling : +* getgravconst +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dsinit + ( + gravconsttype whichconst, + double cosim, double emsq, double argpo, double s1, double s2, + double s3, double s4, double s5, double sinim, double ss1, + double ss2, double ss3, double ss4, double ss5, double sz1, + double sz3, double sz11, double sz13, double sz21, double sz23, + double sz31, double sz33, double t, double tc, double gsto, + double mo, double mdot, double no, double nodeo, double nodedot, + double xpidot, double z1, double z3, double z11, double z13, + double z21, double z23, double z31, double z33, double ecco, + double eccsq, double& em, double& argpm, double& inclm, double& mm, + double& nm, double& nodem, + int& irez, + double& atime, double& d2201, double& d2211, double& d3210, double& d3222, + double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, + double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, + double& dnodt, double& domdt, double& del1, double& del2, double& del3, + double& xfact, double& xlamo, double& xli, double& xni + ) +{ + /* --------------------- local variables ------------------------ */ + const double twopi = 2.0 * pi; + + double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221 , f311 , + f321 , f322 , f330 , f441 , f442 , f522 , f523 , + f542 , f543 , g200 , g201 , g211 , g300 , g310 , + g322 , g410 , g422 , g520 , g521 , g532 , g533 , + ses , sgs , sghl , sghs , shs , shll , sis , + sini2 , sls , temp , temp1 , theta , xno2 , q22 , + q31 , q33 , root22, root44, root54, rptim , root32, + root52, x2o3 , xke , znl , emo , zns , emsqo, + tumin, mu, radiusearthkm, j2, j3, j4, j3oj2; + + q22 = 1.7891679e-6; + q31 = 2.1460748e-6; + q33 = 2.2123015e-7; + root22 = 1.7891679e-6; + root44 = 7.3636953e-9; + root54 = 2.1765803e-9; + rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec + root32 = 3.7393792e-7; + root52 = 1.1428639e-7; + x2o3 = 2.0 / 3.0; + znl = 1.5835218e-4; + zns = 1.19459e-5; + + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + + /* -------------------- deep space initialization ------------ */ + irez = 0; + if ((nm < 0.0052359877) && (nm > 0.0034906585)) + irez = 1; + if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5)) + irez = 2; + + /* ------------------------ do solar terms ------------------- */ + ses = ss1 * zns * ss5; + sis = ss2 * zns * (sz11 + sz13); + sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq); + sghs = ss4 * zns * (sz31 + sz33 - 6.0); + shs = -zns * ss2 * (sz21 + sz23); + // sgp4fix for 180 deg incl + if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) + shs = 0.0; + if (sinim != 0.0) + shs = shs / sinim; + sgs = sghs - cosim * shs; + + /* ------------------------- do lunar terms ------------------ */ + dedt = ses + s1 * znl * s5; + didt = sis + s2 * znl * (z11 + z13); + dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq); + sghl = s4 * znl * (z31 + z33 - 6.0); + shll = -znl * s2 * (z21 + z23); + // sgp4fix for 180 deg incl + if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) + shll = 0.0; + domdt = sgs + sghl; + dnodt = shs; + if (sinim != 0.0) + { + domdt = domdt - cosim / sinim * shll; + dnodt = dnodt + shll / sinim; + } + + /* ----------- calculate deep space resonance effects -------- */ + dndt = 0.0; + theta = fmod(gsto + tc * rptim, twopi); + em = em + dedt * t; + inclm = inclm + didt * t; + argpm = argpm + domdt * t; + nodem = nodem + dnodt * t; + mm = mm + dmdt * t; + // sgp4fix for negative inclinations + // the following if statement should be commented out + //if (inclm < 0.0) + // { + // inclm = -inclm; + // argpm = argpm - pi; + // nodem = nodem + pi; + // } + + /* -------------- initialize the resonance terms ------------- */ + if (irez != 0) + { + aonv = pow(nm / xke, x2o3); + + /* ---------- geopotential resonance for 12 hour orbits ------ */ + if (irez == 2) + { + cosisq = cosim * cosim; + emo = em; + em = ecco; + emsqo = emsq; + emsq = eccsq; + eoc = em * emsq; + g201 = -0.306 - (em - 0.64) * 0.440; + + if (em <= 0.65) + { + g211 = 3.616 - 13.2470 * em + 16.2900 * emsq; + g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc; + g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc; + g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc; + g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc; + g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc; + } + else + { + g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc; + g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc; + g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc; + g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc; + g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc; + if (em > 0.715) + g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc; + else + g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq; + } + if (em < 0.7) + { + g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc; + g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc; + g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc; + } + else + { + g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc; + g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc; + g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc; + } + + sini2= sinim * sinim; + f220 = 0.75 * (1.0 + 2.0 * cosim+cosisq); + f221 = 1.5 * sini2; + f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); + f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); + f441 = 35.0 * sini2 * f220; + f442 = 39.3750 * sini2 * sini2; + f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) + + 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) ); + f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim + + 10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq)); + f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq * + (-12.0 + 8.0 * cosim + 10.0 * cosisq)); + f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq * + (12.0 + 8.0 * cosim - 10.0 * cosisq)); + xno2 = nm * nm; + ainv2 = aonv * aonv; + temp1 = 3.0 * xno2 * ainv2; + temp = temp1 * root22; + d2201 = temp * f220 * g201; + d2211 = temp * f221 * g211; + temp1 = temp1 * aonv; + temp = temp1 * root32; + d3210 = temp * f321 * g310; + d3222 = temp * f322 * g322; + temp1 = temp1 * aonv; + temp = 2.0 * temp1 * root44; + d4410 = temp * f441 * g410; + d4422 = temp * f442 * g422; + temp1 = temp1 * aonv; + temp = temp1 * root52; + d5220 = temp * f522 * g520; + d5232 = temp * f523 * g532; + temp = 2.0 * temp1 * root54; + d5421 = temp * f542 * g521; + d5433 = temp * f543 * g533; + xlamo = fmod(mo + nodeo + nodeo-theta - theta, twopi); + xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no; + em = emo; + emsq = emsqo; + } + + /* ---------------- synchronous resonance terms -------------- */ + if (irez == 1) + { + g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); + g310 = 1.0 + 2.0 * emsq; + g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq); + f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); + f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); + f330 = 1.0 + cosim; + f330 = 1.875 * f330 * f330 * f330; + del1 = 3.0 * nm * nm * aonv * aonv; + del2 = 2.0 * del1 * f220 * g200 * q22; + del3 = 3.0 * del1 * f330 * g300 * q33 * aonv; + del1 = del1 * f311 * g310 * q31 * aonv; + xlamo = fmod(mo + nodeo + argpo - theta, twopi); + xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no; + } + + /* ------------ for sgp4, initialize the integrator ---------- */ + xli = xlamo; + xni = no; + atime = 0.0; + nm = no + dndt; + } + +//#include "debug3.cpp" +} // end dsinit + +/*----------------------------------------------------------------------------- +* +* procedure dspace +* +* this procedure provides deep space contributions to mean elements for +* perturbing third body. these effects have been averaged over one +* revolution of the sun and moon. for earth resonance effects, the +* effects have been averaged over no revolutions of the satellite. +* (mean motion) +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - +* dedt - +* del1, del2, del3 - +* didt - +* dmdt - +* dnodt - +* domdt - +* irez - flag for resonance 0-none, 1-one day, 2-half day +* argpo - argument of perigee +* argpdot - argument of perigee dot (rate) +* t - time +* tc - +* gsto - gst +* xfact - +* xlamo - +* no - mean motion +* atime - +* em - eccentricity +* ft - +* argpm - argument of perigee +* inclm - inclination +* xli - +* mm - mean anomaly +* xni - mean motion +* nodem - right ascension of ascending node +* +* outputs : +* atime - +* em - eccentricity +* argpm - argument of perigee +* inclm - inclination +* xli - +* mm - mean anomaly +* xni - +* nodem - right ascension of ascending node +* dndt - +* nm - mean motion +* +* locals : +* delt - +* ft - +* theta - +* x2li - +* x2omi - +* xl - +* xldot - +* xnddt - +* xndt - +* xomi - +* +* coupling : +* none - +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dspace + ( + int irez, + double d2201, double d2211, double d3210, double d3222, double d4410, + double d4422, double d5220, double d5232, double d5421, double d5433, + double dedt, double del1, double del2, double del3, double didt, + double dmdt, double dnodt, double domdt, double argpo, double argpdot, + double t, double tc, double gsto, double xfact, double xlamo, + double no, + double& atime, double& em, double& argpm, double& inclm, double& xli, + double& mm, double& xni, double& nodem, double& dndt, double& nm + ) +{ + const double twopi = 2.0 * pi; + int iretn , iret; + double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32, + g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp; + + ft = 0.0; + fasx2 = 0.13130908; + fasx4 = 2.8843198; + fasx6 = 0.37448087; + g22 = 5.7686396; + g32 = 0.95240898; + g44 = 1.8014998; + g52 = 1.0508330; + g54 = 4.4108898; + rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec + stepp = 720.0; + stepn = -720.0; + step2 = 259200.0; + + /* ----------- calculate deep space resonance effects ----------- */ + dndt = 0.0; + theta = fmod(gsto + tc * rptim, twopi); + em = em + dedt * t; + + inclm = inclm + didt * t; + argpm = argpm + domdt * t; + nodem = nodem + dnodt * t; + mm = mm + dmdt * t; + + // sgp4fix for negative inclinations + // the following if statement should be commented out + // if (inclm < 0.0) + // { + // inclm = -inclm; + // argpm = argpm - pi; + // nodem = nodem + pi; + // } + + /* - update resonances : numerical (euler-maclaurin) integration - */ + /* ------------------------- epoch restart ---------------------- */ + // sgp4fix for propagator problems + // the following integration works for negative time steps and periods + // the specific changes are unknown because the original code was so convoluted + + ft = 0.0; + atime = 0.0; + if (irez != 0) + { + if ((atime == 0.0) || ((t >= 0.0) && (atime < 0.0)) || + ((t < 0.0) && (atime >= 0.0))) + { + if (t >= 0.0) + delt = stepp; + else + delt = stepn; + atime = 0.0; + xni = no; + xli = xlamo; + } + iretn = 381; // added for do loop + iret = 0; // added for loop + while (iretn == 381) + { + if ((fabs(t) < fabs(atime)) || (iret == 351)) + { + if (t >= 0.0) + delt = stepn; + else + delt = stepp; + iret = 351; + iretn = 381; + } + else + { + if (t > 0.0) // error if prev if has atime:=0.0 and t:=0.0 (ge) + delt = stepp; + else + delt = stepn; + if (fabs(t - atime) >= stepp) + { + iret = 0; + iretn = 381; + } + else + { + ft = t - atime; + iretn = 0; + } + } + + /* ------------------- dot terms calculated ------------- */ + /* ----------- near - synchronous resonance terms ------- */ + if (irez != 2) + { + xndt = del1 * sin(xli - fasx2) + del2 * sin(2.0 * (xli - fasx4)) + + del3 * sin(3.0 * (xli - fasx6)); + xldot = xni + xfact; + xnddt = del1 * cos(xli - fasx2) + + 2.0 * del2 * cos(2.0 * (xli - fasx4)) + + 3.0 * del3 * cos(3.0 * (xli - fasx6)); + xnddt = xnddt * xldot; + } + else + { + /* --------- near - half-day resonance terms -------- */ + xomi = argpo + argpdot * atime; + x2omi = xomi + xomi; + x2li = xli + xli; + xndt = d2201 * sin(x2omi + xli - g22) + d2211 * sin(xli - g22) + + d3210 * sin(xomi + xli - g32) + d3222 * sin(-xomi + xli - g32)+ + d4410 * sin(x2omi + x2li - g44)+ d4422 * sin(x2li - g44) + + d5220 * sin(xomi + xli - g52) + d5232 * sin(-xomi + xli - g52)+ + d5421 * sin(xomi + x2li - g54) + d5433 * sin(-xomi + x2li - g54); + xldot = xni + xfact; + xnddt = d2201 * cos(x2omi + xli - g22) + d2211 * cos(xli - g22) + + d3210 * cos(xomi + xli - g32) + d3222 * cos(-xomi + xli - g32) + + d5220 * cos(xomi + xli - g52) + d5232 * cos(-xomi + xli - g52) + + 2.0 * (d4410 * cos(x2omi + x2li - g44) + + d4422 * cos(x2li - g44) + d5421 * cos(xomi + x2li - g54) + + d5433 * cos(-xomi + x2li - g54)); + xnddt = xnddt * xldot; + } + + /* ----------------------- integrator ------------------- */ + if (iretn == 381) + { + xli = xli + xldot * delt + xndt * step2; + xni = xni + xndt * delt + xnddt * step2; + atime = atime + delt; + } + } // while iretn = 381 + + nm = xni + xndt * ft + xnddt * ft * ft * 0.5; + xl = xli + xldot * ft + xndt * ft * ft * 0.5; + if (irez != 1) + { + mm = xl - 2.0 * nodem + 2.0 * theta; + dndt = nm - no; + } + else + { + mm = xl - nodem - argpm + theta; + dndt = nm - no; + } + nm = no + dndt; + } + +//#include "debug4.cpp" +} // end dsspace + +/*----------------------------------------------------------------------------- +* +* procedure initl +* +* this procedure initializes the spg4 propagator. all the initialization is +* consolidated here instead of having multiple loops inside other routines. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* ecco - eccentricity 0.0 - 1.0 +* epoch - epoch time in days from jan 0, 1950. 0 hr +* inclo - inclination of satellite +* no - mean motion of satellite +* satn - satellite number +* +* outputs : +* ainv - 1.0 / a +* ao - semi major axis +* con41 - +* con42 - 1.0 - 5.0 cos(i) +* cosio - cosine of inclination +* cosio2 - cosio squared +* eccsq - eccentricity squared +* method - flag for deep space 'd', 'n' +* omeosq - 1.0 - ecco * ecco +* posq - semi-parameter squared +* rp - radius of perigee +* rteosq - square root of (1.0 - ecco*ecco) +* sinio - sine of inclination +* gsto - gst at time of observation rad +* no - mean motion of satellite +* +* locals : +* ak - +* d1 - +* del - +* adel - +* po - +* +* coupling : +* getgravconst +* gstime - find greenwich sidereal time from the julian date +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void initl + ( + int satn, gravconsttype whichconst, + double ecco, double epoch, double inclo, double& no, + char& method, + double& ainv, double& ao, double& con41, double& con42, double& cosio, + double& cosio2,double& eccsq, double& omeosq, double& posq, + double& rp, double& rteosq,double& sinio , double& gsto + ) +{ + /* --------------------- local variables ------------------------ */ + double ak, d1, del, adel, po, x2o3, j2, xke, + tumin, mu, radiusearthkm, j3, j4, j3oj2; + + // sgp4fix use old way of finding gst + int ids70; + double ts70, ds70, tfrac, c1, thgr70, fk5r, c1p2p; + const double twopi = 2.0 * pi; + + /* ----------------------- earth constants ---------------------- */ + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + x2o3 = 2.0 / 3.0; + + /* ------------- calculate auxillary epoch quantities ---------- */ + eccsq = ecco * ecco; + omeosq = 1.0 - eccsq; + rteosq = sqrt(omeosq); + cosio = cos(inclo); + cosio2 = cosio * cosio; + + /* ------------------ un-kozai the mean motion ----------------- */ + ak = pow(xke / no, x2o3); + d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq); + del = d1 / (ak * ak); + adel = ak * (1.0 - del * del - del * + (1.0 / 3.0 + 134.0 * del * del / 81.0)); + del = d1/(adel * adel); + no = no / (1.0 + del); + + ao = pow(xke / no, x2o3); + sinio = sin(inclo); + po = ao * omeosq; + con42 = 1.0 - 5.0 * cosio2; + con41 = -con42-cosio2-cosio2; + ainv = 1.0 / ao; + posq = po * po; + rp = ao * (1.0 - ecco); + method = 'n'; + + // sgp4fix modern approach to finding sidereal timew + // gsto = gstime(epoch + 2433281.5); + + // sgp4fix use old way of finding gst + // count integer number of days from 0 jan 1970 + ts70 = epoch - 7305.0; + ids70 = floor(ts70 + 1.0e-8); + ds70 = ids70; + tfrac = ts70 - ds70; + // find greenwich location at epoch + c1 = 1.72027916940703639e-2; + thgr70= 1.7321343856509374; + fk5r = 5.07551419432269442e-15; + c1p2p = c1 + twopi; + gsto = fmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi); + if ( gsto < 0.0 ) + gsto = gsto + twopi; + +//#include "debug5.cpp" +} // end initl + +/*----------------------------------------------------------------------------- +* +* procedure sgp4init +* +* this procedure initializes variables for sgp4. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* satn - satellite number +* bstar - sgp4 type drag coefficient kg/m2er +* ecco - eccentricity +* epoch - epoch time in days from jan 0, 1950. 0 hr +* argpo - argument of perigee (output if ds) +* inclo - inclination +* mo - mean anomaly (output if ds) +* no - mean motion +* nodeo - right ascension of ascending node +* +* outputs : +* satrec - common values for subsequent calls +* return code - non-zero on error. +* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er +* 2 - mean motion less than 0.0 +* 3 - pert elements, ecc < 0.0 or ecc > 1.0 +* 4 - semi-latus rectum < 0.0 +* 5 - epoch elements are sub-orbital +* 6 - satellite has decayed +* +* locals : +* cnodm , snodm , cosim , sinim , cosomm , sinomm +* cc1sq , cc2 , cc3 +* coef , coef1 +* cosio4 - +* day - +* dndt - +* em - eccentricity +* emsq - eccentricity squared +* eeta - +* etasq - +* gam - +* argpm - argument of perigee +* nodem - +* inclm - inclination +* mm - mean anomaly +* nm - mean motion +* perige - perigee +* pinvsq - +* psisq - +* qzms24 - +* rtemsq - +* s1, s2, s3, s4, s5, s6, s7 - +* sfour - +* ss1, ss2, ss3, ss4, ss5, ss6, ss7 - +* sz1, sz2, sz3 +* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - +* tc - +* temp - +* temp1, temp2, temp3 - +* tsi - +* xpidot - +* xhdot1 - +* z1, z2, z3 - +* z11, z12, z13, z21, z22, z23, z31, z32, z33 - +* +* coupling : +* getgravconst- +* initl - +* dscom - +* dpper - +* dsinit - +* sgp4 - +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +int sgp4init + ( + gravconsttype whichconst, const int satn, const double epoch, + const double xbstar, const double xecco, const double xargpo, + const double xinclo, const double xmo, const double xno, + const double xnodeo, elsetrec& satrec + ) +{ + /* --------------------- local variables ------------------------ */ + double ao, ainv, con42, cosio, sinio, cosio2, eccsq, + omeosq, posq, rp, rteosq, + cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq , + cc2 , cc3 , coef , coef1 , cosio4, day , dndt , + em , emsq , eeta , etasq , gam , argpm , nodem , + inclm , mm , nm , perige, pinvsq, psisq , qzms24, + rtemsq, s1 , s2 , s3 , s4 , s5 , s6 , + s7 , sfour , ss1 = 0 , ss2 = 0 , ss3 = 0 , ss4 = 0 , ss5 = 0 , + ss6 = 0 , ss7 = 0 , sz1 = 0 , sz2 = 0 , sz3 = 0 , sz11 = 0 , sz12 = 0 , + sz13 = 0 , sz21 = 0 , sz22 = 0 , sz23 = 0 , sz31 = 0 , sz32 = 0 , sz33 = 0 , + tc , temp , temp1 , temp2 , temp3 , tsi , xpidot, + xhdot1, z1 , z2 , z3 , z11 , z12 , z13 , + z21 , z22 , z23 , z31 , z32 , z33, + qzms2t, ss, j2, j3oj2, j4, x2o3, r[3], v[3], + tumin, mu, radiusearthkm, xke, j3; + + /* ------------------------ initialization --------------------- */ + // sgp4fix divisor for divide by zero check on inclination + const double temp4 = 1.0 + cos(pi-1.0e-9); + + /* ----------- set all near earth variables to zero ------------ */ + satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0; + satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0; + satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0; + satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0; + satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0; + satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0; + satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0; + satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0; + satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0; + + /* ----------- set all deep space variables to zero ------------ */ + satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0; + satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0; + satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0; + satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0; + satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0; + satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0; + satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0; + satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0; + satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0; + satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0; + satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0; + satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0; + satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0; + satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0; + satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0; + satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0; + satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0; + satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0; + satrec.xli = 0.0; satrec.xni = 0.0; + + // sgp4fix - note the following variables are also passed directly via satrec. + // it is possible to streamline the sgp4init call by deleting the "x" + // variables, but the user would need to set the satrec.* values first. we + // include the additional assignments in case twoline2rv is not used. + satrec.bstar = xbstar; + satrec.ecco = xecco; + satrec.argpo = xargpo; + satrec.inclo = xinclo; + satrec.mo = xmo; + satrec.no = xno; + satrec.nodeo = xnodeo; + + /* ------------------------ earth constants ----------------------- */ + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + ss = 78.0 / radiusearthkm + 1.0; + qzms2t = pow(((120.0 - 78.0) / radiusearthkm), 4); + x2o3 = 2.0 / 3.0; + + satrec.init = 'y'; + satrec.t = 0.0; + + initl + ( + satn, whichconst, satrec.ecco, epoch, satrec.inclo, satrec.no, satrec.method, + ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq, + posq, rp, rteosq, sinio, satrec.gsto + ); + satrec.error = 0; + + if (rp < 1.0) + { +// printf("# *** satn%d epoch elts sub-orbital ***\n", satn); + satrec.error = 5; + } + + if ((omeosq >= 0.0 ) || ( satrec.no >= 0.0)) + { + satrec.isimp = 0; + if (rp < (220.0 / radiusearthkm + 1.0)) + satrec.isimp = 1; + sfour = ss; + qzms24 = qzms2t; + perige = (rp - 1.0) * radiusearthkm; + + /* - for perigees below 156 km, s and qoms2t are altered - */ + if (perige < 156.0) + { + sfour = perige - 78.0; + if (perige < 98.0) + sfour = 20.0; + qzms24 = pow(((120.0 - sfour) / radiusearthkm), 4.0); + sfour = sfour / radiusearthkm + 1.0; + } + pinvsq = 1.0 / posq; + + tsi = 1.0 / (ao - sfour); + satrec.eta = ao * satrec.ecco * tsi; + etasq = satrec.eta * satrec.eta; + eeta = satrec.ecco * satrec.eta; + psisq = fabs(1.0 - etasq); + coef = qzms24 * pow(tsi, 4.0); + coef1 = coef / pow(psisq, 3.5); + cc2 = coef1 * satrec.no * (ao * (1.0 + 1.5 * etasq + eeta * + (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec.con41 * + (8.0 + 3.0 * etasq * (8.0 + etasq))); + satrec.cc1 = satrec.bstar * cc2; + cc3 = 0.0; + if (satrec.ecco > 1.0e-4) + cc3 = -2.0 * coef * tsi * j3oj2 * satrec.no * sinio / satrec.ecco; + satrec.x1mth2 = 1.0 - cosio2; + satrec.cc4 = 2.0* satrec.no * coef1 * ao * omeosq * + (satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco * + (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) * + (-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq * + (1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 * + (2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec.argpo))); + satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * + (etasq + eeta) + eeta * etasq); + cosio4 = cosio2 * cosio2; + temp1 = 1.5 * j2 * pinvsq * satrec.no; + temp2 = 0.5 * temp1 * j2 * pinvsq; + temp3 = -0.46875 * j4 * pinvsq * pinvsq * satrec.no; + satrec.mdot = satrec.no + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 * + temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4); + satrec.argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 * + (7.0 - 114.0 * cosio2 + 395.0 * cosio4) + + temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4); + xhdot1 = -temp1 * cosio; + satrec.nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + + 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio; + xpidot = satrec.argpdot+ satrec.nodedot; + satrec.omgcof = satrec.bstar * cc3 * cos(satrec.argpo); + satrec.xmcof = 0.0; + if (satrec.ecco > 1.0e-4) + satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta; + satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1; + satrec.t2cof = 1.5 * satrec.cc1; + // sgp4fix for divide by zero with xinco = 180 deg + if (fabs(cosio+1.0) > 1.5e-12) + satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio); + else + satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4; + satrec.aycof = -0.5 * j3oj2 * sinio; + satrec.delmo = pow((1.0 + satrec.eta * cos(satrec.mo)), 3); + satrec.sinmao = sin(satrec.mo); + satrec.x7thm1 = 7.0 * cosio2 - 1.0; + + /* --------------- deep space initialization ------------- */ + if ((2*pi / satrec.no) >= 225.0) + { + satrec.method = 'd'; + satrec.isimp = 1; + tc = 0.0; + inclm = satrec.inclo; + + dscom + ( + epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo, + satrec.no, snodm, cnodm, sinim, cosim,sinomm, cosomm, + day, satrec.e3, satrec.ee2, em, emsq, gam, + satrec.peo, satrec.pgho, satrec.pho, satrec.pinco, + satrec.plo, rtemsq, satrec.se2, satrec.se3, + satrec.sgh2, satrec.sgh3, satrec.sgh4, + satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, + satrec.sl2, satrec.sl3, satrec.sl4, s1, s2, s3, s4, s5, + s6, s7, ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3, + sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33, + satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2, + satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, + satrec.xl3, satrec.xl4, nm, z1, z2, z3, z11, + z12, z13, z21, z22, z23, z31, z32, z33, + satrec.zmol, satrec.zmos + ); + dpper + ( + satrec.e3, satrec.ee2, satrec.peo, satrec.pgho, + satrec.pho, satrec.pinco, satrec.plo, satrec.se2, + satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4, + satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, + satrec.sl2, satrec.sl3, satrec.sl4, satrec.t, + satrec.xgh2,satrec.xgh3,satrec.xgh4, satrec.xh2, + satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, + satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init, + satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo + ); + + argpm = 0.0; + nodem = 0.0; + mm = 0.0; + + dsinit + ( + whichconst, + cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4, + ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc, + satrec.gsto, satrec.mo, satrec.mdot, satrec.no, satrec.nodeo, + satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33, + satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem, + satrec.irez, satrec.atime, + satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222 , + satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232, + satrec.d5421, satrec.d5433, satrec.dedt, satrec.didt, + satrec.dmdt, dndt, satrec.dnodt, satrec.domdt , + satrec.del1, satrec.del2, satrec.del3, satrec.xfact, + satrec.xlamo, satrec.xli, satrec.xni + ); + } + + /* ----------- set variables if not deep space ----------- */ + if (satrec.isimp != 1) + { + cc1sq = satrec.cc1 * satrec.cc1; + satrec.d2 = 4.0 * ao * tsi * cc1sq; + temp = satrec.d2 * tsi * satrec.cc1 / 3.0; + satrec.d3 = (17.0 * ao + sfour) * temp; + satrec.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * + satrec.cc1; + satrec.t3cof = satrec.d2 + 2.0 * cc1sq; + satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 * + (12.0 * satrec.d2 + 10.0 * cc1sq)); + satrec.t5cof = 0.2 * (3.0 * satrec.d4 + + 12.0 * satrec.cc1 * satrec.d3 + + 6.0 * satrec.d2 * satrec.d2 + + 15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq)); + } + } // if omeosq = 0 ... + + /* finally propogate to zero epoch to initialise all others. */ + if(satrec.error == 0) + sgp4(whichconst, satrec, 0.0, r, v); + + satrec.init = 'n'; + +//#include "debug6.cpp" + return satrec.error; +} // end sgp4init + +/*----------------------------------------------------------------------------- +* +* procedure sgp4 +* +* this procedure is the sgp4 prediction model from space command. this is an +* updated and combined version of sgp4 and sdp4, which were originally +* published separately in spacetrack report #3. this version follows the +* methodology from the aiaa paper (2006) describing the history and +* development of the code. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* satrec - initialised structure from sgp4init() call. +* tsince - time eince epoch (minutes) +* +* outputs : +* r - position vector km +* v - velocity km/sec +* return code - non-zero on error. +* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er +* 2 - mean motion less than 0.0 +* 3 - pert elements, ecc < 0.0 or ecc > 1.0 +* 4 - semi-latus rectum < 0.0 +* 5 - epoch elements are sub-orbital +* 6 - satellite has decayed +* +* locals : +* am - +* axnl, aynl - +* betal - +* cosim , sinim , cosomm , sinomm , cnod , snod , cos2u , +* sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip , +* cosisq , cossu , sinsu , cosu , sinu +* delm - +* delomg - +* dndt - +* eccm - +* emsq - +* ecose - +* el2 - +* eo1 - +* eccp - +* esine - +* argpm - +* argpp - +* omgadf - +* pl - +* r - +* rtemsq - +* rdotl - +* rl - +* rvdot - +* rvdotl - +* su - +* t2 , t3 , t4 , tc +* tem5, temp , temp1 , temp2 , tempa , tempe , templ +* u , ux , uy , uz , vx , vy , vz +* inclm - inclination +* mm - mean anomaly +* nm - mean motion +* nodem - right asc of ascending node +* xinc - +* xincp - +* xl - +* xlm - +* mp - +* xmdf - +* xmx - +* xmy - +* nodedf - +* xnode - +* nodep - +* np - +* +* coupling : +* getgravconst- +* dpper +* dpspace +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +int sgp4 + ( + gravconsttype whichconst, elsetrec& satrec, double tsince, + double r[3], double v[3] + ) +{ + double am , axnl , aynl , betal , cosim , cnod , + cos2u, coseo1, cosi , cosip , cosisq, cossu , cosu, + delm , delomg, em , emsq , ecose , el2 , eo1 , + ep , esine , argpm, argpp , argpdf, pl, mrt = 0.0, + mvt , rdotl , rl , rvdot , rvdotl, sinim , + sin2u, sineo1, sini , sinip , sinsu , sinu , + snod , su , t2 , t3 , t4 , tem5 , temp, + temp1, temp2 , tempa, tempe , templ , u , ux , + uy , uz , vx , vy , vz , inclm , mm , + nm , nodem, xinc , xincp , xl , xlm , mp , + xmdf , xmx , xmy , nodedf, xnode , nodep, tc , dndt, + twopi, x2o3 , j2 , j3 , tumin, j4 , xke , j3oj2, radiusearthkm, + mu, vkmpersec; + int ktr; + + /* ------------------ set mathematical constants --------------- */ + // sgp4fix divisor for divide by zero check on inclination + const double temp4 = 1.0 + cos(pi-1.0e-9); + twopi = 2.0 * pi; + x2o3 = 2.0 / 3.0; + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + vkmpersec = radiusearthkm * xke/60.0; + + /* --------------------- clear sgp4 error flag ----------------- */ + satrec.t = tsince; + satrec.error = 0; + + /* ------- update for secular gravity and atmospheric drag ----- */ + xmdf = satrec.mo + satrec.mdot * satrec.t; + argpdf = satrec.argpo + satrec.argpdot * satrec.t; + nodedf = satrec.nodeo + satrec.nodedot * satrec.t; + argpm = argpdf; + mm = xmdf; + t2 = satrec.t * satrec.t; + nodem = nodedf + satrec.nodecf * t2; + tempa = 1.0 - satrec.cc1 * satrec.t; + tempe = satrec.bstar * satrec.cc4 * satrec.t; + templ = satrec.t2cof * t2; + + if (satrec.isimp != 1) + { + delomg = satrec.omgcof * satrec.t; + delm = satrec.xmcof * + (pow((1.0 + satrec.eta * cos(xmdf)), 3) - + satrec.delmo); + temp = delomg + delm; + mm = xmdf + temp; + argpm = argpdf - temp; + t3 = t2 * satrec.t; + t4 = t3 * satrec.t; + tempa = tempa - satrec.d2 * t2 - satrec.d3 * t3 - + satrec.d4 * t4; + tempe = tempe + satrec.bstar * satrec.cc5 * (sin(mm) - + satrec.sinmao); + templ = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof + + satrec.t * satrec.t5cof); + } + + nm = satrec.no; + em = satrec.ecco; + inclm = satrec.inclo; + if (satrec.method == 'd') + { + tc = satrec.t; + dspace + ( + satrec.irez, + satrec.d2201, satrec.d2211, satrec.d3210, + satrec.d3222, satrec.d4410, satrec.d4422, + satrec.d5220, satrec.d5232, satrec.d5421, + satrec.d5433, satrec.dedt, satrec.del1, + satrec.del2, satrec.del3, satrec.didt, + satrec.dmdt, satrec.dnodt, satrec.domdt, + satrec.argpo, satrec.argpdot, satrec.t, tc, + satrec.gsto, satrec.xfact, satrec.xlamo, + satrec.no, satrec.atime, + em, argpm, inclm, satrec.xli, mm, satrec.xni, + nodem, dndt, nm + ); + } // if method = d + + if (nm <= 0.0) + { +// printf("# error nm %f\n", nm); + satrec.error = 2; + } + am = pow((xke / nm),x2o3) * tempa * tempa; + nm = xke / pow(am, 1.5); + em = em - tempe; + + // fix tolerance for error recognition + if ((em >= 1.0) || (em < -0.001) || (am < 0.95)) + { +// printf("# error em %f\n", em); + satrec.error = 1; + } + if (em < 0.0) + em = 1.0e-6; + mm = mm + satrec.no * templ; + xlm = mm + argpm + nodem; + emsq = em * em; + temp = 1.0 - emsq; + + nodem = fmod(nodem, twopi); + argpm = fmod(argpm, twopi); + xlm = fmod(xlm, twopi); + mm = fmod(xlm - argpm - nodem, twopi); + + /* ----------------- compute extra mean quantities ------------- */ + sinim = sin(inclm); + cosim = cos(inclm); + + /* -------------------- add lunar-solar periodics -------------- */ + ep = em; + xincp = inclm; + argpp = argpm; + nodep = nodem; + mp = mm; + sinip = sinim; + cosip = cosim; + if (satrec.method == 'd') + { + dpper + ( + satrec.e3, satrec.ee2, satrec.peo, + satrec.pgho, satrec.pho, satrec.pinco, + satrec.plo, satrec.se2, satrec.se3, + satrec.sgh2, satrec.sgh3, satrec.sgh4, + satrec.sh2, satrec.sh3, satrec.si2, + satrec.si3, satrec.sl2, satrec.sl3, + satrec.sl4, satrec.t, satrec.xgh2, + satrec.xgh3, satrec.xgh4, satrec.xh2, + satrec.xh3, satrec.xi2, satrec.xi3, + satrec.xl2, satrec.xl3, satrec.xl4, + satrec.zmol, satrec.zmos, satrec.inclo, + 'n', ep, xincp, nodep, argpp, mp + ); + if (xincp < 0.0) + { + xincp = -xincp; + nodep = nodep + pi; + argpp = argpp - pi; + } + if ((ep < 0.0 ) || ( ep > 1.0)) + { + // printf("# error ep %f\n", ep); + satrec.error = 3; + } + } // if method = d + + /* -------------------- long period periodics ------------------ */ + if (satrec.method == 'd') + { + sinip = sin(xincp); + cosip = cos(xincp); + satrec.aycof = -0.5*j3oj2*sinip; + // sgp4fix for divide by zero for xincp = 180 deg + if (fabs(cosip+1.0) > 1.5e-12) + satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip); + else + satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4; + } + axnl = ep * cos(argpp); + temp = 1.0 / (am * (1.0 - ep * ep)); + aynl = ep* sin(argpp) + temp * satrec.aycof; + xl = mp + argpp + nodep + temp * satrec.xlcof * axnl; + + /* --------------------- solve kepler's equation --------------- */ + u = fmod(xl - nodep, twopi); + eo1 = u; + tem5 = 9999.9; + ktr = 1; + // sgp4fix for kepler iteration + // the following iteration needs better limits on corrections + while (( fabs(tem5) >= 1.0e-12) && (ktr <= 10) ) + { + sineo1 = sin(eo1); + coseo1 = cos(eo1); + tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl; + tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5; + if(fabs(tem5) >= 0.95) + tem5 = tem5 > 0.0 ? 0.95 : -0.95; + eo1 = eo1 + tem5; + ktr = ktr + 1; + } + + /* ------------- short period preliminary quantities ----------- */ + ecose = axnl*coseo1 + aynl*sineo1; + esine = axnl*sineo1 - aynl*coseo1; + el2 = axnl*axnl + aynl*aynl; + pl = am*(1.0-el2); + if (pl < 0.0) + { +// printf("# error pl %f\n", pl); + satrec.error = 4; + } + else + { + rl = am * (1.0 - ecose); + rdotl = sqrt(am) * esine/rl; + rvdotl = sqrt(pl) / rl; + betal = sqrt(1.0 - el2); + temp = esine / (1.0 + betal); + sinu = am / rl * (sineo1 - aynl - axnl * temp); + cosu = am / rl * (coseo1 - axnl + aynl * temp); + su = atan2(sinu, cosu); + sin2u = (cosu + cosu) * sinu; + cos2u = 1.0 - 2.0 * sinu * sinu; + temp = 1.0 / pl; + temp1 = 0.5 * j2 * temp; + temp2 = temp1 * temp; + + /* -------------- update for short period periodics ------------ */ + if (satrec.method == 'd') + { + cosisq = cosip * cosip; + satrec.con41 = 3.0*cosisq - 1.0; + satrec.x1mth2 = 1.0 - cosisq; + satrec.x7thm1 = 7.0*cosisq - 1.0; + } + mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) + + 0.5 * temp1 * satrec.x1mth2 * cos2u; + su = su - 0.25 * temp2 * satrec.x7thm1 * sin2u; + xnode = nodep + 1.5 * temp2 * cosip * sin2u; + xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u; + mvt = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / xke; + rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u + + 1.5 * satrec.con41) / xke; + + /* --------------------- orientation vectors ------------------- */ + sinsu = sin(su); + cossu = cos(su); + snod = sin(xnode); + cnod = cos(xnode); + sini = sin(xinc); + cosi = cos(xinc); + xmx = -snod * cosi; + xmy = cnod * cosi; + ux = xmx * sinsu + cnod * cossu; + uy = xmy * sinsu + snod * cossu; + uz = sini * sinsu; + vx = xmx * cossu - cnod * sinsu; + vy = xmy * cossu - snod * sinsu; + vz = sini * cossu; + + /* --------- position and velocity (in km and km/sec) ---------- */ + r[0] = (mrt * ux)* radiusearthkm; + r[1] = (mrt * uy)* radiusearthkm; + r[2] = (mrt * uz)* radiusearthkm; + v[0] = (mvt * ux + rvdot * vx) * vkmpersec; + v[1] = (mvt * uy + rvdot * vy) * vkmpersec; + v[2] = (mvt * uz + rvdot * vz) * vkmpersec; + } // if pl > 0 + + // sgp4fix for decaying satellites + if (mrt < 1.0) + { +// printf("# decay condition %11.6f \n",mrt); + satrec.error = 6; + } + + +//#include "debug7.cpp" + return satrec.error; +} // end sgp4 + + +/* ----------------------------------------------------------------------------- +* +* function gstime +* +* this function finds the greenwich sidereal time. +* +* author : david vallado 719-573-2600 1 mar 2001 +* +* inputs description range / units +* jdut1 - julian date in ut1 days from 4713 bc +* +* outputs : +* gstime - greenwich sidereal time 0 to 2pi rad +* +* locals : +* temp - temporary variable for doubles rad +* tut1 - julian centuries from the +* jan 1, 2000 12 h epoch (ut1) +* +* coupling : +* none +* +* references : +* vallado 2004, 191, eq 3-45 +* --------------------------------------------------------------------------- */ + +double gstime + ( + double jdut1 + ) + { + const double twopi = 2.0 * pi; + const double deg2rad = pi / 180.0; + double temp, tut1; + + tut1 = (jdut1 - 2451545.0) / 36525.0; + temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 + + (876600.0*3600 + 8640184.812866) * tut1 + 67310.54841; // sec + temp = fmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad + + // ------------------------ check quadrants --------------------- + if (temp < 0.0) + temp += twopi; + + return temp; + } // end gstime + +/* ----------------------------------------------------------------------------- +* +* function getgravconst +* +* this function gets constants for the propagator. note that mu is identified to +* facilitiate comparisons with newer models. the common useage is wgs72. +* +* author : david vallado 719-573-2600 21 jul 2006 +* +* inputs : +* whichconst - which set of constants to use wgs72old, wgs72, wgs84 +* +* outputs : +* tumin - minutes in one time unit +* mu - earth gravitational parameter +* radiusearthkm - radius of the earth in km +* xke - reciprocal of tumin +* j2, j3, j4 - un-normalized zonal harmonic values +* j3oj2 - j3 divided by j2 +* +* locals : +* +* coupling : +* none +* +* references : +* norad spacetrack report #3 +* vallado, crawford, hujsak, kelso 2006 + --------------------------------------------------------------------------- */ + +void getgravconst + ( + gravconsttype whichconst, + double& tumin, + double& mu, + double& radiusearthkm, + double& xke, + double& j2, + double& j3, + double& j4, + double& j3oj2 + ) + { + + switch (whichconst) + { + // -- wgs-72 low precision str#3 constants -- + case wgs72old: + mu = 398600.79964; // in km3 / s2 + radiusearthkm = 6378.135; // km + xke = 0.0743669161; + tumin = 1.0 / xke; + j2 = 0.001082616; + j3 = -0.00000253881; + j4 = -0.00000165597; + j3oj2 = j3 / j2; + break; + // ------------ wgs-72 constants ------------ + case wgs72: + mu = 398600.8; // in km3 / s2 + radiusearthkm = 6378.135; // km + xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); + tumin = 1.0 / xke; + j2 = 0.001082616; + j3 = -0.00000253881; + j4 = -0.00000165597; + j3oj2 = j3 / j2; + break; + case wgs84: + // ------------ wgs-84 constants ------------ + mu = 398600.5; // in km3 / s2 + radiusearthkm = 6378.137; // km + xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); + tumin = 1.0 / xke; + j2 = 0.00108262998905; + j3 = -0.00000253215306; + j4 = -0.00000161098761; + j3oj2 = j3 / j2; + break; + default: + fprintf(stderr,"unknown gravity option (%d)\n",whichconst); + break; + } + + } // end getgravconst + + + + + diff --git a/contrib/sgp4/sgp4unit.h b/contrib/sgp4/sgp4unit.h index 48c113d7..f16acbd9 100644 --- a/contrib/sgp4/sgp4unit.h +++ b/contrib/sgp4/sgp4unit.h @@ -1,117 +1,117 @@ -#ifndef _sgp4unit_ -#define _sgp4unit_ -/* ---------------------------------------------------------------- -* -* sgp4unit.h -* -* this file contains the sgp4 procedures for analytical propagation -* of a satellite. the code was originally released in the 1980 and 1986 -* spacetrack papers. a detailed discussion of the theory and history -* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, -* and kelso. -* -* companion code for -* fundamentals of astrodynamics and applications -* 2007 -* by david vallado -* -* (w) 719-573-2600, email dvallado@agi.com -* -* current : -* 20 apr 07 david vallado -* misc fixes for constants -* changes : -* 11 aug 06 david vallado -* chg lyddane choice back to strn3, constants, misc doc -* 15 dec 05 david vallado -* misc fixes -* 26 jul 05 david vallado -* fixes for paper -* note that each fix is preceded by a -* comment with "sgp4fix" and an explanation of -* what was changed -* 10 aug 04 david vallado -* 2nd printing baseline working -* 14 may 01 david vallado -* 2nd edition baseline -* 80 norad -* original baseline -* ---------------------------------------------------------------- */ - -#include -#include - -// -------------------------- structure declarations ---------------------------- -typedef enum -{ - wgs72old, - wgs72, - wgs84 -} gravconsttype; - -typedef struct elsetrec -{ - long int satnum; - int epochyr, epochtynumrev; - int error; - char init, method; - - /* Near Earth */ - int isimp; - double aycof , con41 , cc1 , cc4 , cc5 , d2 , d3 , d4 , - delmo , eta , argpdot, omgcof , sinmao , t , t2cof, t3cof , - t4cof , t5cof , x1mth2 , x7thm1 , mdot , nodedot, xlcof , xmcof , - nodecf; - - /* Deep Space */ - int irez; - double d2201 , d2211 , d3210 , d3222 , d4410 , d4422 , d5220 , d5232 , - d5421 , d5433 , dedt , del1 , del2 , del3 , didt , dmdt , - dnodt , domdt , e3 , ee2 , peo , pgho , pho , pinco , - plo , se2 , se3 , sgh2 , sgh3 , sgh4 , sh2 , sh3 , - si2 , si3 , sl2 , sl3 , sl4 , gsto , xfact , xgh2 , - xgh3 , xgh4 , xh2 , xh3 , xi2 , xi3 , xl2 , xl3 , - xl4 , xlamo , zmol , zmos , atime , xli , xni; - - double a , altp , alta , epochdays, jdsatepoch , nddot , ndot , - bstar , rcse , inclo , nodeo , ecco , argpo , mo , - no; -} elsetrec; - -// --------------------------- function declarations ---------------------------- -int sgp4init - ( - gravconsttype whichconst, const int satn, const double epoch, - const double xbstar, const double xecco, const double xargpo, - const double xinclo, const double xmo, const double xno, - const double xnodeo, - elsetrec& satrec - ); - -int sgp4 - ( - gravconsttype whichconst, - elsetrec& satrec, double tsince, - double r[], double v[] - ); - -double gstime - ( - double - ); - -void getgravconst - ( - gravconsttype, - double&, - double&, - double&, - double&, - double&, - double&, - double&, - double& - ); - -#endif - +#ifndef _sgp4unit_ +#define _sgp4unit_ +/* ---------------------------------------------------------------- +* +* sgp4unit.h +* +* this file contains the sgp4 procedures for analytical propagation +* of a satellite. the code was originally released in the 1980 and 1986 +* spacetrack papers. a detailed discussion of the theory and history +* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, +* and kelso. +* +* companion code for +* fundamentals of astrodynamics and applications +* 2007 +* by david vallado +* +* (w) 719-573-2600, email dvallado@agi.com +* +* current : +* 20 apr 07 david vallado +* misc fixes for constants +* changes : +* 11 aug 06 david vallado +* chg lyddane choice back to strn3, constants, misc doc +* 15 dec 05 david vallado +* misc fixes +* 26 jul 05 david vallado +* fixes for paper +* note that each fix is preceded by a +* comment with "sgp4fix" and an explanation of +* what was changed +* 10 aug 04 david vallado +* 2nd printing baseline working +* 14 may 01 david vallado +* 2nd edition baseline +* 80 norad +* original baseline +* ---------------------------------------------------------------- */ + +#include +#include + +// -------------------------- structure declarations ---------------------------- +typedef enum +{ + wgs72old, + wgs72, + wgs84 +} gravconsttype; + +typedef struct elsetrec +{ + long int satnum; + int epochyr, epochtynumrev; + int error; + char init, method; + + /* Near Earth */ + int isimp; + double aycof , con41 , cc1 , cc4 , cc5 , d2 , d3 , d4 , + delmo , eta , argpdot, omgcof , sinmao , t , t2cof, t3cof , + t4cof , t5cof , x1mth2 , x7thm1 , mdot , nodedot, xlcof , xmcof , + nodecf; + + /* Deep Space */ + int irez; + double d2201 , d2211 , d3210 , d3222 , d4410 , d4422 , d5220 , d5232 , + d5421 , d5433 , dedt , del1 , del2 , del3 , didt , dmdt , + dnodt , domdt , e3 , ee2 , peo , pgho , pho , pinco , + plo , se2 , se3 , sgh2 , sgh3 , sgh4 , sh2 , sh3 , + si2 , si3 , sl2 , sl3 , sl4 , gsto , xfact , xgh2 , + xgh3 , xgh4 , xh2 , xh3 , xi2 , xi3 , xl2 , xl3 , + xl4 , xlamo , zmol , zmos , atime , xli , xni; + + double a , altp , alta , epochdays, jdsatepoch , nddot , ndot , + bstar , rcse , inclo , nodeo , ecco , argpo , mo , + no; +} elsetrec; + +// --------------------------- function declarations ---------------------------- +int sgp4init + ( + gravconsttype whichconst, const int satn, const double epoch, + const double xbstar, const double xecco, const double xargpo, + const double xinclo, const double xmo, const double xno, + const double xnodeo, + elsetrec& satrec + ); + +int sgp4 + ( + gravconsttype whichconst, + elsetrec& satrec, double tsince, + double r[], double v[] + ); + +double gstime + ( + double + ); + +void getgravconst + ( + gravconsttype, + double&, + double&, + double&, + double&, + double&, + double&, + double&, + double& + ); + +#endif + diff --git a/globalfunctions/timevalOperations.cpp b/globalfunctions/timevalOperations.cpp index ae49ef21..1228da04 100644 --- a/globalfunctions/timevalOperations.cpp +++ b/globalfunctions/timevalOperations.cpp @@ -1,99 +1,99 @@ -#include "timevalOperations.h" - -timeval& operator+=(timeval& lhs, const timeval& rhs) { - int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec; - sum += rhs.tv_sec * 1000000. + rhs.tv_usec; - lhs.tv_sec = sum / 1000000; - lhs.tv_usec = sum - lhs.tv_sec * 1000000; - return lhs; -} - -timeval operator+(timeval lhs, const timeval& rhs) { - lhs += rhs; - return lhs; -} - -timeval& operator-=(timeval& lhs, const timeval& rhs) { - int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec; - sum -= rhs.tv_sec * 1000000. + rhs.tv_usec; - lhs.tv_sec = sum / 1000000; - lhs.tv_usec = sum - lhs.tv_sec * 1000000; - return lhs; -} - -timeval operator-(timeval lhs, const timeval& rhs) { - lhs -= rhs; - return lhs; -} - -double operator/(const timeval& lhs, const timeval& rhs) { - double lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; - double rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; - return lhs64 / rhs64; -} - -timeval& operator/=(timeval& lhs, double scalar) { - int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec; - product /= scalar; - lhs.tv_sec = product / 1000000; - lhs.tv_usec = product - lhs.tv_sec * 1000000; - return lhs; -} - -timeval operator/(timeval lhs, double scalar) { - lhs /= scalar; - return lhs; -} - -timeval& operator*=(timeval& lhs, double scalar) { - int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec; - product *= scalar; - lhs.tv_sec = product / 1000000; - lhs.tv_usec = product - lhs.tv_sec * 1000000; - return lhs; -} - -timeval operator*(timeval lhs, double scalar) { - lhs *= scalar; - return lhs; -} - -timeval operator*(double scalar, timeval rhs) { - rhs *= scalar; - return rhs; -} - -bool operator==(const timeval& lhs, const timeval& rhs) { - int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; - int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; - return lhs64 == rhs64; -} -bool operator!=(const timeval& lhs, const timeval& rhs) { - return !operator==(lhs, rhs); -} -bool operator<(const timeval& lhs, const timeval& rhs) { - int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; - int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; - return lhs64 < rhs64; -} -bool operator>(const timeval& lhs, const timeval& rhs) { - return operator<(rhs, lhs); -} -bool operator<=(const timeval& lhs, const timeval& rhs) { - return !operator>(lhs, rhs); -} -bool operator>=(const timeval& lhs, const timeval& rhs) { - return !operator<(lhs, rhs); -} - -double timevalOperations::toDouble(const timeval timeval) { - double result = timeval.tv_sec * 1000000. + timeval.tv_usec; - return result / 1000000.; -} - -timeval timevalOperations::toTimeval(const double seconds) { - timeval tval; - tval.tv_sec = seconds; - tval.tv_usec = seconds *(double) 1e6 - (tval.tv_sec *1e6); - return tval; -} +#include "timevalOperations.h" + +timeval& operator+=(timeval& lhs, const timeval& rhs) { + int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec; + sum += rhs.tv_sec * 1000000. + rhs.tv_usec; + lhs.tv_sec = sum / 1000000; + lhs.tv_usec = sum - lhs.tv_sec * 1000000; + return lhs; +} + +timeval operator+(timeval lhs, const timeval& rhs) { + lhs += rhs; + return lhs; +} + +timeval& operator-=(timeval& lhs, const timeval& rhs) { + int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec; + sum -= rhs.tv_sec * 1000000. + rhs.tv_usec; + lhs.tv_sec = sum / 1000000; + lhs.tv_usec = sum - lhs.tv_sec * 1000000; + return lhs; +} + +timeval operator-(timeval lhs, const timeval& rhs) { + lhs -= rhs; + return lhs; +} + +double operator/(const timeval& lhs, const timeval& rhs) { + double lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; + double rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; + return lhs64 / rhs64; +} + +timeval& operator/=(timeval& lhs, double scalar) { + int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec; + product /= scalar; + lhs.tv_sec = product / 1000000; + lhs.tv_usec = product - lhs.tv_sec * 1000000; + return lhs; +} + +timeval operator/(timeval lhs, double scalar) { + lhs /= scalar; + return lhs; +} + +timeval& operator*=(timeval& lhs, double scalar) { + int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec; + product *= scalar; + lhs.tv_sec = product / 1000000; + lhs.tv_usec = product - lhs.tv_sec * 1000000; + return lhs; +} + +timeval operator*(timeval lhs, double scalar) { + lhs *= scalar; + return lhs; +} + +timeval operator*(double scalar, timeval rhs) { + rhs *= scalar; + return rhs; +} + +bool operator==(const timeval& lhs, const timeval& rhs) { + int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; + int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; + return lhs64 == rhs64; +} +bool operator!=(const timeval& lhs, const timeval& rhs) { + return !operator==(lhs, rhs); +} +bool operator<(const timeval& lhs, const timeval& rhs) { + int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; + int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; + return lhs64 < rhs64; +} +bool operator>(const timeval& lhs, const timeval& rhs) { + return operator<(rhs, lhs); +} +bool operator<=(const timeval& lhs, const timeval& rhs) { + return !operator>(lhs, rhs); +} +bool operator>=(const timeval& lhs, const timeval& rhs) { + return !operator<(lhs, rhs); +} + +double timevalOperations::toDouble(const timeval timeval) { + double result = timeval.tv_sec * 1000000. + timeval.tv_usec; + return result / 1000000.; +} + +timeval timevalOperations::toTimeval(const double seconds) { + timeval tval; + tval.tv_sec = seconds; + tval.tv_usec = seconds *(double) 1e6 - (tval.tv_sec *1e6); + return tval; +} diff --git a/osal/FreeRTOS/BinSemaphUsingTask.cpp b/osal/FreeRTOS/BinSemaphUsingTask.cpp index 8c831bbe..dd1e48ca 100644 --- a/osal/FreeRTOS/BinSemaphUsingTask.cpp +++ b/osal/FreeRTOS/BinSemaphUsingTask.cpp @@ -1,95 +1,95 @@ -#include "../../osal/FreeRTOS/BinSemaphUsingTask.h" -#include "../../osal/FreeRTOS/TaskManagement.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -BinarySemaphoreUsingTask::BinarySemaphoreUsingTask() { - handle = TaskManagement::getCurrentTaskHandle(); - if(handle == nullptr) { - sif::error << "Could not retrieve task handle. Please ensure the" - "constructor was called inside a task." << std::endl; - } - xTaskNotifyGive(handle); -} - -BinarySemaphoreUsingTask::~BinarySemaphoreUsingTask() { - // Clear notification value on destruction. - xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, nullptr); -} - -ReturnValue_t BinarySemaphoreUsingTask::acquire(TimeoutType timeoutType, - uint32_t timeoutMs) { - TickType_t timeout = 0; - if(timeoutType == TimeoutType::POLLING) { - timeout = 0; - } - else if(timeoutType == TimeoutType::WAITING){ - timeout = pdMS_TO_TICKS(timeoutMs); - } - else { - timeout = portMAX_DELAY; - } - return acquireWithTickTimeout(timeoutType, timeout); -} - -ReturnValue_t BinarySemaphoreUsingTask::acquireWithTickTimeout( - TimeoutType timeoutType, TickType_t timeoutTicks) { - BaseType_t returncode = ulTaskNotifyTake(pdTRUE, timeoutTicks); - if (returncode == pdPASS) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - return SemaphoreIF::SEMAPHORE_TIMEOUT; - } -} - -ReturnValue_t BinarySemaphoreUsingTask::release() { - return release(this->handle); -} - -ReturnValue_t BinarySemaphoreUsingTask::release( - TaskHandle_t taskHandle) { - if(getSemaphoreCounter(taskHandle) == 1) { - return SemaphoreIF::SEMAPHORE_NOT_OWNED; - } - BaseType_t returncode = xTaskNotifyGive(taskHandle); - if (returncode == pdPASS) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - // This should never happen. - return HasReturnvaluesIF::RETURN_FAILED; - } -} - -TaskHandle_t BinarySemaphoreUsingTask::getTaskHandle() { - return handle; -} - -uint8_t BinarySemaphoreUsingTask::getSemaphoreCounter() const { - return getSemaphoreCounter(this->handle); -} - -uint8_t BinarySemaphoreUsingTask::getSemaphoreCounter( - TaskHandle_t taskHandle) { - uint32_t notificationValue; - xTaskNotifyAndQuery(taskHandle, 0, eNoAction, ¬ificationValue); - return notificationValue; -} - -// Be careful with the stack size here. This is called from an ISR! -ReturnValue_t BinarySemaphoreUsingTask::releaseFromISR( - TaskHandle_t taskHandle, BaseType_t * higherPriorityTaskWoken) { - if(getSemaphoreCounterFromISR(taskHandle, higherPriorityTaskWoken) == 1) { - return SemaphoreIF::SEMAPHORE_NOT_OWNED; - } - vTaskNotifyGiveFromISR(taskHandle, higherPriorityTaskWoken); - return HasReturnvaluesIF::RETURN_OK; -} - -uint8_t BinarySemaphoreUsingTask::getSemaphoreCounterFromISR( - TaskHandle_t taskHandle, BaseType_t* higherPriorityTaskWoken) { - uint32_t notificationValue = 0; - xTaskNotifyAndQueryFromISR(taskHandle, 0, eNoAction, ¬ificationValue, - higherPriorityTaskWoken); - return notificationValue; -} +#include "../../osal/FreeRTOS/BinSemaphUsingTask.h" +#include "../../osal/FreeRTOS/TaskManagement.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +BinarySemaphoreUsingTask::BinarySemaphoreUsingTask() { + handle = TaskManagement::getCurrentTaskHandle(); + if(handle == nullptr) { + sif::error << "Could not retrieve task handle. Please ensure the" + "constructor was called inside a task." << std::endl; + } + xTaskNotifyGive(handle); +} + +BinarySemaphoreUsingTask::~BinarySemaphoreUsingTask() { + // Clear notification value on destruction. + xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, nullptr); +} + +ReturnValue_t BinarySemaphoreUsingTask::acquire(TimeoutType timeoutType, + uint32_t timeoutMs) { + TickType_t timeout = 0; + if(timeoutType == TimeoutType::POLLING) { + timeout = 0; + } + else if(timeoutType == TimeoutType::WAITING){ + timeout = pdMS_TO_TICKS(timeoutMs); + } + else { + timeout = portMAX_DELAY; + } + return acquireWithTickTimeout(timeoutType, timeout); +} + +ReturnValue_t BinarySemaphoreUsingTask::acquireWithTickTimeout( + TimeoutType timeoutType, TickType_t timeoutTicks) { + BaseType_t returncode = ulTaskNotifyTake(pdTRUE, timeoutTicks); + if (returncode == pdPASS) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + return SemaphoreIF::SEMAPHORE_TIMEOUT; + } +} + +ReturnValue_t BinarySemaphoreUsingTask::release() { + return release(this->handle); +} + +ReturnValue_t BinarySemaphoreUsingTask::release( + TaskHandle_t taskHandle) { + if(getSemaphoreCounter(taskHandle) == 1) { + return SemaphoreIF::SEMAPHORE_NOT_OWNED; + } + BaseType_t returncode = xTaskNotifyGive(taskHandle); + if (returncode == pdPASS) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + // This should never happen. + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +TaskHandle_t BinarySemaphoreUsingTask::getTaskHandle() { + return handle; +} + +uint8_t BinarySemaphoreUsingTask::getSemaphoreCounter() const { + return getSemaphoreCounter(this->handle); +} + +uint8_t BinarySemaphoreUsingTask::getSemaphoreCounter( + TaskHandle_t taskHandle) { + uint32_t notificationValue; + xTaskNotifyAndQuery(taskHandle, 0, eNoAction, ¬ificationValue); + return notificationValue; +} + +// Be careful with the stack size here. This is called from an ISR! +ReturnValue_t BinarySemaphoreUsingTask::releaseFromISR( + TaskHandle_t taskHandle, BaseType_t * higherPriorityTaskWoken) { + if(getSemaphoreCounterFromISR(taskHandle, higherPriorityTaskWoken) == 1) { + return SemaphoreIF::SEMAPHORE_NOT_OWNED; + } + vTaskNotifyGiveFromISR(taskHandle, higherPriorityTaskWoken); + return HasReturnvaluesIF::RETURN_OK; +} + +uint8_t BinarySemaphoreUsingTask::getSemaphoreCounterFromISR( + TaskHandle_t taskHandle, BaseType_t* higherPriorityTaskWoken) { + uint32_t notificationValue = 0; + xTaskNotifyAndQueryFromISR(taskHandle, 0, eNoAction, ¬ificationValue, + higherPriorityTaskWoken); + return notificationValue; +} diff --git a/osal/FreeRTOS/BinSemaphUsingTask.h b/osal/FreeRTOS/BinSemaphUsingTask.h index 2d2cf159..f3c0b0ea 100644 --- a/osal/FreeRTOS/BinSemaphUsingTask.h +++ b/osal/FreeRTOS/BinSemaphUsingTask.h @@ -1,76 +1,76 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ -#define FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ - -#include "../../returnvalues/HasReturnvaluesIF.h" -#include "../../tasks/SemaphoreIF.h" - -#include -#include - -/** - * @brief Binary Semaphore implementation using the task notification value. - * The notification value should therefore not be used - * for other purposes. - * @details - * Additional information: https://www.freertos.org/RTOS-task-notifications.html - * and general semaphore documentation. - */ -class BinarySemaphoreUsingTask: public SemaphoreIF, - public HasReturnvaluesIF { -public: - static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; - - //! @brief Default ctor - BinarySemaphoreUsingTask(); - //! @brief Default dtor - virtual~ BinarySemaphoreUsingTask(); - - ReturnValue_t acquire(TimeoutType timeoutType = TimeoutType::BLOCKING, - uint32_t timeoutMs = portMAX_DELAY) override; - ReturnValue_t release() override; - uint8_t getSemaphoreCounter() const override; - static uint8_t getSemaphoreCounter(TaskHandle_t taskHandle); - static uint8_t getSemaphoreCounterFromISR(TaskHandle_t taskHandle, - BaseType_t* higherPriorityTaskWoken); - - /** - * Same as acquire() with timeout in FreeRTOS ticks. - * @param timeoutTicks - * @return - @c RETURN_OK on success - * - @c RETURN_FAILED on failure - */ - ReturnValue_t acquireWithTickTimeout( - TimeoutType timeoutType = TimeoutType::BLOCKING, - TickType_t timeoutTicks = portMAX_DELAY); - - /** - * Get handle to the task related to the semaphore. - * @return - */ - TaskHandle_t getTaskHandle(); - - /** - * Wrapper function to give back semaphore from handle - * @param semaphore - * @return - @c RETURN_OK on success - * - @c RETURN_FAILED on failure - */ - static ReturnValue_t release(TaskHandle_t taskToNotify); - - /** - * Wrapper function to give back semaphore from handle when called from an ISR - * @param semaphore - * @param higherPriorityTaskWoken This will be set to pdPASS if a task with - * a higher priority was unblocked. A context switch should be requested - * from an ISR if this is the case (see TaskManagement functions) - * @return - @c RETURN_OK on success - * - @c RETURN_FAILED on failure - */ - static ReturnValue_t releaseFromISR(TaskHandle_t taskToNotify, - BaseType_t * higherPriorityTaskWoken); - -protected: - TaskHandle_t handle; -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ +#define FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ + +#include "../../returnvalues/HasReturnvaluesIF.h" +#include "../../tasks/SemaphoreIF.h" + +#include +#include + +/** + * @brief Binary Semaphore implementation using the task notification value. + * The notification value should therefore not be used + * for other purposes. + * @details + * Additional information: https://www.freertos.org/RTOS-task-notifications.html + * and general semaphore documentation. + */ +class BinarySemaphoreUsingTask: public SemaphoreIF, + public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; + + //! @brief Default ctor + BinarySemaphoreUsingTask(); + //! @brief Default dtor + virtual~ BinarySemaphoreUsingTask(); + + ReturnValue_t acquire(TimeoutType timeoutType = TimeoutType::BLOCKING, + uint32_t timeoutMs = portMAX_DELAY) override; + ReturnValue_t release() override; + uint8_t getSemaphoreCounter() const override; + static uint8_t getSemaphoreCounter(TaskHandle_t taskHandle); + static uint8_t getSemaphoreCounterFromISR(TaskHandle_t taskHandle, + BaseType_t* higherPriorityTaskWoken); + + /** + * Same as acquire() with timeout in FreeRTOS ticks. + * @param timeoutTicks + * @return - @c RETURN_OK on success + * - @c RETURN_FAILED on failure + */ + ReturnValue_t acquireWithTickTimeout( + TimeoutType timeoutType = TimeoutType::BLOCKING, + TickType_t timeoutTicks = portMAX_DELAY); + + /** + * Get handle to the task related to the semaphore. + * @return + */ + TaskHandle_t getTaskHandle(); + + /** + * Wrapper function to give back semaphore from handle + * @param semaphore + * @return - @c RETURN_OK on success + * - @c RETURN_FAILED on failure + */ + static ReturnValue_t release(TaskHandle_t taskToNotify); + + /** + * Wrapper function to give back semaphore from handle when called from an ISR + * @param semaphore + * @param higherPriorityTaskWoken This will be set to pdPASS if a task with + * a higher priority was unblocked. A context switch should be requested + * from an ISR if this is the case (see TaskManagement functions) + * @return - @c RETURN_OK on success + * - @c RETURN_FAILED on failure + */ + static ReturnValue_t releaseFromISR(TaskHandle_t taskToNotify, + BaseType_t * higherPriorityTaskWoken); + +protected: + TaskHandle_t handle; +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ */ diff --git a/osal/FreeRTOS/BinarySemaphore.cpp b/osal/FreeRTOS/BinarySemaphore.cpp index f1c78473..8cc3c495 100644 --- a/osal/FreeRTOS/BinarySemaphore.cpp +++ b/osal/FreeRTOS/BinarySemaphore.cpp @@ -1,108 +1,108 @@ -#include "../../osal/FreeRTOS/BinarySemaphore.h" -#include "../../osal/FreeRTOS/TaskManagement.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -BinarySemaphore::BinarySemaphore() { - handle = xSemaphoreCreateBinary(); - if(handle == nullptr) { - sif::error << "Semaphore: Binary semaph creation failure" << std::endl; - } - // Initiated semaphore must be given before it can be taken. - xSemaphoreGive(handle); -} - -BinarySemaphore::~BinarySemaphore() { - vSemaphoreDelete(handle); -} - -BinarySemaphore::BinarySemaphore(BinarySemaphore&& s) { - handle = xSemaphoreCreateBinary(); - if(handle == nullptr) { - sif::error << "Binary semaphore creation failure" << std::endl; - } - xSemaphoreGive(handle); -} - -BinarySemaphore& BinarySemaphore::operator =( - BinarySemaphore&& s) { - if(&s != this) { - handle = xSemaphoreCreateBinary(); - if(handle == nullptr) { - sif::error << "Binary semaphore creation failure" << std::endl; - } - xSemaphoreGive(handle); - } - return *this; -} - -ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType, - uint32_t timeoutMs) { - TickType_t timeout = 0; - if(timeoutType == TimeoutType::POLLING) { - timeout = 0; - } - else if(timeoutType == TimeoutType::WAITING){ - timeout = pdMS_TO_TICKS(timeoutMs); - } - else { - timeout = portMAX_DELAY; - } - return acquireWithTickTimeout(timeoutType, timeout); -} - -ReturnValue_t BinarySemaphore::acquireWithTickTimeout(TimeoutType timeoutType, - TickType_t timeoutTicks) { - if(handle == nullptr) { - return SemaphoreIF::SEMAPHORE_INVALID; - } - - BaseType_t returncode = xSemaphoreTake(handle, timeoutTicks); - if (returncode == pdPASS) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - return SemaphoreIF::SEMAPHORE_TIMEOUT; - } -} - -ReturnValue_t BinarySemaphore::release() { - return release(handle); -} - -ReturnValue_t BinarySemaphore::release(SemaphoreHandle_t semaphore) { - if (semaphore == nullptr) { - return SemaphoreIF::SEMAPHORE_INVALID; - } - BaseType_t returncode = xSemaphoreGive(semaphore); - if (returncode == pdPASS) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - return SemaphoreIF::SEMAPHORE_NOT_OWNED; - } -} - -uint8_t BinarySemaphore::getSemaphoreCounter() const { - return uxSemaphoreGetCount(handle); -} - -SemaphoreHandle_t BinarySemaphore::getSemaphore() { - return handle; -} - - -// Be careful with the stack size here. This is called from an ISR! -ReturnValue_t BinarySemaphore::releaseFromISR( - SemaphoreHandle_t semaphore, BaseType_t * higherPriorityTaskWoken) { - if (semaphore == nullptr) { - return SemaphoreIF::SEMAPHORE_INVALID; - } - BaseType_t returncode = xSemaphoreGiveFromISR(semaphore, - higherPriorityTaskWoken); - if (returncode == pdPASS) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - return SemaphoreIF::SEMAPHORE_NOT_OWNED; - } -} +#include "../../osal/FreeRTOS/BinarySemaphore.h" +#include "../../osal/FreeRTOS/TaskManagement.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +BinarySemaphore::BinarySemaphore() { + handle = xSemaphoreCreateBinary(); + if(handle == nullptr) { + sif::error << "Semaphore: Binary semaph creation failure" << std::endl; + } + // Initiated semaphore must be given before it can be taken. + xSemaphoreGive(handle); +} + +BinarySemaphore::~BinarySemaphore() { + vSemaphoreDelete(handle); +} + +BinarySemaphore::BinarySemaphore(BinarySemaphore&& s) { + handle = xSemaphoreCreateBinary(); + if(handle == nullptr) { + sif::error << "Binary semaphore creation failure" << std::endl; + } + xSemaphoreGive(handle); +} + +BinarySemaphore& BinarySemaphore::operator =( + BinarySemaphore&& s) { + if(&s != this) { + handle = xSemaphoreCreateBinary(); + if(handle == nullptr) { + sif::error << "Binary semaphore creation failure" << std::endl; + } + xSemaphoreGive(handle); + } + return *this; +} + +ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType, + uint32_t timeoutMs) { + TickType_t timeout = 0; + if(timeoutType == TimeoutType::POLLING) { + timeout = 0; + } + else if(timeoutType == TimeoutType::WAITING){ + timeout = pdMS_TO_TICKS(timeoutMs); + } + else { + timeout = portMAX_DELAY; + } + return acquireWithTickTimeout(timeoutType, timeout); +} + +ReturnValue_t BinarySemaphore::acquireWithTickTimeout(TimeoutType timeoutType, + TickType_t timeoutTicks) { + if(handle == nullptr) { + return SemaphoreIF::SEMAPHORE_INVALID; + } + + BaseType_t returncode = xSemaphoreTake(handle, timeoutTicks); + if (returncode == pdPASS) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + return SemaphoreIF::SEMAPHORE_TIMEOUT; + } +} + +ReturnValue_t BinarySemaphore::release() { + return release(handle); +} + +ReturnValue_t BinarySemaphore::release(SemaphoreHandle_t semaphore) { + if (semaphore == nullptr) { + return SemaphoreIF::SEMAPHORE_INVALID; + } + BaseType_t returncode = xSemaphoreGive(semaphore); + if (returncode == pdPASS) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + return SemaphoreIF::SEMAPHORE_NOT_OWNED; + } +} + +uint8_t BinarySemaphore::getSemaphoreCounter() const { + return uxSemaphoreGetCount(handle); +} + +SemaphoreHandle_t BinarySemaphore::getSemaphore() { + return handle; +} + + +// Be careful with the stack size here. This is called from an ISR! +ReturnValue_t BinarySemaphore::releaseFromISR( + SemaphoreHandle_t semaphore, BaseType_t * higherPriorityTaskWoken) { + if (semaphore == nullptr) { + return SemaphoreIF::SEMAPHORE_INVALID; + } + BaseType_t returncode = xSemaphoreGiveFromISR(semaphore, + higherPriorityTaskWoken); + if (returncode == pdPASS) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + return SemaphoreIF::SEMAPHORE_NOT_OWNED; + } +} diff --git a/osal/FreeRTOS/BinarySemaphore.h b/osal/FreeRTOS/BinarySemaphore.h index 5a32088a..c6cedc53 100644 --- a/osal/FreeRTOS/BinarySemaphore.h +++ b/osal/FreeRTOS/BinarySemaphore.h @@ -1,107 +1,107 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ -#define FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ - -#include "../../returnvalues/HasReturnvaluesIF.h" -#include "../../tasks/SemaphoreIF.h" - -#include -#include - -/** - * @brief OS Tool to achieve synchronization of between tasks or between - * task and ISR. The default semaphore implementation creates a - * binary semaphore, which can only be taken once. - * @details - * Documentation: https://www.freertos.org/Embedded-RTOS-Binary-Semaphores.html - * - * Please note that if the semaphore implementation is only related to - * the synchronization of one task, the new task notifications can be used, - * also see the BinSemaphUsingTask and CountingSemaphUsingTask classes. - * These use the task notification value instead of a queue and are - * faster and more efficient. - * - * @author R. Mueller - * @ingroup osal - */ -class BinarySemaphore: public SemaphoreIF, - public HasReturnvaluesIF { -public: - static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; - - //! @brief Default ctor - BinarySemaphore(); - //! @brief Copy ctor, deleted explicitely. - BinarySemaphore(const BinarySemaphore&) = delete; - //! @brief Copy assignment, deleted explicitely. - BinarySemaphore& operator=(const BinarySemaphore&) = delete; - //! @brief Move ctor - BinarySemaphore (BinarySemaphore &&); - //! @brief Move assignment - BinarySemaphore & operator=(BinarySemaphore &&); - //! @brief Destructor - virtual ~BinarySemaphore(); - - uint8_t getSemaphoreCounter() const override; - - /** - * Take the binary semaphore. - * If the semaphore has already been taken, the task will be blocked - * for a maximum of #timeoutMs or until the semaphore is given back, - * for example by an ISR or another task. - * @param timeoutMs - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout - */ - ReturnValue_t acquire(TimeoutType timeoutType = - TimeoutType::BLOCKING, uint32_t timeoutMs = portMAX_DELAY) override; - - /** - * Same as lockBinarySemaphore() with timeout in FreeRTOS ticks. - * @param timeoutTicks - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout - */ - ReturnValue_t acquireWithTickTimeout(TimeoutType timeoutType = - TimeoutType::BLOCKING, TickType_t timeoutTicks = portMAX_DELAY); - - /** - * Release the binary semaphore. - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is - * already available. - */ - ReturnValue_t release() override; - - /** - * Get Handle to the semaphore. - * @return - */ - SemaphoreHandle_t getSemaphore(); - - /** - * Wrapper function to give back semaphore from handle - * @param semaphore - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is - * already available. - */ - static ReturnValue_t release(SemaphoreHandle_t semaphore); - - /** - * Wrapper function to give back semaphore from handle when called from an ISR - * @param semaphore - * @param higherPriorityTaskWoken This will be set to pdPASS if a task with - * a higher priority was unblocked. A context switch from an ISR should - * then be requested (see TaskManagement functions) - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is - * already available. - */ - static ReturnValue_t releaseFromISR(SemaphoreHandle_t semaphore, - BaseType_t * higherPriorityTaskWoken); - -protected: - SemaphoreHandle_t handle; -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ +#define FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ + +#include "../../returnvalues/HasReturnvaluesIF.h" +#include "../../tasks/SemaphoreIF.h" + +#include +#include + +/** + * @brief OS Tool to achieve synchronization of between tasks or between + * task and ISR. The default semaphore implementation creates a + * binary semaphore, which can only be taken once. + * @details + * Documentation: https://www.freertos.org/Embedded-RTOS-Binary-Semaphores.html + * + * Please note that if the semaphore implementation is only related to + * the synchronization of one task, the new task notifications can be used, + * also see the BinSemaphUsingTask and CountingSemaphUsingTask classes. + * These use the task notification value instead of a queue and are + * faster and more efficient. + * + * @author R. Mueller + * @ingroup osal + */ +class BinarySemaphore: public SemaphoreIF, + public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; + + //! @brief Default ctor + BinarySemaphore(); + //! @brief Copy ctor, deleted explicitely. + BinarySemaphore(const BinarySemaphore&) = delete; + //! @brief Copy assignment, deleted explicitely. + BinarySemaphore& operator=(const BinarySemaphore&) = delete; + //! @brief Move ctor + BinarySemaphore (BinarySemaphore &&); + //! @brief Move assignment + BinarySemaphore & operator=(BinarySemaphore &&); + //! @brief Destructor + virtual ~BinarySemaphore(); + + uint8_t getSemaphoreCounter() const override; + + /** + * Take the binary semaphore. + * If the semaphore has already been taken, the task will be blocked + * for a maximum of #timeoutMs or until the semaphore is given back, + * for example by an ISR or another task. + * @param timeoutMs + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout + */ + ReturnValue_t acquire(TimeoutType timeoutType = + TimeoutType::BLOCKING, uint32_t timeoutMs = portMAX_DELAY) override; + + /** + * Same as lockBinarySemaphore() with timeout in FreeRTOS ticks. + * @param timeoutTicks + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout + */ + ReturnValue_t acquireWithTickTimeout(TimeoutType timeoutType = + TimeoutType::BLOCKING, TickType_t timeoutTicks = portMAX_DELAY); + + /** + * Release the binary semaphore. + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is + * already available. + */ + ReturnValue_t release() override; + + /** + * Get Handle to the semaphore. + * @return + */ + SemaphoreHandle_t getSemaphore(); + + /** + * Wrapper function to give back semaphore from handle + * @param semaphore + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is + * already available. + */ + static ReturnValue_t release(SemaphoreHandle_t semaphore); + + /** + * Wrapper function to give back semaphore from handle when called from an ISR + * @param semaphore + * @param higherPriorityTaskWoken This will be set to pdPASS if a task with + * a higher priority was unblocked. A context switch from an ISR should + * then be requested (see TaskManagement functions) + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is + * already available. + */ + static ReturnValue_t releaseFromISR(SemaphoreHandle_t semaphore, + BaseType_t * higherPriorityTaskWoken); + +protected: + SemaphoreHandle_t handle; +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ */ diff --git a/osal/FreeRTOS/CountingSemaphUsingTask.cpp b/osal/FreeRTOS/CountingSemaphUsingTask.cpp index e66671ac..a47341bc 100644 --- a/osal/FreeRTOS/CountingSemaphUsingTask.cpp +++ b/osal/FreeRTOS/CountingSemaphUsingTask.cpp @@ -1,114 +1,114 @@ -#include "../../osal/FreeRTOS/CountingSemaphUsingTask.h" -#include "../../osal/FreeRTOS/TaskManagement.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -CountingSemaphoreUsingTask::CountingSemaphoreUsingTask(const uint8_t maxCount, - uint8_t initCount): maxCount(maxCount) { - if(initCount > maxCount) { - sif::error << "CountingSemaphoreUsingTask: Max count bigger than " - "intial cout. Setting initial count to max count." << std::endl; - initCount = maxCount; - } - - handle = TaskManagement::getCurrentTaskHandle(); - if(handle == nullptr) { - sif::error << "CountingSemaphoreUsingTask: Could not retrieve task " - "handle. Please ensure the constructor was called inside a " - "task." << std::endl; - } - - uint32_t oldNotificationValue; - xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, - &oldNotificationValue); - if(oldNotificationValue != 0) { - sif::warning << "CountinSemaphoreUsingTask: Semaphore initiated but " - "current notification value is not 0. Please ensure the " - "notification value is not used for other purposes!" << std::endl; - } - for(int i = 0; i < initCount; i++) { - xTaskNotifyGive(handle); - } -} - -CountingSemaphoreUsingTask::~CountingSemaphoreUsingTask() { - // Clear notification value on destruction. - // If this is not desired, don't call the destructor - // (or implement a boolean which disables the reset) - xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, nullptr); -} - -ReturnValue_t CountingSemaphoreUsingTask::acquire(TimeoutType timeoutType, - uint32_t timeoutMs) { - TickType_t timeout = 0; - if(timeoutType == TimeoutType::POLLING) { - timeout = 0; - } - else if(timeoutType == TimeoutType::WAITING){ - timeout = pdMS_TO_TICKS(timeoutMs); - } - else { - timeout = portMAX_DELAY; - } - return acquireWithTickTimeout(timeoutType, timeout); - -} - -ReturnValue_t CountingSemaphoreUsingTask::acquireWithTickTimeout( - TimeoutType timeoutType, TickType_t timeoutTicks) { - // Decrement notfication value without resetting it. - BaseType_t oldCount = ulTaskNotifyTake(pdFALSE, timeoutTicks); - if (getSemaphoreCounter() == oldCount - 1) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - return SemaphoreIF::SEMAPHORE_TIMEOUT; - } -} - -ReturnValue_t CountingSemaphoreUsingTask::release() { - if(getSemaphoreCounter() == maxCount) { - return SemaphoreIF::SEMAPHORE_NOT_OWNED; - } - return release(handle); -} - -ReturnValue_t CountingSemaphoreUsingTask::release( - TaskHandle_t taskToNotify) { - BaseType_t returncode = xTaskNotifyGive(taskToNotify); - if (returncode == pdPASS) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - // This should never happen. - return HasReturnvaluesIF::RETURN_FAILED; - } -} - - -uint8_t CountingSemaphoreUsingTask::getSemaphoreCounter() const { - uint32_t notificationValue = 0; - xTaskNotifyAndQuery(handle, 0, eNoAction, ¬ificationValue); - return notificationValue; -} - -TaskHandle_t CountingSemaphoreUsingTask::getTaskHandle() { - return handle; -} - -ReturnValue_t CountingSemaphoreUsingTask::releaseFromISR( - TaskHandle_t taskToNotify, BaseType_t* higherPriorityTaskWoken) { - vTaskNotifyGiveFromISR(taskToNotify, higherPriorityTaskWoken); - return HasReturnvaluesIF::RETURN_OK; -} - -uint8_t CountingSemaphoreUsingTask::getSemaphoreCounterFromISR( - TaskHandle_t task, BaseType_t* higherPriorityTaskWoken) { - uint32_t notificationValue; - xTaskNotifyAndQueryFromISR(task, 0, eNoAction, ¬ificationValue, - higherPriorityTaskWoken); - return notificationValue; -} - -uint8_t CountingSemaphoreUsingTask::getMaxCount() const { - return maxCount; -} +#include "../../osal/FreeRTOS/CountingSemaphUsingTask.h" +#include "../../osal/FreeRTOS/TaskManagement.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +CountingSemaphoreUsingTask::CountingSemaphoreUsingTask(const uint8_t maxCount, + uint8_t initCount): maxCount(maxCount) { + if(initCount > maxCount) { + sif::error << "CountingSemaphoreUsingTask: Max count bigger than " + "intial cout. Setting initial count to max count." << std::endl; + initCount = maxCount; + } + + handle = TaskManagement::getCurrentTaskHandle(); + if(handle == nullptr) { + sif::error << "CountingSemaphoreUsingTask: Could not retrieve task " + "handle. Please ensure the constructor was called inside a " + "task." << std::endl; + } + + uint32_t oldNotificationValue; + xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, + &oldNotificationValue); + if(oldNotificationValue != 0) { + sif::warning << "CountinSemaphoreUsingTask: Semaphore initiated but " + "current notification value is not 0. Please ensure the " + "notification value is not used for other purposes!" << std::endl; + } + for(int i = 0; i < initCount; i++) { + xTaskNotifyGive(handle); + } +} + +CountingSemaphoreUsingTask::~CountingSemaphoreUsingTask() { + // Clear notification value on destruction. + // If this is not desired, don't call the destructor + // (or implement a boolean which disables the reset) + xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, nullptr); +} + +ReturnValue_t CountingSemaphoreUsingTask::acquire(TimeoutType timeoutType, + uint32_t timeoutMs) { + TickType_t timeout = 0; + if(timeoutType == TimeoutType::POLLING) { + timeout = 0; + } + else if(timeoutType == TimeoutType::WAITING){ + timeout = pdMS_TO_TICKS(timeoutMs); + } + else { + timeout = portMAX_DELAY; + } + return acquireWithTickTimeout(timeoutType, timeout); + +} + +ReturnValue_t CountingSemaphoreUsingTask::acquireWithTickTimeout( + TimeoutType timeoutType, TickType_t timeoutTicks) { + // Decrement notfication value without resetting it. + BaseType_t oldCount = ulTaskNotifyTake(pdFALSE, timeoutTicks); + if (getSemaphoreCounter() == oldCount - 1) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + return SemaphoreIF::SEMAPHORE_TIMEOUT; + } +} + +ReturnValue_t CountingSemaphoreUsingTask::release() { + if(getSemaphoreCounter() == maxCount) { + return SemaphoreIF::SEMAPHORE_NOT_OWNED; + } + return release(handle); +} + +ReturnValue_t CountingSemaphoreUsingTask::release( + TaskHandle_t taskToNotify) { + BaseType_t returncode = xTaskNotifyGive(taskToNotify); + if (returncode == pdPASS) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + // This should never happen. + return HasReturnvaluesIF::RETURN_FAILED; + } +} + + +uint8_t CountingSemaphoreUsingTask::getSemaphoreCounter() const { + uint32_t notificationValue = 0; + xTaskNotifyAndQuery(handle, 0, eNoAction, ¬ificationValue); + return notificationValue; +} + +TaskHandle_t CountingSemaphoreUsingTask::getTaskHandle() { + return handle; +} + +ReturnValue_t CountingSemaphoreUsingTask::releaseFromISR( + TaskHandle_t taskToNotify, BaseType_t* higherPriorityTaskWoken) { + vTaskNotifyGiveFromISR(taskToNotify, higherPriorityTaskWoken); + return HasReturnvaluesIF::RETURN_OK; +} + +uint8_t CountingSemaphoreUsingTask::getSemaphoreCounterFromISR( + TaskHandle_t task, BaseType_t* higherPriorityTaskWoken) { + uint32_t notificationValue; + xTaskNotifyAndQueryFromISR(task, 0, eNoAction, ¬ificationValue, + higherPriorityTaskWoken); + return notificationValue; +} + +uint8_t CountingSemaphoreUsingTask::getMaxCount() const { + return maxCount; +} diff --git a/osal/FreeRTOS/CountingSemaphUsingTask.h b/osal/FreeRTOS/CountingSemaphUsingTask.h index e3733382..8977258c 100644 --- a/osal/FreeRTOS/CountingSemaphUsingTask.h +++ b/osal/FreeRTOS/CountingSemaphUsingTask.h @@ -1,102 +1,102 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ -#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ - -#include "../../osal/FreeRTOS/CountingSemaphUsingTask.h" -#include "../../tasks/SemaphoreIF.h" - -extern "C" { -#include -#include -} - -/** - * @brief Couting Semaphore implementation which uses the notification value - * of the task. The notification value should therefore not be used - * for other purposes. - * @details - * Additional information: https://www.freertos.org/RTOS-task-notifications.html - * and general semaphore documentation. - */ -class CountingSemaphoreUsingTask: public SemaphoreIF { -public: - CountingSemaphoreUsingTask(const uint8_t maxCount, uint8_t initCount); - virtual ~CountingSemaphoreUsingTask(); - - /** - * Acquire the counting semaphore. - * If no semaphores are available, the task will be blocked - * for a maximum of #timeoutMs or until one is given back, - * for example by an ISR or another task. - * @param timeoutMs - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout - */ - ReturnValue_t acquire(TimeoutType timeoutType = TimeoutType::BLOCKING, - uint32_t timeoutMs = portMAX_DELAY) override; - - /** - * Release a semaphore, increasing the number of available counting - * semaphores up to the #maxCount value. - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are - * already available. - */ - ReturnValue_t release() override; - - uint8_t getSemaphoreCounter() const override; - /** - * Get the semaphore counter from an ISR. - * @param task - * @param higherPriorityTaskWoken This will be set to pdPASS if a task with - * a higher priority was unblocked. A context switch should be requested - * from an ISR if this is the case (see TaskManagement functions) - * @return - */ - static uint8_t getSemaphoreCounterFromISR(TaskHandle_t task, - BaseType_t* higherPriorityTaskWoken); - - /** - * Acquire with a timeout value in ticks - * @param timeoutTicks - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout - */ - ReturnValue_t acquireWithTickTimeout( - TimeoutType timeoutType = TimeoutType::BLOCKING, - TickType_t timeoutTicks = portMAX_DELAY); - - /** - * Get handle to the task related to the semaphore. - * @return - */ - TaskHandle_t getTaskHandle(); - - /** - * Release semaphore of task by supplying task handle - * @param taskToNotify - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are - * already available. - */ - static ReturnValue_t release(TaskHandle_t taskToNotify); - /** - * Release seamphore of a task from an ISR. - * @param taskToNotify - * @param higherPriorityTaskWoken This will be set to pdPASS if a task with - * a higher priority was unblocked. A context switch should be requested - * from an ISR if this is the case (see TaskManagement functions) - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are - * already available. - */ - static ReturnValue_t releaseFromISR(TaskHandle_t taskToNotify, - BaseType_t* higherPriorityTaskWoken); - - uint8_t getMaxCount() const; - -private: - TaskHandle_t handle; - const uint8_t maxCount; -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ +#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ + +#include "../../osal/FreeRTOS/CountingSemaphUsingTask.h" +#include "../../tasks/SemaphoreIF.h" + +extern "C" { +#include +#include +} + +/** + * @brief Couting Semaphore implementation which uses the notification value + * of the task. The notification value should therefore not be used + * for other purposes. + * @details + * Additional information: https://www.freertos.org/RTOS-task-notifications.html + * and general semaphore documentation. + */ +class CountingSemaphoreUsingTask: public SemaphoreIF { +public: + CountingSemaphoreUsingTask(const uint8_t maxCount, uint8_t initCount); + virtual ~CountingSemaphoreUsingTask(); + + /** + * Acquire the counting semaphore. + * If no semaphores are available, the task will be blocked + * for a maximum of #timeoutMs or until one is given back, + * for example by an ISR or another task. + * @param timeoutMs + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout + */ + ReturnValue_t acquire(TimeoutType timeoutType = TimeoutType::BLOCKING, + uint32_t timeoutMs = portMAX_DELAY) override; + + /** + * Release a semaphore, increasing the number of available counting + * semaphores up to the #maxCount value. + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are + * already available. + */ + ReturnValue_t release() override; + + uint8_t getSemaphoreCounter() const override; + /** + * Get the semaphore counter from an ISR. + * @param task + * @param higherPriorityTaskWoken This will be set to pdPASS if a task with + * a higher priority was unblocked. A context switch should be requested + * from an ISR if this is the case (see TaskManagement functions) + * @return + */ + static uint8_t getSemaphoreCounterFromISR(TaskHandle_t task, + BaseType_t* higherPriorityTaskWoken); + + /** + * Acquire with a timeout value in ticks + * @param timeoutTicks + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout + */ + ReturnValue_t acquireWithTickTimeout( + TimeoutType timeoutType = TimeoutType::BLOCKING, + TickType_t timeoutTicks = portMAX_DELAY); + + /** + * Get handle to the task related to the semaphore. + * @return + */ + TaskHandle_t getTaskHandle(); + + /** + * Release semaphore of task by supplying task handle + * @param taskToNotify + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are + * already available. + */ + static ReturnValue_t release(TaskHandle_t taskToNotify); + /** + * Release seamphore of a task from an ISR. + * @param taskToNotify + * @param higherPriorityTaskWoken This will be set to pdPASS if a task with + * a higher priority was unblocked. A context switch should be requested + * from an ISR if this is the case (see TaskManagement functions) + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are + * already available. + */ + static ReturnValue_t releaseFromISR(TaskHandle_t taskToNotify, + BaseType_t* higherPriorityTaskWoken); + + uint8_t getMaxCount() const; + +private: + TaskHandle_t handle; + const uint8_t maxCount; +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ */ diff --git a/osal/FreeRTOS/CountingSemaphore.cpp b/osal/FreeRTOS/CountingSemaphore.cpp index 0b218e54..d1310a6a 100644 --- a/osal/FreeRTOS/CountingSemaphore.cpp +++ b/osal/FreeRTOS/CountingSemaphore.cpp @@ -1,43 +1,43 @@ -#include "../../osal/FreeRTOS/CountingSemaphore.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" -#include "../../osal/FreeRTOS/TaskManagement.h" - -#include - -// Make sure #define configUSE_COUNTING_SEMAPHORES 1 is set in -// free FreeRTOSConfig.h file. -CountingSemaphore::CountingSemaphore(const uint8_t maxCount, uint8_t initCount): - maxCount(maxCount), initCount(initCount) { - if(initCount > maxCount) { - sif::error << "CountingSemaphoreUsingTask: Max count bigger than " - "intial cout. Setting initial count to max count." << std::endl; - initCount = maxCount; - } - - handle = xSemaphoreCreateCounting(maxCount, initCount); - if(handle == nullptr) { - sif::error << "CountingSemaphore: Creation failure" << std::endl; - } -} - -CountingSemaphore::CountingSemaphore(CountingSemaphore&& other): - maxCount(other.maxCount), initCount(other.initCount) { - handle = xSemaphoreCreateCounting(other.maxCount, other.initCount); - if(handle == nullptr) { - sif::error << "CountingSemaphore: Creation failure" << std::endl; - } -} - -CountingSemaphore& CountingSemaphore::operator =( - CountingSemaphore&& other) { - handle = xSemaphoreCreateCounting(other.maxCount, other.initCount); - if(handle == nullptr) { - sif::error << "CountingSemaphore: Creation failure" << std::endl; - } - return * this; -} - - -uint8_t CountingSemaphore::getMaxCount() const { - return maxCount; -} +#include "../../osal/FreeRTOS/CountingSemaphore.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include "../../osal/FreeRTOS/TaskManagement.h" + +#include + +// Make sure #define configUSE_COUNTING_SEMAPHORES 1 is set in +// free FreeRTOSConfig.h file. +CountingSemaphore::CountingSemaphore(const uint8_t maxCount, uint8_t initCount): + maxCount(maxCount), initCount(initCount) { + if(initCount > maxCount) { + sif::error << "CountingSemaphoreUsingTask: Max count bigger than " + "intial cout. Setting initial count to max count." << std::endl; + initCount = maxCount; + } + + handle = xSemaphoreCreateCounting(maxCount, initCount); + if(handle == nullptr) { + sif::error << "CountingSemaphore: Creation failure" << std::endl; + } +} + +CountingSemaphore::CountingSemaphore(CountingSemaphore&& other): + maxCount(other.maxCount), initCount(other.initCount) { + handle = xSemaphoreCreateCounting(other.maxCount, other.initCount); + if(handle == nullptr) { + sif::error << "CountingSemaphore: Creation failure" << std::endl; + } +} + +CountingSemaphore& CountingSemaphore::operator =( + CountingSemaphore&& other) { + handle = xSemaphoreCreateCounting(other.maxCount, other.initCount); + if(handle == nullptr) { + sif::error << "CountingSemaphore: Creation failure" << std::endl; + } + return * this; +} + + +uint8_t CountingSemaphore::getMaxCount() const { + return maxCount; +} diff --git a/osal/FreeRTOS/CountingSemaphore.h b/osal/FreeRTOS/CountingSemaphore.h index 9432ed81..ae2f62ae 100644 --- a/osal/FreeRTOS/CountingSemaphore.h +++ b/osal/FreeRTOS/CountingSemaphore.h @@ -1,34 +1,34 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ -#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ -#include "../../osal/FreeRTOS/BinarySemaphore.h" - -/** - * @brief Counting semaphores, which can be acquire more than once. - * @details - * See: https://www.freertos.org/CreateCounting.html - * API of counting semaphores is almost identical to binary semaphores, - * so we just inherit from binary semaphore and provide the respective - * constructors. - */ -class CountingSemaphore: public BinarySemaphore { -public: - CountingSemaphore(const uint8_t maxCount, uint8_t initCount); - //! @brief Copy ctor, disabled - CountingSemaphore(const CountingSemaphore&) = delete; - //! @brief Copy assignment, disabled - CountingSemaphore& operator=(const CountingSemaphore&) = delete; - //! @brief Move ctor - CountingSemaphore (CountingSemaphore &&); - //! @brief Move assignment - CountingSemaphore & operator=(CountingSemaphore &&); - - /* Same API as binary semaphore otherwise. acquire() can be called - * until there are not semaphores left and release() can be called - * until maxCount is reached. */ - uint8_t getMaxCount() const; -private: - const uint8_t maxCount; - uint8_t initCount = 0; -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ +#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ +#include "../../osal/FreeRTOS/BinarySemaphore.h" + +/** + * @brief Counting semaphores, which can be acquire more than once. + * @details + * See: https://www.freertos.org/CreateCounting.html + * API of counting semaphores is almost identical to binary semaphores, + * so we just inherit from binary semaphore and provide the respective + * constructors. + */ +class CountingSemaphore: public BinarySemaphore { +public: + CountingSemaphore(const uint8_t maxCount, uint8_t initCount); + //! @brief Copy ctor, disabled + CountingSemaphore(const CountingSemaphore&) = delete; + //! @brief Copy assignment, disabled + CountingSemaphore& operator=(const CountingSemaphore&) = delete; + //! @brief Move ctor + CountingSemaphore (CountingSemaphore &&); + //! @brief Move assignment + CountingSemaphore & operator=(CountingSemaphore &&); + + /* Same API as binary semaphore otherwise. acquire() can be called + * until there are not semaphores left and release() can be called + * until maxCount is reached. */ + uint8_t getMaxCount() const; +private: + const uint8_t maxCount; + uint8_t initCount = 0; +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ */ diff --git a/osal/FreeRTOS/SemaphoreFactory.cpp b/osal/FreeRTOS/SemaphoreFactory.cpp index 8575cf4a..beb0d096 100644 --- a/osal/FreeRTOS/SemaphoreFactory.cpp +++ b/osal/FreeRTOS/SemaphoreFactory.cpp @@ -1,59 +1,59 @@ -#include "../../osal/FreeRTOS/BinarySemaphore.h" -#include "../../osal/FreeRTOS/BinSemaphUsingTask.h" -#include "../../osal/FreeRTOS/CountingSemaphore.h" -#include "../../osal/FreeRTOS/CountingSemaphUsingTask.h" -#include "../../tasks/SemaphoreFactory.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; - -static const uint32_t USE_REGULAR_SEMAPHORES = 0; -static const uint32_t USE_TASK_NOTIFICATIONS = 1; - -SemaphoreFactory::SemaphoreFactory() { -} - -SemaphoreFactory::~SemaphoreFactory() { - delete factoryInstance; -} - -SemaphoreFactory* SemaphoreFactory::instance() { - if (factoryInstance == nullptr){ - factoryInstance = new SemaphoreFactory(); - } - return SemaphoreFactory::factoryInstance; -} - -SemaphoreIF* SemaphoreFactory::createBinarySemaphore(uint32_t argument) { - if(argument == USE_REGULAR_SEMAPHORES) { - return new BinarySemaphore(); - } - else if(argument == USE_TASK_NOTIFICATIONS) { - return new BinarySemaphoreUsingTask(); - } - else { - sif::warning << "SemaphoreFactory: Invalid argument, return regular" - "binary semaphore" << std::endl; - return new BinarySemaphore(); - } -} - -SemaphoreIF* SemaphoreFactory::createCountingSemaphore(uint8_t maxCount, - uint8_t initCount, uint32_t argument) { - if(argument == USE_REGULAR_SEMAPHORES) { - return new CountingSemaphore(maxCount, initCount); - } - else if(argument == USE_TASK_NOTIFICATIONS) { - return new CountingSemaphoreUsingTask(maxCount, initCount); - } - else { - sif::warning << "SemaphoreFactory: Invalid argument, return regular" - "binary semaphore" << std::endl; - return new CountingSemaphore(maxCount, initCount); - } - -} - -void SemaphoreFactory::deleteSemaphore(SemaphoreIF* semaphore) { - delete semaphore; -} +#include "../../osal/FreeRTOS/BinarySemaphore.h" +#include "../../osal/FreeRTOS/BinSemaphUsingTask.h" +#include "../../osal/FreeRTOS/CountingSemaphore.h" +#include "../../osal/FreeRTOS/CountingSemaphUsingTask.h" +#include "../../tasks/SemaphoreFactory.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; + +static const uint32_t USE_REGULAR_SEMAPHORES = 0; +static const uint32_t USE_TASK_NOTIFICATIONS = 1; + +SemaphoreFactory::SemaphoreFactory() { +} + +SemaphoreFactory::~SemaphoreFactory() { + delete factoryInstance; +} + +SemaphoreFactory* SemaphoreFactory::instance() { + if (factoryInstance == nullptr){ + factoryInstance = new SemaphoreFactory(); + } + return SemaphoreFactory::factoryInstance; +} + +SemaphoreIF* SemaphoreFactory::createBinarySemaphore(uint32_t argument) { + if(argument == USE_REGULAR_SEMAPHORES) { + return new BinarySemaphore(); + } + else if(argument == USE_TASK_NOTIFICATIONS) { + return new BinarySemaphoreUsingTask(); + } + else { + sif::warning << "SemaphoreFactory: Invalid argument, return regular" + "binary semaphore" << std::endl; + return new BinarySemaphore(); + } +} + +SemaphoreIF* SemaphoreFactory::createCountingSemaphore(uint8_t maxCount, + uint8_t initCount, uint32_t argument) { + if(argument == USE_REGULAR_SEMAPHORES) { + return new CountingSemaphore(maxCount, initCount); + } + else if(argument == USE_TASK_NOTIFICATIONS) { + return new CountingSemaphoreUsingTask(maxCount, initCount); + } + else { + sif::warning << "SemaphoreFactory: Invalid argument, return regular" + "binary semaphore" << std::endl; + return new CountingSemaphore(maxCount, initCount); + } + +} + +void SemaphoreFactory::deleteSemaphore(SemaphoreIF* semaphore) { + delete semaphore; +} diff --git a/osal/FreeRTOS/TaskManagement.cpp b/osal/FreeRTOS/TaskManagement.cpp index 16682d36..b77f12a9 100644 --- a/osal/FreeRTOS/TaskManagement.cpp +++ b/osal/FreeRTOS/TaskManagement.cpp @@ -1,24 +1,24 @@ -#include "../../osal/FreeRTOS/TaskManagement.h" - -void TaskManagement::vRequestContextSwitchFromTask() { - vTaskDelay(0); -} - -void TaskManagement::requestContextSwitch( - CallContext callContext = CallContext::TASK) { - if(callContext == CallContext::ISR) { - // This function depends on the partmacro.h definition for the specific device - vRequestContextSwitchFromISR(); - } else { - vRequestContextSwitchFromTask(); - } -} - -TaskHandle_t TaskManagement::getCurrentTaskHandle() { - return xTaskGetCurrentTaskHandle(); -} - -size_t TaskManagement::getTaskStackHighWatermark( - TaskHandle_t task) { - return uxTaskGetStackHighWaterMark(task) * sizeof(StackType_t); -} +#include "../../osal/FreeRTOS/TaskManagement.h" + +void TaskManagement::vRequestContextSwitchFromTask() { + vTaskDelay(0); +} + +void TaskManagement::requestContextSwitch( + CallContext callContext = CallContext::TASK) { + if(callContext == CallContext::ISR) { + // This function depends on the partmacro.h definition for the specific device + vRequestContextSwitchFromISR(); + } else { + vRequestContextSwitchFromTask(); + } +} + +TaskHandle_t TaskManagement::getCurrentTaskHandle() { + return xTaskGetCurrentTaskHandle(); +} + +size_t TaskManagement::getTaskStackHighWatermark( + TaskHandle_t task) { + return uxTaskGetStackHighWaterMark(task) * sizeof(StackType_t); +} diff --git a/osal/FreeRTOS/TaskManagement.h b/osal/FreeRTOS/TaskManagement.h index 43003d76..4b7fe3eb 100644 --- a/osal/FreeRTOS/TaskManagement.h +++ b/osal/FreeRTOS/TaskManagement.h @@ -1,64 +1,64 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ -#define FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ - -#include "../../returnvalues/HasReturnvaluesIF.h" - -extern "C" { -#include -#include -} -#include - -/** - * Architecture dependant portmacro.h function call. - * Should be implemented in bsp. - */ -extern void vRequestContextSwitchFromISR(); - -/*! - * Used by functions to tell if they are being called from - * within an ISR or from a regular task. This is required because FreeRTOS - * has different functions for handling semaphores and messages from within - * an ISR and task. - */ -enum class CallContext { - TASK = 0x00,//!< task_context - ISR = 0xFF //!< isr_context -}; - - -class TaskManagement { -public: - /** - * @brief In this function, a function dependant on the portmacro.h header - * function calls to request a context switch can be specified. - * This can be used if sending to the queue from an ISR caused a task - * to unblock and a context switch is required. - */ - static void requestContextSwitch(CallContext callContext); - - /** - * If task preemption in FreeRTOS is disabled, a context switch - * can be requested manually by calling this function. - */ - static void vRequestContextSwitchFromTask(void); - - /** - * @return The current task handle - */ - static TaskHandle_t getCurrentTaskHandle(); - - /** - * Get returns the minimum amount of remaining stack space in words - * that was a available to the task since the task started executing. - * Please note that the actual value in bytes depends - * on the stack depth type. - * E.g. on a 32 bit machine, a value of 200 means 800 bytes. - * @return Smallest value of stack remaining since the task was started in - * words. - */ - static size_t getTaskStackHighWatermark( - TaskHandle_t task = nullptr); -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ +#define FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ + +#include "../../returnvalues/HasReturnvaluesIF.h" + +extern "C" { +#include +#include +} +#include + +/** + * Architecture dependant portmacro.h function call. + * Should be implemented in bsp. + */ +extern void vRequestContextSwitchFromISR(); + +/*! + * Used by functions to tell if they are being called from + * within an ISR or from a regular task. This is required because FreeRTOS + * has different functions for handling semaphores and messages from within + * an ISR and task. + */ +enum class CallContext { + TASK = 0x00,//!< task_context + ISR = 0xFF //!< isr_context +}; + + +class TaskManagement { +public: + /** + * @brief In this function, a function dependant on the portmacro.h header + * function calls to request a context switch can be specified. + * This can be used if sending to the queue from an ISR caused a task + * to unblock and a context switch is required. + */ + static void requestContextSwitch(CallContext callContext); + + /** + * If task preemption in FreeRTOS is disabled, a context switch + * can be requested manually by calling this function. + */ + static void vRequestContextSwitchFromTask(void); + + /** + * @return The current task handle + */ + static TaskHandle_t getCurrentTaskHandle(); + + /** + * Get returns the minimum amount of remaining stack space in words + * that was a available to the task since the task started executing. + * Please note that the actual value in bytes depends + * on the stack depth type. + * E.g. on a 32 bit machine, a value of 200 means 800 bytes. + * @return Smallest value of stack remaining since the task was started in + * words. + */ + static size_t getTaskStackHighWatermark( + TaskHandle_t task = nullptr); +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ */ diff --git a/osal/linux/BinarySemaphore.cpp b/osal/linux/BinarySemaphore.cpp index 5216ff37..8c0eeae7 100644 --- a/osal/linux/BinarySemaphore.cpp +++ b/osal/linux/BinarySemaphore.cpp @@ -1,149 +1,149 @@ -#include "../../osal/linux/BinarySemaphore.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -extern "C" { -#include -#include -} - -BinarySemaphore::BinarySemaphore() { - // Using unnamed semaphores for now - initSemaphore(); -} - -BinarySemaphore::~BinarySemaphore() { - sem_destroy(&handle); -} - -BinarySemaphore::BinarySemaphore(BinarySemaphore&& s) { - initSemaphore(); -} - -BinarySemaphore& BinarySemaphore::operator =( - BinarySemaphore&& s) { - initSemaphore(); - return * this; -} - -ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType, - uint32_t timeoutMs) { - int result = 0; - if(timeoutType == TimeoutType::POLLING) { - result = sem_trywait(&handle); - } - else if(timeoutType == TimeoutType::BLOCKING) { - result = sem_wait(&handle); - } - else if(timeoutType == TimeoutType::WAITING){ - timespec timeOut; - clock_gettime(CLOCK_REALTIME, &timeOut); - uint64_t nseconds = timeOut.tv_sec * 1000000000 + timeOut.tv_nsec; - nseconds += timeoutMs * 1000000; - timeOut.tv_sec = nseconds / 1000000000; - timeOut.tv_nsec = nseconds - timeOut.tv_sec * 1000000000; - result = sem_timedwait(&handle, &timeOut); - if(result != 0 and errno == EINVAL) { - sif::debug << "BinarySemaphore::acquire: Invalid time value possible" - << std::endl; - } - } - if(result == 0) { - return HasReturnvaluesIF::RETURN_OK; - } - - switch(errno) { - case(EAGAIN): - // Operation could not be performed without blocking (for sem_trywait) - case(ETIMEDOUT): - // Semaphore is 0 - return SemaphoreIF::SEMAPHORE_TIMEOUT; - case(EINVAL): - // Semaphore invalid - return SemaphoreIF::SEMAPHORE_INVALID; - case(EINTR): - // Call was interrupted by signal handler - sif::debug << "BinarySemaphore::acquire: Signal handler interrupted." - "Code " << strerror(errno) << std::endl; - /* No break */ - default: - return HasReturnvaluesIF::RETURN_FAILED; - } -} - -ReturnValue_t BinarySemaphore::release() { - return BinarySemaphore::release(&this->handle); -} - -ReturnValue_t BinarySemaphore::release(sem_t *handle) { - ReturnValue_t countResult = checkCount(handle, 1); - if(countResult != HasReturnvaluesIF::RETURN_OK) { - return countResult; - } - - int result = sem_post(handle); - if(result == 0) { - return HasReturnvaluesIF::RETURN_OK; - } - - switch(errno) { - case(EINVAL): - // Semaphore invalid - return SemaphoreIF::SEMAPHORE_INVALID; - case(EOVERFLOW): - // SEM_MAX_VALUE overflow. This should never happen - default: - return HasReturnvaluesIF::RETURN_FAILED; - } -} - -uint8_t BinarySemaphore::getSemaphoreCounter() const { - // And another ugly cast :-D - return getSemaphoreCounter(const_cast(&this->handle)); -} - -uint8_t BinarySemaphore::getSemaphoreCounter(sem_t *handle) { - int value = 0; - int result = sem_getvalue(handle, &value); - if (result == 0) { - return value; - } - else if(result != 0 and errno == EINVAL) { - // Could be called from interrupt, use lightweight printf - printf("BinarySemaphore::getSemaphoreCounter: Invalid semaphore\n"); - return 0; - } - else { - // This should never happen. - return 0; - } -} - -void BinarySemaphore::initSemaphore(uint8_t initCount) { - auto result = sem_init(&handle, true, initCount); - if(result == -1) { - switch(errno) { - case(EINVAL): - // Value exceeds SEM_VALUE_MAX - case(ENOSYS): - // System does not support process-shared semaphores - sif::error << "BinarySemaphore: Init failed with" << strerror(errno) - << std::endl; - } - } -} - -ReturnValue_t BinarySemaphore::checkCount(sem_t* handle, uint8_t maxCount) { - int value = getSemaphoreCounter(handle); - if(value >= maxCount) { - if(maxCount == 1 and value > 1) { - // Binary Semaphore special case. - // This is a config error use lightweight printf is this is called - // from an interrupt - printf("BinarySemaphore::release: Value of binary semaphore greater" - " than 1!\n"); - return HasReturnvaluesIF::RETURN_FAILED; - } - return SemaphoreIF::SEMAPHORE_NOT_OWNED; - } - return HasReturnvaluesIF::RETURN_OK; -} +#include "../../osal/linux/BinarySemaphore.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +extern "C" { +#include +#include +} + +BinarySemaphore::BinarySemaphore() { + // Using unnamed semaphores for now + initSemaphore(); +} + +BinarySemaphore::~BinarySemaphore() { + sem_destroy(&handle); +} + +BinarySemaphore::BinarySemaphore(BinarySemaphore&& s) { + initSemaphore(); +} + +BinarySemaphore& BinarySemaphore::operator =( + BinarySemaphore&& s) { + initSemaphore(); + return * this; +} + +ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType, + uint32_t timeoutMs) { + int result = 0; + if(timeoutType == TimeoutType::POLLING) { + result = sem_trywait(&handle); + } + else if(timeoutType == TimeoutType::BLOCKING) { + result = sem_wait(&handle); + } + else if(timeoutType == TimeoutType::WAITING){ + timespec timeOut; + clock_gettime(CLOCK_REALTIME, &timeOut); + uint64_t nseconds = timeOut.tv_sec * 1000000000 + timeOut.tv_nsec; + nseconds += timeoutMs * 1000000; + timeOut.tv_sec = nseconds / 1000000000; + timeOut.tv_nsec = nseconds - timeOut.tv_sec * 1000000000; + result = sem_timedwait(&handle, &timeOut); + if(result != 0 and errno == EINVAL) { + sif::debug << "BinarySemaphore::acquire: Invalid time value possible" + << std::endl; + } + } + if(result == 0) { + return HasReturnvaluesIF::RETURN_OK; + } + + switch(errno) { + case(EAGAIN): + // Operation could not be performed without blocking (for sem_trywait) + case(ETIMEDOUT): + // Semaphore is 0 + return SemaphoreIF::SEMAPHORE_TIMEOUT; + case(EINVAL): + // Semaphore invalid + return SemaphoreIF::SEMAPHORE_INVALID; + case(EINTR): + // Call was interrupted by signal handler + sif::debug << "BinarySemaphore::acquire: Signal handler interrupted." + "Code " << strerror(errno) << std::endl; + /* No break */ + default: + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +ReturnValue_t BinarySemaphore::release() { + return BinarySemaphore::release(&this->handle); +} + +ReturnValue_t BinarySemaphore::release(sem_t *handle) { + ReturnValue_t countResult = checkCount(handle, 1); + if(countResult != HasReturnvaluesIF::RETURN_OK) { + return countResult; + } + + int result = sem_post(handle); + if(result == 0) { + return HasReturnvaluesIF::RETURN_OK; + } + + switch(errno) { + case(EINVAL): + // Semaphore invalid + return SemaphoreIF::SEMAPHORE_INVALID; + case(EOVERFLOW): + // SEM_MAX_VALUE overflow. This should never happen + default: + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +uint8_t BinarySemaphore::getSemaphoreCounter() const { + // And another ugly cast :-D + return getSemaphoreCounter(const_cast(&this->handle)); +} + +uint8_t BinarySemaphore::getSemaphoreCounter(sem_t *handle) { + int value = 0; + int result = sem_getvalue(handle, &value); + if (result == 0) { + return value; + } + else if(result != 0 and errno == EINVAL) { + // Could be called from interrupt, use lightweight printf + printf("BinarySemaphore::getSemaphoreCounter: Invalid semaphore\n"); + return 0; + } + else { + // This should never happen. + return 0; + } +} + +void BinarySemaphore::initSemaphore(uint8_t initCount) { + auto result = sem_init(&handle, true, initCount); + if(result == -1) { + switch(errno) { + case(EINVAL): + // Value exceeds SEM_VALUE_MAX + case(ENOSYS): + // System does not support process-shared semaphores + sif::error << "BinarySemaphore: Init failed with" << strerror(errno) + << std::endl; + } + } +} + +ReturnValue_t BinarySemaphore::checkCount(sem_t* handle, uint8_t maxCount) { + int value = getSemaphoreCounter(handle); + if(value >= maxCount) { + if(maxCount == 1 and value > 1) { + // Binary Semaphore special case. + // This is a config error use lightweight printf is this is called + // from an interrupt + printf("BinarySemaphore::release: Value of binary semaphore greater" + " than 1!\n"); + return HasReturnvaluesIF::RETURN_FAILED; + } + return SemaphoreIF::SEMAPHORE_NOT_OWNED; + } + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/osal/linux/BinarySemaphore.h b/osal/linux/BinarySemaphore.h index 4181de85..e9bb8bb6 100644 --- a/osal/linux/BinarySemaphore.h +++ b/osal/linux/BinarySemaphore.h @@ -1,81 +1,81 @@ -#ifndef FRAMEWORK_OSAL_LINUX_BINARYSEMPAHORE_H_ -#define FRAMEWORK_OSAL_LINUX_BINARYSEMPAHORE_H_ - -#include "../../returnvalues/HasReturnvaluesIF.h" -#include "../../tasks/SemaphoreIF.h" - -extern "C" { -#include -} - -/** - * @brief OS Tool to achieve synchronization of between tasks or between - * task and ISR. The default semaphore implementation creates a - * binary semaphore, which can only be taken once. - * @details - * See: http://www.man7.org/linux/man-pages/man7/sem_overview.7.html - * @author R. Mueller - * @ingroup osal - */ -class BinarySemaphore: public SemaphoreIF, - public HasReturnvaluesIF { -public: - static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; - - //! @brief Default ctor - BinarySemaphore(); - //! @brief Copy ctor, deleted explicitely. - BinarySemaphore(const BinarySemaphore&) = delete; - //! @brief Copy assignment, deleted explicitely. - BinarySemaphore& operator=(const BinarySemaphore&) = delete; - //! @brief Move ctor - BinarySemaphore (BinarySemaphore &&); - //! @brief Move assignment - BinarySemaphore & operator=(BinarySemaphore &&); - //! @brief Destructor - virtual ~BinarySemaphore(); - - void initSemaphore(uint8_t initCount = 1); - - uint8_t getSemaphoreCounter() const override; - static uint8_t getSemaphoreCounter(sem_t* handle); - - /** - * Take the binary semaphore. - * If the semaphore has already been taken, the task will be blocked - * for a maximum of #timeoutMs or until the semaphore is given back, - * for example by an ISR or another task. - * @param timeoutMs - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout - */ - ReturnValue_t acquire(TimeoutType timeoutType = TimeoutType::BLOCKING, - uint32_t timeoutMs = 0) override; - - /** - * Release the binary semaphore. - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is - * already available. - */ - virtual ReturnValue_t release() override; - /** - * This static function can be used to release a semaphore by providing - * its handle. - * @param handle - * @return - */ - static ReturnValue_t release(sem_t* handle); - - /** Checks the validity of the semaphore count against a specified - * known maxCount - * @param handle - * @param maxCount - * @return - */ - static ReturnValue_t checkCount(sem_t* handle, uint8_t maxCount); -protected: - sem_t handle; -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ */ +#ifndef FRAMEWORK_OSAL_LINUX_BINARYSEMPAHORE_H_ +#define FRAMEWORK_OSAL_LINUX_BINARYSEMPAHORE_H_ + +#include "../../returnvalues/HasReturnvaluesIF.h" +#include "../../tasks/SemaphoreIF.h" + +extern "C" { +#include +} + +/** + * @brief OS Tool to achieve synchronization of between tasks or between + * task and ISR. The default semaphore implementation creates a + * binary semaphore, which can only be taken once. + * @details + * See: http://www.man7.org/linux/man-pages/man7/sem_overview.7.html + * @author R. Mueller + * @ingroup osal + */ +class BinarySemaphore: public SemaphoreIF, + public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; + + //! @brief Default ctor + BinarySemaphore(); + //! @brief Copy ctor, deleted explicitely. + BinarySemaphore(const BinarySemaphore&) = delete; + //! @brief Copy assignment, deleted explicitely. + BinarySemaphore& operator=(const BinarySemaphore&) = delete; + //! @brief Move ctor + BinarySemaphore (BinarySemaphore &&); + //! @brief Move assignment + BinarySemaphore & operator=(BinarySemaphore &&); + //! @brief Destructor + virtual ~BinarySemaphore(); + + void initSemaphore(uint8_t initCount = 1); + + uint8_t getSemaphoreCounter() const override; + static uint8_t getSemaphoreCounter(sem_t* handle); + + /** + * Take the binary semaphore. + * If the semaphore has already been taken, the task will be blocked + * for a maximum of #timeoutMs or until the semaphore is given back, + * for example by an ISR or another task. + * @param timeoutMs + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout + */ + ReturnValue_t acquire(TimeoutType timeoutType = TimeoutType::BLOCKING, + uint32_t timeoutMs = 0) override; + + /** + * Release the binary semaphore. + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is + * already available. + */ + virtual ReturnValue_t release() override; + /** + * This static function can be used to release a semaphore by providing + * its handle. + * @param handle + * @return + */ + static ReturnValue_t release(sem_t* handle); + + /** Checks the validity of the semaphore count against a specified + * known maxCount + * @param handle + * @param maxCount + * @return + */ + static ReturnValue_t checkCount(sem_t* handle, uint8_t maxCount); +protected: + sem_t handle; +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ */ diff --git a/osal/linux/CountingSemaphore.cpp b/osal/linux/CountingSemaphore.cpp index ef32539b..18339399 100644 --- a/osal/linux/CountingSemaphore.cpp +++ b/osal/linux/CountingSemaphore.cpp @@ -1,54 +1,54 @@ -#include "../../osal/linux/CountingSemaphore.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -CountingSemaphore::CountingSemaphore(const uint8_t maxCount, uint8_t initCount): - maxCount(maxCount), initCount(initCount) { - if(initCount > maxCount) { - sif::error << "CountingSemaphoreUsingTask: Max count bigger than " - "intial cout. Setting initial count to max count." << std::endl; - initCount = maxCount; - } - - initSemaphore(initCount); -} - -CountingSemaphore::CountingSemaphore(CountingSemaphore&& other): - maxCount(other.maxCount), initCount(other.initCount) { - initSemaphore(initCount); -} - -CountingSemaphore& CountingSemaphore::operator =( - CountingSemaphore&& other) { - initSemaphore(other.initCount); - return * this; -} - -ReturnValue_t CountingSemaphore::release() { - ReturnValue_t result = checkCount(&handle, maxCount); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - return CountingSemaphore::release(&this->handle); -} - -ReturnValue_t CountingSemaphore::release(sem_t* handle) { - int result = sem_post(handle); - if(result == 0) { - return HasReturnvaluesIF::RETURN_OK; - } - - switch(errno) { - case(EINVAL): - // Semaphore invalid - return SemaphoreIF::SEMAPHORE_INVALID; - case(EOVERFLOW): - // SEM_MAX_VALUE overflow. This should never happen - default: - return HasReturnvaluesIF::RETURN_FAILED; - } -} - -uint8_t CountingSemaphore::getMaxCount() const { - return maxCount; -} - +#include "../../osal/linux/CountingSemaphore.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +CountingSemaphore::CountingSemaphore(const uint8_t maxCount, uint8_t initCount): + maxCount(maxCount), initCount(initCount) { + if(initCount > maxCount) { + sif::error << "CountingSemaphoreUsingTask: Max count bigger than " + "intial cout. Setting initial count to max count." << std::endl; + initCount = maxCount; + } + + initSemaphore(initCount); +} + +CountingSemaphore::CountingSemaphore(CountingSemaphore&& other): + maxCount(other.maxCount), initCount(other.initCount) { + initSemaphore(initCount); +} + +CountingSemaphore& CountingSemaphore::operator =( + CountingSemaphore&& other) { + initSemaphore(other.initCount); + return * this; +} + +ReturnValue_t CountingSemaphore::release() { + ReturnValue_t result = checkCount(&handle, maxCount); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return CountingSemaphore::release(&this->handle); +} + +ReturnValue_t CountingSemaphore::release(sem_t* handle) { + int result = sem_post(handle); + if(result == 0) { + return HasReturnvaluesIF::RETURN_OK; + } + + switch(errno) { + case(EINVAL): + // Semaphore invalid + return SemaphoreIF::SEMAPHORE_INVALID; + case(EOVERFLOW): + // SEM_MAX_VALUE overflow. This should never happen + default: + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +uint8_t CountingSemaphore::getMaxCount() const { + return maxCount; +} + diff --git a/osal/linux/CountingSemaphore.h b/osal/linux/CountingSemaphore.h index afe21a61..e0fbb992 100644 --- a/osal/linux/CountingSemaphore.h +++ b/osal/linux/CountingSemaphore.h @@ -1,37 +1,37 @@ -#ifndef FRAMEWORK_OSAL_LINUX_COUNTINGSEMAPHORE_H_ -#define FRAMEWORK_OSAL_LINUX_COUNTINGSEMAPHORE_H_ -#include "../../osal/linux/BinarySemaphore.h" - -/** - * @brief Counting semaphores, which can be acquired more than once. - * @details - * See: https://www.freertos.org/CreateCounting.html - * API of counting semaphores is almost identical to binary semaphores, - * so we just inherit from binary semaphore and provide the respective - * constructors. - */ -class CountingSemaphore: public BinarySemaphore { -public: - CountingSemaphore(const uint8_t maxCount, uint8_t initCount); - //! @brief Copy ctor, disabled - CountingSemaphore(const CountingSemaphore&) = delete; - //! @brief Copy assignment, disabled - CountingSemaphore& operator=(const CountingSemaphore&) = delete; - //! @brief Move ctor - CountingSemaphore (CountingSemaphore &&); - //! @brief Move assignment - CountingSemaphore & operator=(CountingSemaphore &&); - - ReturnValue_t release() override; - static ReturnValue_t release(sem_t* sem); - /* Same API as binary semaphore otherwise. acquire() can be called - * until there are not semaphores left and release() can be called - * until maxCount is reached. */ - - uint8_t getMaxCount() const; -private: - const uint8_t maxCount; - uint8_t initCount = 0; -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ */ +#ifndef FRAMEWORK_OSAL_LINUX_COUNTINGSEMAPHORE_H_ +#define FRAMEWORK_OSAL_LINUX_COUNTINGSEMAPHORE_H_ +#include "../../osal/linux/BinarySemaphore.h" + +/** + * @brief Counting semaphores, which can be acquired more than once. + * @details + * See: https://www.freertos.org/CreateCounting.html + * API of counting semaphores is almost identical to binary semaphores, + * so we just inherit from binary semaphore and provide the respective + * constructors. + */ +class CountingSemaphore: public BinarySemaphore { +public: + CountingSemaphore(const uint8_t maxCount, uint8_t initCount); + //! @brief Copy ctor, disabled + CountingSemaphore(const CountingSemaphore&) = delete; + //! @brief Copy assignment, disabled + CountingSemaphore& operator=(const CountingSemaphore&) = delete; + //! @brief Move ctor + CountingSemaphore (CountingSemaphore &&); + //! @brief Move assignment + CountingSemaphore & operator=(CountingSemaphore &&); + + ReturnValue_t release() override; + static ReturnValue_t release(sem_t* sem); + /* Same API as binary semaphore otherwise. acquire() can be called + * until there are not semaphores left and release() can be called + * until maxCount is reached. */ + + uint8_t getMaxCount() const; +private: + const uint8_t maxCount; + uint8_t initCount = 0; +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ */ diff --git a/osal/linux/SemaphoreFactory.cpp b/osal/linux/SemaphoreFactory.cpp index bcb5d16f..e4710933 100644 --- a/osal/linux/SemaphoreFactory.cpp +++ b/osal/linux/SemaphoreFactory.cpp @@ -1,33 +1,33 @@ -#include "../../tasks/SemaphoreFactory.h" -#include "BinarySemaphore.h" -#include "CountingSemaphore.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; - -SemaphoreFactory::SemaphoreFactory() { -} - -SemaphoreFactory::~SemaphoreFactory() { - delete factoryInstance; -} - -SemaphoreFactory* SemaphoreFactory::instance() { - if (factoryInstance == nullptr){ - factoryInstance = new SemaphoreFactory(); - } - return SemaphoreFactory::factoryInstance; -} - -SemaphoreIF* SemaphoreFactory::createBinarySemaphore(uint32_t arguments) { - return new BinarySemaphore(); -} - -SemaphoreIF* SemaphoreFactory::createCountingSemaphore(const uint8_t maxCount, - uint8_t initCount, uint32_t arguments) { - return new CountingSemaphore(maxCount, initCount); -} - -void SemaphoreFactory::deleteSemaphore(SemaphoreIF* semaphore) { - delete semaphore; -} +#include "../../tasks/SemaphoreFactory.h" +#include "BinarySemaphore.h" +#include "CountingSemaphore.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; + +SemaphoreFactory::SemaphoreFactory() { +} + +SemaphoreFactory::~SemaphoreFactory() { + delete factoryInstance; +} + +SemaphoreFactory* SemaphoreFactory::instance() { + if (factoryInstance == nullptr){ + factoryInstance = new SemaphoreFactory(); + } + return SemaphoreFactory::factoryInstance; +} + +SemaphoreIF* SemaphoreFactory::createBinarySemaphore(uint32_t arguments) { + return new BinarySemaphore(); +} + +SemaphoreIF* SemaphoreFactory::createCountingSemaphore(const uint8_t maxCount, + uint8_t initCount, uint32_t arguments) { + return new CountingSemaphore(maxCount, initCount); +} + +void SemaphoreFactory::deleteSemaphore(SemaphoreIF* semaphore) { + delete semaphore; +} diff --git a/serialize/SerialBufferAdapter.cpp b/serialize/SerialBufferAdapter.cpp index 812cd34b..1c11afd4 100644 --- a/serialize/SerialBufferAdapter.cpp +++ b/serialize/SerialBufferAdapter.cpp @@ -1,129 +1,129 @@ -#include "../serialize/SerialBufferAdapter.h" -#include "../serviceinterface/ServiceInterfaceStream.h" -#include - -template -SerialBufferAdapter::SerialBufferAdapter(const uint8_t* buffer, - count_t bufferLength, bool serializeLength) : - serializeLength(serializeLength), - constBuffer(buffer), buffer(nullptr), - bufferLength(bufferLength) {} - -template -SerialBufferAdapter::SerialBufferAdapter(uint8_t* buffer, - count_t bufferLength, bool serializeLength) : - serializeLength(serializeLength), constBuffer(buffer), buffer(buffer), - bufferLength(bufferLength) {} - - -template -SerialBufferAdapter::~SerialBufferAdapter() { -} - -template -ReturnValue_t SerialBufferAdapter::serialize(uint8_t** buffer, - size_t* size, size_t maxSize, Endianness streamEndianness) const { - if (serializeLength) { - ReturnValue_t result = SerializeAdapter::serialize(&bufferLength, - buffer, size, maxSize, streamEndianness); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - } - - if (*size + bufferLength > maxSize) { - return BUFFER_TOO_SHORT; - } - - if (this->constBuffer != nullptr) { - std::memcpy(*buffer, this->constBuffer, bufferLength); - } - else if (this->buffer != nullptr) { - // This will propably be never reached, constBuffer should always be - // set if non-const buffer is set. - std::memcpy(*buffer, this->buffer, bufferLength); - } - else { - return HasReturnvaluesIF::RETURN_FAILED; - } - *size += bufferLength; - (*buffer) += bufferLength; - return HasReturnvaluesIF::RETURN_OK; - -} - -template -size_t SerialBufferAdapter::getSerializedSize() const { - if (serializeLength) { - return bufferLength + SerializeAdapter::getSerializedSize(&bufferLength); - } else { - return bufferLength; - } -} - -template -ReturnValue_t SerialBufferAdapter::deSerialize(const uint8_t** buffer, - size_t* size, Endianness streamEndianness) { - if (this->buffer == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - - if(serializeLength){ - count_t lengthField = 0; - ReturnValue_t result = SerializeAdapter::deSerialize(&lengthField, - buffer, size, streamEndianness); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - if(lengthField > bufferLength) { - return TOO_MANY_ELEMENTS; - } - bufferLength = lengthField; - } - - if (bufferLength <= *size) { - *size -= bufferLength; - std::memcpy(this->buffer, *buffer, bufferLength); - (*buffer) += bufferLength; - return HasReturnvaluesIF::RETURN_OK; - } - else { - return STREAM_TOO_SHORT; - } -} - -template -uint8_t * SerialBufferAdapter::getBuffer() { - if(buffer == nullptr) { - sif::error << "Wrong access function for stored type !" - " Use getConstBuffer()." << std::endl; - return nullptr; - } - return buffer; -} - -template -const uint8_t * SerialBufferAdapter::getConstBuffer() { - if(constBuffer == nullptr) { - sif::error << "SerialBufferAdapter::getConstBuffer:" - " Buffers are unitialized!" << std::endl; - return nullptr; - } - return constBuffer; -} - -template -void SerialBufferAdapter::setBuffer(uint8_t* buffer, - count_t bufferLength) { - this->buffer = buffer; - this->constBuffer = buffer; - this->bufferLength = bufferLength; -} - - -//forward Template declaration for linker -template class SerialBufferAdapter; -template class SerialBufferAdapter; -template class SerialBufferAdapter; -template class SerialBufferAdapter; - +#include "../serialize/SerialBufferAdapter.h" +#include "../serviceinterface/ServiceInterfaceStream.h" +#include + +template +SerialBufferAdapter::SerialBufferAdapter(const uint8_t* buffer, + count_t bufferLength, bool serializeLength) : + serializeLength(serializeLength), + constBuffer(buffer), buffer(nullptr), + bufferLength(bufferLength) {} + +template +SerialBufferAdapter::SerialBufferAdapter(uint8_t* buffer, + count_t bufferLength, bool serializeLength) : + serializeLength(serializeLength), constBuffer(buffer), buffer(buffer), + bufferLength(bufferLength) {} + + +template +SerialBufferAdapter::~SerialBufferAdapter() { +} + +template +ReturnValue_t SerialBufferAdapter::serialize(uint8_t** buffer, + size_t* size, size_t maxSize, Endianness streamEndianness) const { + if (serializeLength) { + ReturnValue_t result = SerializeAdapter::serialize(&bufferLength, + buffer, size, maxSize, streamEndianness); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + } + + if (*size + bufferLength > maxSize) { + return BUFFER_TOO_SHORT; + } + + if (this->constBuffer != nullptr) { + std::memcpy(*buffer, this->constBuffer, bufferLength); + } + else if (this->buffer != nullptr) { + // This will propably be never reached, constBuffer should always be + // set if non-const buffer is set. + std::memcpy(*buffer, this->buffer, bufferLength); + } + else { + return HasReturnvaluesIF::RETURN_FAILED; + } + *size += bufferLength; + (*buffer) += bufferLength; + return HasReturnvaluesIF::RETURN_OK; + +} + +template +size_t SerialBufferAdapter::getSerializedSize() const { + if (serializeLength) { + return bufferLength + SerializeAdapter::getSerializedSize(&bufferLength); + } else { + return bufferLength; + } +} + +template +ReturnValue_t SerialBufferAdapter::deSerialize(const uint8_t** buffer, + size_t* size, Endianness streamEndianness) { + if (this->buffer == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + + if(serializeLength){ + count_t lengthField = 0; + ReturnValue_t result = SerializeAdapter::deSerialize(&lengthField, + buffer, size, streamEndianness); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if(lengthField > bufferLength) { + return TOO_MANY_ELEMENTS; + } + bufferLength = lengthField; + } + + if (bufferLength <= *size) { + *size -= bufferLength; + std::memcpy(this->buffer, *buffer, bufferLength); + (*buffer) += bufferLength; + return HasReturnvaluesIF::RETURN_OK; + } + else { + return STREAM_TOO_SHORT; + } +} + +template +uint8_t * SerialBufferAdapter::getBuffer() { + if(buffer == nullptr) { + sif::error << "Wrong access function for stored type !" + " Use getConstBuffer()." << std::endl; + return nullptr; + } + return buffer; +} + +template +const uint8_t * SerialBufferAdapter::getConstBuffer() { + if(constBuffer == nullptr) { + sif::error << "SerialBufferAdapter::getConstBuffer:" + " Buffers are unitialized!" << std::endl; + return nullptr; + } + return constBuffer; +} + +template +void SerialBufferAdapter::setBuffer(uint8_t* buffer, + count_t bufferLength) { + this->buffer = buffer; + this->constBuffer = buffer; + this->bufferLength = bufferLength; +} + + +//forward Template declaration for linker +template class SerialBufferAdapter; +template class SerialBufferAdapter; +template class SerialBufferAdapter; +template class SerialBufferAdapter; + diff --git a/serialize/SerialBufferAdapter.h b/serialize/SerialBufferAdapter.h index c3dfcd8f..9a89e18b 100644 --- a/serialize/SerialBufferAdapter.h +++ b/serialize/SerialBufferAdapter.h @@ -1,78 +1,78 @@ -#ifndef SERIALBUFFERADAPTER_H_ -#define SERIALBUFFERADAPTER_H_ - -#include "../serialize/SerializeIF.h" -#include "../serialize/SerializeAdapter.h" - -/** - * This adapter provides an interface for SerializeIF to serialize or deserialize - * buffers with no length header but a known size. - * - * Additionally, the buffer length can be serialized too and will be put in - * front of the serialized buffer. - * - * Can be used with SerialLinkedListAdapter by declaring a SerializeElement with - * SerialElement>. - * Right now, the SerialBufferAdapter must always - * be initialized with the buffer and size ! - * - * \ingroup serialize - */ -template -class SerialBufferAdapter: public SerializeIF { -public: - - /** - * Constructor for constant uint8_t buffer. Length field can be serialized optionally. - * Type of length can be supplied as template type. - * @param buffer - * @param bufferLength - * @param serializeLength - */ - SerialBufferAdapter(const uint8_t* buffer, count_t bufferLength, - bool serializeLength = false); - - /** - * Constructor for non-constant uint8_t buffer. - * Length field can be serialized optionally. - * Type of length can be supplied as template type. - * @param buffer - * @param bufferLength - * @param serializeLength Length field will be serialized with size count_t - */ - SerialBufferAdapter(uint8_t* buffer, count_t bufferLength, - bool serializeLength = false); - - virtual ~SerialBufferAdapter(); - - virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, - size_t maxSize, Endianness streamEndianness) const override; - - virtual size_t getSerializedSize() const override; - - /** - * @brief This function deserializes a buffer into the member buffer. - * @details - * If a length field is present, it is ignored, as the size should have - * been set in the constructor. If the size is not known beforehand, - * consider using SerialFixedArrayListAdapter instead. - * @param buffer [out] Resulting buffer - * @param size remaining size to deserialize, should be larger than buffer - * + size field size - * @param bigEndian - * @return - */ - virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, - Endianness streamEndianness) override; - - uint8_t * getBuffer(); - const uint8_t * getConstBuffer(); - void setBuffer(uint8_t* buffer, count_t bufferLength); -private: - bool serializeLength = false; - const uint8_t *constBuffer = nullptr; - uint8_t *buffer = nullptr; - count_t bufferLength = 0; -}; - -#endif /* SERIALBUFFERADAPTER_H_ */ +#ifndef SERIALBUFFERADAPTER_H_ +#define SERIALBUFFERADAPTER_H_ + +#include "../serialize/SerializeIF.h" +#include "../serialize/SerializeAdapter.h" + +/** + * This adapter provides an interface for SerializeIF to serialize or deserialize + * buffers with no length header but a known size. + * + * Additionally, the buffer length can be serialized too and will be put in + * front of the serialized buffer. + * + * Can be used with SerialLinkedListAdapter by declaring a SerializeElement with + * SerialElement>. + * Right now, the SerialBufferAdapter must always + * be initialized with the buffer and size ! + * + * \ingroup serialize + */ +template +class SerialBufferAdapter: public SerializeIF { +public: + + /** + * Constructor for constant uint8_t buffer. Length field can be serialized optionally. + * Type of length can be supplied as template type. + * @param buffer + * @param bufferLength + * @param serializeLength + */ + SerialBufferAdapter(const uint8_t* buffer, count_t bufferLength, + bool serializeLength = false); + + /** + * Constructor for non-constant uint8_t buffer. + * Length field can be serialized optionally. + * Type of length can be supplied as template type. + * @param buffer + * @param bufferLength + * @param serializeLength Length field will be serialized with size count_t + */ + SerialBufferAdapter(uint8_t* buffer, count_t bufferLength, + bool serializeLength = false); + + virtual ~SerialBufferAdapter(); + + virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, + size_t maxSize, Endianness streamEndianness) const override; + + virtual size_t getSerializedSize() const override; + + /** + * @brief This function deserializes a buffer into the member buffer. + * @details + * If a length field is present, it is ignored, as the size should have + * been set in the constructor. If the size is not known beforehand, + * consider using SerialFixedArrayListAdapter instead. + * @param buffer [out] Resulting buffer + * @param size remaining size to deserialize, should be larger than buffer + * + size field size + * @param bigEndian + * @return + */ + virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, + Endianness streamEndianness) override; + + uint8_t * getBuffer(); + const uint8_t * getConstBuffer(); + void setBuffer(uint8_t* buffer, count_t bufferLength); +private: + bool serializeLength = false; + const uint8_t *constBuffer = nullptr; + uint8_t *buffer = nullptr; + count_t bufferLength = 0; +}; + +#endif /* SERIALBUFFERADAPTER_H_ */ diff --git a/tasks/SemaphoreFactory.h b/tasks/SemaphoreFactory.h index 2c41a135..01c09d1b 100644 --- a/tasks/SemaphoreFactory.h +++ b/tasks/SemaphoreFactory.h @@ -1,50 +1,50 @@ -#ifndef FSFW_TASKS_SEMAPHOREFACTORY_H_ -#define FSFW_TASKS_SEMAPHOREFACTORY_H_ - -#include "../tasks/SemaphoreIF.h" - -/** - * Creates Semaphore. - * This class is a "singleton" interface, i.e. it provides an - * interface, but also is the base class for a singleton. - */ -class SemaphoreFactory { -public: - virtual ~SemaphoreFactory(); - /** - * Returns the single instance of SemaphoreFactory. - * The implementation of #instance is found in its subclasses. - * Thus, we choose link-time variability of the instance. - */ - static SemaphoreFactory* instance(); - - /** - * Create a binary semaphore. - * Creator function for a binary semaphore which may only be acquired once - * @param argument Can be used to pass implementation specific information. - * @return Pointer to newly created semaphore class instance. - */ - SemaphoreIF* createBinarySemaphore(uint32_t arguments = 0); - /** - * Create a counting semaphore. - * Creator functons for a counting semaphore which may be acquired multiple - * times. - * @param count Semaphore can be taken count times. - * @param initCount Initial count value. - * @param argument Can be used to pass implementation specific information. - * @return - */ - SemaphoreIF* createCountingSemaphore(const uint8_t maxCount, - uint8_t initCount, uint32_t arguments = 0); - - void deleteSemaphore(SemaphoreIF* semaphore); - -private: - /** - * External instantiation is not allowed. - */ - SemaphoreFactory(); - static SemaphoreFactory* factoryInstance; -}; - -#endif /* FSFW_TASKS_SEMAPHOREFACTORY_H_ */ +#ifndef FSFW_TASKS_SEMAPHOREFACTORY_H_ +#define FSFW_TASKS_SEMAPHOREFACTORY_H_ + +#include "../tasks/SemaphoreIF.h" + +/** + * Creates Semaphore. + * This class is a "singleton" interface, i.e. it provides an + * interface, but also is the base class for a singleton. + */ +class SemaphoreFactory { +public: + virtual ~SemaphoreFactory(); + /** + * Returns the single instance of SemaphoreFactory. + * The implementation of #instance is found in its subclasses. + * Thus, we choose link-time variability of the instance. + */ + static SemaphoreFactory* instance(); + + /** + * Create a binary semaphore. + * Creator function for a binary semaphore which may only be acquired once + * @param argument Can be used to pass implementation specific information. + * @return Pointer to newly created semaphore class instance. + */ + SemaphoreIF* createBinarySemaphore(uint32_t arguments = 0); + /** + * Create a counting semaphore. + * Creator functons for a counting semaphore which may be acquired multiple + * times. + * @param count Semaphore can be taken count times. + * @param initCount Initial count value. + * @param argument Can be used to pass implementation specific information. + * @return + */ + SemaphoreIF* createCountingSemaphore(const uint8_t maxCount, + uint8_t initCount, uint32_t arguments = 0); + + void deleteSemaphore(SemaphoreIF* semaphore); + +private: + /** + * External instantiation is not allowed. + */ + SemaphoreFactory(); + static SemaphoreFactory* factoryInstance; +}; + +#endif /* FSFW_TASKS_SEMAPHOREFACTORY_H_ */ diff --git a/tasks/SemaphoreIF.h b/tasks/SemaphoreIF.h index 30d4ed88..dd327e23 100644 --- a/tasks/SemaphoreIF.h +++ b/tasks/SemaphoreIF.h @@ -1,68 +1,68 @@ -#ifndef FRAMEWORK_TASKS_SEMAPHOREIF_H_ -#define FRAMEWORK_TASKS_SEMAPHOREIF_H_ -#include "../returnvalues/FwClassIds.h" -#include "../returnvalues/HasReturnvaluesIF.h" -#include - -/** - * @brief Generic interface for semaphores, which can be used to achieve - * task synchronization. This is a generic interface which can be - * used for both binary semaphores and counting semaphores. - * @details - * A semaphore is a synchronization primitive. - * See: https://en.wikipedia.org/wiki/Semaphore_(programming) - * A semaphore can be used to achieve task synchonization and track the - * availability of resources by using either the binary or the counting - * semaphore types. - * - * If mutual exlcusion of a resource is desired, a mutex should be used, - * which is a special form of a semaphore and has an own interface. - */ -class SemaphoreIF { -public: - /** - * Different types of timeout for the mutex lock. - */ - enum TimeoutType { - POLLING, //!< If mutex is not available, return immediately - WAITING, //!< Wait a specified time for the mutex to become available - BLOCKING //!< Block indefinitely until the mutex becomes available. - }; - - virtual~ SemaphoreIF() {}; - - static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; - //! Semaphore timeout - static constexpr ReturnValue_t SEMAPHORE_TIMEOUT = MAKE_RETURN_CODE(1); - //! The current semaphore can not be given, because it is not owned - static constexpr ReturnValue_t SEMAPHORE_NOT_OWNED = MAKE_RETURN_CODE(2); - static constexpr ReturnValue_t SEMAPHORE_INVALID = MAKE_RETURN_CODE(3); - - /** - * Generic call to acquire a semaphore. - * If there are no more semaphores to be taken (for a counting semaphore, - * a semaphore may be taken more than once), the taks will block - * for a maximum of timeoutMs while trying to acquire the semaphore. - * This can be used to achieve task synchrnization. - * @param timeoutMs - * @return - c RETURN_OK for successfull acquisition - */ - virtual ReturnValue_t acquire(TimeoutType timeoutType = - TimeoutType::BLOCKING, uint32_t timeoutMs = 0) = 0; - - /** - * Corrensponding call to release a semaphore. - * @return -@c RETURN_OK for successfull release - */ - virtual ReturnValue_t release() = 0; - - /** - * If the semaphore is a counting semaphore then the semaphores current - * count value is returned. If the semaphore is a binary semaphore then 1 - * is returned if the semaphore is available, and 0 is returned if the - * semaphore is not available. - */ - virtual uint8_t getSemaphoreCounter() const = 0; -}; - -#endif /* FRAMEWORK_TASKS_SEMAPHOREIF_H_ */ +#ifndef FRAMEWORK_TASKS_SEMAPHOREIF_H_ +#define FRAMEWORK_TASKS_SEMAPHOREIF_H_ +#include "../returnvalues/FwClassIds.h" +#include "../returnvalues/HasReturnvaluesIF.h" +#include + +/** + * @brief Generic interface for semaphores, which can be used to achieve + * task synchronization. This is a generic interface which can be + * used for both binary semaphores and counting semaphores. + * @details + * A semaphore is a synchronization primitive. + * See: https://en.wikipedia.org/wiki/Semaphore_(programming) + * A semaphore can be used to achieve task synchonization and track the + * availability of resources by using either the binary or the counting + * semaphore types. + * + * If mutual exlcusion of a resource is desired, a mutex should be used, + * which is a special form of a semaphore and has an own interface. + */ +class SemaphoreIF { +public: + /** + * Different types of timeout for the mutex lock. + */ + enum TimeoutType { + POLLING, //!< If mutex is not available, return immediately + WAITING, //!< Wait a specified time for the mutex to become available + BLOCKING //!< Block indefinitely until the mutex becomes available. + }; + + virtual~ SemaphoreIF() {}; + + static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; + //! Semaphore timeout + static constexpr ReturnValue_t SEMAPHORE_TIMEOUT = MAKE_RETURN_CODE(1); + //! The current semaphore can not be given, because it is not owned + static constexpr ReturnValue_t SEMAPHORE_NOT_OWNED = MAKE_RETURN_CODE(2); + static constexpr ReturnValue_t SEMAPHORE_INVALID = MAKE_RETURN_CODE(3); + + /** + * Generic call to acquire a semaphore. + * If there are no more semaphores to be taken (for a counting semaphore, + * a semaphore may be taken more than once), the taks will block + * for a maximum of timeoutMs while trying to acquire the semaphore. + * This can be used to achieve task synchrnization. + * @param timeoutMs + * @return - c RETURN_OK for successfull acquisition + */ + virtual ReturnValue_t acquire(TimeoutType timeoutType = + TimeoutType::BLOCKING, uint32_t timeoutMs = 0) = 0; + + /** + * Corrensponding call to release a semaphore. + * @return -@c RETURN_OK for successfull release + */ + virtual ReturnValue_t release() = 0; + + /** + * If the semaphore is a counting semaphore then the semaphores current + * count value is returned. If the semaphore is a binary semaphore then 1 + * is returned if the semaphore is available, and 0 is returned if the + * semaphore is not available. + */ + virtual uint8_t getSemaphoreCounter() const = 0; +}; + +#endif /* FRAMEWORK_TASKS_SEMAPHOREIF_H_ */