diff --git a/navmon.proto b/navmon.proto index c8da44e..95f934e 100644 --- a/navmon.proto +++ b/navmon.proto @@ -35,7 +35,12 @@ message NavMonMessage { required uint32 gnssID =3; required uint32 gnssSV =4; required bytes contents =5; - optional uint32 sigid = 6; + optional uint32 sigid = 6; + optional bytes reserved1 = 7; + optional bytes reserved2 = 8; + optional bytes sar = 9; + optional bytes spare = 10; + optional bytes crc = 11; } message GalileoFnav { diff --git a/ubx.cc b/ubx.cc index 35d02dd..387d4bd 100644 --- a/ubx.cc +++ b/ubx.cc @@ -3,6 +3,7 @@ #include "fmt/format.h" #include "fmt/printf.h" #include "bits.hh" +#include "navmon.hh" using namespace std; uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, std::basic_string_view str) @@ -60,7 +61,12 @@ std::basic_string buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, st return msg; } -basic_string getInavFromSFRBXMsg(std::basic_string_view msg) +basic_string getInavFromSFRBXMsg(std::basic_string_view msg, + basic_string& reserved1, + basic_string& reserved2, + basic_string& sar, + basic_string& spare, + basic_string& crc) { // byte order adjustment std::basic_string payload; @@ -74,10 +80,14 @@ basic_string getInavFromSFRBXMsg(std::basic_string_view msg) for (i=0,j= 4;i<15;i++,j+=8) setbitu(crc_buff,j,8,getbitu(payload.c_str() ,i*8,8)); for (i=0,j=118;i<11;i++,j+=8) setbitu(crc_buff,j,8,getbitu(payload.c_str()+16,i*8,8)); if (rtk_crc24q(crc_buff,25) != getbitu(payload.c_str()+16,82,24)) { - cout << "CRC mismatch, " << rtk_crc24q(crc_buff, 25) << " != " << getbitu(payload.c_str()+16,82,24) < buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, st std::basic_string buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, const std::initializer_list& str); -std::basic_string getInavFromSFRBXMsg(std::basic_string_view msg); +std::basic_string getInavFromSFRBXMsg(std::basic_string_view msg, + std::basic_string& reserved1, + std::basic_string& reserved2, + std::basic_string& sar, + std::basic_string& spare, + std::basic_string& crc); + std::basic_string getGPSFromSFRBXMsg(std::basic_string_view msg); std::basic_string getGlonassFromSFRBXMsg(std::basic_string_view msg); std::basic_string getBeidouFromSFRBXMsg(std::basic_string_view msg); diff --git a/ubxtool.cc b/ubxtool.cc index a82cd39..ceab36f 100644 --- a/ubxtool.cc +++ b/ubxtool.cc @@ -927,7 +927,7 @@ int main(int argc, char** argv) enableUBXMessageOnPort(fd, 0x02, 0x59, ubxport); // UBX-RXM-RLM } - if (doDEBUG) { cerr< reserved1, reserved2, sar, spare, crc; + auto inav = getInavFromSFRBXMsg(payload, reserved1, reserved2, sar, spare, crc); unsigned int wtype = getbitu(&inav[0], 0, 6); uint32_t satTOW; @@ -1466,6 +1467,11 @@ int main(int argc, char** argv) nmm.mutable_gi()->set_gnsssv(id.second); nmm.mutable_gi()->set_sigid(sigid); nmm.mutable_gi()->set_contents((const char*)&inav[0], inav.size()); + nmm.mutable_gi()->set_reserved1((const char*)&reserved1[0], reserved1.size()); + nmm.mutable_gi()->set_reserved2((const char*)&reserved2[0], reserved2.size()); + nmm.mutable_gi()->set_sar((const char*) &sar[0], sar.size()); + nmm.mutable_gi()->set_crc((const char*) &crc[0], crc.size()); + nmm.mutable_gi()->set_spare((const char*)&spare[0], spare.size()); ns.emitNMM( nmm); }