Index: trunk/BNC/src/RTCM3/RTCM3Decoder.cpp =================================================================== --- trunk/BNC/src/RTCM3/RTCM3Decoder.cpp (revision 4284) +++ trunk/BNC/src/RTCM3/RTCM3Decoder.cpp (revision 4284) @@ -0,0 +1,444 @@ +// Part of BNC, a utility for retrieving decoding and +// converting GNSS data streams from NTRIP broadcasters. +// +// Copyright (C) 2007 +// German Federal Agency for Cartography and Geodesy (BKG) +// http://www.bkg.bund.de +// Czech Technical University Prague, Department of Geodesy +// http://www.fsv.cvut.cz +// +// Email: euref-ip@bkg.bund.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation, version 2. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + +/* ------------------------------------------------------------------------- + * BKG NTRIP Client + * ------------------------------------------------------------------------- + * + * Class: RTCM3Decoder + * + * Purpose: RTCM3 Decoder + * + * Author: L. Mervart + * + * Created: 24-Aug-2006 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include + +#include "RTCM3Decoder.h" +#include "../RTCM/rtcm_utils.h" +#include "bncconst.h" +#include "bncapp.h" +#include "bncutils.h" +#include "bncsettings.h" + +using namespace std; + +#ifndef isinf +# define isinf(x) 0 +#endif + +// Error Handling +//////////////////////////////////////////////////////////////////////////// +void RTCM3Error(const char*, ...) { +} + +// Constructor +//////////////////////////////////////////////////////////////////////////// +RTCM3Decoder::RTCM3Decoder(const QString& staID, bncRawFile* rawFile) : + GPSDecoder() { + + _staID = staID; + _rawFile = rawFile; + + bncSettings settings; + _checkMountPoint = settings.value("miscMount").toString(); + + connect(this, SIGNAL(newGPSEph(gpsephemeris*)), + (bncApp*) qApp, SLOT(slotNewGPSEph(gpsephemeris*))); + connect(this, SIGNAL(newGlonassEph(glonassephemeris*)), + (bncApp*) qApp, SLOT(slotNewGlonassEph(glonassephemeris*))); + connect(this, SIGNAL(newGalileoEph(galileoephemeris*)), + (bncApp*) qApp, SLOT(slotNewGalileoEph(galileoephemeris*))); + + // Mode can be either observations or corrections + // ---------------------------------------------- + _mode = unknown; + + // Antenna position (used for decoding of message 1003) + // ---------------------------------------------------- + _antXYZ[0] = _antXYZ[1] = _antXYZ[2] = 0; + +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +RTCM3Decoder::~RTCM3Decoder() { + QMapIterator it(_coDecoders); + while (it.hasNext()) { + it.next(); + delete it.value(); + } +} + +// +//////////////////////////////////////////////////////////////////////////// +t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector& errmsg) { + + errmsg.clear(); + + bool decoded = false; + + // If read from file, mode is always uknown + // ---------------------------------------- + if (_rawFile) { + _mode = unknown; + _staID = _rawFile->staID(); + } + + // Try to decode Clock and Orbit Corrections + // ----------------------------------------- + if (_mode == unknown || _mode == corrections) { + + // Find the corresponding coDecoder + // -------------------------------- + if (!_coDecoders.contains(_staID.toAscii())) { + _coDecoders[_staID.toAscii()] = new RTCM3coDecoder(_staID); + } + RTCM3coDecoder* coDecoder = _coDecoders[_staID.toAscii()]; + + if ( coDecoder->Decode(buffer, bufLen, errmsg) == success ) { + decoded = true; + if (!_rawFile && _mode == unknown) { + _mode = corrections; + } + } + } + + // Find the corresponding parser, initialize a new parser if necessary + // ------------------------------------------------------------------- + bool newParser = !_parsers.contains(_staID.toAscii()); + RTCM3ParserData& parser = _parsers[_staID.toAscii()]; + if (newParser) { + memset(&parser, 0, sizeof(parser)); + parser.rinex3 = 0; + double secGPS; + currentGPSWeeks(parser.GPSWeek, secGPS); + parser.GPSTOW = int(secGPS); + } + + // Get Glonass Slot Numbers from Global Array + // ------------------------------------------ + bncApp* app = (bncApp*) qApp; + app->getGlonassSlotNums(parser.GLOFreq); + + // Remaining part decodes the Observations + // --------------------------------------- + if (_mode == unknown || _mode == observations || + _checkMountPoint == _staID || _checkMountPoint == "ALL") { + + for (int iByte = 0; iByte < bufLen; iByte++) { + + parser.Message[parser.MessageSize++] = buffer[iByte]; + + if (parser.MessageSize >= parser.NeedBytes) { + + while (int rr = RTCM3Parser(&parser)) { + + // RTCMv3 message types + // -------------------- + _typeList.push_back(parser.blocktype); + + // RTCMv3 antenna descriptor + // ------------------------- + if (rr == 1007 || rr == 1008 || rr == 1033) { + _antType.push_back(parser.antenna); + } + + // RTCMv3 antenna XYZ + // ------------------ + else if (rr == 1005) { + _antList.push_back(t_antInfo()); + _antList.back().type = t_antInfo::ARP; + _antList.back().xx = parser.antX * 1e-4; + _antList.back().yy = parser.antY * 1e-4; + _antList.back().zz = parser.antZ * 1e-4; + _antList.back().message = rr; + + // Remember station position for 1003 message decoding + _antXYZ[0] = parser.antX * 1e-4; + _antXYZ[1] = parser.antY * 1e-4; + _antXYZ[2] = parser.antZ * 1e-4; + } + + // RTCMv3 antenna XYZ-H + // -------------------- + else if(rr == 1006) { + _antList.push_back(t_antInfo()); + _antList.back().type = t_antInfo::ARP; + _antList.back().xx = parser.antX * 1e-4; + _antList.back().yy = parser.antY * 1e-4; + _antList.back().zz = parser.antZ * 1e-4; + _antList.back().height = parser.antH * 1e-4; + _antList.back().height_f = true; + _antList.back().message = rr; + + // Remember station position for 1003 message decoding + _antXYZ[0] = parser.antX * 1e-4; + _antXYZ[1] = parser.antY * 1e-4; + _antXYZ[2] = parser.antZ * 1e-4; + } + + // GNSS Observations + // ----------------- + else if (rr == 1 || rr == 2) { + decoded = true; + + if (!parser.init) { + HandleHeader(&parser); + parser.init = 1; + } + + if (rr == 2) { + emit(newMessage( (_staID + + ": No valid RINEX! All values are modulo 299792.458!").toAscii(), + true)); + } + + gnssdata& gnssData = parser.Data; + + for (int iSat = 0; iSat < gnssData.numsats; iSat++) { + + t_obs obs; + int satID = gnssData.satellites[iSat]; + + // GPS + // --- + if (satID >= PRN_GPS_START && satID <= PRN_GPS_END) { + obs.satSys = 'G'; + obs.satNum = satID; + } + + // Glonass + // ------- + else if (satID >= PRN_GLONASS_START && satID <= PRN_GLONASS_END) { + obs.satSys = 'R'; + obs.satNum = satID - PRN_GLONASS_START + 1; + if (obs.satNum <= PRN_GLONASS_NUM && + parser.GLOFreq[obs.satNum-1] != 0) { + obs.slotNum = parser.GLOFreq[obs.satNum-1] - 100; + } + else { + continue; + } + } + + // Galileo + // ------- + else if (satID >= PRN_GALILEO_START && satID <= PRN_GALILEO_END) { + obs.satSys = 'E'; + obs.satNum = satID - PRN_GALILEO_START + 1; + } + + // WAAS + // ---- + else if (satID >= PRN_WAAS_START && satID <= PRN_WAAS_END) { + obs.satSys = 'S'; + obs.satNum = satID - PRN_WAAS_START + 20; + } + + // Giove A and B + // ------------- + else if (satID >= PRN_GIOVE_START && satID <= PRN_GIOVE_END) { + obs.satSys = 'E'; + obs.satNum = satID - PRN_GIOVE_START + PRN_GIOVE_OFFSET; + } + + // Unknown System + // -------------- + else { + continue; + } + + obs.GPSWeek = gnssData.week; + obs.GPSWeeks = gnssData.timeofweek / 1000.0; + + QString prn = QString("%1%2").arg(obs.satSys) + .arg(obs.satNum, 2, 10, QChar('0')); + + // Handle loss-of-lock flags + // ------------------------- + const int maxSlipCnt = 100; + if (!_slip_cnt_L1.contains(prn)) { + _slip_cnt_L1[prn] = 0; + _slip_cnt_L2[prn] = 0; + _slip_cnt_L5[prn] = 0; + } + if (GNSSDF2_LOCKLOSSL1 & gnssData.dataflags2[iSat]) { + if (_slip_cnt_L1[prn] < maxSlipCnt) { + ++_slip_cnt_L1[prn]; + } + else { + _slip_cnt_L1[prn] = 1; + } + obs.slip_cnt_L1 = _slip_cnt_L1[prn]; + } + if (GNSSDF2_LOCKLOSSL2 & gnssData.dataflags2[iSat]) { + if (_slip_cnt_L2[prn] < maxSlipCnt) { + ++_slip_cnt_L2[prn]; + } + else { + _slip_cnt_L2[prn] = 1; + } + obs.slip_cnt_L2 = _slip_cnt_L2[prn]; + } + if (GNSSDF2_LOCKLOSSL5 & gnssData.dataflags2[iSat]) { + if (_slip_cnt_L5[prn] < maxSlipCnt) { + ++_slip_cnt_L5[prn]; + } + else { + _slip_cnt_L5[prn] = 1; + } + obs.slip_cnt_L5 = _slip_cnt_L5[prn]; + } + + // Loop over all data types + // ------------------------ + for (int iEntry = 0; iEntry < GNSSENTRY_NUMBER; ++iEntry) { + + unsigned df = (1 << iEntry); + + if (df & gnssData.dataflags[iSat]) { + + if (iEntry == GNSSENTRY_C1DATA) { + obs.C1 = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_C2DATA) { + obs.C2 = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_P1DATA) { + obs.P1 = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_P2DATA) { + obs.P2 = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_L1CDATA) { + obs.L1C = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_L1PDATA) { + obs.L1P = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_L2CDATA) { + obs.L2C = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_L2PDATA) { + obs.L2P = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_D1CDATA) { + obs.D1C = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_D1PDATA) { + obs.D1P = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_S1CDATA) { + obs.S1C = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_S1PDATA) { + obs.S1P = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_D2CDATA) { + obs.D2C = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_D2PDATA) { + obs.D2P = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_S2CDATA) { + obs.S2C = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_S2PDATA) { + obs.S2P = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_C5DATA) { + obs.C5 = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_L5DATA) { + obs.L5 = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_D5DATA) { + obs.D5 = gnssData.measdata[iSat][iEntry]; + } + else if (iEntry == GNSSENTRY_S5DATA) { + obs.S5 = gnssData.measdata[iSat][iEntry]; + } + } + } + _obsList.push_back(obs); + } + } + + // GPS Ephemeris + // ------------- + else if (rr == 1019) { + decoded = true; + emit newGPSEph(new gpsephemeris(parser.ephemerisGPS)); + } + + // GLONASS Ephemeris + // ----------------- + else if (rr == 1020) { + decoded = true; + emit newGlonassEph(new glonassephemeris(parser.ephemerisGLONASS)); + } + + // Galileo Ephemeris + // ----------------- + else if (rr == 1045) { + decoded = true; + emit newGalileoEph(new galileoephemeris(parser.ephemerisGALILEO)); + } + } + } + } + if (!_rawFile && _mode == unknown && decoded) { + _mode = observations; + } + } + + if (decoded) { + app->storeGlonassSlotNums(parser.GLOFreq); + return success; + } + else { + return failure; + } +} + +// Time of Corrections +////////////////////////////////////////////////////////////////////////////// +int RTCM3Decoder::corrGPSEpochTime() const { + if (_mode == corrections && _coDecoders.size() > 0) { + return _coDecoders.begin().value()->corrGPSEpochTime(); + } + else { + return -1; + } +} Index: trunk/BNC/src/RTCM3/RTCM3Decoder.h =================================================================== --- trunk/BNC/src/RTCM3/RTCM3Decoder.h (revision 4284) +++ trunk/BNC/src/RTCM3/RTCM3Decoder.h (revision 4284) @@ -0,0 +1,72 @@ +// Part of BNC, a utility for retrieving decoding and +// converting GNSS data streams from NTRIP broadcasters. +// +// Copyright (C) 2007 +// German Federal Agency for Cartography and Geodesy (BKG) +// http://www.bkg.bund.de +// Czech Technical University Prague, Department of Geodesy +// http://www.fsv.cvut.cz +// +// Email: euref-ip@bkg.bund.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation, version 2. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + +#ifndef RTCM3DECODER_H +#define RTCM3DECODER_H + +#include +#include + +#include "../RTCM/GPSDecoder.h" +#include "../RTCM/GPSDecoder.h" +#include "RTCM3coDecoder.h" +#include "bncrawfile.h" + +extern "C" { +#include "rtcm3torinex.h" +} + +class RTCM3Decoder : public QObject, public GPSDecoder { +Q_OBJECT + public: + RTCM3Decoder(const QString& staID, bncRawFile* rawFile); + virtual ~RTCM3Decoder(); + virtual t_irc Decode(char* buffer, int bufLen, std::vector& errmsg); + virtual int corrGPSEpochTime() const; + + signals: + void newMessage(QByteArray msg,bool showOnScreen); + void newGPSEph(gpsephemeris* gpseph); + void newGlonassEph(glonassephemeris* glonasseph); + void newGalileoEph(galileoephemeris* galileoeph); + + private: + enum t_mode{unknown = 0, observations, corrections}; + + QString _staID; + QString _checkMountPoint; + QMap _parsers; + QMap _coDecoders; + t_mode _mode; + + double _antXYZ[3]; + bncRawFile* _rawFile; + + QMap _slip_cnt_L1; + QMap _slip_cnt_L2; + QMap _slip_cnt_L5; +}; + +#endif + Index: trunk/BNC/src/RTCM3/RTCM3coDecoder.cpp =================================================================== --- trunk/BNC/src/RTCM3/RTCM3coDecoder.cpp (revision 4284) +++ trunk/BNC/src/RTCM3/RTCM3coDecoder.cpp (revision 4284) @@ -0,0 +1,396 @@ +// Part of BNC, a utility for retrieving decoding and +// converting GNSS data streams from NTRIP broadcasters. +// +// Copyright (C) 2007 +// German Federal Agency for Cartography and Geodesy (BKG) +// http://www.bkg.bund.de +// Czech Technical University Prague, Department of Geodesy +// http://www.fsv.cvut.cz +// +// Email: euref-ip@bkg.bund.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation, version 2. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + +/* ------------------------------------------------------------------------- + * BKG NTRIP Client + * ------------------------------------------------------------------------- + * + * Class: RTCM3coDecoder + * + * Purpose: RTCM3 Clock Orbit Decoder + * + * Author: L. Mervart + * + * Created: 05-May-2008 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include +#include + +#include "RTCM3coDecoder.h" +#include "bncutils.h" +#include "bncrinex.h" +#include "bncapp.h" +#include "bncsettings.h" +#include "rtcm3torinex.h" + +using namespace std; + +// Constructor +//////////////////////////////////////////////////////////////////////////// +RTCM3coDecoder::RTCM3coDecoder(const QString& staID) { + + _staID = staID; + + // File Output + // ----------- + bncSettings settings; + QString path = settings.value("corrPath").toString(); + if (!path.isEmpty()) { + expandEnvVar(path); + if ( path.length() > 0 && path[path.length()-1] != QDir::separator() ) { + path += QDir::separator(); + } + _fileNameSkl = path + staID; + } + _out = 0; + _GPSweeks = -1.0; + + connect(this, SIGNAL(newCorrLine(QString, QString, long)), + (bncApp*) qApp, SLOT(slotNewCorrLine(QString, QString, long))); + + memset(&_co, 0, sizeof(_co)); + memset(&_bias, 0, sizeof(_bias)); +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +RTCM3coDecoder::~RTCM3coDecoder() { + delete _out; +} + +// Reopen Output File +//////////////////////////////////////////////////////////////////////// +void RTCM3coDecoder::reopen(const QString& fileNameSkl, QString& fileName, + ofstream*& out) { + + if (!fileNameSkl.isEmpty()) { + + bncSettings settings; + + QDateTime datTim = currentDateAndTimeGPS(); + + QString hlpStr = bncRinex::nextEpochStr(datTim, + settings.value("corrIntr").toString()); + + QString fileNameHlp = fileNameSkl + + QString("%1").arg(datTim.date().dayOfYear(), 3, 10, QChar('0')) + + hlpStr + datTim.toString(".yyC"); + + if (fileName == fileNameHlp) { + return; + } + else { + fileName = fileNameHlp; + } + + delete out; + if ( Qt::CheckState(settings.value("rnxAppend").toInt()) == Qt::Checked) { + out = new ofstream( fileName.toAscii().data(), + ios_base::out | ios_base::app ); + } + else { + out = new ofstream( fileName.toAscii().data() ); + } + } +} + +// +//////////////////////////////////////////////////////////////////////////// +t_irc RTCM3coDecoder::Decode(char* buffer, int bufLen, vector& errmsg) { + + errmsg.clear(); + + _buffer.append(QByteArray(buffer,bufLen)); + + t_irc retCode = failure; + + while(_buffer.size()) { + + int bytesused = 0; + struct ClockOrbit co_sav; + memcpy(&co_sav, &_co, sizeof(co_sav)); // save state + + GCOB_RETURN irc = GetClockOrbitBias(&_co, &_bias, _buffer.data(), + _buffer.size(), &bytesused); + + if (irc <= -30) { // not enough data - restore state and exit loop + memcpy(&_co, &co_sav, sizeof(co_sav)); + break; + } + + else if (irc < 0) { // error - skip 1 byte and retry + memset(&_co, 0, sizeof(_co)); + memset(&_bias, 0, sizeof(_bias)); + _buffer = _buffer.mid(bytesused ? bytesused : 1); + } + + else { // OK or MESSAGEFOLLOWS + _buffer = _buffer.mid(bytesused); + + if ( irc == GCOBR_OK && + (_co.NumberOfGPSSat > 0 || _co.NumberOfGLONASSSat > 0 || + _bias.NumberOfGPSSat > 0 || _bias.NumberOfGLONASSSat > 0) ) { + + reopen(_fileNameSkl, _fileName, _out); + + // Guess GPS week and sec using system time + // ---------------------------------------- + int GPSweek; + double GPSweeksHlp; + currentGPSWeeks(GPSweek, GPSweeksHlp); + + // Correction Epoch from GPSEpochTime + // ---------------------------------- + if (_co.NumberOfGPSSat > 0 || _bias.NumberOfGPSSat > 0) { + int GPSEpochTime = (_co.NumberOfGPSSat > 0) ? + _co.GPSEpochTime : _bias.GPSEpochTime; + if (GPSweeksHlp > GPSEpochTime + 86400.0) { + GPSweek += 1; + } + else if (GPSweeksHlp < GPSEpochTime - 86400.0) { + GPSweek -= 1; + } + _GPSweeks = GPSEpochTime; + } + + // Correction Epoch from Glonass Epoch + // ----------------------------------- + else if (_co.NumberOfGLONASSSat > 0 || _bias.NumberOfGLONASSSat > 0){ + int GLONASSEpochTime = (_co.NumberOfGLONASSSat > 0) ? + _co.GLONASSEpochTime : _bias.GLONASSEpochTime; + + // Second of day (GPS time) from Glonass Epoch + // ------------------------------------------- + QDate date = dateAndTimeFromGPSweek(GPSweek, GPSweeksHlp).date(); + int leapSecond = gnumleap(date.year(), date.month(), date.day()); + int GPSDaySec = GLONASSEpochTime - 3 * 3600 + leapSecond; + + int weekDay = int(GPSweeksHlp/86400.0); + int GPSDaySecHlp = int(GPSweeksHlp) - weekDay * 86400; + + // Handle the difference between system clock and correction epoch + // --------------------------------------------------------------- + if (GPSDaySec < GPSDaySecHlp - 3600) { + weekDay += 1; + if (weekDay > 6) { + weekDay = 0; + GPSweek += 1; + } + } + else if (GPSDaySec > GPSDaySecHlp + 3600) { + weekDay -= 1; + if (weekDay < 0) { + weekDay = 6; + GPSweek -= 1; + } + } + + _GPSweeks = weekDay * 86400.0 + GPSDaySec; + } + + QStringList asciiLines = corrsToASCIIlines(GPSweek, _GPSweeks, + _co, &_bias); + + long coTime = GPSweek * 7*24*3600 + long(floor(_GPSweeks+0.5)); + + QStringListIterator it(asciiLines); + while (it.hasNext()) { + QString line = it.next(); + printLine(line, coTime); + } + + retCode = success; + memset(&_co, 0, sizeof(_co)); + memset(&_bias, 0, sizeof(_bias)); + } + } + } + + if (retCode != success) { + _GPSweeks = -1.0; + } + return retCode; +} + +// +//////////////////////////////////////////////////////////////////////////// +void RTCM3coDecoder::printLine(const QString& line, long coTime) { + if (_out) { + *_out << line.toAscii().data() << endl; + _out->flush(); + } + + emit newCorrLine(line, _staID, coTime); +} + +// +//////////////////////////////////////////////////////////////////////////// +QStringList RTCM3coDecoder::corrsToASCIIlines(int GPSweek, double GPSweeks, + const ClockOrbit& co, + const Bias* bias) { + + QStringList retLines; + + // Loop over all satellites (GPS and Glonass) + // ------------------------------------------ + if (co.NumberOfGPSSat > 0 || co.NumberOfGLONASSSat > 0) { + QString line1; + line1.sprintf("! Orbits/Clocks: %d GPS %d Glonass", + co.NumberOfGPSSat, co.NumberOfGLONASSSat); + retLines << line1; + } + for (int ii = 0; ii < CLOCKORBIT_NUMGPS+co.NumberOfGLONASSSat; ii++) { + char sysCh = ' '; + if (ii < co.NumberOfGPSSat) { + sysCh = 'G'; + } + else if (ii >= CLOCKORBIT_NUMGPS) { + sysCh = 'R'; + } + + if (sysCh != ' ') { + + QString linePart; + linePart.sprintf("%d %d %d %.1f %c%2.2d", + co.messageType, co.UpdateInterval, GPSweek, GPSweeks, + sysCh, co.Sat[ii].ID); + + // Combined message (orbit and clock) + // ---------------------------------- + if ( co.messageType == COTYPE_GPSCOMBINED || + co.messageType == COTYPE_GLONASSCOMBINED ) { + QString line; + line.sprintf(" %3d" + " %8.3f %8.3f %8.3f %8.3f" + " %10.5f %10.5f %10.5f %10.5f" + " %10.5f", + co.Sat[ii].IOD, + co.Sat[ii].Clock.DeltaA0, + co.Sat[ii].Orbit.DeltaRadial, + co.Sat[ii].Orbit.DeltaAlongTrack, + co.Sat[ii].Orbit.DeltaCrossTrack, + co.Sat[ii].Clock.DeltaA1, + co.Sat[ii].Orbit.DotDeltaRadial, + co.Sat[ii].Orbit.DotDeltaAlongTrack, + co.Sat[ii].Orbit.DotDeltaCrossTrack, + co.Sat[ii].Clock.DeltaA2); + retLines << linePart+line; + } + + // Orbits only + // ----------- + else if ( co.messageType == COTYPE_GPSORBIT || + co.messageType == COTYPE_GLONASSORBIT ) { + QString line; + line.sprintf(" %3d" + " %8.3f %8.3f %8.3f" + " %10.5f %10.5f %10.5f", + co.Sat[ii].IOD, + co.Sat[ii].Orbit.DeltaRadial, + co.Sat[ii].Orbit.DeltaAlongTrack, + co.Sat[ii].Orbit.DeltaCrossTrack, + co.Sat[ii].Orbit.DotDeltaRadial, + co.Sat[ii].Orbit.DotDeltaAlongTrack, + co.Sat[ii].Orbit.DotDeltaCrossTrack); + retLines << linePart+line; + } + + // Clocks only + // ----------- + else if ( co.messageType == COTYPE_GPSCLOCK || + co.messageType == COTYPE_GLONASSCLOCK ) { + QString line; + line.sprintf(" %3d %8.3f %10.5f %10.5f", + co.Sat[ii].IOD, + co.Sat[ii].Clock.DeltaA0, + co.Sat[ii].Clock.DeltaA1, + co.Sat[ii].Clock.DeltaA2); + retLines << linePart+line; + } + + // User Range Accuracy + // ------------------- + else if ( co.messageType == COTYPE_GPSURA || + co.messageType == COTYPE_GLONASSURA ) { + QString line; + line.sprintf(" %3d %f", + co.Sat[ii].IOD, co.Sat[ii].UserRangeAccuracy); + retLines << linePart+line; + } + + // High-Resolution Clocks + // ---------------------- + else if ( co.messageType == COTYPE_GPSHR || + co.messageType == COTYPE_GLONASSHR ) { + QString line; + line.sprintf(" %3d %8.3f", + co.Sat[ii].IOD, co.Sat[ii].hrclock); + retLines << linePart+line; + } + } + } + + // Loop over all satellites (GPS and Glonass) + // ------------------------------------------ + if (bias) { + if (bias->NumberOfGPSSat > 0 || bias->NumberOfGLONASSSat > 0) { + QString line1; + line1.sprintf("! Biases: %d GPS %d Glonass", + bias->NumberOfGPSSat, bias->NumberOfGLONASSSat); + retLines << line1; + } + for (int ii = 0; ii < CLOCKORBIT_NUMGPS + bias->NumberOfGLONASSSat; ii++) { + char sysCh = ' '; + int messageType; + if (ii < bias->NumberOfGPSSat) { + sysCh = 'G'; + messageType = BTYPE_GPS; + } + else if (ii >= CLOCKORBIT_NUMGPS) { + sysCh = 'R'; + messageType = BTYPE_GLONASS; + } + if (sysCh != ' ') { + QString line; + line.sprintf("%d %d %d %.1f %c%2.2d %d", + messageType, bias->UpdateInterval, GPSweek, GPSweeks, + sysCh, bias->Sat[ii].ID, + bias->Sat[ii].NumberOfCodeBiases); + for (int jj = 0; jj < bias->Sat[ii].NumberOfCodeBiases; jj++) { + QString hlp; + hlp.sprintf(" %d %8.3f", bias->Sat[ii].Biases[jj].Type, + bias->Sat[ii].Biases[jj].Bias); + line += hlp; + } + retLines << line; + } + } + } + + return retLines; +} Index: trunk/BNC/src/RTCM3/RTCM3coDecoder.h =================================================================== --- trunk/BNC/src/RTCM3/RTCM3coDecoder.h (revision 4284) +++ trunk/BNC/src/RTCM3/RTCM3coDecoder.h (revision 4284) @@ -0,0 +1,66 @@ +// Part of BNC, a utility for retrieving decoding and +// converting GNSS data streams from NTRIP broadcasters. +// +// Copyright (C) 2007 +// German Federal Agency for Cartography and Geodesy (BKG) +// http://www.bkg.bund.de +// Czech Technical University Prague, Department of Geodesy +// http://www.fsv.cvut.cz +// +// Email: euref-ip@bkg.bund.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation, version 2. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + +#ifndef RTCM3CODECODER_H +#define RTCM3CODECODER_H + +#include + +#include +#include + +#include "RTCM/GPSDecoder.h" + +extern "C" { +#include "clock_orbit_rtcm.h" +} + +class RTCM3coDecoder : public QObject, public GPSDecoder { +Q_OBJECT + public: + RTCM3coDecoder(const QString& staID); + virtual ~RTCM3coDecoder(); + virtual t_irc Decode(char* buffer, int bufLen, std::vector& errmsg); + virtual int corrGPSEpochTime() const {return (int) _GPSweeks;} + + static QStringList corrsToASCIIlines(int GPSweek, double GPSweeks, + const ClockOrbit& co, const Bias* bias); + static void reopen(const QString& fileNameSkl, QString& fileName, + std::ofstream*& out); + signals: + void newCorrLine(QString line, QString staID, long coTime); + + private: + void printLine(const QString& line, long coTime); + std::ofstream* _out; + QString _staID; + QString _fileNameSkl; + QString _fileName; + QByteArray _buffer; + double _GPSweeks; + ClockOrbit _co; + Bias _bias; +}; + +#endif Index: trunk/BNC/src/RTCM3/ephemeris.cpp =================================================================== --- trunk/BNC/src/RTCM3/ephemeris.cpp (revision 4284) +++ trunk/BNC/src/RTCM3/ephemeris.cpp (revision 4284) @@ -0,0 +1,1191 @@ +#include +#include +#include +#include +#include + +#include + +#include "ephemeris.h" +#include "bncutils.h" +#include "timeutils.h" +#include "bnctime.h" +#include "bncapp.h" + +using namespace std; + +// Returns CRC24 +//////////////////////////////////////////////////////////////////////////// +static unsigned long CRC24(long size, const unsigned char *buf) { + unsigned long crc = 0; + int ii; + while (size--) { + crc ^= (*buf++) << (16); + for(ii = 0; ii < 8; ii++) { + crc <<= 1; + if (crc & 0x1000000) { + crc ^= 0x01864cfb; + } + } + } + return crc; +} + +// Set GPS Satellite Position +//////////////////////////////////////////////////////////////////////////// +void t_ephGPS::set(const gpsephemeris* ee) { + + _receptDateTime = currentDateAndTimeGPS(); + + _prn = QString("G%1").arg(ee->satellite, 2, 10, QChar('0')); + + _TOC.set(ee->GPSweek, ee->TOC); + _clock_bias = ee->clock_bias; + _clock_drift = ee->clock_drift; + _clock_driftrate = ee->clock_driftrate; + + _IODE = ee->IODE; + _Crs = ee->Crs; + _Delta_n = ee->Delta_n; + _M0 = ee->M0; + + _Cuc = ee->Cuc; + _e = ee->e; + _Cus = ee->Cus; + _sqrt_A = ee->sqrt_A; + + _TOEsec = ee->TOE; + _Cic = ee->Cic; + _OMEGA0 = ee->OMEGA0; + _Cis = ee->Cis; + + _i0 = ee->i0; + _Crc = ee->Crc; + _omega = ee->omega; + _OMEGADOT = ee->OMEGADOT; + + _IDOT = ee->IDOT; + _L2Codes = 0.0; + _TOEweek = ee->GPSweek; + _L2PFlag = 0.0; + + if (ee->URAindex <= 6) { + _ura = ceil(10.0*pow(2.0, 1.0+((double)ee->URAindex)/2.0))/10.0; + } + else { + _ura = ceil(10.0*pow(2.0, ((double)ee->URAindex)/2.0))/10.0; + } + _health = ee->SVhealth; + _TGD = ee->TGD; + _IODC = ee->IODC; + + _TOT = 0.9999e9; + _fitInterval = 0.0; + + _ok = true; +} + +// Compute GPS Satellite Position (virtual) +//////////////////////////////////////////////////////////////////////////// +void t_ephGPS::position(int GPSweek, double GPSweeks, + double* xc, + double* vv) const { + + + static const double omegaEarth = 7292115.1467e-11; + static const double gmWGS = 398.6005e12; + + memset(xc, 0, 4*sizeof(double)); + memset(vv, 0, 3*sizeof(double)); + + double a0 = _sqrt_A * _sqrt_A; + if (a0 == 0) { + return; + } + + double n0 = sqrt(gmWGS/(a0*a0*a0)); + + bncTime tt(GPSweek, GPSweeks); + double tk = tt - bncTime(_TOEweek, _TOEsec); + + double n = n0 + _Delta_n; + double M = _M0 + n*tk; + double E = M; + double E_last; + do { + E_last = E; + E = M + _e*sin(E); + } while ( fabs(E-E_last)*a0 > 0.001 ); + double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) ); + double u0 = v + _omega; + double sin2u0 = sin(2*u0); + double cos2u0 = cos(2*u0); + double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0; + double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0; + double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0; + double xp = r*cos(u); + double yp = r*sin(u); + double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - + omegaEarth*_TOEsec; + + double sinom = sin(OM); + double cosom = cos(OM); + double sini = sin(i); + double cosi = cos(i); + xc[0] = xp*cosom - yp*cosi*sinom; + xc[1] = xp*sinom + yp*cosi*cosom; + xc[2] = yp*sini; + + double tc = tt - _TOC; + xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc; + + // Velocity + // -------- + double tanv2 = tan(v/2); + double dEdM = 1 / (1 - _e*cos(E)); + double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) + * dEdM * n; + double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv; + double dotom = _OMEGADOT - omegaEarth; + double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv; + double dotr = a0 * _e*sin(E) * dEdM * n + + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv; + double dotx = dotr*cos(u) - r*sin(u)*dotu; + double doty = dotr*sin(u) + r*cos(u)*dotu; + + vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr + - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA + + yp*sini*sinom*doti; // dX / di + + vv[1] = sinom *dotx + cosi*cosom *doty + + xp*cosom*dotom - yp*cosi*sinom*dotom + - yp*sini*cosom*doti; + + vv[2] = sini *doty + yp*cosi *doti; + + // Relativistic Correction + // ----------------------- + xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c; +} + +// build up RTCM3 for GPS +//////////////////////////////////////////////////////////////////////////// +#define GPSTOINT(type, value) static_cast(round(value)) + +#define GPSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \ + |(GPSTOINT(long long,b)&((1ULL<= 8) { \ + buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}} + +#define GPSADDBITSFLOAT(a,b,c) {long long i = GPSTOINT(long long,(b)/(c)); \ + GPSADDBITS(a,i)}; + +int t_ephGPS::RTCM3(unsigned char *buffer) { + + unsigned char *startbuffer = buffer; + buffer= buffer+3; + int size = 0; + int numbits = 0; + unsigned long long bitbuffer = 0; + if (_ura <= 2.40){ + _ura = 0; + } + else if (_ura <= 3.40){ + _ura = 1; + } + else if (_ura <= 6.85){ + _ura = 2; + } + else if (_ura <= 9.65){ + _ura = 3; + } + else if (_ura <= 13.65){ + _ura = 4; + } + else if (_ura <= 24.00){ + _ura = 5; + } + else if (_ura <= 48.00){ + _ura = 6; + } + else if (_ura <= 96.00){ + _ura = 7; + } + else if (_ura <= 192.00){ + _ura = 8; + } + else if (_ura <= 384.00){ + _ura = 9; + } + else if (_ura <= 768.00){ + _ura = 10; + } + else if (_ura <= 1536.00){ + _ura = 11; + } + else if (_ura <= 1536.00){ + _ura = 12; + } + else if (_ura <= 2072.00){ + _ura = 13; + } + else if (_ura <= 6144.00){ + _ura = 14; + } + else{ + _ura = 15; + } + + GPSADDBITS(12, 1019) + GPSADDBITS(6,_prn.right((_prn.length()-1)).toInt()) + GPSADDBITS(10, _TOC.gpsw()) + GPSADDBITS(4, _ura) + GPSADDBITS(2,_L2Codes) + GPSADDBITSFLOAT(14, _IDOT, M_PI/static_cast(1<<30) + /static_cast(1<<13)) + GPSADDBITS(8, _IODE) + GPSADDBITS(16, static_cast(_TOC.gpssec())>>4) + GPSADDBITSFLOAT(8, _clock_driftrate, 1.0/static_cast(1<<30) + /static_cast(1<<25)) + GPSADDBITSFLOAT(16, _clock_drift, 1.0/static_cast(1<<30) + /static_cast(1<<13)) + GPSADDBITSFLOAT(22, _clock_bias, 1.0/static_cast(1<<30) + /static_cast(1<<1)) + GPSADDBITS(10, _IODC) + GPSADDBITSFLOAT(16, _Crs, 1.0/static_cast(1<<5)) + GPSADDBITSFLOAT(16, _Delta_n, M_PI/static_cast(1<<30) + /static_cast(1<<13)) + GPSADDBITSFLOAT(32, _M0, M_PI/static_cast(1<<30)/static_cast(1<<1)) + GPSADDBITSFLOAT(16, _Cuc, 1.0/static_cast(1<<29)) + GPSADDBITSFLOAT(32, _e, 1.0/static_cast(1<<30)/static_cast(1<<3)) + GPSADDBITSFLOAT(16, _Cus, 1.0/static_cast(1<<29)) + GPSADDBITSFLOAT(32, _sqrt_A, 1.0/static_cast(1<<19)) + GPSADDBITS(16, static_cast(_TOEsec)>>4) + GPSADDBITSFLOAT(16, _Cic, 1.0/static_cast(1<<29)) + GPSADDBITSFLOAT(32, _OMEGA0, M_PI/static_cast(1<<30) + /static_cast(1<<1)) + GPSADDBITSFLOAT(16, _Cis, 1.0/static_cast(1<<29)) + GPSADDBITSFLOAT(32, _i0, M_PI/static_cast(1<<30)/static_cast(1<<1)) + GPSADDBITSFLOAT(16, _Crc, 1.0/static_cast(1<<5)) + GPSADDBITSFLOAT(32, _omega, M_PI/static_cast(1<<30) + /static_cast(1<<1)) + GPSADDBITSFLOAT(24, _OMEGADOT, M_PI/static_cast(1<<30) + /static_cast(1<<13)) + GPSADDBITSFLOAT(8, _TGD, 1.0/static_cast(1<<30)/static_cast(1<<1)) + GPSADDBITS(6, _health) + GPSADDBITS(1, _L2PFlag) + GPSADDBITS(1, 0) /* GPS fit interval */ + + startbuffer[0]=0xD3; + startbuffer[1]=(size >> 8); + startbuffer[2]=size; + unsigned long i = CRC24(size+3, startbuffer); + buffer[size++] = i >> 16; + buffer[size++] = i >> 8; + buffer[size++] = i; + size += 3; + return size; +} + +// Derivative of the state vector using a simple force model (static) +//////////////////////////////////////////////////////////////////////////// +ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv, + double* acc) { + + // State vector components + // ----------------------- + ColumnVector rr = xv.rows(1,3); + ColumnVector vv = xv.rows(4,6); + + // Acceleration + // ------------ + static const double GM = 398.60044e12; + static const double AE = 6378136.0; + static const double OMEGA = 7292115.e-11; + static const double C20 = -1082.6257e-6; + + double rho = rr.norm_Frobenius(); + double t1 = -GM/(rho*rho*rho); + double t2 = 3.0/2.0 * C20 * (GM*AE*AE) / (rho*rho*rho*rho*rho); + double t3 = OMEGA * OMEGA; + double t4 = 2.0 * OMEGA; + double z2 = rr(3) * rr(3); + + // Vector of derivatives + // --------------------- + ColumnVector va(6); + va(1) = vv(1); + va(2) = vv(2); + va(3) = vv(3); + va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0]; + va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1]; + va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3) + acc[2]; + + return va; +} + +// Compute Glonass Satellite Position (virtual) +//////////////////////////////////////////////////////////////////////////// +void t_ephGlo::position(int GPSweek, double GPSweeks, + double* xc, double* vv) const { + + static const double nominalStep = 10.0; + + memset(xc, 0, 4*sizeof(double)); + memset(vv, 0, 3*sizeof(double)); + + double dtPos = bncTime(GPSweek, GPSweeks) - _tt; + + int nSteps = int(fabs(dtPos) / nominalStep) + 1; + double step = dtPos / nSteps; + + double acc[3]; + acc[0] = _x_acceleration * 1.e3; + acc[1] = _y_acceleration * 1.e3; + acc[2] = _z_acceleration * 1.e3; + for (int ii = 1; ii <= nSteps; ii++) { + _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv); + _tt = _tt + step; + } + + // Position and Velocity + // --------------------- + xc[0] = _xv(1); + xc[1] = _xv(2); + xc[2] = _xv(3); + + vv[0] = _xv(4); + vv[1] = _xv(5); + vv[2] = _xv(6); + + // Clock Correction + // ---------------- + double dtClk = bncTime(GPSweek, GPSweeks) - _TOC; + xc[3] = -_tau + _gamma * dtClk; +} + +// IOD of Glonass Ephemeris (virtual) +//////////////////////////////////////////////////////////////////////////// +int t_ephGlo::IOD() const { + bncTime tMoscow = _TOC - _gps_utc + 3 * 3600.0; + return int(tMoscow.daysec() / 900); +} + +// Set Glonass Ephemeris +//////////////////////////////////////////////////////////////////////////// +void t_ephGlo::set(const glonassephemeris* ee) { + + _receptDateTime = currentDateAndTimeGPS(); + + _prn = QString("R%1").arg(ee->almanac_number, 2, 10, QChar('0')); + + int ww = ee->GPSWeek; + int tow = ee->GPSTOW; + updatetime(&ww, &tow, ee->tb*1000, 0); // Moscow -> GPS + + // Check the day once more + // ----------------------- + { + const double secPerDay = 24 * 3600.0; + const double secPerWeek = 7 * secPerDay; + int ww_old = ww; + int tow_old = tow; + int currentWeek; + double currentSec; + currentGPSWeeks(currentWeek, currentSec); + bncTime currentTime(currentWeek, currentSec); + bncTime hTime(ww, (double) tow); + + bool changed = false; + if (hTime - currentTime > secPerDay/2.0) { + changed = true; + tow -= secPerDay; + if (tow < 0) { + tow += secPerWeek; + ww -= 1; + } + } + else if (hTime - currentTime < -secPerDay/2.0) { + changed = true; + tow += secPerDay; + if (tow > secPerWeek) { + tow -= secPerWeek; + ww += 1; + } + } + + if (changed && ((bncApp*) qApp)->mode() == bncApp::batchPostProcessing) { + bncTime newHTime(ww, (double) tow); + cout << "GLONASS " << ee->almanac_number << " Time Changed at " + << currentTime.datestr() << " " << currentTime.timestr() + << endl + << "old: " << hTime.datestr() << " " << hTime.timestr() + << endl + << "new: " << newHTime.datestr() << " " << newHTime.timestr() + << endl + << "eph: " << ee->GPSWeek << " " << ee->GPSTOW << " " << ee->tb + << endl + << "ww, tow (old): " << ww_old << " " << tow_old + << endl + << "ww, tow (new): " << ww << " " << tow + << endl << endl; + } + } + + bncTime hlpTime(ww, (double) tow); + unsigned year, month, day; + hlpTime.civil_date(year, month, day); + _gps_utc = gnumleap(year, month, day); + + _TOC.set(ww, tow); + _E = ee->E; + _tau = ee->tau; + _gamma = ee->gamma; + _x_pos = ee->x_pos; + _x_velocity = ee->x_velocity; + _x_acceleration = ee->x_acceleration; + _y_pos = ee->y_pos; + _y_velocity = ee->y_velocity; + _y_acceleration = ee->y_acceleration; + _z_pos = ee->z_pos; + _z_velocity = ee->z_velocity; + _z_acceleration = ee->z_acceleration; + _health = 0; + _frequency_number = ee->frequency_number; + _tki = ee->tk-3*60*60; if (_tki < 0) _tki += 86400; + + // Initialize status vector + // ------------------------ + _tt = _TOC; + + _xv(1) = _x_pos * 1.e3; + _xv(2) = _y_pos * 1.e3; + _xv(3) = _z_pos * 1.e3; + _xv(4) = _x_velocity * 1.e3; + _xv(5) = _y_velocity * 1.e3; + _xv(6) = _z_velocity * 1.e3; + + _ok = true; +} + +// build up RTCM3 for GLONASS +//////////////////////////////////////////////////////////////////////////// +#define GLONASSTOINT(type, value) static_cast(round(value)) + +#define GLONASSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \ + |(GLONASSTOINT(long long,b)&((1ULL<<(a))-1)); \ + numbits += (a); \ + while(numbits >= 8) { \ + buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}} +#define GLONASSADDBITSFLOATM(a,b,c) {int s; long long i; \ + if(b < 0.0) \ + { \ + s = 1; \ + i = GLONASSTOINT(long long,(-b)/(c)); \ + if(!i) s = 0; \ + } \ + else \ + { \ + s = 0; \ + i = GLONASSTOINT(long long,(b)/(c)); \ + } \ + GLONASSADDBITS(1,s) \ + GLONASSADDBITS(a-1,i)} + +int t_ephGlo::RTCM3(unsigned char *buffer) +{ + + int size = 0; + int numbits = 0; + long long bitbuffer = 0; + unsigned char *startbuffer = buffer; + buffer= buffer+3; + + GLONASSADDBITS(12, 1020) + GLONASSADDBITS(6, _prn.right((_prn.length()-1)).toInt()) + GLONASSADDBITS(5, 7+_frequency_number) + GLONASSADDBITS(1, 0) + GLONASSADDBITS(1, 0) + GLONASSADDBITS(2, 0) + _tki=_tki+3*60*60; + GLONASSADDBITS(5, static_cast(_tki)/(60*60)) + GLONASSADDBITS(6, (static_cast(_tki)/60)%60) + GLONASSADDBITS(1, (static_cast(_tki)/30)%30) + GLONASSADDBITS(1, _health) + GLONASSADDBITS(1, 0) + unsigned long long timeofday = (static_cast(_tt.gpssec()+3*60*60-_gps_utc)%86400); + GLONASSADDBITS(7, timeofday/60/15) + GLONASSADDBITSFLOATM(24, _x_velocity*1000, 1000.0/static_cast(1<<20)) + GLONASSADDBITSFLOATM(27, _x_pos*1000, 1000.0/static_cast(1<<11)) + GLONASSADDBITSFLOATM(5, _x_acceleration*1000, 1000.0/static_cast(1<<30)) + GLONASSADDBITSFLOATM(24, _y_velocity*1000, 1000.0/static_cast(1<<20)) + GLONASSADDBITSFLOATM(27, _y_pos*1000, 1000.0/static_cast(1<<11)) + GLONASSADDBITSFLOATM(5, _y_acceleration*1000, 1000.0/static_cast(1<<30)) + GLONASSADDBITSFLOATM(24, _z_velocity*1000, 1000.0/static_cast(1<<20)) + GLONASSADDBITSFLOATM(27,_z_pos*1000, 1000.0/static_cast(1<<11)) + GLONASSADDBITSFLOATM(5, _z_acceleration*1000, 1000.0/static_cast(1<<30)) + GLONASSADDBITS(1, 0) + GLONASSADDBITSFLOATM(11, _gamma, 1.0/static_cast(1<<30) + /static_cast(1<<10)) + GLONASSADDBITS(2, 0) /* GLONASS-M P */ + GLONASSADDBITS(1, 0) /* GLONASS-M ln(3) */ + GLONASSADDBITSFLOATM(22, _tau, 1.0/static_cast(1<<30)) + GLONASSADDBITS(5, 0) /* GLONASS-M delta tau */ + GLONASSADDBITS(5, _E) + GLONASSADDBITS(1, 0) /* GLONASS-M P4 */ + GLONASSADDBITS(4, 0) /* GLONASS-M FT */ + GLONASSADDBITS(11, 0) /* GLONASS-M NT */ + GLONASSADDBITS(2, 0) /* GLONASS-M active? */ + GLONASSADDBITS(1, 0) /* GLONASS additional data */ + GLONASSADDBITS(11, 0) /* GLONASS NA */ + GLONASSADDBITS(32, 0) /* GLONASS tau C */ + GLONASSADDBITS(5, 0) /* GLONASS-M N4 */ + GLONASSADDBITS(22, 0) /* GLONASS-M tau GPS */ + GLONASSADDBITS(1, 0) /* GLONASS-M ln(5) */ + GLONASSADDBITS(7, 0) /* Reserved */ + + startbuffer[0]=0xD3; + startbuffer[1]=(size >> 8); + startbuffer[2]=size; + unsigned long i = CRC24(size+3, startbuffer); + buffer[size++] = i >> 16; + buffer[size++] = i >> 8; + buffer[size++] = i; + size += 3; + return size; +} + +// Set Galileo Satellite Position +//////////////////////////////////////////////////////////////////////////// +void t_ephGal::set(const galileoephemeris* ee) { + + _receptDateTime = currentDateAndTimeGPS(); + + _prn = QString("E%1").arg(ee->satellite, 2, 10, QChar('0')); + + _TOC.set(ee->Week, ee->TOC); + _clock_bias = ee->clock_bias; + _clock_drift = ee->clock_drift; + _clock_driftrate = ee->clock_driftrate; + + _IODnav = ee->IODnav; + _Crs = ee->Crs; + _Delta_n = ee->Delta_n; + _M0 = ee->M0; + + _Cuc = ee->Cuc; + _e = ee->e; + _Cus = ee->Cus; + _sqrt_A = ee->sqrt_A; + + _TOEsec = ee->TOE; + _Cic = ee->Cic; + _OMEGA0 = ee->OMEGA0; + _Cis = ee->Cis; + + _i0 = ee->i0; + _Crc = ee->Crc; + _omega = ee->omega; + _OMEGADOT = ee->OMEGADOT; + + _IDOT = ee->IDOT; + _TOEweek = ee->Week; + + _SISA = ee->SISA; + _E5aHS = ee->E5aHS; + _BGD_1_5A = ee->BGD_1_5A; + _BGD_1_5B = ee->BGD_1_5B; + + _TOT = 0.9999e9; + + _ok = true; +} + +// Compute Galileo Satellite Position (virtual) +//////////////////////////////////////////////////////////////////////////// +void t_ephGal::position(int GPSweek, double GPSweeks, + double* xc, + double* vv) const { + + static const double omegaEarth = 7292115.1467e-11; + static const double gmWGS = 398.6005e12; + + memset(xc, 0, 4*sizeof(double)); + memset(vv, 0, 3*sizeof(double)); + + double a0 = _sqrt_A * _sqrt_A; + if (a0 == 0) { + return; + } + + double n0 = sqrt(gmWGS/(a0*a0*a0)); + + bncTime tt(GPSweek, GPSweeks); + double tk = tt - bncTime(_TOC.gpsw(), _TOEsec); + + double n = n0 + _Delta_n; + double M = _M0 + n*tk; + double E = M; + double E_last; + do { + E_last = E; + E = M + _e*sin(E); + } while ( fabs(E-E_last)*a0 > 0.001 ); + double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) ); + double u0 = v + _omega; + double sin2u0 = sin(2*u0); + double cos2u0 = cos(2*u0); + double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0; + double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0; + double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0; + double xp = r*cos(u); + double yp = r*sin(u); + double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - + omegaEarth*_TOEsec; + + double sinom = sin(OM); + double cosom = cos(OM); + double sini = sin(i); + double cosi = cos(i); + xc[0] = xp*cosom - yp*cosi*sinom; + xc[1] = xp*sinom + yp*cosi*cosom; + xc[2] = yp*sini; + + double tc = tt - _TOC; + xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc; + + // Velocity + // -------- + double tanv2 = tan(v/2); + double dEdM = 1 / (1 - _e*cos(E)); + double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) + * dEdM * n; + double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv; + double dotom = _OMEGADOT - omegaEarth; + double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv; + double dotr = a0 * _e*sin(E) * dEdM * n + + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv; + double dotx = dotr*cos(u) - r*sin(u)*dotu; + double doty = dotr*sin(u) + r*cos(u)*dotu; + + vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr + - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA + + yp*sini*sinom*doti; // dX / di + + vv[1] = sinom *dotx + cosi*cosom *doty + + xp*cosom*dotom - yp*cosi*sinom*dotom + - yp*sini*cosom*doti; + + vv[2] = sini *doty + yp*cosi *doti; + + // Relativistic Correction + // ----------------------- + // xc(4) -= 4.442807633e-10 * _e * sqrt(a0) *sin(E); + xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c; +} + +// build up RTCM3 for Galileo +//////////////////////////////////////////////////////////////////////////// +#define GALILEOTOINT(type, value) static_cast(round(value)) + +#define GALILEOADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \ + |(GALILEOTOINT(long long,b)&((1LL<= 8) { \ + buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}} +#define GALILEOADDBITSFLOAT(a,b,c) {long long i = GALILEOTOINT(long long,(b)/(c)); \ + GALILEOADDBITS(a,i)}; + +int t_ephGal::RTCM3(unsigned char *buffer) { + int size = 0; + int numbits = 0; + long long bitbuffer = 0; + unsigned char *startbuffer = buffer; + buffer= buffer+3; + + GALILEOADDBITS(12, /*inav ? 1046 :*/ 1045) + GALILEOADDBITS(6, _prn.right((_prn.length()-1)).toInt()) + GALILEOADDBITS(12, _TOC.gpsw()) + GALILEOADDBITS(10, _IODnav) + GALILEOADDBITS(8, _SISA) + GALILEOADDBITSFLOAT(14, _IDOT, M_PI/static_cast(1<<30) + /static_cast(1<<13)) + GALILEOADDBITS(14, _TOC.gpssec()/60) + GALILEOADDBITSFLOAT(6, _clock_driftrate, 1.0/static_cast(1<<30) + /static_cast(1<<29)) + GALILEOADDBITSFLOAT(21, _clock_drift, 1.0/static_cast(1<<30) + /static_cast(1<<16)) + GALILEOADDBITSFLOAT(31, _clock_bias, 1.0/static_cast(1<<30) + /static_cast(1<<4)) + GALILEOADDBITSFLOAT(16, _Crs, 1.0/static_cast(1<<5)) + GALILEOADDBITSFLOAT(16, _Delta_n, M_PI/static_cast(1<<30) + /static_cast(1<<13)) + GALILEOADDBITSFLOAT(32, _M0, M_PI/static_cast(1<<30)/static_cast(1<<1)) + GALILEOADDBITSFLOAT(16, _Cuc, 1.0/static_cast(1<<29)) + GALILEOADDBITSFLOAT(32, _e, 1.0/static_cast(1<<30)/static_cast(1<<3)) + GALILEOADDBITSFLOAT(16, _Cus, 1.0/static_cast(1<<29)) + GALILEOADDBITSFLOAT(32, _sqrt_A, 1.0/static_cast(1<<19)) + GALILEOADDBITS(14, _TOEsec/60) + GALILEOADDBITSFLOAT(16, _Cic, 1.0/static_cast(1<<29)) + GALILEOADDBITSFLOAT(32, _OMEGA0, M_PI/static_cast(1<<30) + /static_cast(1<<1)) + GALILEOADDBITSFLOAT(16, _Cis, 1.0/static_cast(1<<29)) + GALILEOADDBITSFLOAT(32, _i0, M_PI/static_cast(1<<30)/static_cast(1<<1)) + GALILEOADDBITSFLOAT(16, _Crc, 1.0/static_cast(1<<5)) + GALILEOADDBITSFLOAT(32, _omega, M_PI/static_cast(1<<30) + /static_cast(1<<1)) + GALILEOADDBITSFLOAT(24, _OMEGADOT, M_PI/static_cast(1<<30) + /static_cast(1<<13)) + GALILEOADDBITSFLOAT(10, _BGD_1_5A, 1.0/static_cast(1<<30) + /static_cast(1<<2)) + /*if(inav) + { + GALILEOADDBITSFLOAT(10, _BGD_1_5B, 1.0/static_cast(1<<30) + /static_cast(1<<2)) + GALILEOADDBITS(2, _E5bHS) + GALILEOADDBITS(1, flags & MNFGALEPHF_E5BDINVALID) + } + else*/ + { + GALILEOADDBITS(2, _E5aHS) + GALILEOADDBITS(1, /*flags & MNFGALEPHF_E5ADINVALID*/0) + } + _TOEsec = 0.9999E9; + GALILEOADDBITS(20, _TOEsec) + + GALILEOADDBITS(/*inav ? 1 :*/ 3, 0) /* fill up */ + + startbuffer[0]=0xD3; + startbuffer[1]=(size >> 8); + startbuffer[2]=size; + unsigned long i = CRC24(size+3, startbuffer); + buffer[size++] = i >> 16; + buffer[size++] = i >> 8; + buffer[size++] = i; + size += 3; + return size; +} + +// Constructor +////////////////////////////////////////////////////////////////////////////// +t_ephGPS::t_ephGPS(float rnxVersion, const QStringList& lines) { + + const int nLines = 8; + + _ok = false; + + if (lines.size() != nLines) { + return; + } + + // RINEX Format + // ------------ + int fieldLen = 19; + + int pos[4]; + pos[0] = (rnxVersion <= 2.12) ? 3 : 4; + pos[1] = pos[0] + fieldLen; + pos[2] = pos[1] + fieldLen; + pos[3] = pos[2] + fieldLen; + + // Read eight lines + // ---------------- + for (int iLine = 0; iLine < nLines; iLine++) { + QString line = lines[iLine]; + + if ( iLine == 0 ) { + QTextStream in(line.left(pos[1]).toAscii()); + + int year, month, day, hour, min; + double sec; + + in >> _prn >> year >> month >> day >> hour >> min >> sec; + + if (_prn.at(0) != 'G') { + _prn = QString("G%1").arg(_prn.toInt(), 2, 10, QLatin1Char('0')); + } + + if (year < 80) { + year += 2000; + } + else if (year < 100) { + year += 1900; + } + + _TOC.set(year, month, day, hour, min, sec); + + if ( readDbl(line, pos[1], fieldLen, _clock_bias ) || + readDbl(line, pos[2], fieldLen, _clock_drift ) || + readDbl(line, pos[3], fieldLen, _clock_driftrate) ) { + return; + } + } + + else if ( iLine == 1 ) { + if ( readDbl(line, pos[0], fieldLen, _IODE ) || + readDbl(line, pos[1], fieldLen, _Crs ) || + readDbl(line, pos[2], fieldLen, _Delta_n) || + readDbl(line, pos[3], fieldLen, _M0 ) ) { + return; + } + } + + else if ( iLine == 2 ) { + if ( readDbl(line, pos[0], fieldLen, _Cuc ) || + readDbl(line, pos[1], fieldLen, _e ) || + readDbl(line, pos[2], fieldLen, _Cus ) || + readDbl(line, pos[3], fieldLen, _sqrt_A) ) { + return; + } + } + + else if ( iLine == 3 ) { + if ( readDbl(line, pos[0], fieldLen, _TOEsec) || + readDbl(line, pos[1], fieldLen, _Cic ) || + readDbl(line, pos[2], fieldLen, _OMEGA0) || + readDbl(line, pos[3], fieldLen, _Cis ) ) { + return; + } + } + + else if ( iLine == 4 ) { + if ( readDbl(line, pos[0], fieldLen, _i0 ) || + readDbl(line, pos[1], fieldLen, _Crc ) || + readDbl(line, pos[2], fieldLen, _omega ) || + readDbl(line, pos[3], fieldLen, _OMEGADOT) ) { + return; + } + } + + else if ( iLine == 5 ) { + if ( readDbl(line, pos[0], fieldLen, _IDOT ) || + readDbl(line, pos[1], fieldLen, _L2Codes) || + readDbl(line, pos[2], fieldLen, _TOEweek ) || + readDbl(line, pos[3], fieldLen, _L2PFlag) ) { + return; + } + } + + else if ( iLine == 6 ) { + if ( readDbl(line, pos[0], fieldLen, _ura ) || + readDbl(line, pos[1], fieldLen, _health) || + readDbl(line, pos[2], fieldLen, _TGD ) || + readDbl(line, pos[3], fieldLen, _IODC ) ) { + return; + } + } + + else if ( iLine == 7 ) { + if ( readDbl(line, pos[0], fieldLen, _TOT) ) { + return; + } + readDbl(line, pos[1], fieldLen, _fitInterval); // _fitInterval optional + } + } + + _ok = true; +} + +// Constructor +////////////////////////////////////////////////////////////////////////////// +t_ephGlo::t_ephGlo(float rnxVersion, const QStringList& lines) { + + const int nLines = 4; + + _ok = false; + + if (lines.size() != nLines) { + return; + } + + // RINEX Format + // ------------ + int fieldLen = 19; + + int pos[4]; + pos[0] = (rnxVersion <= 2.12) ? 3 : 4; + pos[1] = pos[0] + fieldLen; + pos[2] = pos[1] + fieldLen; + pos[3] = pos[2] + fieldLen; + + // Read four lines + // --------------- + for (int iLine = 0; iLine < nLines; iLine++) { + QString line = lines[iLine]; + + if ( iLine == 0 ) { + QTextStream in(line.left(pos[1]).toAscii()); + + int year, month, day, hour, min; + double sec; + + in >> _prn >> year >> month >> day >> hour >> min >> sec; + + if (_prn.at(0) != 'R') { + _prn = QString("R%1").arg(_prn.toInt(), 2, 10, QLatin1Char('0')); + } + + if (year < 80) { + year += 2000; + } + else if (year < 100) { + year += 1900; + } + + _gps_utc = gnumleap(year, month, day); + + _TOC.set(year, month, day, hour, min, sec); + _TOC = _TOC + _gps_utc; + + if ( readDbl(line, pos[1], fieldLen, _tau ) || + readDbl(line, pos[2], fieldLen, _gamma) || + readDbl(line, pos[3], fieldLen, _tki ) ) { + return; + } + + _tau = -_tau; + } + + else if ( iLine == 1 ) { + if ( readDbl(line, pos[0], fieldLen, _x_pos ) || + readDbl(line, pos[1], fieldLen, _x_velocity ) || + readDbl(line, pos[2], fieldLen, _x_acceleration) || + readDbl(line, pos[3], fieldLen, _health ) ) { + return; + } + } + + else if ( iLine == 2 ) { + if ( readDbl(line, pos[0], fieldLen, _y_pos ) || + readDbl(line, pos[1], fieldLen, _y_velocity ) || + readDbl(line, pos[2], fieldLen, _y_acceleration ) || + readDbl(line, pos[3], fieldLen, _frequency_number) ) { + return; + } + } + + else if ( iLine == 3 ) { + if ( readDbl(line, pos[0], fieldLen, _z_pos ) || + readDbl(line, pos[1], fieldLen, _z_velocity ) || + readDbl(line, pos[2], fieldLen, _z_acceleration) || + readDbl(line, pos[3], fieldLen, _E ) ) { + return; + } + } + } + + // Initialize status vector + // ------------------------ + _tt = _TOC; + _xv.ReSize(6); + _xv(1) = _x_pos * 1.e3; + _xv(2) = _y_pos * 1.e3; + _xv(3) = _z_pos * 1.e3; + _xv(4) = _x_velocity * 1.e3; + _xv(5) = _y_velocity * 1.e3; + _xv(6) = _z_velocity * 1.e3; + + _ok = true; +} + +// Constructor +////////////////////////////////////////////////////////////////////////////// +t_ephGal::t_ephGal(float /* rnxVersion */, const QStringList& /* lines */) { + + _ok = false; +} + +// +////////////////////////////////////////////////////////////////////////////// +QString t_eph::rinexDateStr(const bncTime& tt, const QString& prn, + double version) { + + QString datStr; + + unsigned year, month, day, hour, min; + double sec; + tt.civil_date(year, month, day); + tt.civil_time(hour, min, sec); + + QTextStream out(&datStr); + + if (version < 3.0) { + QString prnHlp = prn.mid(1,2); if (prnHlp[0] == '0') prnHlp[0] = ' '; + out << prnHlp << QString(" %1 %2 %3 %4 %5%6") + .arg(year % 100, 2, 10, QChar('0')) + .arg(month, 2) + .arg(day, 2) + .arg(hour, 2) + .arg(min, 2) + .arg(sec, 5, 'f',1); + } + else { + out << prn << QString(" %1 %2 %3 %4 %5 %6") + .arg(year, 4) + .arg(month, 2, 10, QChar('0')) + .arg(day, 2, 10, QChar('0')) + .arg(hour, 2, 10, QChar('0')) + .arg(min, 2, 10, QChar('0')) + .arg(int(sec), 2, 10, QChar('0')); + } + + return datStr; +} + +// RINEX Format String +////////////////////////////////////////////////////////////////////////////// +QString t_ephGPS::toString(double version) const { + + QString rnxStr = rinexDateStr(_TOC, _prn, version); + + QTextStream out(&rnxStr); + + out << QString("%1%2%3\n") + .arg(_clock_bias, 19, 'e', 12) + .arg(_clock_drift, 19, 'e', 12) + .arg(_clock_driftrate, 19, 'e', 12); + + QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n"; + + out << QString(fmt) + .arg(_IODE, 19, 'e', 12) + .arg(_Crs, 19, 'e', 12) + .arg(_Delta_n, 19, 'e', 12) + .arg(_M0, 19, 'e', 12); + + out << QString(fmt) + .arg(_Cuc, 19, 'e', 12) + .arg(_e, 19, 'e', 12) + .arg(_Cus, 19, 'e', 12) + .arg(_sqrt_A, 19, 'e', 12); + + out << QString(fmt) + .arg(_TOEsec, 19, 'e', 12) + .arg(_Cic, 19, 'e', 12) + .arg(_OMEGA0, 19, 'e', 12) + .arg(_Cis, 19, 'e', 12); + + out << QString(fmt) + .arg(_i0, 19, 'e', 12) + .arg(_Crc, 19, 'e', 12) + .arg(_omega, 19, 'e', 12) + .arg(_OMEGADOT, 19, 'e', 12); + + out << QString(fmt) + .arg(_IDOT, 19, 'e', 12) + .arg(_L2Codes, 19, 'e', 12) + .arg(_TOEweek, 19, 'e', 12) + .arg(_L2PFlag, 19, 'e', 12); + + out << QString(fmt) + .arg(_ura, 19, 'e', 12) + .arg(_health, 19, 'e', 12) + .arg(_TGD, 19, 'e', 12) + .arg(_IODC, 19, 'e', 12); + + out << QString(fmt) + .arg(_TOT, 19, 'e', 12) + .arg(_fitInterval, 19, 'e', 12) + .arg("", 19, QChar(' ')) + .arg("", 19, QChar(' ')); + + return rnxStr; +} + +// RINEX Format String +////////////////////////////////////////////////////////////////////////////// +QString t_ephGlo::toString(double version) const { + + QString rnxStr = rinexDateStr(_TOC-_gps_utc, _prn, version); + + QTextStream out(&rnxStr); + + out << QString("%1%2%3\n") + .arg(-_tau, 19, 'e', 12) + .arg(_gamma, 19, 'e', 12) + .arg(_tki, 19, 'e', 12); + + QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n"; + + out << QString(fmt) + .arg(_x_pos, 19, 'e', 12) + .arg(_x_velocity, 19, 'e', 12) + .arg(_x_acceleration, 19, 'e', 12) + .arg(_health, 19, 'e', 12); + + out << QString(fmt) + .arg(_y_pos, 19, 'e', 12) + .arg(_y_velocity, 19, 'e', 12) + .arg(_y_acceleration, 19, 'e', 12) + .arg(_frequency_number, 19, 'e', 12); + + out << QString(fmt) + .arg(_z_pos, 19, 'e', 12) + .arg(_z_velocity, 19, 'e', 12) + .arg(_z_acceleration, 19, 'e', 12) + .arg(_E, 19, 'e', 12); + + return rnxStr; +} + +// RINEX Format String +////////////////////////////////////////////////////////////////////////////// +QString t_ephGal::toString(double version) const { + + QString rnxStr = rinexDateStr(_TOC, _prn, version); + + QTextStream out(&rnxStr); + + out << QString("%1%2%3\n") + .arg(_clock_bias, 19, 'e', 12) + .arg(_clock_drift, 19, 'e', 12) + .arg(_clock_driftrate, 19, 'e', 12); + + QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n"; + + out << QString(fmt) + .arg(_IODnav, 19, 'e', 12) + .arg(_Crs, 19, 'e', 12) + .arg(_Delta_n, 19, 'e', 12) + .arg(_M0, 19, 'e', 12); + + out << QString(fmt) + .arg(_Cuc, 19, 'e', 12) + .arg(_e, 19, 'e', 12) + .arg(_Cus, 19, 'e', 12) + .arg(_sqrt_A, 19, 'e', 12); + + out << QString(fmt) + .arg(_TOEsec, 19, 'e', 12) + .arg(_Cic, 19, 'e', 12) + .arg(_OMEGA0, 19, 'e', 12) + .arg(_Cis, 19, 'e', 12); + + out << QString(fmt) + .arg(_i0, 19, 'e', 12) + .arg(_Crc, 19, 'e', 12) + .arg(_omega, 19, 'e', 12) + .arg(_OMEGADOT, 19, 'e', 12); + + out << QString(fmt) + .arg(_IDOT, 19, 'e', 12) + .arg("", 19, QChar(' ')) + .arg(_TOEweek, 19, 'e', 12) + .arg("", 19, QChar(' ')); + + out << QString(fmt) + .arg(_SISA, 19, 'e', 12) + .arg(_E5aHS, 19, 'e', 12) + .arg(_BGD_1_5A, 19, 'e', 12) + .arg(_BGD_1_5B, 19, 'e', 12); + + out << QString(fmt) + .arg(_TOT, 19, 'e', 12) + .arg("", 19, QChar(' ')) + .arg("", 19, QChar(' ')) + .arg("", 19, QChar(' ')); + + return rnxStr; +} + Index: trunk/BNC/src/RTCM3/ephemeris.h =================================================================== --- trunk/BNC/src/RTCM3/ephemeris.h (revision 4284) +++ trunk/BNC/src/RTCM3/ephemeris.h (revision 4284) @@ -0,0 +1,231 @@ +#ifndef EPHEMERIS_H +#define EPHEMERIS_H + +#include +#include +#include +#include +#include "bnctime.h" +extern "C" { +#include "rtcm3torinex.h" +} + +class t_eph { + public: + + enum e_type {unknown, GPS, GLONASS, Galileo}; + + t_eph() {_ok = false;} + virtual ~t_eph() {}; + + static bool earlierTime(const t_eph* eph1, const t_eph* eph2) { + return eph1->_TOC < eph2->_TOC; + } + + virtual e_type type() const = 0; + virtual QString toString(double version) const = 0; + virtual void position(int GPSweek, double GPSweeks, + double* xc, double* vv) const = 0; + virtual int IOD() const = 0; + virtual int RTCM3(unsigned char *) = 0; + + bool ok() const {return _ok;} + bncTime TOC() const {return _TOC;} + bool isNewerThan(const t_eph* eph) const { + return earlierTime(eph, this); + } + QString prn() const {return _prn;} + const QDateTime& receptDateTime() const {return _receptDateTime;} + + void position(int GPSweek, double GPSweeks, + double& xx, double& yy, double& zz, double& cc) const { + double tmp_xx[4]; + double tmp_vv[4]; + position(GPSweek, GPSweeks, tmp_xx, tmp_vv); + + xx = tmp_xx[0]; + yy = tmp_xx[1]; + zz = tmp_xx[2]; + cc = tmp_xx[3]; + } + + static QString rinexDateStr(const bncTime& tt, const QString& prn, + double version); + + protected: + QString _prn; + bncTime _TOC; + QDateTime _receptDateTime; + bool _ok; +}; + + +class t_ephGPS : public t_eph { + public: + t_ephGPS() { } + t_ephGPS(float rnxVersion, const QStringList& lines); + virtual ~t_ephGPS() {} + + virtual e_type type() const {return t_eph::GPS;} + + virtual QString toString(double version) const; + + void set(const gpsephemeris* ee); + + virtual void position(int GPSweek, double GPSweeks, + double* xc, + double* vv) const; + + virtual int IOD() const { return static_cast(_IODC); } + + virtual int RTCM3(unsigned char *); + + private: + double _clock_bias; // [s] + double _clock_drift; // [s/s] + double _clock_driftrate; // [s/s^2] + + double _IODE; + double _Crs; // [m] + double _Delta_n; // [rad/s] + double _M0; // [rad] + + double _Cuc; // [rad] + double _e; // + double _Cus; // [rad] + double _sqrt_A; // [m^0.5] + + double _TOEsec; // [s] + double _Cic; // [rad] + double _OMEGA0; // [rad] + double _Cis; // [rad] + + double _i0; // [rad] + double _Crc; // [m] + double _omega; // [rad] + double _OMEGADOT; // [rad/s] + + double _IDOT; // [rad/s] + double _L2Codes; // Codes on L2 channel + double _TOEweek; + double _L2PFlag; // L2 P data flag + + double _ura; // SV accuracy + double _health; // SV health + double _TGD; // [s] + double _IODC; + + double _TOT; // Transmisstion time + double _fitInterval; // Fit interval +}; + +class t_ephGlo : public t_eph { + public: + t_ephGlo() { _xv.ReSize(6); } + t_ephGlo(float rnxVersion, const QStringList& lines); + + virtual ~t_ephGlo() {} + + virtual e_type type() const {return t_eph::GLONASS;} + + virtual QString toString(double version) const; + + virtual void position(int GPSweek, double GPSweeks, + double* xc, + double* vv) const; + + virtual int IOD() const; + + virtual int RTCM3(unsigned char *); + + void set(const glonassephemeris* ee); + + int slotNum() const {return int(_frequency_number);} + + private: + static ColumnVector glo_deriv(double /* tt */, const ColumnVector& xv, + double* acc); + + mutable bncTime _tt; // time + mutable ColumnVector _xv; // status vector (position, velocity) at time _tt + + double _gps_utc; + double _tau; // [s] + double _gamma; // + double _tki; // message frame time + + double _x_pos; // [km] + double _x_velocity; // [km/s] + double _x_acceleration; // [km/s^2] + double _health; // 0 = O.K. + + double _y_pos; // [km] + double _y_velocity; // [km/s] + double _y_acceleration; // [km/s^2] + double _frequency_number; // ICD-GLONASS data position + + double _z_pos; // [km] + double _z_velocity; // [km/s] + double _z_acceleration; // [km/s^2] + double _E; // Age of Information [days] +}; + +class t_ephGal : public t_eph { + public: + t_ephGal() { } + t_ephGal(float rnxVersion, const QStringList& lines); + virtual ~t_ephGal() {} + + virtual QString toString(double version) const; + + virtual e_type type() const {return t_eph::Galileo;} + + void set(const galileoephemeris* ee); + + virtual void position(int GPSweek, double GPSweeks, + double* xc, + double* vv) const; + + virtual int IOD() const { return static_cast(_IODnav); } + + virtual int RTCM3(unsigned char *); + + private: + double _clock_bias; // [s] + double _clock_drift; // [s/s] + double _clock_driftrate; // [s/s^2] + + double _IODnav; + double _Crs; // [m] + double _Delta_n; // [rad/s] + double _M0; // [rad] + + double _Cuc; // [rad] + double _e; // + double _Cus; // [rad] + double _sqrt_A; // [m^0.5] + + double _TOEsec; // [s] + double _Cic; // [rad] + double _OMEGA0; // [rad] + double _Cis; // [rad] + + double _i0; // [rad] + double _Crc; // [m] + double _omega; // [rad] + double _OMEGADOT; // [rad/s] + + double _IDOT; // [rad/s] + // + double _TOEweek; + // spare + + double _SISA; // Signal In Space Accuracy + double _E5aHS; // E5a Health Status + double _BGD_1_5A; // group delay [s] + double _BGD_1_5B; // group delay [s] + + double _TOT; // [s] +}; + +#endif Index: trunk/BNC/src/RTCM3/timeutils.cpp =================================================================== --- trunk/BNC/src/RTCM3/timeutils.cpp (revision 4284) +++ trunk/BNC/src/RTCM3/timeutils.cpp (revision 4284) @@ -0,0 +1,133 @@ +/* ----------------------------------------------------------------------------- + * + * Function : djul + * + * Purpose : computes the modified julian date (mjd) from + * year, month and day + * + * Author : Z. Lukes + * + * Created : 13-OCT-2001 + * + * Changes : + * + * ---------------------------------------------------------------------------*/ + +#include + +#ifndef NO_CVS_HEADER +static const char *const cvsid = "$Header: /home/cvs/cvsroot/gpss_src/cpp/src/common/rtgnss/timeutils.cpp,v 1.1 2007/04/02 16:30:26 cvs Exp $"; +#endif + +double djul(long jj, long mm, double tt) { + long ii, kk; + double djul ; + + if( mm <= 2 ) { + jj = jj - 1; + mm = mm + 12; + } + + ii = jj/100; + kk = 2 - ii + ii/4; + djul = (365.25*jj - fmod( 365.25*jj, 1.0 )) - 679006.0; + djul = djul + floor( 30.6001*(mm + 1) ) + tt + kk; + return djul; +} + +/* ----------------------------------------------------------------------------- + * + * Function : gpjd + * + * Purpose : computes the modified julian date (mjd) from + * gpsweek number and number of seconds past last + * saturday/sunday midnight + * + * Author : Z. Lukes + * + * Created : 13-OCT-2001 + * + * Changes : + * + * ---------------------------------------------------------------------------*/ + +double gpjd(double second, int nweek) { + double deltat; + + // days since starting epoch of gps weeks (sunday 06-jan-80) + + deltat = nweek*7.0 + second/86400.0 ; + + // mod. julian date + + return( 44244.0 + deltat) ; +} + +/* ----------------------------------------------------------------------------- + * + * Function : jdgp + * + * Purpose : compute number of seconds past midnight of last + * saturday/sunday and gps week number of current + * date given in modified julian date + * + * Author : Z. Lukes + * + * Created : 13-OCT-2001 + * + * Changes : + * + * ---------------------------------------------------------------------------*/ + +void jdgp(double tjul, double & second, long & nweek) { + double deltat; + + deltat = tjul - 44244.0 ; + + // current gps week + + nweek = (long) floor(deltat/7.0); + + // seconds past midnight of last weekend + + second = (deltat - (nweek)*7.0)*86400.0; + +} + +/* ----------------------------------------------------------------------------- + * + * Function : djul + * + * Purpose : compute year,month,day of month from + * modified julian date (mjd=jul. date-2400000.5) + * + * Author : Z. Lukes + * + * Created : 13-OCT-2001 + * + * Changes : + * + * ---------------------------------------------------------------------------*/ + +void jmt(double djul, long& jj, long& mm, double& dd) { + long ih, ih1, ih2 ; + double t1, t2, t3, t4; + + t1 = 1.0 + djul - fmod( djul, 1.0 ) + 2400000.0; + t4 = fmod( djul, 1.0 ); + ih = long( (t1 - 1867216.25)/36524.25 ); + t2 = t1 + 1 + ih - ih/4; + t3 = t2 - 1720995.0; + ih1 = long( (t3 - 122.1)/365.25 ); + t1 = 365.25*ih1 - fmod( 365.25*ih1, 1.0 ); + ih2 = long( (t3 - t1)/30.6001 ); + dd = t3 - t1 - (int)( 30.6001*ih2 ) + t4; + mm = ih2 - 1; + + if ( ih2 > 13 ) mm = ih2 - 13; + + jj = ih1; + + if ( mm <= 2 ) jj = jj + 1; + +} Index: trunk/BNC/src/RTCM3/timeutils.h =================================================================== --- trunk/BNC/src/RTCM3/timeutils.h (revision 4284) +++ trunk/BNC/src/RTCM3/timeutils.h (revision 4284) @@ -0,0 +1,9 @@ +#ifndef TIMEUTILS_H +#define TIMEUTILS_H + +double djul(long j1, long m1, double tt); +double gpjd(double second, int nweek) ; +void jdgp(double tjul, double & second, long & nweek); +void jmt (double djul, long& jj, long& mm, double& dd); + +#endif