Index: trunk/BNC/src/bncantex.cpp =================================================================== --- trunk/BNC/src/bncantex.cpp (revision 10951) +++ trunk/BNC/src/bncantex.cpp (revision 10955) @@ -192,4 +192,22 @@ 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,9 +466,281 @@ } +// 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,16 +785,44 @@ 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,11 +830,13 @@ 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));