committed for backup purposes
bert hubert
parent
ec9f934aea
commit
661a7e0e23
|
|
@ -0,0 +1,30 @@
|
|||
#include "beidou.hh"
|
||||
#include "bits.hh"
|
||||
|
||||
std::basic_string<uint8_t> getCondensedBeidouMessage(std::basic_string_view<uint8_t> 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<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);
|
||||
}
|
||||
|
|
@ -0,0 +1,165 @@
|
|||
#pragma once
|
||||
#include <string>
|
||||
#include <stdint.h>
|
||||
#include "bits.hh"
|
||||
#include <math.h>
|
||||
std::basic_string<uint8_t> getCondensedBeidouMessage(std::basic_string_view<uint8_t> payload);
|
||||
int beidouBitconv(int their);
|
||||
|
||||
|
||||
struct BeidouMessage
|
||||
{
|
||||
uint8_t strtype;
|
||||
|
||||
std::basic_string_view<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> cond)
|
||||
{
|
||||
return bbitu(44, 7);
|
||||
}
|
||||
|
||||
int a0gps, a1gps, a0gal, a1gal, a0glo, a1glo, a0utc, a1utc;
|
||||
int8_t deltaTLS;
|
||||
|
||||
int parse5(std::basic_string_view<uint8_t> 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;
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
#include "gps.hh"
|
||||
|
||||
// this strips out spare bits + parity, and leaves 10 clean 24 bit words
|
||||
std::basic_string<uint8_t> getGlonassMessage(std::basic_string_view<uint8_t> 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<uint8_t>(buffer, 16);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,121 @@
|
|||
#pragma once
|
||||
#include <bitset>
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include "bits.hh"
|
||||
#include <iostream>
|
||||
std::basic_string<uint8_t> getGlonassessage(std::basic_string_view<uint8_t> payload);
|
||||
|
||||
struct GlonassMessage
|
||||
{
|
||||
uint8_t strtype;
|
||||
|
||||
int parse(std::basic_string_view<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> gstr)
|
||||
{
|
||||
n4=getbitu(&gstr[0], 85-36, 5);
|
||||
taugps = getbitsglonass(&gstr[0], 85-31, 22);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
Loading…
Reference in New Issue