Changeset 11110 in ntrip for trunk/BNC/src/bncantex.cpp

Timestamp:

Oct 22, 2026, 11:10:34 AM (32 hours ago)

Author:

stuerze

Message:

clock combination: some updates regarding attitude correction

File:

• trunk/BNC/src/bncantex.cpp (modified) (6 diffs)

trunk/BNC/src/bncantex.cpp

@@ -385 +385 @@
 }
 
-// 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 +404 @@
 
   // 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 +452 @@
                   : 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 +648 @@
         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 +679 @@
       st.inTurn = false;
     }
-    st.yaw = psiNom;
   }
 
@@ -837 +887 @@
       // 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