fix CNAV parsing for HAS
parent
4860006fdc
commit
8ddeff2a6b
24
septool.cc
24
septool.cc
|
@ -392,10 +392,11 @@ try
|
||||||
uint8_t src;
|
uint8_t src;
|
||||||
uint8_t ign1;
|
uint8_t ign1;
|
||||||
uint8_t rxChannel;
|
uint8_t rxChannel;
|
||||||
uint8_t navBits[48];
|
uint8_t navBits[64];
|
||||||
} __attribute__((packed));
|
} __attribute__((packed));
|
||||||
|
|
||||||
SEPCnav sc;
|
SEPCnav sc;
|
||||||
|
|
||||||
auto str = res.first.getPayload();
|
auto str = res.first.getPayload();
|
||||||
memcpy(&sc, str.c_str(), sizeof(sc));
|
memcpy(&sc, str.c_str(), sizeof(sc));
|
||||||
int sigid = sc.src & 31;
|
int sigid = sc.src & 31;
|
||||||
|
@ -405,17 +406,32 @@ try
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::string cnav((char*)sc.navBits, 48);
|
std::string cnav((char*)sc.navBits, 64);
|
||||||
// byte order adjustment
|
// byte order adjustment
|
||||||
std::basic_string<uint8_t> payload;
|
std::basic_string<uint8_t> payload;
|
||||||
|
|
||||||
for(unsigned int i = 0 ; i < 12; ++i) {
|
for(unsigned int i = 0 ; i < 16; ++i) {
|
||||||
payload.append(1, sc.navBits[4 * i + 3]);
|
payload.append(1, sc.navBits[4 * i + 3]);
|
||||||
payload.append(1, sc.navBits[4 * i + 2]);
|
payload.append(1, sc.navBits[4 * i + 2]);
|
||||||
payload.append(1, sc.navBits[4 * i + 1]);
|
payload.append(1, sc.navBits[4 * i + 1]);
|
||||||
payload.append(1, sc.navBits[4 * i + 0]);
|
payload.append(1, sc.navBits[4 * i + 0]);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
unsigned char crc_buff[58]={0};
|
||||||
|
unsigned int i;
|
||||||
|
for (i=0; i< 462;i++)
|
||||||
|
setbitu(crc_buff, 2+i, 1,getbitu(payload.c_str(),i, 1));
|
||||||
|
|
||||||
|
int calccrc=rtk_crc24q(crc_buff,58);
|
||||||
|
int realcrc= getbitu(payload.c_str(), 14+448, 24);
|
||||||
|
if (calccrc != realcrc) {
|
||||||
|
cerr << "CRC mismatch, " << calccrc << " != " << realcrc <<endl;
|
||||||
|
}
|
||||||
|
// else
|
||||||
|
// cerr<<"Checksum Correct: "<<calccrc << " = " << realcrc<<endl;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
NavMonMessage nmm;
|
NavMonMessage nmm;
|
||||||
double t = utcFromGST(sc.wn - 1024, sc.towMsec / 1000.0);
|
double t = utcFromGST(sc.wn - 1024, sc.towMsec / 1000.0);
|
||||||
// cerr<<t<< " " <<si.wn - 1024 <<" " <<si.towMsec /1000.0 <<" " << g_dtLS<<endl;
|
// cerr<<t<< " " <<si.wn - 1024 <<" " <<si.towMsec /1000.0 <<" " << g_dtLS<<endl;
|
||||||
|
|
Loading…
Reference in New Issue