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

Timestamp:

Jul 3, 2026, 11:10:42 AM (26 hours ago)

Author:

stuerze

Message:

Possibility to select how satellite attitude is modelled when converting Antenna Phase Center (APC) corrections to
Center-of-Mass (CoM) positions required for SP3 output

File:

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

trunk/BNC/src/bncantex.cpp

@@ -192 +192 @@
             line.indexOf("NavIC") == 0 ){
           newAntMap->antName = line.mid(20,3);
+          if (line.indexOf("BLOCK I") == 0) {
+            // Extract GPS block type: "BLOCK IIF   " → "IIF"
+            QString bt = line.mid(0, 20).trimmed();
+            if (bt.startsWith("BLOCK "))
+              newAntMap->blockType = bt.mid(6);
+          }
+          else if (line.indexOf("GALILEO") == 0) {
+            // "GALILEO-1" → "1" (IOV), "GALILEO-2" → "2" (FOC)
+            QString bt = line.mid(0, 20).trimmed();
+            if (bt.startsWith("GALILEO-"))
+              newAntMap->blockType = bt.mid(8);
+          }
+          else if (line.indexOf("BEIDOU") == 0) {
+            // "BEIDOU-2M" → "2M", "BEIDOU-3M-CAST" → "3M-CAST", etc.
+            QString bt = line.mid(0, 20).trimmed();
+            if (bt.startsWith("BEIDOU-"))
+              newAntMap->blockType = bt.mid(7);
+          }
         }
         else {
@@ -448 +466 @@
 }
 
