Index: trunk/BNC/src/bncantex.cpp =================================================================== --- trunk/BNC/src/bncantex.cpp (revision 11104) +++ trunk/BNC/src/bncantex.cpp (revision 11110) @@ -385,21 +385,14 @@ } -// GLONASS Yaw Angle (Sun-pointing law overridden near noon/midnight when -// the satellite cannot mechanically keep up, following the GLONASS-M -// "yaw-fixed" behaviour described in Dilssner, Springer, Flohrer, Dow -// (2011), "The GLONASS-M satellite yaw-attitude model", Advances in Space -// Research 47(1), 160-171. -// -// Unlike GPS/Galileo/BeiDou, which are commonly approximated by the -// nominal Sun-pointing yaw-steering law of Bar-Sever (1996) at all times, -// GLONASS-M has been found to stop tracking that law and hold a constant -// (frozen) yaw angle whenever the required yaw rate would exceed the -// satellite's slew capability - which happens close to the orbit -// noon/midnight points whenever the Sun's elevation above the orbital -// plane (the "beta" angle) is small. The maximum yaw rate used below -// (0.25 deg/s) and the general approach follow that paper; satellite -// telemetry was not available to validate the exact rate against this -// installation's GLONASS satellites, so it should be checked against -// independently-known attitude/orbit residuals if high accuracy matters. +// GLONASS Yaw Angle following the GLONASS-M yaw-attitude model of Dilssner, +// Springer, Gienger, Dow (2011), "The GLONASS-M satellite yaw-attitude +// model", Advances in Space Research 47(1), 160-171: +// - noon turn: when the nominal yaw rate exceeds the maximum hardware rate +// (0.25 deg/s), the satellite yaws at that rate in the direction of the +// nominal yaw rate until it has caught up with the nominal yaw; +// - midnight turn: in the Earth's shadow the satellite yaws at the maximum +// rate until it reaches the nominal yaw at shadow exit and holds it until +// leaving the shadow. +// The maximum rate is applied to all GLONASS satellites (K/K2 like M). //////////////////////////////////////////////////////////////////////////// double bncAntex::glonassYawAngle(const QString& prn, double Mjd, @@ -411,10 +404,9 @@ // This bounds the gap BETWEEN CONSECUTIVE CALLS for a given satellite - // (not how long the freeze itself has lasted - a real low-beta passage - // can legitimately stay frozen far longer than this). If there was no + // (not how long a turn lasts). If there was no // call for longer than this, something else interrupted normal epoch-by- // epoch processing (a stream outage, a maneuver-flagged-unhealthy - // period, ...), so the stored frozen value is more likely stale than a - // still-valid freeze, and we fall back to the current nominal value + // period, ...), so the stored yaw is more likely stale than a + // still-valid turn, and we fall back to the current nominal value // instead of trusting it indefinitely. const double MAX_CALL_GAP = 1800.0 / 86400.0; // 30 minutes, in days @@ -460,41 +452,90 @@ : 1e9; + // Sign of the nominal yaw rate: dPsi/dt = nRate * tanBeta * cos(mu) / denom + double psiDotNomSign = (tanBeta * cos(mu) >= 0.0) ? 1.0 : -1.0; + + // Earth's shadow (cylinder): around midnight for |mu| < muShadow + // ---------------------------------------------------------------- + const double RE = 6378137.0; + double cosMuShadow = sqrt(std::max(0.0, 1.0 - RE * RE / (r * r))) / cos(beta); + bool inShadow = (cosMuShadow < 1.0 && cos(mu) > cosMuShadow); + double muExit = (cosMuShadow < 1.0) ? acos(cosMuShadow) : 0.0; + double psiExit = atan2(-tanBeta, sin(muExit)); // nominal yaw at shadow exit + t_glonassYaw& st = _glonassYaw[prn]; double callGap = st.valid ? (Mjd - st.lastCallMjd) : 0.0; + if (st.valid && callGap > MAX_CALL_GAP) { + _glonassYawLog += QString().asprintf( + "%s glonassYaw STALE-RESET Mjd=%.6f beta=%6.3f mu=%7.2f rate=%6.3f" + " old=%7.2f new=%7.2f gap=%.1fmin\n", + prn.toLatin1().data(), Mjd, beta*180.0/M_PI, mu*180.0/M_PI, + psiRate*180.0/M_PI, st.yaw*180.0/M_PI, psiNom*180.0/M_PI, + callGap*1440.0); + st.valid = false; + st.fixed = false; + } + double step = MAX_YAW_RATE * callGap * 86400.0; // max. yaw change since the last call double psiEff; - if (psiRate > MAX_YAW_RATE) { - if (!st.valid || callGap > MAX_CALL_GAP) { - if (st.valid) { // i.e. it was a genuine gap, not cold-start - _glonassYawLog += QString().asprintf( - "%s glonassYaw STALE-RESET Mjd=%.6f beta=%6.3f mu=%7.2f rate=%6.3f" - " old=%7.2f new=%7.2f gap=%.1fmin\n", - prn.toLatin1().data(), Mjd, beta*180.0/M_PI, mu*180.0/M_PI, - psiRate*180.0/M_PI, st.yaw*180.0/M_PI, psiNom*180.0/M_PI, - callGap*1440.0); - } - st.yaw = psiNom; // no prior history, or the gap is too long to trust it - } + + if (inShadow) { + // Midnight turn: in the shadow the satellite yaws at the maximum rate + // until it reaches the nominal yaw at shadow exit and holds it there if (!st.fixed) { + st.yaw = st.valid ? st.yaw : psiNom; + st.turnSign = psiDotNomSign; + st.fixed = true; _glonassYawLog += QString().asprintf( - "%s glonassYaw FIXED-ENTER Mjd=%.6f beta=%6.3f mu=%7.2f rate=%6.3f yaw=%7.2f\n", + "%s glonassYaw SHADOW-ENTER Mjd=%.6f beta=%6.3f mu=%7.2f yaw=%7.2f exit=%7.2f\n", + prn.toLatin1().data(), Mjd, beta*180.0/M_PI, mu*180.0/M_PI, + st.yaw*180.0/M_PI, psiExit*180.0/M_PI); + psiEff = st.yaw; + } + else if (remainder(st.turnSign * (psiExit - st.yaw), 2.0 * M_PI) > step) { + psiEff = st.yaw + st.turnSign * step; + } + else { + psiEff = psiExit; + } + } + else if (psiRate > MAX_YAW_RATE) { + // Noon turn (or shadow-free midnight turn): the satellite yaws at the + // maximum rate in the direction of the nominal yaw rate + if (!st.valid || !st.fixed) { + st.yaw = st.valid ? st.yaw : psiNom; + st.turnSign = psiDotNomSign; + st.fixed = true; + _glonassYawLog += QString().asprintf( + "%s glonassYaw TURN-ENTER Mjd=%.6f beta=%6.3f mu=%7.2f rate=%6.3f yaw=%7.2f\n", prn.toLatin1().data(), Mjd, beta*180.0/M_PI, mu*180.0/M_PI, psiRate*180.0/M_PI, st.yaw*180.0/M_PI); - st.fixed = true; - } - psiEff = st.yaw; // hold the frozen yaw angle + psiEff = st.yaw; + } + else { + psiEff = st.yaw + st.turnSign * step; + } + } + else if (st.valid && st.fixed && + remainder(st.turnSign * (psiNom - st.yaw), 2.0 * M_PI) > step) { + // after the turn or shadow exit, the satellite still lags behind and + // keeps yawing at the maximum rate until it has caught up + psiEff = st.yaw + st.turnSign * step; } else { + // Nominal tracking if (st.fixed) { _glonassYawLog += QString().asprintf( - "%s glonassYaw FIXED-EXIT Mjd=%.6f beta=%6.3f mu=%7.2f rate=%6.3f" - " frozen=%7.2f nominal=%7.2f\n", + "%s glonassYaw TURN-EXIT Mjd=%.6f beta=%6.3f mu=%7.2f rate=%6.3f" + " yaw=%7.2f nominal=%7.2f\n", prn.toLatin1().data(), Mjd, beta*180.0/M_PI, mu*180.0/M_PI, psiRate*180.0/M_PI, st.yaw*180.0/M_PI, psiNom*180.0/M_PI); st.fixed = false; } - st.yaw = psiNom; psiEff = psiNom; } - st.lastCallMjd = Mjd; // update on every call, fixed or not, to detect gaps + while (psiEff > M_PI) psiEff -= 2.0 * M_PI; + while (psiEff < -M_PI) psiEff += 2.0 * M_PI; + + st.yaw = psiEff; + st.lastCallMjd = Mjd; // update on every call to detect gaps st.valid = true; @@ -607,11 +648,21 @@ psiNom*180.0/M_PI, psiEff*180.0/M_PI, psiDotNomAbs*psiDotNomSign*180.0/M_PI, psiDotMax*180.0/M_PI); - st.inTurn = true; + st.inTurn = true; + st.turnSign = psiDotNomSign; } else { // Continuing the turn: integrate at constrained rate double dt = callGap * 86400.0; // [s] - psiEff = st.yaw + psiDotNomSign * psiDotMax * dt; + psiEff = st.yaw + st.turnSign * psiDotMax * dt; } // Wrap to [-pi, pi] + while (psiEff > M_PI) psiEff -= 2.0 * M_PI; + while (psiEff < -M_PI) psiEff += 2.0 * M_PI; + } + else if (st.valid && st.inTurn && + remainder(st.turnSign * (psiNom - st.yaw), 2.0 * M_PI) > psiDotMax * callGap * 86400.0) { + // End of the turn: the nominal yaw rate is below the maximum again, but + // the satellite still lags behind and keeps yawing at the maximum rate + // until it has caught up with the nominal yaw (Kouba 2009) + psiEff = st.yaw + st.turnSign * psiDotMax * callGap * 86400.0; while (psiEff > M_PI) psiEff -= 2.0 * M_PI; while (psiEff < -M_PI) psiEff += 2.0 * M_PI; @@ -628,5 +679,4 @@ st.inTurn = false; } - st.yaw = psiNom; } @@ -837,5 +887,5 @@ // ATT_NOMINAL: simple nominal Sun-pointing for all systems. // ATT_COMPUTED or ATT_EXTERNAL: use per-system attitude models. - // GLONASS → yaw-fixed model (Dilssner et al. 2011) + // GLONASS → noon/shadow turn model (Dilssner et al. 2011) // GPS → noon/midnight turn model (Kouba 2009/2015) // others → simple nominal Sun-pointing