tool inadvertently did not send 0 words
parent
d047360d8b
commit
11b82c858f
170
ubxtool.cc
170
ubxtool.cc
|
@ -24,6 +24,8 @@ struct timespec g_gstutc;
|
||||||
uint16_t g_wn;
|
uint16_t g_wn;
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
|
uint16_t g_srcid{2};
|
||||||
|
|
||||||
#define BAUDRATE B921600
|
#define BAUDRATE B921600
|
||||||
#define MODEMDEVICE "/dev/ttyACM0"
|
#define MODEMDEVICE "/dev/ttyACM0"
|
||||||
|
|
||||||
|
@ -427,7 +429,7 @@ int main(int argc, char** argv)
|
||||||
nmm.set_type(NavMonMessage::RFDataType);
|
nmm.set_type(NavMonMessage::RFDataType);
|
||||||
nmm.set_localutcseconds(g_gstutc.tv_sec);
|
nmm.set_localutcseconds(g_gstutc.tv_sec);
|
||||||
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
|
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
|
||||||
nmm.set_sourceid(1); // XXX source id
|
nmm.set_sourceid(g_srcid); // XXX source id
|
||||||
|
|
||||||
nmm.mutable_rfd()->set_gnssid(gnssid);
|
nmm.mutable_rfd()->set_gnssid(gnssid);
|
||||||
nmm.mutable_rfd()->set_gnsssv(sv);
|
nmm.mutable_rfd()->set_gnsssv(sv);
|
||||||
|
@ -462,7 +464,7 @@ int main(int argc, char** argv)
|
||||||
nmm.set_type(NavMonMessage::ObserverPositionType);
|
nmm.set_type(NavMonMessage::ObserverPositionType);
|
||||||
nmm.set_localutcseconds(g_gstutc.tv_sec);
|
nmm.set_localutcseconds(g_gstutc.tv_sec);
|
||||||
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
|
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
|
||||||
nmm.set_sourceid(1); // XXX source id
|
nmm.set_sourceid(g_srcid); // XXX source id
|
||||||
nmm.mutable_op()->set_x(p.ecefX /100.0);
|
nmm.mutable_op()->set_x(p.ecefX /100.0);
|
||||||
nmm.mutable_op()->set_y(p.ecefY /100.0);
|
nmm.mutable_op()->set_y(p.ecefY /100.0);
|
||||||
nmm.mutable_op()->set_z(p.ecefZ /100.0);
|
nmm.mutable_op()->set_z(p.ecefZ /100.0);
|
||||||
|
@ -470,102 +472,102 @@ int main(int argc, char** argv)
|
||||||
emitNMM(1, nmm);
|
emitNMM(1, nmm);
|
||||||
}
|
}
|
||||||
else if(msg.getClass() == 2 && msg.getType() == 0x13) { // SFRBX
|
else if(msg.getClass() == 2 && msg.getType() == 0x13) { // SFRBX
|
||||||
try {
|
try {
|
||||||
pair<int,int> id = make_pair(payload[0], payload[1]);
|
pair<int,int> id = make_pair(payload[0], payload[1]);
|
||||||
if(id.first != 2)
|
if(id.first != 2)
|
||||||
continue; // not Galileo
|
continue; // not Galileo
|
||||||
auto inav = getInavFromSFRBXMsg(payload);
|
auto inav = getInavFromSFRBXMsg(payload);
|
||||||
unsigned int wtype = getbitu(&inav[0], 0, 6);
|
unsigned int wtype = getbitu(&inav[0], 0, 6);
|
||||||
tv[id] = timestamp;
|
tv[id] = timestamp;
|
||||||
|
|
||||||
cerr<<"gnssid "<<id.first<<" sv "<<id.second<<" " << wtype << endl;
|
cerr<<"gnssid "<<id.first<<" sv "<<id.second<<" " << wtype << endl;
|
||||||
uint32_t satTOW;
|
uint32_t satTOW;
|
||||||
int msgTOW{0};
|
int msgTOW{0};
|
||||||
if(getTOWFromInav(inav, &satTOW, &g_wn)) {
|
if(getTOWFromInav(inav, &satTOW, &g_wn)) {
|
||||||
cerr<<" "<<wtype<<" sv "<<id.second<<" tow "<<satTOW << " % 30 = "<< satTOW % 30<<", implied start of cycle: "<<(satTOW - (satTOW %30)) <<endl;
|
cerr<<" "<<wtype<<" sv "<<id.second<<" tow "<<satTOW << " % 30 = "<< satTOW % 30<<", implied start of cycle: "<<(satTOW - (satTOW %30)) <<endl;
|
||||||
if(curCycleTOW != satTOW - (satTOW %30))
|
if(curCycleTOW != satTOW - (satTOW %30))
|
||||||
curCycleTOWFresh=false;
|
curCycleTOWFresh=false;
|
||||||
curCycleTOW = satTOW - (satTOW %30);
|
msgTOW = satTOW;
|
||||||
nextCycleTOW = curCycleTOW + 30;
|
curCycleTOW = satTOW - (satTOW %30);
|
||||||
}
|
nextCycleTOW = curCycleTOW + 30;
|
||||||
else {
|
|
||||||
cerr<<" "<<wtype<<" sv "<<id.second<<" tow ";
|
|
||||||
if(wtype == 2) {
|
|
||||||
cerr<<"infered to be 1 "<<nextCycleTOW + 1<<endl;
|
|
||||||
curCycleTOWFresh=false;
|
|
||||||
msgTOW = nextCycleTOW + 1;
|
|
||||||
}
|
}
|
||||||
else if(wtype == 4) {
|
else {
|
||||||
cerr<<"infered to be 3 "<<nextCycleTOW + 3<<endl;
|
cerr<<" "<<wtype<<" sv "<<id.second<<" tow ";
|
||||||
msgTOW = nextCycleTOW + 3;
|
if(wtype == 2) {
|
||||||
curCycleTOWFresh=false;
|
cerr<<"infered to be 1 "<<nextCycleTOW + 1<<endl;
|
||||||
} // next have '6' which sets TOW
|
curCycleTOWFresh=false;
|
||||||
else if(wtype==7 || wtype == 9) {
|
msgTOW = nextCycleTOW + 1;
|
||||||
if(curCycleTOWFresh) {
|
|
||||||
cerr<<"infered to be 7 "<< curCycleTOW + 7<<endl;
|
|
||||||
msgTOW = curCycleTOW + 7;
|
|
||||||
}
|
}
|
||||||
else {
|
else if(wtype == 4) {
|
||||||
cerr<<"infered to be 7 "<< nextCycleTOW + 7<<endl;
|
cerr<<"infered to be 3 "<<nextCycleTOW + 3<<endl;
|
||||||
msgTOW = nextCycleTOW + 7;
|
msgTOW = nextCycleTOW + 3;
|
||||||
|
curCycleTOWFresh=false;
|
||||||
|
} // next have '6' which sets TOW
|
||||||
|
else if(wtype==7 || wtype == 9) {
|
||||||
|
if(curCycleTOWFresh) {
|
||||||
|
cerr<<"infered to be 7 "<< curCycleTOW + 7<<endl;
|
||||||
|
msgTOW = curCycleTOW + 7;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
cerr<<"infered to be 7 "<< nextCycleTOW + 7<<endl;
|
||||||
|
msgTOW = nextCycleTOW + 7;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(wtype==8 || wtype == 10) {
|
||||||
|
if(curCycleTOWFresh) {
|
||||||
|
cerr<<"infered to be 9 "<< curCycleTOW + 9<<endl;
|
||||||
|
msgTOW = curCycleTOW + 9;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
cerr<<"infered to be 9 "<< nextCycleTOW + 9<<endl;
|
||||||
|
msgTOW = nextCycleTOW + 9;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(wtype==1) {
|
||||||
|
if(curCycleTOWFresh) {
|
||||||
|
cerr<<"infered to be 21 "<< curCycleTOW + 21<<endl;
|
||||||
|
msgTOW = curCycleTOW + 21;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
cerr<<"infered to be 21 "<< nextCycleTOW + 21<<endl;
|
||||||
|
msgTOW = nextCycleTOW + 21;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(wtype==3) {
|
||||||
|
if(curCycleTOWFresh) {
|
||||||
|
msgTOW = curCycleTOW + 23;
|
||||||
|
cerr<<"infered to be 23 "<< curCycleTOW + 23 <<endl;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
cerr<<"infered to be 23 "<< nextCycleTOW + 23 <<endl;
|
||||||
|
msgTOW = nextCycleTOW + 23;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else { // dummy
|
||||||
|
cerr<<"what kind of wtype is this"<<endl;
|
||||||
|
continue;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if(wtype==8 || wtype == 10) {
|
|
||||||
if(curCycleTOWFresh) {
|
|
||||||
cerr<<"infered to be 9 "<< curCycleTOW + 9<<endl;
|
|
||||||
msgTOW = curCycleTOW + 9;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
cerr<<"infered to be 9 "<< nextCycleTOW + 9<<endl;
|
|
||||||
msgTOW = nextCycleTOW + 9;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if(wtype==1) {
|
|
||||||
if(curCycleTOWFresh) {
|
|
||||||
cerr<<"infered to be 21 "<< curCycleTOW + 21<<endl;
|
|
||||||
msgTOW = curCycleTOW + 21;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
cerr<<"infered to be 21 "<< nextCycleTOW + 21<<endl;
|
|
||||||
msgTOW = nextCycleTOW + 21;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if(wtype==3) {
|
|
||||||
if(curCycleTOWFresh) {
|
|
||||||
msgTOW = curCycleTOW + 23;
|
|
||||||
cerr<<"infered to be 23 "<< curCycleTOW + 23 <<endl;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
cerr<<"infered to be 23 "<< nextCycleTOW + 23 <<endl;
|
|
||||||
msgTOW = nextCycleTOW + 23;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else { // dummy
|
|
||||||
cerr<<"what kind of wtype is this"<<endl;
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
NavMonMessage nmm;
|
NavMonMessage nmm;
|
||||||
nmm.set_sourceid(1);
|
nmm.set_sourceid(g_srcid);
|
||||||
nmm.set_type(NavMonMessage::GalileoInavType);
|
nmm.set_type(NavMonMessage::GalileoInavType);
|
||||||
nmm.set_localutcseconds(g_gstutc.tv_sec);
|
nmm.set_localutcseconds(g_gstutc.tv_sec);
|
||||||
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
|
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
|
||||||
|
|
||||||
nmm.mutable_gi()->set_gnsswn(g_wn);
|
nmm.mutable_gi()->set_gnsswn(g_wn);
|
||||||
nmm.mutable_gi()->set_gnsstow(msgTOW);
|
nmm.mutable_gi()->set_gnsstow(msgTOW);
|
||||||
nmm.mutable_gi()->set_gnssid(id.first);
|
nmm.mutable_gi()->set_gnssid(id.first);
|
||||||
nmm.mutable_gi()->set_gnsssv(id.second);
|
nmm.mutable_gi()->set_gnsssv(id.second);
|
||||||
nmm.mutable_gi()->set_contents((const char*)&inav[0], inav.size());
|
nmm.mutable_gi()->set_contents((const char*)&inav[0], inav.size());
|
||||||
|
|
||||||
emitNMM(1, nmm);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
if(0 && lasttv.count(id)) {
|
emitNMM(1, nmm);
|
||||||
fmt::fprintf(stderr, "gnssid %d sv %d wtype %d, %d:%d -> %d:%d, delta=%d\n",
|
|
||||||
payload[0], payload[1], wtype, lasttv[id].tv_sec, lasttv[id].tv_usec, tv[id].tv_sec, tv[id].tv_usec, tv[id].tv_usec - lasttv[id].tv_usec);
|
|
||||||
}
|
if(0 && lasttv.count(id)) {
|
||||||
lasttv[id]=tv[id];
|
fmt::fprintf(stderr, "gnssid %d sv %d wtype %d, %d:%d -> %d:%d, delta=%d\n",
|
||||||
|
payload[0], payload[1], wtype, lasttv[id].tv_sec, lasttv[id].tv_usec, tv[id].tv_sec, tv[id].tv_usec, tv[id].tv_usec - lasttv[id].tv_usec);
|
||||||
|
}
|
||||||
|
lasttv[id]=tv[id];
|
||||||
}
|
}
|
||||||
catch(CRCMismatch& cm) {
|
catch(CRCMismatch& cm) {
|
||||||
cerr<<"Had CRC mismatch!"<<endl;
|
cerr<<"Had CRC mismatch!"<<endl;
|
||||||
|
@ -589,7 +591,7 @@ int main(int argc, char** argv)
|
||||||
auto db = (int)payload[10+12*n];
|
auto db = (int)payload[10+12*n];
|
||||||
|
|
||||||
NavMonMessage nmm;
|
NavMonMessage nmm;
|
||||||
nmm.set_sourceid(1);
|
nmm.set_sourceid(g_srcid);
|
||||||
nmm.set_localutcseconds(g_gstutc.tv_sec);
|
nmm.set_localutcseconds(g_gstutc.tv_sec);
|
||||||
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
|
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue