INSTINCT Code Coverage Report

Directory: src/
File: Navigation/GNSS/Satellite/Ephemeris/GLONASSEphemeris.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 62 65 95.4%
Functions: 6 7 85.7%
Branches: 49 88 55.7%
Line Branch Exec Source
1 // This file is part of INSTINCT, the INS Toolkit for Integrated
2 // Navigation Concepts and Training by the Institute of Navigation of
3 // the University of Stuttgart, Germany.
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9 #include "GLONASSEphemeris.hpp"
10 #include <cstdint>
11
12 #include "Navigation/Constants.hpp"
13 #include "Navigation/Math/NumericalIntegration.hpp"
14 #include "Navigation/Transformations/CoordinateFrames.hpp"
15
16 #include "util/Logger.hpp"
17
18 namespace NAV
19 {
20
21 3628 GLONASSEphemeris::GLONASSEphemeris(const InsTime& toc, double tau_c,
22 double tau_n, double gamma_n, bool health,
23 Eigen::Vector3d pos, Eigen::Vector3d vel, Eigen::Vector3d accelLuniSolar,
24 3628 int8_t frequencyNumber)
25 : SatNavData(SatNavData::GLONASSEphemeris, toc),
26 3628 tau_c(tau_c),
27 3628 toc(toc),
28 3628 tau_n(tau_n),
29 3628 gamma_n(gamma_n),
30 3628 health(health),
31 3628 PZ90_pos(std::move(pos)),
32 3628 PZ90_vel(std::move(vel)),
33 3628 PZ90_accelLuniSolar(std::move(accelLuniSolar)),
34 3628 frequencyNumber(frequencyNumber) {}
35
36 #ifdef TESTING
37
38 11033 GLONASSEphemeris::GLONASSEphemeris(int32_t year, int32_t month, int32_t day, int32_t hour, int32_t minute, double second,
39 double m_tau_n, double gamma_n, double /* messageFrameTime */,
40 double satPos_x, double satVel_x, double satAccel_x, double health,
41 double satPos_y, double satVel_y, double satAccel_y, double frequencyNumber,
42 double satPos_z, double satVel_z, double satAccel_z, double /* ageOfOperationInfo */,
43 double /* statusFlags */, double /* L1L2groupDelayDifference */, double /* URAI */, double /* healthFlags */,
44 11033 double tau_c)
45
1/2
✓ Branch 1 taken 11033 times.
✗ Branch 2 not taken.
 11033 : SatNavData(SatNavData::GLONASSEphemeris, InsTime(year, month, day, hour, minute, second, UTC)),
46 11033 tau_c(tau_c),
47 11033 toc(refTime),
48 11033 tau_n(-m_tau_n),
49 11033 gamma_n(gamma_n),
50 11033 health(static_cast<bool>(health)),
51 ✗ PZ90_pos(satPos_x * 1e3, satPos_y * 1e3, satPos_z * 1e3),
52
1/2
✓ Branch 1 taken 11033 times.
✗ Branch 2 not taken.
 11033 PZ90_vel(satVel_x * 1e3, satVel_y * 1e3, satVel_z * 1e3),
53
1/2
✓ Branch 1 taken 11033 times.
✗ Branch 2 not taken.
 11033 PZ90_accelLuniSolar(satAccel_x * 1e3, satAccel_y * 1e3, satAccel_z * 1e3),
54
2/4
✓ Branch 2 taken 11033 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11033 times.
✗ Branch 6 not taken.
 22066 frequencyNumber(static_cast<int8_t>(frequencyNumber))
55 11033 {}
56
57 #endif
58
59 962 Clock::Corrections GLONASSEphemeris::calcClockCorrections(const InsTime& recvTime, double dist, const Frequency& /* freq */) const
60 {
61 LOG_DATA("Calc Sat Clock corrections at receiver time {}", recvTime.toGPSweekTow());
62 LOG_DATA(" toc {} (Time of clock)", toc.toGPSweekTow());
63
64 // Time at transmission
65
2/4
✓ Branch 2 taken 962 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 962 times.
✗ Branch 6 not taken.
 962 InsTime transTime = recvTime - std::chrono::duration<double>(dist / InsConst::C);
66
67 // SV clock time offset [s]
68
1/2
✓ Branch 1 taken 962 times.
✗ Branch 2 not taken.
 962 double dt_sv = -tau_n + gamma_n * static_cast<double>((transTime - toc).count());
69 LOG_DATA(" dt_sv {} [s] (SV clock time offset)", dt_sv);
70
71
2/4
✓ Branch 2 taken 962 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 962 times.
✗ Branch 6 not taken.
 962 transTime += std::chrono::duration<double>(dt_sv);
72 LOG_DATA(" transTime {} (Time at transmission)", transTime.toGPSweekTow());
73
74 962 return { .transmitTime = transTime, .bias = dt_sv, .drift = -gamma_n };
75 }
76
77 2290 Orbit::PosVelAccel GLONASSEphemeris::calcSatelliteData(const InsTime& transTime, Orbit::Calc calc) const
78 {
79 LOG_DATA("Calc Sat Position at transmit time {}", transTime.toGPSweekTow());
80 LOG_DATA(" toc {} (Time of clock)", toc.toGPSweekTow());
81
82 // Calculates the position and velocity derivative for the satellite position
83 // y State [x, y, z, v_x, v_y, v_z]^T
84 // c Constant values needed to calculate the derivatives
85 // Returns the derivative ∂/∂t [x, y, z, v_x, v_y, v_z]^T
86 75183 auto calcPosVelDerivative = [](const Eigen::Matrix<double, 6, 1>& y, int /* z */, const Eigen::Vector3d& accelLuniSolar, double /* t */ = 0.0) {
87 // 0 1 2 3 4 5
88 // ∂/∂t [x, y, z, v_x, v_y, v_z]^T
89
2/4
✓ Branch 1 taken 75183 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 75183 times.
✗ Branch 5 not taken.
 75183 Eigen::Matrix<double, 6, 1> y_dot = Eigen::Matrix<double, 6, 1>::Zero();
90
91 enum State : uint8_t
92 {
93 X,
94 Y,
95 Z,
96 VX,
97 VY,
98 VZ
99 };
100
101
2/4
✓ Branch 1 taken 75183 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 75183 times.
✗ Branch 5 not taken.
 75183 double r = y.topRows<3>().norm();
102
103 75183 double omega_ie2 = std::pow(InsConst::GLO::omega_ie, 2);
104
105 75183 double a = 1.5 * InsConst::GLO::J2 * InsConst::GLO::MU * std::pow(InsConst::GLO::a, 2) / std::pow(r, 5);
106 75183 double c = -InsConst::GLO::MU / std::pow(r, 3) - a * (1. - 5. * std::pow(y(Z), 2) / std::pow(r, 2));
107
108
3/6
✓ Branch 1 taken 75183 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 75183 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 75183 times.
✗ Branch 8 not taken.
 75183 y_dot.topRows<3>() = y.bottomRows<3>();
109 75183 y_dot(3) = (c + omega_ie2) * y(X) + 2 * InsConst::GLO::omega_ie * y(VY) + accelLuniSolar.x();
110 75183 y_dot(4) = (c + omega_ie2) * y(Y) - 2 * InsConst::GLO::omega_ie * y(VX) + accelLuniSolar.y();
111 75183 y_dot(5) = (c - 2. * a) * y(Z) + accelLuniSolar.z();
112
113 75183 return y_dot;
114 };
115
116 // State [x, y, z, v_x, v_y, v_z]^T
117
1/2
✓ Branch 1 taken 2290 times.
✗ Branch 2 not taken.
 2290 Eigen::Matrix<double, 6, 1> y;
118
2/4
✓ Branch 1 taken 2290 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2290 times.
✗ Branch 5 not taken.
 2290 y << PZ90_pos, PZ90_vel;
119
120 // Time to integrate in [s]
121
1/2
✓ Branch 1 taken 2290 times.
✗ Branch 2 not taken.
 2290 double dt = static_cast<double>((transTime - toc).count());
122 LOG_DATA(" dt {} [s] (Time to integrate)", dt);
123
124
2/2
✓ Branch 1 taken 18630 times.
✓ Branch 2 taken 2290 times.
 20920 while (std::abs(dt) > 1e-9)
125 {
126
2/2
✓ Branch 0 taken 11805 times.
✓ Branch 1 taken 6825 times.
 18630 double step = dt > 0 ? _h : -_h;
127
2/2
✓ Branch 1 taken 2288 times.
✓ Branch 2 taken 16342 times.
 18630 if (std::abs(dt) < _h)
128 {
129 2288 step = dt;
130 }
131 LOG_DATA(" step {:0.2f}, pos {}, vel {}", step, y.topRows<3>().transpose(), y.bottomRows<3>().transpose());
132 ;
133
1/2
✓ Branch 1 taken 18630 times.
✗ Branch 2 not taken.
 18630 y = RungeKutta4(y, std::array<int, 4>{}, step, calcPosVelDerivative, PZ90_accelLuniSolar);
134 18630 dt -= step;
135 }
136 LOG_DATA(" pos {}, vel {} (end state)", y.topRows<3>().transpose(), y.bottomRows<3>().transpose());
137
138
2/4
✓ Branch 1 taken 2290 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2290 times.
✗ Branch 5 not taken.
 2290 Eigen::Vector3d e_pos = Eigen::Vector3d::Zero();
139
2/4
✓ Branch 1 taken 2290 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2290 times.
✗ Branch 5 not taken.
 2290 Eigen::Vector3d e_vel = Eigen::Vector3d::Zero();
140
2/4
✓ Branch 1 taken 2290 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2290 times.
✗ Branch 5 not taken.
 2290 Eigen::Vector3d e_accel = Eigen::Vector3d::Zero();
141
142
1/2
✓ Branch 1 taken 2290 times.
✗ Branch 2 not taken.
 2290 if (calc & Calc_Position)
143 {
144
2/4
✓ Branch 1 taken 2290 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2290 times.
✗ Branch 5 not taken.
 2290 e_pos = y.topRows<3>(); // trafo::pz90toWGS84_pos(y.topRows<3>());
145 // LOG_DATA(" pos (WGS84) {}", e_pos.transpose());
146 }
147
2/2
✓ Branch 1 taken 1328 times.
✓ Branch 2 taken 962 times.
 2290 if (calc & Calc_Velocity)
148 {
149
2/4
✓ Branch 1 taken 1328 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1328 times.
✗ Branch 5 not taken.
 1328 e_vel = y.bottomRows<3>(); // trafo::pz90toWGS84(y.bottomRows<3>(), y.topRows<3>());
150 // LOG_DATA(" vel (WGS84) {}", e_vel.transpose());
151 }
152
2/2
✓ Branch 1 taken 663 times.
✓ Branch 2 taken 1627 times.
 2290 if (calc & Calc_Acceleration)
153 {
154
1/2
✓ Branch 1 taken 663 times.
✗ Branch 2 not taken.
 663 Eigen::Matrix<double, 6, 1> y_dot = calcPosVelDerivative(y, 0, PZ90_accelLuniSolar, 0.0);
155
2/4
✓ Branch 1 taken 663 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 663 times.
✗ Branch 5 not taken.
 663 e_accel = y_dot.bottomRows<3>(); // trafo::pz90toWGS84(y_dot.bottomRows<3>(), y.topRows<3>());
156 // LOG_DATA(" accel (WGS84) {}", e_accel.transpose());
157 }
158
159 return { .e_pos = e_pos,
160 .e_vel = e_vel,
161
3/6
✓ Branch 1 taken 2290 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2290 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2290 times.
✗ Branch 8 not taken.
 4580 .e_accel = e_accel };
162 }
163
164 972 bool GLONASSEphemeris::isHealthy() const
165 {
166 972 return !health;
167 }
168
169 ✗ double GLONASSEphemeris::calcSatellitePositionVariance() const
170 {
171 ✗ return 5 * 5;
172 }
173
174 } // namespace NAV
175