diff --git a/beidou.cc b/beidou.cc new file mode 100644 index 0000000..cb8f78e --- /dev/null +++ b/beidou.cc @@ -0,0 +1,30 @@ +#include "beidou.hh" +#include "bits.hh" + +std::basic_string getCondensedBeidouMessage(std::basic_string_view payload) +{ + uint8_t buffer[(26+9*22)/8]; + + setbitu(buffer, 0, 26, getbitu(&payload[0], 2, 26)); + + for(int w = 1 ; w < 10; ++w) { + setbitu(buffer, 26+22*(w-1), 22, getbitu(&payload[0], 2 + w*32, 22)); + } + + return std::basic_string(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); +} diff --git a/beidou.hh b/beidou.hh new file mode 100644 index 0000000..d4ae692 --- /dev/null +++ b/beidou.hh @@ -0,0 +1,165 @@ +#pragma once +#include +#include +#include "bits.hh" +#include +std::basic_string getCondensedBeidouMessage(std::basic_string_view payload); +int beidouBitconv(int their); + + +struct BeidouMessage +{ + uint8_t strtype; + + std::basic_string_view g_cond; + int bbitu(int bit, int len) + { + return getbitu(&g_cond[0], beidouBitconv(bit), len); + }; + int bbits(int bit, int len) + { + return getbits(&g_cond[0], beidouBitconv(bit), len); + }; + + + int fraid, sow; // part of every message (thanks!) + + int parse(std::basic_string_view cond, uint8_t* pageno) + { + g_cond = cond; + if(pageno) + *pageno=0; + fraid = bbitu(16, 3); + + sow = bbitu(19, 20); + if(bbitu(1, 11) != 1810) + throw std::runtime_error("BeiDou preamble not 1810: "+std::to_string(bbitu(1,11))); + + if(fraid == 1) { + parse1(cond); + } + else if(fraid == 2) { + parse2(cond); + } + else if(fraid == 3) { + parse3(cond); + } + else if(fraid == 4) { + int tmp = parse4(cond); + if(pageno) *pageno=tmp; + } + else if(fraid == 5) { + int tmp=parse5(cond); + if(pageno) *pageno = tmp; + } + + return fraid; + } + + uint8_t sath1, aodc, urai, aode; + uint16_t t0c; + int wn{-1}, a0, a1, a2; + + void parse1(std::basic_string_view cond) + { + sath1 = bbitu(43,1 ); + aodc = bbitu(31+13, 5); + urai = bbitu(31+12+1+5, 4); + wn = bbitu(61, 13); + t0c = bbitu(74, 17); + a0 = bbits(226, 24); // this should be af0, af1, af2 + a1 = bbits(258, 22); + a2 = bbits(215, 11); + aode = bbitu(288, 5); + } + + int t0eMSB{-1}; + uint32_t sqrtA, e; + int32_t deltan; + int32_t cuc, cus, crc, crs; + int32_t m0; + + + uint32_t getT0e() const { return 8*((t0eMSB << 15) + t0eLSB); } // seconds + double getDeltan()const { return ldexp(deltan *M_PI, -43); } //radians/s + double getSqrtA() const { return ldexp(sqrtA, -19); } + double getE() const { return ldexp(e, -33); } + double getCuc() const { return ldexp(cuc, -31); } // radians + double getCus() const { return ldexp(cus, -31); } // radians + double getCrc() const { return ldexp(crc, -6); } // meters + double getCrs() const { return ldexp(crs, -6); } // meters + double getM0() const { return ldexp(m0 * M_PI, -31); } // radians + + + void parse2(std::basic_string_view cond) + { + deltan = bbits(43, 16); + cuc = bbits(67, 18); + m0 = bbits(93, 32); + e = bbitu(133,32); + cus = bbits(181, 18); + crc = bbits(199, 18); + sqrtA = bbitu(251, 32); + + t0eMSB = bbitu(291, 2); + + + } + + int t0eLSB{-1}; + int i0; + int32_t cic, cis; + int32_t omegadot, Omega0, idot, omega; + + double getMu() const { return 3.986004418 * pow(10.0, 14.0); } // m^3/s^2 + double getOmegaE() const { return 7.2921150 * pow(10, -5); } // rad/s + double getI0() const { return ldexp(i0 * M_PI, -31); } // radians + double getCic() const { return ldexp(cic, -31); } // radians + double getCis() const { return ldexp(cis, -31); } // radians + double getOmegadot() const { return ldexp(omegadot * M_PI, -43); } // radians/s + double getOmega0() const { return ldexp(Omega0 * M_PI, -31); } // radians + double getIdot() const { return ldexp(idot * M_PI, -43); } // radians/s + double getOmega() const { return ldexp(omega * M_PI, -31); } // radians + void parse3(std::basic_string_view cond) + { + t0eLSB = bbitu(43, 15); + i0 = bbits(66, 32); + cic = bbits(106, 18); + omegadot = bbits(132, 24); + cis = bbits(160, 18); + idot = bbits(190, 14); + Omega0 = bbits(212, 32); + omega = bbits(252, 32); + + } + + // 4 is all almanac + int parse4(std::basic_string_view cond) + { + return bbitu(44, 7); + } + + int a0gps, a1gps, a0gal, a1gal, a0glo, a1glo, a0utc, a1utc; + int8_t deltaTLS; + + int parse5(std::basic_string_view cond) + { + int pageno = bbitu(44, 7); + if(pageno == 9) { + a0gps = bbits(97, 14); + a1gps = bbits(111, 16); + a0gal = bbits(135, 14); + a1gal = bbits(157, 16); + a0glo = bbits(181, 14); + a1glo = bbits(195, 16); + } + if(pageno == 10) { + a0utc = bbits(91, 32); + a1utc = bbits(131, 24); + deltaTLS = bbits(31+12+1+7, 8); + } + return pageno; + + } +}; + diff --git a/glonass.cc b/glonass.cc new file mode 100644 index 0000000..878f9da --- /dev/null +++ b/glonass.cc @@ -0,0 +1,16 @@ +#include "gps.hh" + +// this strips out spare bits + parity, and leaves 10 clean 24 bit words +std::basic_string getGlonassMessage(std::basic_string_view payload) +{ + uint8_t buffer[4*4]; + + for(int w = 0 ; w < 4; ++w) { + setbitu(buffer, 32*w, 32, getbitu(&payload[0], w*32, 32)); + } + + return std::basic_string(buffer, 16); + +} + + diff --git a/glonass.hh b/glonass.hh new file mode 100644 index 0000000..c1a788c --- /dev/null +++ b/glonass.hh @@ -0,0 +1,121 @@ +#pragma once +#include +#include +#include +#include "bits.hh" +#include +std::basic_string getGlonassessage(std::basic_string_view payload); + +struct GlonassMessage +{ + uint8_t strtype; + + int parse(std::basic_string_view gstr) + { + strtype = getbitu(&gstr[0], 1, 4); + if(strtype == 1) { + parse1(gstr); + } + else if(strtype == 2) { + parse2(gstr); + } + else if(strtype == 3) { + parse3(gstr); + } + else if(strtype == 4) { + parse4(gstr); + } + else if(strtype == 5) { + parse5(gstr); + } + + return strtype; + } + + /* The GLONASS day starts at 00:00 Moscow time, which is on UTC+3 by definition. + This means midnight is 21:00 UTC the previous day. + Various GLONASS things relate to "the day", so it is important to note which day we are at + */ + uint8_t hour, minute, seconds, P1; + int32_t x, dx, ddx; + + void parse1(std::basic_string_view gstr) + { + hour = getbitu(&gstr[0], 9, 5); + minute = getbitu(&gstr[0], 14, 6); + seconds = 30*getbitu(&gstr[0], 20, 1); + P1 = getbitu(&gstr[0], 85-78, 2); + x=getbitsglonass(&gstr[0], 85-35, 27); // 2^-11 + dx=getbitsglonass(&gstr[0], 85-64, 24); // 2^-20 + ddx=getbitsglonass(&gstr[0], 85-40, 5); // 2^-30 + } + + uint8_t Bn, Tb, P2; + int32_t y, dy, ddy; + + /* The GLONASS ephemeris centered on the "Tb-th" interval, from the start of the Moscow day. + An interval is 15 minutes long, plus a spacer of length described by P1. If P1 is zero, there is no spacer. + */ + + void parse2(std::basic_string_view gstr) + { + Bn = getbitu(&gstr[0], 85-80, 3); // Health bit, only look at MSB, ignore the rest. 0 is ok. + Tb = getbitu(&gstr[0], 85-76, 7); + P2 = getbitu(&gstr[0], 85-77, 1); + + y=getbitsglonass(&gstr[0], 85-35, 27); // 2^-11, in kilometers + dy=getbitsglonass(&gstr[0], 85-64, 24); // 2^-20, in kilometers + ddy=getbitsglonass(&gstr[0], 85-40, 5); // 2^-30, in kilometers + + } + + int32_t z, dz, ddz; + + + bool P, P3; + uint16_t gamman; + void parse3(std::basic_string_view gstr) + { + z = getbitsglonass(&gstr[0], 85-35, 27); // 2^-11 + dz = getbitsglonass(&gstr[0], 85-64, 24); // 2^-20 + ddz = getbitsglonass(&gstr[0], 85-40, 5); // 2^-30 + + P = getbitu(&gstr[0], 85 - 66, 1); + P3 = getbitu(&gstr[0], 85 - 80, 1); + + gamman = getbitu(&gstr[0], 85 - 79, 11); + } + + + /* NT is the 'day number' within the current four-year-plan, which run in blocks from 1996. + Not yet sure if this starts from 0 or not (I guess 1) + */ + + uint16_t NT; + uint8_t FT, En, deltaTaun, M; + uint32_t taun; + bool P4; + + void parse4(std::basic_string_view gstr) + { + NT = getbitu(&gstr[0], 85-26, 11); + FT = getbitu(&gstr[0], 85-33, 4); + M = getbitu(&gstr[0], 85-10, 2); + taun = getbitsglonass(&gstr[0], 85-80, 22); + En = getbitu(&gstr[0], 85-53, 5); + P4 = getbitu(&gstr[0], 85-34, 1); + // missing delta tau n + } + + uint8_t n4; // counting from 1996 ('n4=1'), this is the 4-year plan index we are currently in + uint16_t taugps; + void parse5(std::basic_string_view gstr) + { + n4=getbitu(&gstr[0], 85-36, 5); + taugps = getbitsglonass(&gstr[0], 85-31, 22); + + } + + + +};