Changeset 884 in ntrip for trunk/BNS/bnseph.cpp
- Timestamp:
- May 8, 2008, 3:36:12 PM (16 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/BNS/bnseph.cpp
r839 r884 85 85 void t_bnseph::readEph() { 86 86 87 gpsEph* ep = new gpsEph; 88 89 bool flagGlonass = false; 90 91 const int NUMLINES = 8; 92 93 for (int ii = 1; ii <= NUMLINES; ii++) { 94 87 88 t_eph* eph = 0; 89 90 QByteArray line = _socket->readLine(); 91 QTextStream in(line); 92 QString prn; 93 94 in >> prn; 95 96 int numlines = 0; 97 if (prn.indexOf('R') != -1) { 98 eph = new t_ephGlo(); 99 numlines = 4; 100 } 101 else { 102 eph = new t_ephGPS(); 103 numlines = 8; 104 } 105 106 QStringList lines; 107 lines << line; 108 109 for (int ii = 2; ii <= numlines; ii++) { 95 110 QByteArray line = _socket->readLine(); 96 97 if (flagGlonass) { 98 if (ii == 4) { 99 delete ep; 100 return; 101 } 102 else { 103 continue; 104 } 105 } 106 107 QTextStream in(line); 111 lines << line; 112 } 113 114 eph->read(lines); 115 116 emit(newEph(eph)); 117 } 118 119 // Read GPS Ephemeris 120 //////////////////////////////////////////////////////////////////////////// 121 void t_ephGPS::read(const QStringList& lines) { 122 123 for (int ii = 1; ii <= lines.size(); ii++) { 124 QTextStream in(lines.at(ii-1).toAscii()); 108 125 109 126 if (ii == 1) { 110 in >> ep->prn;111 112 if (ep->prn.indexOf('R') != -1) {113 flagGlonass = true;114 continue;115 }116 117 127 double year, month, day, hour, minute, second; 118 in >> year >> month >> day >> hour >> minute >> second119 >> ep->clock_bias >> ep->clock_drift >> ep->clock_driftrate;128 in >> _prn >> year >> month >> day >> hour >> minute >> second 129 >> _clock_bias >> _clock_drift >> _clock_driftrate; 120 130 121 131 if (year < 100) year += 2000; … … 124 134 QTime(int(hour), int(minute), int(second)), Qt::UTC); 125 135 int week; 126 GPSweekFromDateAndTime(dateTime, week, ep->TOC);127 ep->GPSweek = week;136 GPSweekFromDateAndTime(dateTime, week, _TOC); 137 _GPSweek = week; 128 138 } 129 139 else if (ii == 2) { 130 in >> ep->IODE >> ep->Crs >> ep->Delta_n >> ep->M0;140 in >> _IODE >> _Crs >> _Delta_n >> _M0; 131 141 } 132 142 else if (ii == 3) { 133 in >> ep->Cuc >> ep->e >> ep->Cus >> ep->sqrt_A;143 in >> _Cuc >> _e >> _Cus >> _sqrt_A; 134 144 } 135 145 else if (ii == 4) { 136 in >> ep->TOE >> ep->Cic >> ep->OMEGA0 >> ep->Cis;146 in >> _TOE >> _Cic >> _OMEGA0 >> _Cis; 137 147 } 138 148 else if (ii == 5) { 139 in >> ep->i0 >> ep->Crc >> ep->omega >> ep->OMEGADOT;149 in >> _i0 >> _Crc >> _omega >> _OMEGADOT; 140 150 } 141 151 else if (ii == 6) { 142 in >> ep->IDOT;152 in >> _IDOT; 143 153 } 144 154 else if (ii == 7) { 145 155 double hlp, health; 146 in >> hlp >> health >> ep->TGD >> ep->IODC;156 in >> hlp >> health >> _TGD >> _IODC; 147 157 } 148 158 else if (ii == 8) { 149 in >> ep->TOW; 150 } 151 } 152 153 emit(newEph(ep)); 154 } 159 in >> _TOW; 160 } 161 } 162 } 163 164 // Compute GPS Satellite Position 165 //////////////////////////////////////////////////////////////////////////// 166 void t_ephGPS::position(int GPSweek, double GPSweeks, ColumnVector& xc, 167 ColumnVector& vv) const { 168 169 const static double secPerWeek = 7 * 86400.0; 170 const static double omegaEarth = 7292115.1467e-11; 171 const static double gmWGS = 398.6005e12; 172 173 if (xc.Nrows() < 4) { 174 xc.ReSize(4); 175 } 176 xc = 0.0; 177 178 if (vv.Nrows() < 3) { 179 vv.ReSize(3); 180 } 181 vv = 0.0; 182 183 double a0 = _sqrt_A * _sqrt_A; 184 if (a0 == 0) { 185 return; 186 } 187 188 double n0 = sqrt(gmWGS/(a0*a0*a0)); 189 double tk = GPSweeks - _TOE; 190 if (GPSweek != _GPSweek) { 191 tk += (GPSweek - _GPSweek) * secPerWeek; 192 } 193 double n = n0 + _Delta_n; 194 double M = _M0 + n*tk; 195 double E = M; 196 double E_last; 197 do { 198 E_last = E; 199 E = M + _e*sin(E); 200 } while ( fabs(E-E_last)*a0 > 0.001 ); 201 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) ); 202 double u0 = v + _omega; 203 double sin2u0 = sin(2*u0); 204 double cos2u0 = cos(2*u0); 205 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0; 206 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0; 207 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0; 208 double xp = r*cos(u); 209 double yp = r*sin(u); 210 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - 211 omegaEarth*_TOE; 212 213 double sinom = sin(OM); 214 double cosom = cos(OM); 215 double sini = sin(i); 216 double cosi = cos(i); 217 xc(1) = xp*cosom - yp*cosi*sinom; 218 xc(2) = xp*sinom + yp*cosi*cosom; 219 xc(3) = yp*sini; 220 221 double tc = GPSweeks - _TOC; 222 if (GPSweek != _GPSweek) { 223 tc += (GPSweek - _GPSweek) * secPerWeek; 224 } 225 xc(4) = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc 226 - 4.442807633e-10 * _e * sqrt(a0) *sin(E); 227 228 // Velocity 229 // -------- 230 double tanv2 = tan(v/2); 231 double dEdM = 1 / (1 - _e*cos(E)); 232 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) 233 * dEdM * n; 234 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv; 235 double dotom = _OMEGADOT - omegaEarth; 236 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv; 237 double dotr = a0 * _e*sin(E) * dEdM * n 238 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv; 239 double dotx = dotr*cos(u) - r*sin(u)*dotu; 240 double doty = dotr*sin(u) + r*cos(u)*dotu; 241 242 vv(1) = cosom *dotx - cosi*sinom *doty // dX / dr 243 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA 244 + yp*sini*sinom*doti; // dX / di 245 246 vv(2) = sinom *dotx + cosi*cosom *doty 247 + xp*cosom*dotom - yp*cosi*sinom*dotom 248 - yp*sini*cosom*doti; 249 250 vv(3) = sini *doty + yp*cosi *doti; 251 } 252 253 // Compare Time 254 //////////////////////////////////////////////////////////////////////////// 255 bool t_ephGPS::isNewerThan(const t_eph* ep) const { 256 257 const t_ephGPS* eph = dynamic_cast<const t_ephGPS*>(ep); 258 if (!eph) { 259 return false; 260 } 261 262 if (_GPSweek > eph->_GPSweek || 263 (_GPSweek == eph->_GPSweek && _TOC > eph->_TOC)) { 264 return true; 265 } 266 else { 267 return false; 268 } 269 } 270 271 // 272 //////////////////////////////////////////////////////////////////////////// 273 void t_ephGlo::read(const QStringList& lines) { 274 275 for (int ii = 1; ii <= lines.size(); ii++) { 276 QTextStream in(lines.at(ii-1).toAscii()); 277 278 if (ii == 1) { 279 double year, month, day, hour, minute, second; 280 in >> _prn >> year >> month >> day >> hour >> minute >> second 281 >> _tau >> _gamma; 282 283 if (year < 100) year += 2000; 284 285 QDateTime dateTime(QDate(int(year), int(month), int(day)), 286 QTime(int(hour), int(minute), int(second)), Qt::UTC); 287 int week; 288 GPSweekFromDateAndTime(dateTime, week, _GPSTOW); 289 _GPSweek = week; 290 } 291 else if (ii == 2) { 292 in >>_x_pos >> _x_velocity >> _x_acceleration >> _flags; 293 } 294 else if (ii == 3) { 295 in >>_y_pos >> _y_velocity >> _y_acceleration >> _frequency_number; 296 } 297 else if (ii == 4) { 298 in >>_z_pos >> _z_velocity >> _z_acceleration >> _E; 299 } 300 } 301 } 302 303 // 304 //////////////////////////////////////////////////////////////////////////// 305 bool t_ephGlo::isNewerThan(const t_eph* ep) const { 306 return false; 307 } 308 309 // 310 //////////////////////////////////////////////////////////////////////////// 311 int t_ephGlo::IOD() const { 312 return 0; 313 } 314 315 // Derivative of the state vector using a simple force model (static) 316 //////////////////////////////////////////////////////////////////////////// 317 ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv) { 318 319 // State vector components 320 // ----------------------- 321 ColumnVector rr = xv.rows(1,3); 322 ColumnVector vv = xv.rows(4,6); 323 324 // Acceleration 325 // ------------ 326 const static double GM = 398.60044e12; 327 const static double AE = 6378136.0; 328 const static double OMEGA = 7292115.e-11; 329 const static double C20 = -1082.63e-6; 330 331 double rho = rr.norm_Frobenius(); 332 double t1 = -GM/(rho*rho*rho); 333 double t2 = 3.0/2.0 * C20 * (GM*AE*AE) / (rho*rho*rho*rho*rho); 334 double t3 = OMEGA * OMEGA; 335 double t4 = 2.0 * OMEGA; 336 double z2 = rr(3) * rr(3); 337 338 // Vector of derivatives 339 // --------------------- 340 ColumnVector va(6); 341 va(1) = vv(1); 342 va(2) = vv(2); 343 va(3) = vv(3); 344 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2); 345 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1); 346 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3); 347 348 return va; 349 } 350 351 // 352 //////////////////////////////////////////////////////////////////////////// 353 void t_ephGlo::position(int GPSweek, double GPSweeks, ColumnVector& xc, 354 ColumnVector& vv) const { 355 356 } 357
Note:
See TracChangeset
for help on using the changeset viewer.