127 lines
3.3 KiB
C++
127 lines
3.3 KiB
C++
#include "beidou.hh"
|
|
#include "bits.hh"
|
|
#include <iostream>
|
|
|
|
using namespace std;
|
|
|
|
// with immense gratitude to https://stackoverflow.com/questions/24612436/how-to-check-a-bch15-11-1-code-checksum-for-bds-beidou-satellite-system
|
|
|
|
static int checkbds(int bits)
|
|
{
|
|
static int const at[15] = {1, 2, 4, 8, 3, 6, 12, 11, 5, 10, 7, 14, 15, 13, 9};
|
|
int s, i, j;
|
|
|
|
for(i = 1; i <= 2; i++)
|
|
{
|
|
s = 0;
|
|
for(j = 0; j < 15; j++)
|
|
{
|
|
if(bits & (1<<j))
|
|
{
|
|
s ^= at[(i * j) % 15];
|
|
}
|
|
}
|
|
if(s != 0)
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
std::basic_string<uint8_t> getCondensedBeidouMessage(std::basic_string_view<uint8_t> payload)
|
|
{
|
|
|
|
// payload consists of 32 bit words where we have to ignore the first 2 bits of every word
|
|
|
|
int chunk = getbitu(&payload[0], 17, 15);
|
|
// cout <<"w0 checkbds(chunk0): "<<checkbds(chunk) << endl;
|
|
if(!checkbds(chunk))
|
|
throw std::runtime_error("Beidou checksum error chunk0");
|
|
|
|
uint8_t buffer[(26+9*22)/8];
|
|
|
|
setbitu(buffer, 0, 26, getbitu(&payload[0], 2, 26));
|
|
|
|
for(int w = 1 ; w < 10; ++w) {
|
|
int chunk1 = getbitu(&payload[0], 2+w*32, 11);
|
|
int chunk2 = getbitu(&payload[0], 2+w*32+11, 11);
|
|
int parity1 = getbitu(&payload[0], 2+w*32+22, 4);
|
|
int parity2 = getbitu(&payload[0], 2+w*32+22+4, 4);
|
|
chunk1 = (chunk1<<4) | parity1;
|
|
chunk2 = (chunk2<<4) | parity2;
|
|
//cout <<"w"<<w<<" checkbds(chunk1): "<<checkbds(chunk1) << ", checkbds(chunk2): "<<checkbds(chunk2)<<endl;
|
|
if(!checkbds(chunk1) || !checkbds(chunk2))
|
|
throw std::runtime_error("Beidou checksum error");
|
|
setbitu(buffer, 26+22*(w-1), 22, getbitu(&payload[0], 2 + w*32, 22));
|
|
}
|
|
|
|
return std::basic_string<uint8_t>(buffer, 28);
|
|
}
|
|
|
|
|
|
// the BeiDou ICD lists bits from bit 1
|
|
// In addition, they count the parity bits which we strip out
|
|
// this function converts their bit numbers to ours
|
|
// the first word has 4 parity bits, the rest has 8
|
|
int beidouBitconv(int their)
|
|
{
|
|
int wordcount = their/30;
|
|
|
|
if(!wordcount)
|
|
return their - 1;
|
|
|
|
return their - (1 + 4 + (wordcount -1)*8);
|
|
}
|
|
|
|
|
|
bool processBeidouAlmanac(const BeidouMessage& bm, struct BeidouAlmanacEntry& bae)
|
|
{
|
|
bae.alma = bm.alma;
|
|
int pageno = bm.alma.pageno;
|
|
|
|
if(bm.fraid == 5 && pageno >= 11 && pageno <= 23) {
|
|
/*
|
|
cout<<" AmEpID "<< bm.alma.AmEpID;
|
|
cout << " AmID "<<bm.alma.AmID; */
|
|
if(bm.alma.AmEpID != 3) {
|
|
// cout<<" skipping page because AmEpID tells us to "<<endl;
|
|
return false;
|
|
}
|
|
}
|
|
if(ephAge(bm.sow, bm.alma.getT0e()) < 0) {
|
|
// cout <<" almanac too old t0a "<<bm.alma.getT0e()<<" sow "<<bm.sow << " days " << 1.0*(bm.alma.getT0e()-bm.sow)/86400<< endl;
|
|
return false;
|
|
}
|
|
if(bm.alma.sqrtA == 0) {
|
|
// cout<<"sqrtA==0 - likely not a present satellite "<<endl;
|
|
return false;
|
|
}
|
|
|
|
Point sat;
|
|
if(bm.fraid ==4 && pageno <= 5)
|
|
bae.alma.geo=true;
|
|
else
|
|
bae.alma.geo=false;
|
|
int offset = 0;
|
|
if(bm.fraid == 4)
|
|
offset = 0;
|
|
else if(bm.fraid == 5 && pageno <=6)
|
|
offset = 24;
|
|
else if(bm.fraid == 5 && bm.alma.AmID ==1)
|
|
offset = 20;
|
|
else if(bm.fraid == 5 && bm.alma.AmID ==2)
|
|
offset = 34;
|
|
else if(bm.fraid == 5 && bm.alma.AmID ==3)
|
|
offset = 46;
|
|
|
|
bae.sv = pageno+offset;
|
|
// cout<<" eff-id "<<bae.sv;
|
|
if(bae.sv == 59)
|
|
bae.alma.geo = true;
|
|
return true;
|
|
}
|
|
|
|
|
|
|