+// GPS satellite yaw angle during nominal tracking and noon/midnight turns.
+//
+// GPS satellites perform a "noon turn" (and, for some blocks, a "midnight
+// turn") when the Sun's elevation angle above the orbital plane (beta) is
+// small: the required nominal yaw rate then exceeds the satellite's
+// mechanical maximum, so the satellite yaws at that maximum rate until it
+// catches back up to the nominal Sun-pointing orientation (Kouba 2009/2015,
+// Bar-Sever 1996).
+//
+// The max yaw rates below are the best published estimates per block type:
+//   IIA:   0.12 °/s (Kouba 2009)
+//   IIR:   0.20 °/s (Bar-Sever 1996)
+//   IIR-M: 0.20 °/s
+//   IIF:   0.11 °/s (Kouba 2015)
+//   IIIA:  0.15 °/s (tentative)
+//
+// Returns the effective yaw angle [rad] in the velocity-referenced frame,
+// for use with the same Rodrigues rotation as the GLONASS model. During
+// nominal tracking this equals psiNom and the result is identical to the
+// simple sz×xSun formula.
+////////////////////////////////////////////////////////////////////////////
+double bncAntex::gpsYawAngle(const QString& prn, const QString& blockType,
+                              double Mjd,
+                              const ColumnVector& xSat,
+                              const ColumnVector& vSat,
+                              const ColumnVector& xSun) {
+
+  // Max yaw rate [rad/s] by GPS block type
+  double psiDotMax;
+  if      (blockType == "IIA")   psiDotMax = 0.12 * M_PI / 180.0;
+  else if (blockType == "IIR")   psiDotMax = 0.20 * M_PI / 180.0;
+  else if (blockType == "IIR-M") psiDotMax = 0.20 * M_PI / 180.0;
+  else if (blockType == "IIF")   psiDotMax = 0.11 * M_PI / 180.0;
+  else if (blockType == "IIIA")  psiDotMax = 0.15 * M_PI / 180.0;
+  else return 0.0; // unknown block: caller uses simple Sun-pointing
+
+  const double MAX_CALL_GAP = 1800.0 / 86400.0; // 30 min in days
+
+  // Inertial velocity
+  ColumnVector Omega(3); Omega(1) = 0.0; Omega(2) = 0.0; Omega(3) = t_CST::omega;
+  ColumnVector vInert = vSat + crossproduct(Omega, xSat);
+
+  // Orbital angular momentum vector → orbital rate
+  ColumnVector h     = crossproduct(xSat, vInert);
+  double       hNorm = sqrt(DotProduct(h, h));
+  ColumnVector orbNormal = h / hNorm;
+  double       r    = sqrt(DotProduct(xSat, xSat));
+  double       nRate = hNorm / (r * r); // [rad/s]
+
+  // Beta angle
+  double beta = asin(DotProduct(orbNormal, xSun));
+
+  // Mu: orbit angle from midnight (same geometry as GLONASS)
+  ColumnVector sunProj = xSun - DotProduct(xSun, orbNormal) * orbNormal;
+  sunProj /= sqrt(DotProduct(sunProj, sunProj));
+  ColumnVector eX = -1.0 * sunProj; // midnight direction
+  ColumnVector eY = crossproduct(orbNormal, eX);
+  ColumnVector rHat = xSat / r;
+  double mu = atan2(DotProduct(rHat, eY), DotProduct(rHat, eX));
+
+  // Nominal yaw and its rate
+  double tanBeta = tan(beta);
+  double sinMu   = sin(mu);
+  double psiNom  = atan2(-tanBeta, sinMu);
+  double denom   = tanBeta * tanBeta + sinMu * sinMu;
+  // |dPsi/dt| (always non-negative, sign comes from sign of tanBeta*cos(mu))
+  double psiDotNomAbs = (denom > 1e-12)
+                        ? nRate * fabs(tanBeta * cos(mu)) / denom
+                        : 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;
+
+  t_gpsYaw& st      = _gpsYaw[prn];
+  double    callGap = st.valid ? (Mjd - st.lastCallMjd) : 0.0;
+
+  // Stale state: reset if there has been a gap in calls
+  if (st.valid && callGap > MAX_CALL_GAP) {
+    _gpsYawLog += QString().asprintf(
+      "%s gpsYaw STALE-RESET  Mjd=%.6f beta=%6.3f mu=%7.2f"
+      " old=%7.2f new=%7.2f gap=%.1fmin\n",
+      prn.toLatin1().data(), Mjd, beta*180.0/M_PI, mu*180.0/M_PI,
+      st.yaw*180.0/M_PI, psiNom*180.0/M_PI, callGap*1440.0);
+    st.valid  = false;
+    st.inTurn = false;
+  }
+
+  double psiEff;
+  if (psiDotNomAbs > psiDotMax) {
+    // Constrained: satellite yaws at psiDotMax in the nominal direction
+    if (!st.valid || !st.inTurn) {
+      // Entering a noon/midnight turn
+      psiEff = st.valid ? st.yaw : psiNom;
+      _gpsYawLog += QString().asprintf(
+        "%s gpsYaw TURN-ENTER   Mjd=%.6f beta=%6.3f mu=%7.2f"
+        " psiNom=%7.2f psiEff=%7.2f psiDotNom=%6.3f max=%5.3f\n",
+        prn.toLatin1().data(), Mjd, beta*180.0/M_PI, mu*180.0/M_PI,
+        psiNom*180.0/M_PI, psiEff*180.0/M_PI,
+        psiDotNomAbs*psiDotNomSign*180.0/M_PI, psiDotMax*180.0/M_PI);
+      st.inTurn = true;
+    } else {
+      // Continuing the turn: integrate at constrained rate
+      double dt  = callGap * 86400.0; // [s]
+      psiEff = st.yaw + psiDotNomSign * 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 {
+    // Nominal tracking
+    psiEff = psiNom;
+    if (st.inTurn) {
+      _gpsYawLog += QString().asprintf(
+        "%s gpsYaw TURN-EXIT    Mjd=%.6f beta=%6.3f mu=%7.2f"
+        " frozen=%7.2f nominal=%7.2f\n",
+        prn.toLatin1().data(), Mjd, beta*180.0/M_PI, mu*180.0/M_PI,
+        st.yaw*180.0/M_PI, psiNom*180.0/M_PI);
+      st.inTurn = false;
+    }
+    st.yaw = psiNom;
+  }
+
+  st.yaw         = psiEff;
+  st.lastCallMjd = Mjd;
+  st.valid       = true;
+
+  return psiEff;
+}
+
+//
+// Orbit-Normal Mode Yaw Angle — shared model for Galileo and BDS
+//
+// When |beta| < betaThr the satellite rotates toward yaw = 0 (orbit-normal)
+// at the block-specific maximum yaw rate. When |beta| >= betaThr it returns
+// to nominal yaw-steering (psiNom). Transitions are rate-limited in both
+// directions to match physical satellite behaviour.
+//
+// References:
+//   Galileo IOV/FOC: Kouba (2017), Steigenberger et al. (2018)
+//   BDS MEO/IGSO:    Dai et al. (2015), Wang et al. (2018)
+////////////////////////////////////////////////////////////////////////////
+double bncAntex::onModeYawAngle(const QString& prn,
+                                 double betaThr, double psiDotMax, double Mjd,
+                                 const ColumnVector& xSat,
+                                 const ColumnVector& vSat,
+                                 const ColumnVector& xSun) {
+
+  const double MAX_CALL_GAP = 1800.0 / 86400.0;  // 30 min [days]
+
+  // Inertial velocity
+  ColumnVector Omega(3); Omega(1) = 0.0; Omega(2) = 0.0; Omega(3) = t_CST::omega;
+  ColumnVector vInert = vSat + crossproduct(Omega, xSat);
+
+  // Orbit geometry
+  ColumnVector h        = crossproduct(xSat, vInert);
+  double       hNorm    = sqrt(DotProduct(h, h));
+  ColumnVector orbNormal = h / hNorm;
+  double       r        = sqrt(DotProduct(xSat, xSat));
+
+  double beta = asin(DotProduct(orbNormal, xSun));
+
+  ColumnVector sunProj = xSun - DotProduct(xSun, orbNormal) * orbNormal;
+  sunProj /= sqrt(DotProduct(sunProj, sunProj));
+  ColumnVector eX   = -1.0 * sunProj;
+  ColumnVector eY   = crossproduct(orbNormal, eX);
+  ColumnVector rHat = xSat / r;
+  double mu     = atan2(DotProduct(rHat, eY), DotProduct(rHat, eX));
+  double psiNom = atan2(-tan(beta), sin(mu));
+
+  // Target yaw: orbit-normal (0) when below threshold, nominal otherwise
+  bool   wantsON   = (fabs(beta) < betaThr);
+  double psiTarget = wantsON ? 0.0 : psiNom;
+
+  t_onYaw& st      = _onYaw[prn];
+  double   callGap = st.valid ? (Mjd - st.lastCallMjd) : 0.0;
+
+  if (st.valid && callGap > MAX_CALL_GAP) {
+    _onYawLog += QString().asprintf(
+      "%s onYaw STALE-RESET  Mjd=%.6f beta=%5.2f gap=%.1fmin\n",
+      prn.toLatin1().data(), Mjd, beta*180.0/M_PI, callGap*1440.0);
+    st.valid = false;
+  }
+
+  double psiEff;
+  if (!st.valid) {
+    // Cold start: initialise at psiNom regardless of mode
+    psiEff    = psiNom;
+    st.inON   = false;
+  }
+  else {
+    double dt      = callGap * 86400.0;       // [s]
+    double dpsi    = psiTarget - st.yaw;
+    while (dpsi >  M_PI) dpsi -= 2.0 * M_PI;
+    while (dpsi < -M_PI) dpsi += 2.0 * M_PI;
+    double maxDpsi = psiDotMax * dt;
+
+    if (fabs(dpsi) <= maxDpsi) {
+      // Target reached this step
+      psiEff = psiTarget;
+      bool wasON = st.inON;
+      st.inON    = wantsON;
+      if (!wasON && wantsON && fabs(st.yaw) < 0.5*M_PI/180.0) {
+        _onYawLog += QString().asprintf(
+          "%s onYaw ON-ENTER   Mjd=%.6f beta=%5.2f psiNom=%7.2f\n",
+          prn.toLatin1().data(), Mjd, beta*180.0/M_PI, psiNom*180.0/M_PI);
+      }
+      else if (wasON && !wantsON) {
+        _onYawLog += QString().asprintf(
+          "%s onYaw ON-EXIT    Mjd=%.6f beta=%5.2f psiNom=%7.2f\n",
+          prn.toLatin1().data(), Mjd, beta*180.0/M_PI, psiNom*180.0/M_PI);
+        st.inON = false;
+      }
+    }
+    else {
+      // Still rotating toward target
+      psiEff = st.yaw + (dpsi > 0.0 ? 1.0 : -1.0) * maxDpsi;
+    }
+  }
+
+  while (psiEff >  M_PI) psiEff -= 2.0 * M_PI;
+  while (psiEff < -M_PI) psiEff += 2.0 * M_PI;
+
+  st.yaw         = psiEff;
+  st.lastCallMjd = Mjd;
+  st.valid       = true;
+
+  return psiEff;
+}
+
+//
+////////////////////////////////////////////////////////////////////////////
+double bncAntex::galileoYawAngle(const QString& prn, const QString& blockType,
+                                  double Mjd,
+                                  const ColumnVector& xSat,
+                                  const ColumnVector& vSat,
+                                  const ColumnVector& xSun) {
+  // IOV (type "1") and FOC (type "2"): orbit-normal for |beta| < 2 deg.
+  // Max yaw rate 0.20 deg/s applies to both generations.
+  // Kouba (2017), Steigenberger et al. (2018)
+  Q_UNUSED(blockType);
+  const double betaThr   = 2.0 * M_PI / 180.0;
+  const double psiDotMax = 0.20 * M_PI / 180.0;
+  return onModeYawAngle(prn, betaThr, psiDotMax, Mjd, xSat, vSat, xSun);
+}
+
+//
+////////////////////////////////////////////////////////////////////////////
+double bncAntex::bdsYawAngle(const QString& prn, const QString& blockType,
+                              double Mjd,
+                              const ColumnVector& xSat,
+                              const ColumnVector& vSat,
+                              const ColumnVector& xSun) {
+  // GEO satellites are always in orbit-normal mode (yaw = 0).
+  if (blockType == "2G" || blockType == "3G-CAST")
+    return 0.0;
+
+  double betaThr, psiDotMax;
+  if (blockType == "2M" || blockType == "2I") {
+    // BDS-2 MEO / IGSO: orbit-normal for |beta| < 4 deg (Dai et al. 2015)
+    betaThr   = 4.0 * M_PI / 180.0;
+    psiDotMax = 0.10 * M_PI / 180.0;
+  }
+  else {
+    // BDS-3 (CAST and SECM MEO/IGSO): orbit-normal for |beta| < 3 deg
+    betaThr   = 3.0 * M_PI / 180.0;
+    psiDotMax = 0.15 * M_PI / 180.0;
+  }
+  return onModeYawAngle(prn, betaThr, psiDotMax, Mjd, xSat, vSat, xSun);
+}
+
+//
 // Satellite Antenna Offset
 ////////////////////////////////////////////////////////////////////////////
 t_irc bncAntex::satCoMcorrection(const QString& prn, double Mjd,
                                  const ColumnVector& xSat,
-                                 const ColumnVector& vSat, ColumnVector& dx) {
+                                 const ColumnVector& vSat, ColumnVector& dx,
+                                 e_attMode mode, double externalYaw) {
 
   t_frequency::type frqType = t_frequency::dummy;
@@ -495 +785 @@
       ColumnVector sy, sx;
 
-      // GLONASS: override the nominal Sun-pointing attitude near the
-      // orbit noon/midnight points when the beta angle is small (see
-      // glonassYawAngle() above). Elsewhere GLONASS-M follows the same
-      // nominal law as the other constellations, so the result is
-      // identical to the direct Sun-pointing computation used below.
-      // -----------------------------------------------------------------
-      if (prn[0] == 'R' && vSat.size() == 3) {
-        double psi = glonassYawAngle(prn, Mjd, xSat, vSat, xSun);
-
-        ColumnVector vInert = vSat;
+      // Determine the satellite body frame orientation.
+      //
+      // 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)
+      //   GPS      → noon/midnight turn model (Kouba 2009/2015)
+      //   others   → simple nominal Sun-pointing
+      // ATT_EXTERNAL: caller supplies the yaw angle [rad] directly
+      //   (velocity-referenced frame, same convention as GLONASS/GPS models).
+      // -----------------------------------------------------------------------
+      bool useVelocityFrame = false;
+      double psiEff = 0.0;
+
+      if (mode == ATT_COMPUTED && prn[0] == 'R' && vSat.size() == 3) {
+        psiEff = glonassYawAngle(prn, Mjd, xSat, vSat, xSun);
+        useVelocityFrame = true;
+      }
+      else if (mode == ATT_COMPUTED && prn[0] == 'G' && vSat.size() == 3
+               && !map->blockType.isEmpty()) {
+        psiEff = gpsYawAngle(prn, map->blockType, Mjd, xSat, vSat, xSun);
+        useVelocityFrame = true;
+      }
+      else if (mode == ATT_COMPUTED && prn[0] == 'E' && vSat.size() == 3
+               && !map->blockType.isEmpty()) {
+        psiEff = galileoYawAngle(prn, map->blockType, Mjd, xSat, vSat, xSun);
+        useVelocityFrame = true;
+      }
+      else if (mode == ATT_COMPUTED && prn[0] == 'C' && vSat.size() == 3
+               && !map->blockType.isEmpty()) {
+        psiEff = bdsYawAngle(prn, map->blockType, Mjd, xSat, vSat, xSun);
+        useVelocityFrame = true;
+      }
+      else if (mode == ATT_EXTERNAL && vSat.size() == 3) {
+        psiEff = externalYaw;
+        useVelocityFrame = true;
+      }
+
+      if (useVelocityFrame) {
         ColumnVector Omega(3); Omega(1) = 0.0; Omega(2) = 0.0; Omega(3) = t_CST::omega;
-        vInert += crossproduct(Omega, xSat);
+        ColumnVector vInert = vSat + crossproduct(Omega, xSat);
 
         ColumnVector sy0 = crossproduct(sz, vInert);
@@ -512 +830 @@
         ColumnVector sx0 = crossproduct(sy0, sz);
 
-        // Rodrigues rotation of (sx0, sy0) around sz by angle psi
-        double cosY = cos(psi);
-        double sinY = sin(psi);
+        // Rodrigues rotation of (sx0, sy0) around sz by psiEff
+        double cosY = cos(psiEff);
+        double sinY = sin(psiEff);
         sx = sx0 * cosY + crossproduct(sz, sx0) * sinY;
         sy = sy0 * cosY + crossproduct(sz, sy0) * sinY;
       }
       else {
+        // Nominal Sun-pointing: direct formula (ATT_NOMINAL, or computed
+        // with no block-type-specific model available)
         sy = crossproduct(sz, xSun);
         sy /= sqrt(DotProduct(sy,sy));