172 lines
4.4 KiB
C++
172 lines
4.4 KiB
C++
#include "rtcm.hh"
|
||
#include "bits.hh"
|
||
#include <vector>
|
||
#include <iostream>
|
||
#include "nmmsender.hh"
|
||
#include "CLI/CLI.hpp"
|
||
#include "swrappers.hh"
|
||
#include "sclasses.hh"
|
||
#include "version.hh"
|
||
|
||
using namespace std;
|
||
|
||
bool RTCMReader::get(RTCMFrame& rf)
|
||
{
|
||
int c;
|
||
bool skipped=false;
|
||
while( ((c=fgetc(d_fp)) != -1) && c != 211) {
|
||
skipped=true;
|
||
cerr<<".";
|
||
continue;
|
||
}
|
||
if(skipped)
|
||
cerr<<endl;
|
||
|
||
if(c != 211) {
|
||
cerr<<"EOF"<<endl;
|
||
return false;
|
||
}
|
||
// cout<<"Found preamble"<<endl;
|
||
unsigned char buffer[2];
|
||
if(fread((char*)buffer, 1, 2, d_fp) != 2)
|
||
return false;
|
||
|
||
// cout<<"Got two byte buffer"<<endl;
|
||
// 6 bits reserved, 10 bits of size
|
||
int size = getbitu(buffer, 6, 10);
|
||
size += 3;
|
||
// cout<<"Now reading "<<size<<" bytes"<<endl;
|
||
vector<char> buf(size);
|
||
if((int)fread(&buf[0], 1, size, d_fp) != size)
|
||
return false;
|
||
|
||
// cout<<"Returning true"<<endl;
|
||
rf.payload.assign(&buf[0], size - 3);
|
||
return true;
|
||
}
|
||
|
||
|
||
/*
|
||
Message types:
|
||
Type 1240, Galileo orbit corrections to Broadcast Ephemeris
|
||
Type 1241, Galileo clock corrections to Broadcast Ephemeris
|
||
1057 message for GPS orbit corrections to Broadcast Ephemeris,
|
||
1058 message for GPS clock corrections to Broadcast Ephemeris,
|
||
*/
|
||
|
||
|
||
/* 1057 (GPS), 1240 (Galileo)
|
||
0 12 bits message number
|
||
12 20 bits galileo SOW
|
||
32 4 bits SSR update interval
|
||
36 1 bit multiple message indicator
|
||
37 1 bit Satellite Reference Datum (0: ITRF, 1: regional)
|
||
38 4 bit (IOD SSR)
|
||
42 16 bits SSR provider ID
|
||
58 4 bits SSR solution ID
|
||
62 6 bits number of satellites
|
||
68
|
||
Repeat, starting from pos 68:
|
||
0 6 bits sv
|
||
6 10 bits IODE nav / 8 for GPS, a shift of -2 everywhere below
|
||
16 22 bits delta radial (0.1mm)
|
||
38 20 bits along track (0.4mm)
|
||
58 20 bits cross track (0.4mm)
|
||
78 21 bits dot delta radial (0.001 mm/s)
|
||
99 19 bits dot delta along (0.004 mm/s)
|
||
118 19 bits dot delta cross-track (0.004 mm/s)
|
||
137
|
||
*/
|
||
|
||
|
||
/*
|
||
1058 (GPS), 1241 (Galileo)
|
||
0 12 bits message number
|
||
12 20 bits galileo SOW (GPS Epoch time)
|
||
32 4 bits "UDI" ("SSR Update interval")
|
||
36 1 bit "sync" ("multiple message indicator")
|
||
37 4 bit (IOD SSR, link to 1240 I think)
|
||
41 16 bits SSR provider ID
|
||
57 4 bits SSR solution ID
|
||
61 6 bits number of satellites
|
||
67
|
||
|
||
67
|
||
Repeat, starting from pos 68:
|
||
0 6 bits sv
|
||
6 22 bits dclk[0] 1e-4 meter // 0.1 mm
|
||
28 21 bits dclk[1] 1e-6 meter/s // 0.001 mm/s
|
||
49 27 bits dclk[2] 2e-8 meter // 0.0002mm/s^2
|
||
76
|
||
|
||
Reference time is Epoch Time + 0.5* SSR update interval, which can be zero.
|
||
*/
|
||
|
||
/*
|
||
CLKA[0,1]_DEU1 – containing the SSR corrections regarding the satellites’ Antenna Phase Center
|
||
CLKC[0,1]_DEU1 – containing the SSR corrections regarding the satellites’ Center of Mass.
|
||
*/
|
||
|
||
static char program[]="rtcmtool";
|
||
uint16_t g_srcid{0};
|
||
|
||
int main(int argc, char** argv)
|
||
{
|
||
// time_t starttime=time(0);
|
||
GOOGLE_PROTOBUF_VERIFY_VERSION;
|
||
vector<string> destinations;
|
||
|
||
bool doVERSION{false}, doSTDOUT{false};
|
||
CLI::App app(program);
|
||
app.add_option("--destination,-d", destinations, "Send output to this IPv4/v6 address");
|
||
app.add_option("--station", g_srcid, "Station id")->required();
|
||
app.add_flag("--version", doVERSION, "show program version and copyright");
|
||
app.add_flag("--stdout", doSTDOUT, "Emit output to stdout");
|
||
try {
|
||
app.parse(argc, argv);
|
||
} catch(const CLI::Error &e) {
|
||
return app.exit(e);
|
||
}
|
||
|
||
if(doVERSION) {
|
||
showVersion(program, g_gitHash);
|
||
exit(0);
|
||
}
|
||
|
||
|
||
signal(SIGPIPE, SIG_IGN);
|
||
NMMSender ns;
|
||
ns.d_debug = true;
|
||
for(const auto& s : destinations) {
|
||
auto res=resolveName(s, true, true);
|
||
if(res.empty()) {
|
||
cerr<<"Unable to resolve '"<<s<<"' as destination for data, exiting"<<endl;
|
||
exit(EXIT_FAILURE);
|
||
}
|
||
ns.addDestination(s); // ComboAddress(s, 29603));
|
||
}
|
||
if(doSTDOUT)
|
||
ns.addDestination(1);
|
||
|
||
ns.launch();
|
||
|
||
RTCMReader rr(0);
|
||
RTCMFrame rf;
|
||
cerr<<"Station "<<g_srcid<<endl;
|
||
while(rr.get(rf)) {
|
||
// cerr<<"Got a "<<rf.payload.size()<<" byte frame"<<endl;
|
||
RTCMMessage rm;
|
||
NavMonMessage nmm;
|
||
struct timespec ts;
|
||
clock_gettime(CLOCK_REALTIME, & ts);
|
||
nmm.set_type(NavMonMessage::RTCMMessageType);
|
||
nmm.set_localutcseconds(ts.tv_sec);
|
||
nmm.set_localutcnanoseconds(ts.tv_nsec);
|
||
nmm.set_sourceid(g_srcid);
|
||
nmm.mutable_rm()->set_contents(rf.payload);
|
||
ns.emitNMM(nmm);
|
||
|
||
// rm.parse(rf.payload);
|
||
}
|
||
}
|