spacecruft
/
galmon
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

fix CNAV parsing for HAS

pull/151/head
bert hubert 2021-05-25 13:55:17 +02:00
parent 4860006fdc
commit 8ddeff2a6b
1 changed files with 20 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
septool.cc
View File

@ -392,10 +392,11 @@ try
uint8_t src;
uint8_t ign1;
uint8_t rxChannel;
uint8_t navBits[48];
uint8_t navBits[64];
} __attribute__((packed));
SEPCnav sc;
auto str = res.first.getPayload();
memcpy(&sc, str.c_str(), sizeof(sc));
int sigid = sc.src & 31;
@ -405,17 +406,32 @@ try
continue;
}
std::string cnav((char*)sc.navBits, 48);
std::string cnav((char*)sc.navBits, 64);
// 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 + 2]);
payload.append(1, sc.navBits[4 * i + 1]);
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;
double t = utcFromGST(sc.wn - 1024, sc.towMsec / 1000.0);
// cerr<<t<< " " <<si.wn - 1024 <<" " <<si.towMsec /1000.0 <<" " << g_dtLS<<endl;