mirror of
https://github.com/berthubert/galmon.git
synced 2026-05-15 13:46:55 -04:00
Compare commits
No commits in common. "master" and "neof10n" have entirely different histories.
26 changed files with 270 additions and 308 deletions
|
|
@ -328,7 +328,7 @@ Documents
|
|||
* [GLONASS CDMA](http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD-GLONASS-CDMA-General.-Edition-1.0-2016.pdf)
|
||||
not actually relevant for the CDMA aspects, but has appendices on more
|
||||
precise orbit determinations.
|
||||
* [GPS](https://www.navcen.uscg.gov/sites/default/files/pdf/gps/IS-GPS-200N.pdf)
|
||||
* [GPS](https://www.gps.gov/technical/icwg/IS-GPS-200K.pdf)
|
||||
* [U-blox 8 interface specification](https://www.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_%28UBX-13003221%29_Public.pdf)
|
||||
* [U-blox 9 interface specification](https://www.u-blox.com/sites/default/files/u-blox_ZED-F9P_InterfaceDescription_%28UBX-18010854%29.pdf)
|
||||
* [U-blox 10 interface specification](https://content.u-blox.com/sites/default/files/documents/u-blox-F10-SPG-6.00_InterfaceDescription_UBX-23002975.pdf)
|
||||
|
|
|
|||
|
|
@ -1,8 +1,6 @@
|
|||
#include "beidou.hh"
|
||||
#include "bits.hh"
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include "navmon.hh"
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -31,7 +29,7 @@ static int checkbds(int bits)
|
|||
return 1;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> getCondensedBeidouMessage(const std::vector<uint8_t>& payload)
|
||||
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
|
||||
|
|
@ -58,7 +56,7 @@ std::vector<uint8_t> getCondensedBeidouMessage(const std::vector<uint8_t>& paylo
|
|||
setbitu(buffer, 26+22*(w-1), 22, getbitu(&payload[0], 2 + w*32, 22));
|
||||
}
|
||||
|
||||
return makeVec(buffer, 28);
|
||||
return std::basic_string<uint8_t>(buffer, 28);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
17
beidou.hh
17
beidou.hh
|
|
@ -4,10 +4,9 @@
|
|||
#include "bits.hh"
|
||||
#include <math.h>
|
||||
#include <stdexcept>
|
||||
#include <vector>
|
||||
#include "ephemeris.hh"
|
||||
|
||||
std::vector<uint8_t> getCondensedBeidouMessage(const std::vector<uint8_t>& payload);
|
||||
std::basic_string<uint8_t> getCondensedBeidouMessage(std::basic_string_view<uint8_t> payload);
|
||||
int beidouBitconv(int their);
|
||||
|
||||
/* Geostationary, so D2, so not to be parsed by this parser:
|
||||
|
|
@ -22,7 +21,7 @@ struct BeidouMessage : GPSLikeEphemeris
|
|||
{
|
||||
uint8_t strtype;
|
||||
|
||||
std::vector<uint8_t> g_cond;
|
||||
std::basic_string_view<uint8_t> g_cond;
|
||||
int bbitu(int bit, int len)
|
||||
{
|
||||
return getbitu(&g_cond[0], beidouBitconv(bit), len);
|
||||
|
|
@ -35,7 +34,7 @@ struct BeidouMessage : GPSLikeEphemeris
|
|||
|
||||
int fraid{-1}, sow{-1}; // part of every message (thanks!)
|
||||
|
||||
int parse(const std::vector<uint8_t>& cond, uint8_t* pageno)
|
||||
int parse(std::basic_string_view<uint8_t> cond, uint8_t* pageno)
|
||||
{
|
||||
g_cond = cond;
|
||||
if(pageno)
|
||||
|
|
@ -90,7 +89,7 @@ struct BeidouMessage : GPSLikeEphemeris
|
|||
return {factor * cur, factor * trend};
|
||||
}
|
||||
|
||||
void parse1(const std::vector<uint8_t>& cond)
|
||||
void parse1(std::basic_string_view<uint8_t> cond)
|
||||
{
|
||||
sath1 = bbitu(43,1);
|
||||
aodc = bbitu(31+13, 5);
|
||||
|
|
@ -125,7 +124,7 @@ struct BeidouMessage : GPSLikeEphemeris
|
|||
return -1;
|
||||
}
|
||||
|
||||
void parse2(const std::vector<uint8_t>& cond)
|
||||
void parse2(std::basic_string_view<uint8_t> cond)
|
||||
{
|
||||
deltan = bbits(43, 16);
|
||||
cuc = bbits(67, 18);
|
||||
|
|
@ -152,7 +151,7 @@ struct BeidouMessage : GPSLikeEphemeris
|
|||
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(const std::vector<uint8_t>& cond)
|
||||
void parse3(std::basic_string_view<uint8_t> cond)
|
||||
{
|
||||
t0eLSB = bbitu(43, 15);
|
||||
i0 = bbits(66, 32);
|
||||
|
|
@ -208,7 +207,7 @@ struct BeidouMessage : GPSLikeEphemeris
|
|||
} alma;
|
||||
|
||||
// 4 is all almanac
|
||||
int parse4(const std::vector<uint8_t>& cond)
|
||||
int parse4(std::basic_string_view<uint8_t> cond)
|
||||
{
|
||||
alma.sqrtA = bbitu(51, 24);
|
||||
alma.a1 = bbits(91, 11);
|
||||
|
|
@ -255,7 +254,7 @@ struct BeidouMessage : GPSLikeEphemeris
|
|||
}
|
||||
|
||||
|
||||
int parse5(const std::vector<uint8_t>& cond)
|
||||
int parse5(std::basic_string_view<uint8_t> cond)
|
||||
{
|
||||
alma.pageno = bbitu(44, 7);
|
||||
if(alma.pageno == 9) {
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
#include "galileo.hh"
|
||||
using namespace std;
|
||||
|
||||
void FixHunter::reportInav(const vector<uint8_t>& inav, int32_t gst)
|
||||
void FixHunter::reportInav(const basic_string<uint8_t>& inav, int32_t gst)
|
||||
{
|
||||
int wtype = getbitu(&inav[0], 0, 6);
|
||||
|
||||
|
|
@ -181,7 +181,7 @@ void FixHunter::tryFix(int32_t gst)
|
|||
struct GalileoMessage FixHunter::fillGMFromRS(const std::string& out)
|
||||
{
|
||||
// we need to reconstruct words 1, 2, 3 and 4 and feed them to the parser
|
||||
vector<uint8_t> inav[5];
|
||||
basic_string<uint8_t> inav[5];
|
||||
string inavraw[5];
|
||||
inavraw[4] = out.substr(0, 14);
|
||||
inavraw[3] = out.substr(14, 14);
|
||||
|
|
@ -198,7 +198,7 @@ struct GalileoMessage FixHunter::fillGMFromRS(const std::string& out)
|
|||
for(int n=0; n < 14; ++n)
|
||||
setbitu(tmp, 16 + n*8, 8, getbitu((unsigned char*) inavraw[1].c_str(), 16 + n*8, 8));
|
||||
|
||||
inav[1]= makeVec(tmp, 16);
|
||||
inav[1].assign(tmp, 16);
|
||||
|
||||
struct GalileoMessage gm={};
|
||||
gm.parse(inav[1]);
|
||||
|
|
@ -210,7 +210,7 @@ struct GalileoMessage FixHunter::fillGMFromRS(const std::string& out)
|
|||
setbitu(tmp, 6, 10, getbitu((unsigned char*) inav[1].c_str(), 6, 10)); // fake in IOD from inav1
|
||||
for(int n=0; n < 14; ++n)
|
||||
setbitu(tmp, 16 + n*8, 8, getbitu((unsigned char*) inavraw[i].c_str(), n*8, 8));
|
||||
inav[i]= makeVec(tmp, 16);
|
||||
inav[i].assign(tmp, 16);
|
||||
gm.parse(inav[i]);
|
||||
cout<<"wtype: "<<(int)gm.wtype<<", iod "<<gm.iodnav<<endl;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,11 +5,11 @@
|
|||
class FixHunter
|
||||
{
|
||||
public:
|
||||
void reportInav(const std::vector<uint8_t>& inav, int32_t gst);
|
||||
void reportInav(const std::basic_string<uint8_t>& inav, int32_t gst);
|
||||
private:
|
||||
void tryFix(int32_t gst);
|
||||
struct GalileoMessage fillGMFromRS(const std::string& out);
|
||||
std::vector<uint8_t> inav1, inav2, inav3, inav4, inav16, inav17, inav18, inav19, inav20;
|
||||
std::basic_string<uint8_t> inav1, inav2, inav3, inav4, inav16, inav17, inav18, inav19, inav20;
|
||||
int d_latestiod;
|
||||
uint32_t d_inav16t0r;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#include "bits.hh"
|
||||
#include "galileo.hh"
|
||||
|
||||
bool getTOWFromInav(const std::vector<uint8_t>& inav, uint32_t *satTOW, uint16_t *wn)
|
||||
bool getTOWFromInav(std::basic_string_view<uint8_t> inav, uint32_t *satTOW, uint16_t *wn)
|
||||
{
|
||||
unsigned int wtype = getbitu(&inav[0], 0, 6);
|
||||
if(wtype==0) {
|
||||
|
|
@ -25,7 +25,7 @@ bool getTOWFromInav(const std::vector<uint8_t>& inav, uint32_t *satTOW, uint16_t
|
|||
return false;
|
||||
}
|
||||
|
||||
int GalileoMessage::parseFnav(const std::vector<uint8_t>& page)
|
||||
int GalileoMessage::parseFnav(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
const uint8_t* ptr = &page[0];
|
||||
int offset=0;
|
||||
|
|
|
|||
34
galileo.hh
34
galileo.hh
|
|
@ -7,13 +7,13 @@
|
|||
#include "ephemeris.hh"
|
||||
#include "bits.hh"
|
||||
|
||||
bool getTOWFromInav(const std::vector<uint8_t>& inav, uint32_t *satTOW, uint16_t *wn);
|
||||
bool getTOWFromInav(std::basic_string_view<uint8_t> inav, uint32_t *satTOW, uint16_t *wn);
|
||||
|
||||
struct GalileoMessage : GPSLikeEphemeris
|
||||
{
|
||||
uint8_t wtype;
|
||||
|
||||
typedef void (GalileoMessage::*func_t)(const std::vector<uint8_t>& page);
|
||||
typedef void (GalileoMessage::*func_t)(std::basic_string_view<uint8_t> page);
|
||||
std::map<int, func_t> parsers{
|
||||
{0, &GalileoMessage::parse0},
|
||||
{1, &GalileoMessage::parse1},
|
||||
|
|
@ -34,7 +34,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
};
|
||||
|
||||
|
||||
int parse(const std::vector<uint8_t>& page)
|
||||
int parse(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
wtype = getbitu(&page[0], 0, 6);
|
||||
if(!parsers.count(wtype)) {
|
||||
|
|
@ -46,7 +46,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
return wtype;
|
||||
}
|
||||
|
||||
int parseFnav(const std::vector<uint8_t>& page);
|
||||
int parseFnav(std::basic_string_view<uint8_t> page);
|
||||
|
||||
uint8_t sparetime{0};
|
||||
uint16_t wn{0};
|
||||
|
|
@ -58,7 +58,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
|
||||
|
||||
// spare word, only contains a WN and a TOW, but only if the 'time' field is set to 2
|
||||
void parse0(const std::vector<uint8_t>& page)
|
||||
void parse0(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
sparetime = getbitu(&page[0], 6, 2);
|
||||
if(sparetime == 2) {
|
||||
|
|
@ -164,7 +164,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
|
||||
|
||||
// an ephemeris word
|
||||
void parse1(const std::vector<uint8_t>& page)
|
||||
void parse1(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
iodnav = getbitu(&page[0], 6, 10);
|
||||
t0e = getbitu(&page[0], 16, 14);
|
||||
|
|
@ -174,7 +174,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
}
|
||||
|
||||
// another ephemeris word
|
||||
void parse2(const std::vector<uint8_t>& page)
|
||||
void parse2(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
iodnav = getbitu(&page[0], 6, 10);
|
||||
omega0 = getbits(&page[0], 16, 32);
|
||||
|
|
@ -184,7 +184,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
}
|
||||
|
||||
// yet another ephemeris word
|
||||
void parse3(const std::vector<uint8_t>& page)
|
||||
void parse3(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
iodnav = getbitu(&page[0], 6, 10);
|
||||
omegadot = getbits(&page[0], 16, 24);
|
||||
|
|
@ -264,7 +264,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
|
||||
|
||||
// can't get enough of that ephemeris
|
||||
void parse4(const std::vector<uint8_t>& page)
|
||||
void parse4(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
iodnav = getbitu(&page[0], 6, 10);
|
||||
cic = getbits(&page[0], 22, 16);
|
||||
|
|
@ -277,7 +277,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
}
|
||||
|
||||
// ionospheric disturbance, health, group delay, time
|
||||
void parse5(const std::vector<uint8_t>& page)
|
||||
void parse5(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
ai0 = getbitu(&page[0], 6, 11);
|
||||
ai1 = getbits(&page[0], 17, 11); // ai1 & 2 are signed, 0 not
|
||||
|
|
@ -300,7 +300,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
}
|
||||
|
||||
// time stuff
|
||||
void parse6(const std::vector<uint8_t>& page)
|
||||
void parse6(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
a0 = getbits(&page[0], 6, 32);
|
||||
a1 = getbits(&page[0], 38, 24);
|
||||
|
|
@ -315,7 +315,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
}
|
||||
|
||||
// almanac
|
||||
void parse7(const std::vector<uint8_t>& page)
|
||||
void parse7(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
iodalmanac = getbitu(&page[0], 6, 4);
|
||||
alma1.wnalmanac = wnalmanac = getbitu(&page[0], 10, 2);
|
||||
|
|
@ -331,7 +331,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
|
||||
}
|
||||
// almanac
|
||||
void parse8(const std::vector<uint8_t>& page)
|
||||
void parse8(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
iodalmanac = getbitu(&page[0], 6, 4);
|
||||
alma1.af0 = getbits(&page[0], 10, 16);
|
||||
|
|
@ -350,7 +350,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
}
|
||||
|
||||
// almanac
|
||||
void parse9(const std::vector<uint8_t>& page)
|
||||
void parse9(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
iodalmanac = getbitu(&page[0], 6, 4);
|
||||
alma2.wnalmanac = wnalmanac = getbitu(&page[0], 10, 2);
|
||||
|
|
@ -371,7 +371,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
}
|
||||
|
||||
// almanac + more time stuff (GPS)
|
||||
void parse10(const std::vector<uint8_t>& page)
|
||||
void parse10(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
iodalmanac = getbitu(&page[0], 6, 4);
|
||||
alma3.Omega0 = getbits(&page[0], 10, 16);
|
||||
|
|
@ -391,7 +391,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
|
||||
|
||||
// reduced clock and ephemeris data (redced)
|
||||
void parse16(const std::vector<uint8_t>& page)
|
||||
void parse16(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
deltaAred = getbits(&page[0], 6, 5);
|
||||
exred = getbits(&page[0], 11, 13);
|
||||
|
|
@ -404,7 +404,7 @@ struct GalileoMessage : GPSLikeEphemeris
|
|||
}
|
||||
|
||||
// reed-solomon data
|
||||
void parseRS(const std::vector<uint8_t>& page)
|
||||
void parseRS(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
// see 5.1.13.2 of the Galileo SIS ICD 2.0
|
||||
rs2bitiod = getbitu(&page[0], 6+8, 2);
|
||||
|
|
|
|||
|
|
@ -3,7 +3,6 @@
|
|||
#include <string.h>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include "navmon.hh"
|
||||
using std::cout;
|
||||
using std::endl;
|
||||
|
||||
|
|
@ -13,7 +12,7 @@ static const double J2 = 1082625.75E-9; // IERS: 1.0826359
|
|||
static const double oe = 7.2921151467E-5; // rad/s // IERS: 7.292115
|
||||
|
||||
// this strips out spare bits + parity, and leaves 10 clean 24 bit words
|
||||
std::vector<uint8_t> getGlonassMessage(const std::vector<uint8_t>& payload)
|
||||
std::basic_string<uint8_t> getGlonassMessage(std::basic_string_view<uint8_t> payload)
|
||||
{
|
||||
uint8_t buffer[4*4];
|
||||
|
||||
|
|
@ -21,7 +20,7 @@ std::vector<uint8_t> getGlonassMessage(const std::vector<uint8_t>& payload)
|
|||
setbitu(buffer, 32*w, 32, getbitu(&payload[0], w*32, 32));
|
||||
}
|
||||
|
||||
return makeVec(buffer, 16);
|
||||
return std::basic_string<uint8_t>(buffer, 16);
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
20
glonass.hh
20
glonass.hh
|
|
@ -5,16 +5,14 @@
|
|||
#include <iostream>
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <vector>
|
||||
|
||||
#include "minivec.hh"
|
||||
std::vector<uint8_t> getGlonassessage(const std::vector<uint8_t>& payload);
|
||||
std::basic_string<uint8_t> getGlonassessage(std::basic_string_view<uint8_t> payload);
|
||||
|
||||
struct GlonassMessage
|
||||
{
|
||||
uint8_t strtype;
|
||||
|
||||
int parse(const std::vector<uint8_t>& gstr)
|
||||
int parse(std::basic_string_view<uint8_t> gstr)
|
||||
{
|
||||
strtype = getbitu(&gstr[0], 1, 4);
|
||||
if(strtype == 1) {
|
||||
|
|
@ -62,7 +60,7 @@ struct GlonassMessage
|
|||
|
||||
double getRadius() { return sqrt(getX()*getX() + getY()*getY() + getZ()*getZ()); }
|
||||
|
||||
void parse1(const std::vector<uint8_t>& gstr)
|
||||
void parse1(std::basic_string_view<uint8_t> gstr)
|
||||
{
|
||||
hour = getbitu(&gstr[0], 9, 5);
|
||||
minute = getbitu(&gstr[0], 14, 6);
|
||||
|
|
@ -80,7 +78,7 @@ struct GlonassMessage
|
|||
An interval is 15 minutes long, plus a spacer of length described by P1. If P1 is zero, there is no spacer.
|
||||
*/
|
||||
|
||||
void parse2(const std::vector<uint8_t>& gstr)
|
||||
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);
|
||||
|
|
@ -95,7 +93,7 @@ struct GlonassMessage
|
|||
bool l_n;
|
||||
bool P, P3;
|
||||
uint16_t gamman;
|
||||
void parse3(const std::vector<uint8_t>& gstr)
|
||||
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
|
||||
|
|
@ -123,7 +121,7 @@ struct GlonassMessage
|
|||
return 1000*ldexp(1000000.0*taun, -30);
|
||||
}
|
||||
|
||||
void parse4(const std::vector<uint8_t>& gstr)
|
||||
void parse4(std::basic_string_view<uint8_t> gstr)
|
||||
{
|
||||
NT = getbitu(&gstr[0], 85-26, 11);
|
||||
FT = getbitu(&gstr[0], 85-33, 4);
|
||||
|
|
@ -159,7 +157,7 @@ struct GlonassMessage
|
|||
int32_t taugps;
|
||||
int32_t tauc;
|
||||
|
||||
void parse5(const std::vector<uint8_t>& gstr)
|
||||
void parse5(std::basic_string_view<uint8_t> gstr)
|
||||
{
|
||||
n4=getbitu(&gstr[0], 85-36, 5);
|
||||
taugps = getbitsglonass(&gstr[0], 85-31, 22);
|
||||
|
|
@ -177,7 +175,7 @@ struct GlonassMessage
|
|||
return ldexp(tlambdana, -5);
|
||||
}
|
||||
|
||||
void parse7_9_11_13_15(const std::vector<uint8_t>& gstr)
|
||||
void parse7_9_11_13_15(std::basic_string_view<uint8_t> gstr)
|
||||
{
|
||||
l_n = getbitu(&gstr[0], 85 - 9, 1);
|
||||
omegana = getbitsglonass(&gstr[0], 85-80, 16);
|
||||
|
|
@ -209,7 +207,7 @@ struct GlonassMessage
|
|||
return M_PI*63.0/180 + ldexp(M_PI* deltaina, -20);
|
||||
}
|
||||
|
||||
void parse6_8_10_12_14(const std::vector<uint8_t>& gstr)
|
||||
void parse6_8_10_12_14(std::basic_string_view<uint8_t> gstr)
|
||||
{
|
||||
CnA = getbitu(&gstr[0], 85-80, 1);
|
||||
nA = getbitu(&gstr[0], 85-77, 5);
|
||||
|
|
|
|||
9
gps.cc
9
gps.cc
|
|
@ -1,7 +1,7 @@
|
|||
#include "gps.hh"
|
||||
|
||||
// this strips out spare bits + parity, and leaves 10 clean 24 bit words
|
||||
std::vector<uint8_t> getCondensedGPSMessage(const std::vector<uint8_t>& payload)
|
||||
std::basic_string<uint8_t> getCondensedGPSMessage(std::basic_string_view<uint8_t> payload)
|
||||
{
|
||||
uint8_t buffer[10*24/8];
|
||||
|
||||
|
|
@ -10,11 +10,12 @@ std::vector<uint8_t> getCondensedGPSMessage(const std::vector<uint8_t>& payload)
|
|||
setbitu(buffer, 24*w, 24, getbitu(&payload[0], 2 + w*32, 24));
|
||||
}
|
||||
|
||||
return std::vector<uint8_t>(buffer, buffer+30);
|
||||
return std::basic_string<uint8_t>(buffer, 30);
|
||||
|
||||
}
|
||||
|
||||
// expects input as 24 bit read to to use messages, returns frame number
|
||||
int GPSState::parseGPSMessage(const std::vector<uint8_t>& cond, uint8_t* pageptr)
|
||||
int GPSState::parseGPSMessage(std::basic_string_view<uint8_t> cond, uint8_t* pageptr)
|
||||
{
|
||||
using namespace std;
|
||||
int frame = getbitu(&cond[0], 24+19, 3);
|
||||
|
|
@ -38,8 +39,6 @@ int GPSState::parseGPSMessage(const std::vector<uint8_t>& cond, uint8_t* pageptr
|
|||
wn = 2048 + getbitu(&cond[0], 2*24, 10);
|
||||
ura = getbitu(&cond[0], 2*24+12, 4);
|
||||
gpshealth = getbitu(&cond[0], 2*24+16, 6);
|
||||
iodc = getbitu(&cond[0], 2*24 +22, 2) * 256;
|
||||
iodc += getbitu(&cond[0], 7*24, 8);
|
||||
|
||||
// cerr<<"GPS Week Number: "<< wn <<", URA: "<< (int)ura<<", health: "<<
|
||||
// (int)gpshealth <<endl;
|
||||
|
|
|
|||
7
gps.hh
7
gps.hh
|
|
@ -6,9 +6,8 @@
|
|||
#include <iostream>
|
||||
#include <math.h>
|
||||
#include "ephemeris.hh"
|
||||
#include <vector>
|
||||
|
||||
std::vector<uint8_t> getCondensedGPSMessage(const std::vector<uint8_t>& payload);
|
||||
std::basic_string<uint8_t> getCondensedGPSMessage(std::basic_string_view<uint8_t> payload);
|
||||
|
||||
|
||||
struct GPSAlmanac : GPSLikeEphemeris
|
||||
|
|
@ -134,17 +133,15 @@ struct GPSState : GPSLikeEphemeris
|
|||
uint16_t wnLSF{0};
|
||||
uint8_t dn; // leap second day number
|
||||
// 1 2^-31 2^-43 2^-55 16 second
|
||||
int iodc;
|
||||
int ura;
|
||||
|
||||
|
||||
int gpsiod{-1};
|
||||
|
||||
int getIOD() const
|
||||
{
|
||||
return gpsiod;
|
||||
}
|
||||
int parseGPSMessage(const std::vector<uint8_t>& cond, uint8_t* pageptr=0);
|
||||
int parseGPSMessage(std::basic_string_view<uint8_t> cond, uint8_t* pageptr=0);
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
#pragma once
|
||||
#include <bitset>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include "bits.hh"
|
||||
#include <iostream>
|
||||
|
|
@ -186,7 +185,7 @@ std::pair<double, double> getGPSCNavUTCOffset(int tow, int wn, const T& eph)
|
|||
|
||||
|
||||
template<typename T>
|
||||
int parseGPSCNavMessage(const std::vector<uint8_t>& msg, T& out)
|
||||
int parseGPSCNavMessage(std::basic_string_view<uint8_t> msg, T& out)
|
||||
{
|
||||
using namespace std;
|
||||
int type = getbitu(&msg[0], 14, 6);
|
||||
|
|
|
|||
|
|
@ -188,7 +188,7 @@ int main(int argc, char** argv)
|
|||
static map<int, GalileoMessage> gms;
|
||||
GalileoMessage& gm = gms[nmm.gi().gnsssv()];
|
||||
|
||||
auto inav = makeVec((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size());
|
||||
basic_string<uint8_t> inav((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size());
|
||||
int wtype =gm.parse(inav);
|
||||
wk.emitLine(sv, "src "+to_string(nmm.sourceid())+" wtype " + to_string(wtype));
|
||||
// wk.setStatus(sv, "Hlth: "+std::to_string(getbitu(&inav[0], 67, 2)) +", el="+to_string(g_svstats[sv].el)+", db="+to_string(g_svstats[sv].db) );
|
||||
|
|
|
|||
20
navdump.cc
20
navdump.cc
|
|
@ -387,7 +387,7 @@ try
|
|||
}
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::GalileoInavType) {
|
||||
auto inav = makeVec((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size());
|
||||
basic_string<uint8_t> inav((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size());
|
||||
static map<int, GalileoMessage> gms;
|
||||
static map<pair<int, int>, GalileoMessage> gmwtypes;
|
||||
|
||||
|
|
@ -714,7 +714,7 @@ try
|
|||
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::GalileoFnavType) {
|
||||
auto fnav = makeVec((uint8_t*)nmm.gf().contents().c_str(), nmm.gf().contents().size());
|
||||
basic_string<uint8_t> fnav((uint8_t*)nmm.gf().contents().c_str(), nmm.gf().contents().size());
|
||||
int sv = nmm.gf().gnsssv();
|
||||
if(!svfilter.check(2, sv, nmm.gf().sigid()))
|
||||
continue;
|
||||
|
|
@ -752,7 +752,7 @@ try
|
|||
continue;
|
||||
etstamp();
|
||||
|
||||
auto cond = getCondensedGPSMessage(makeVec((uint8_t*)nmm.gpsi().contents().c_str(), nmm.gpsi().contents().size()));
|
||||
auto cond = getCondensedGPSMessage(std::basic_string<uint8_t>((uint8_t*)nmm.gpsi().contents().c_str(), nmm.gpsi().contents().size()));
|
||||
struct GPSState gs;
|
||||
static map<int, GPSState> eph;
|
||||
|
||||
|
|
@ -951,7 +951,7 @@ try
|
|||
static map<int, GPSCNavState> states;
|
||||
auto& state = states[sv];
|
||||
int type = parseGPSCNavMessage(
|
||||
makeVec((uint8_t*)nmm.gpsc().contents().c_str(),
|
||||
std::basic_string<uint8_t>((uint8_t*)nmm.gpsc().contents().c_str(),
|
||||
nmm.gpsc().contents().size()),
|
||||
state);
|
||||
|
||||
|
|
@ -969,9 +969,9 @@ try
|
|||
continue;
|
||||
etstamp();
|
||||
|
||||
std::vector<uint8_t> cond;
|
||||
std::basic_string<uint8_t> cond;
|
||||
try {
|
||||
cond = getCondensedBeidouMessage(makeVec((uint8_t*)nmm.bid1().contents().c_str(), nmm.bid1().contents().size()));
|
||||
cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bid1().contents().c_str(), nmm.bid1().contents().size()));
|
||||
}
|
||||
catch(std::exception& e) {
|
||||
cout<<"Parsing error"<<endl;
|
||||
|
|
@ -1050,7 +1050,7 @@ try
|
|||
continue;
|
||||
etstamp();
|
||||
|
||||
auto cond = getCondensedBeidouMessage(makeVec((uint8_t*)nmm.bid2().contents().c_str(), nmm.bid2().contents().size()));
|
||||
auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bid2().contents().c_str(), nmm.bid2().contents().size()));
|
||||
BeidouMessage bm;
|
||||
uint8_t pageno;
|
||||
int fraid = bm.parse(cond, &pageno);
|
||||
|
|
@ -1065,7 +1065,7 @@ try
|
|||
static map<int, GlonassMessage> gms;
|
||||
auto& gm = gms[nmm.gloi().gnsssv()];
|
||||
|
||||
int strno = gm.parse(makeVec((uint8_t*)nmm.gloi().contents().c_str(), nmm.gloi().contents().size()));
|
||||
int strno = gm.parse(std::basic_string<uint8_t>((uint8_t*)nmm.gloi().contents().c_str(), nmm.gloi().contents().size()));
|
||||
|
||||
cout<<"Glonass R"<<nmm.gloi().gnsssv()<<" @ "<< ((int)nmm.gloi().freq()-7) <<" strno "<<strno;
|
||||
if(strno == 1) {
|
||||
|
|
@ -1261,7 +1261,7 @@ try
|
|||
continue;
|
||||
|
||||
etstamp();
|
||||
auto sbas = makeVec((uint8_t*)nmm.sbm().contents().c_str(), nmm.sbm().contents().size());
|
||||
basic_string<uint8_t> sbas((uint8_t*)nmm.sbm().contents().c_str(), nmm.sbm().contents().size());
|
||||
cout<<" PRN "<<nmm.sbm().gnsssv()<<" SBAS message type ";
|
||||
|
||||
// Preamble sequence:
|
||||
|
|
@ -1315,7 +1315,7 @@ try
|
|||
}
|
||||
else if(nmm.type() == NavMonMessage::DebuggingType) {
|
||||
|
||||
auto res = parseTrkMeas(makeVec((const uint8_t*)nmm.dm().payload().c_str(), nmm.dm().payload().size()));
|
||||
auto res = parseTrkMeas(basic_string<uint8_t>((const uint8_t*)nmm.dm().payload().c_str(), nmm.dm().payload().size()));
|
||||
if(res.empty())
|
||||
continue;
|
||||
etstamp();
|
||||
|
|
|
|||
41
navmerge.cc
41
navmerge.cc
|
|
@ -55,7 +55,7 @@ set<int> g_bsset;
|
|||
// this means each Galileo message will only get set once
|
||||
map<tuple<uint32_t, std::string, uint32_t, std::string, int16_t>, time_t> g_seen;
|
||||
|
||||
bool g_inavdedup{false}, g_gpsdedup{false};
|
||||
bool g_inavdedup{false};
|
||||
|
||||
/* Goal: do a number of TCP operations that have a combined timeout.
|
||||
maybe some helper:
|
||||
|
|
@ -134,36 +134,20 @@ void recvSession(ComboAddress upstream)
|
|||
continue;
|
||||
|
||||
if(g_inavdedup) {
|
||||
if(nmm.type() == NavMonMessage::GalileoInavType) {
|
||||
std::lock_guard<std::mutex> mut(g_mut);
|
||||
decltype(g_seen)::key_type tup(nmm.gi().gnsssv(),
|
||||
if(nmm.type() == NavMonMessage::GalileoInavType) {
|
||||
std::lock_guard<std::mutex> mut(g_mut);
|
||||
decltype(g_seen)::key_type tup(nmm.gi().gnsssv(),
|
||||
nmm.gi().contents(),
|
||||
nmm.gi().sigid(),
|
||||
nmm.gi().reserved1(),
|
||||
nmm.gi().has_ssp() ? nmm.gi().ssp() : -1);
|
||||
|
||||
if(!g_seen.count(tup))
|
||||
g_buffer.insert({{nmm.localutcseconds(), nmm.localutcnanoseconds()}, part});
|
||||
g_seen[tup]=time(0);
|
||||
}
|
||||
}
|
||||
|
||||
if(g_gpsdedup) {
|
||||
if(nmm.type() == NavMonMessage::GPSInavType) {
|
||||
std::lock_guard<std::mutex> mut(g_mut);
|
||||
decltype(g_seen)::key_type tup(nmm.gpsi().gnsssv(),
|
||||
nmm.gpsi().contents(),
|
||||
nmm.gpsi().sigid(),
|
||||
"",
|
||||
0);
|
||||
|
||||
if(!g_seen.count(tup))
|
||||
g_buffer.insert({{nmm.localutcseconds(), nmm.localutcnanoseconds()}, part});
|
||||
g_seen[tup]=time(0);
|
||||
}
|
||||
}
|
||||
|
||||
if(!g_gpsdedup && !g_inavdedup) {
|
||||
|
||||
if(!g_seen.count(tup))
|
||||
g_buffer.insert({{nmm.localutcseconds(), nmm.localutcnanoseconds()}, part});
|
||||
g_seen[tup]=time(0);
|
||||
}
|
||||
}
|
||||
else {
|
||||
std::lock_guard<std::mutex> mut(g_mut);
|
||||
g_buffer.insert({{nmm.localutcseconds(), nmm.localutcnanoseconds()}, part});
|
||||
}
|
||||
|
|
@ -203,8 +187,7 @@ int main(int argc, char** argv)
|
|||
app.add_option("--destination,-d", destinations, "Send output to this IPv4/v6 address");
|
||||
app.add_option("--drop-stations", badstations, "Drop these station numbers");
|
||||
app.add_option("--listener,-l", listeners, "Make data available on this IPv4/v6 address");
|
||||
app.add_flag("--inavdedup", g_inavdedup, "Only pass on Galileo I/NAV, and dedeup");
|
||||
app.add_flag("--gpsdedup", g_gpsdedup, "Only pass on GPS, and dedeup");
|
||||
app.add_flag("--inavdedup", g_inavdedup, "Only pass on Galileo I/NAV, and dedeup");
|
||||
app.add_flag("--version", doVERSION, "show program version and copyright");
|
||||
app.add_flag("--stdout", doSTDOUT, "Emit output to stdout");
|
||||
|
||||
|
|
|
|||
|
|
@ -347,7 +347,7 @@ string makeHexDump(const string& str)
|
|||
return ret;
|
||||
}
|
||||
|
||||
string makeHexDump(const vector<uint8_t>& str)
|
||||
string makeHexDump(const basic_string<uint8_t>& str)
|
||||
{
|
||||
char tmp[5];
|
||||
string ret;
|
||||
|
|
|
|||
|
|
@ -6,8 +6,6 @@
|
|||
#include <tuple>
|
||||
#include <mutex>
|
||||
#include <limits.h>
|
||||
#include <vector>
|
||||
|
||||
extern const char* g_gitHash;
|
||||
|
||||
|
||||
|
|
@ -85,13 +83,9 @@ void getGalDateFromUTC(time_t t, int& wn, int& tow);
|
|||
void getBeiDouDateFromUTC(time_t t, int&wn, int& sow);
|
||||
|
||||
std::string makeHexDump(const std::string& str);
|
||||
std::string makeHexDump(const std::vector<uint8_t>& str);
|
||||
std::string makeHexDump(const std::basic_string<uint8_t>& str);
|
||||
size_t writen2(int fd, const void *buf, size_t count);
|
||||
void unixDie(const std::string& reason);
|
||||
time_t parseTime(std::string_view in);
|
||||
std::string string_replace(const std::string& str, const std::string& match,
|
||||
const std::string& replacement, unsigned int max_replacements = UINT_MAX);
|
||||
inline const std::vector<uint8_t> makeVec(const uint8_t* ptr, size_t len)
|
||||
{
|
||||
return std::vector(ptr, ptr+len);
|
||||
}
|
||||
|
|
|
|||
18
navparse.cc
18
navparse.cc
|
|
@ -1985,7 +1985,7 @@ try
|
|||
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::GalileoInavType) {
|
||||
auto inav = makeVec((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size());
|
||||
basic_string<uint8_t> inav((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size());
|
||||
int sv = nmm.gi().gnsssv();
|
||||
int sigid;
|
||||
if(nmm.gi().has_sigid())
|
||||
|
|
@ -2174,7 +2174,7 @@ try
|
|||
// ... no idea what this contains
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::GalileoFnavType) {
|
||||
auto fnav = makeVec((uint8_t*)nmm.gf().contents().c_str(), nmm.gf().contents().size());
|
||||
basic_string<uint8_t> fnav((uint8_t*)nmm.gf().contents().c_str(), nmm.gf().contents().size());
|
||||
int sv = nmm.gf().gnsssv();
|
||||
SatID id={2,(uint32_t)sv,6}; // E5a
|
||||
|
||||
|
|
@ -2508,7 +2508,7 @@ try
|
|||
if(doGalileoReportSpeedup)
|
||||
continue; // speedup
|
||||
|
||||
auto ret = parseTrkMeas(makeVec((const uint8_t*)nmm.dm().payload().c_str(), nmm.dm().payload().size()));
|
||||
auto ret = parseTrkMeas(basic_string<uint8_t>((const uint8_t*)nmm.dm().payload().c_str(), nmm.dm().payload().size()));
|
||||
for(const auto& tss : ret) {
|
||||
SatID id{static_cast<uint32_t>(tss.gnss), static_cast<uint32_t>(tss.sv), tss.gnss == 2 ? 1u : 0u};
|
||||
if(g_svstats[id].completeIOD()) {
|
||||
|
|
@ -2561,7 +2561,7 @@ try
|
|||
cout<<"ignoring sigid "<<nmm.gpsi().sigid()<<" for legacy GPS "<<nmm.gpsi().gnsssv()<<endl;
|
||||
continue;
|
||||
}
|
||||
auto cond = getCondensedGPSMessage(makeVec((uint8_t*)nmm.gpsi().contents().c_str(), nmm.gpsi().contents().size()));
|
||||
auto cond = getCondensedGPSMessage(std::basic_string<uint8_t>((uint8_t*)nmm.gpsi().contents().c_str(), nmm.gpsi().contents().size()));
|
||||
SatID id{nmm.gpsi().gnssid(), nmm.gpsi().gnsssv(), nmm.gpsi().sigid()};
|
||||
|
||||
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
||||
|
|
@ -2818,7 +2818,7 @@ try
|
|||
|
||||
GPSCNavState gcns;
|
||||
parseGPSCNavMessage(
|
||||
makeVec((uint8_t*)nmm.gpsc().contents().c_str(),
|
||||
std::basic_string<uint8_t>((uint8_t*)nmm.gpsc().contents().c_str(),
|
||||
nmm.gpsc().contents().size()),
|
||||
gcns);
|
||||
// cout<<"Got a message from "<<makeSatIDName(id)<<endl;
|
||||
|
|
@ -2840,7 +2840,7 @@ try
|
|||
auto& svstat = g_svstats[id];
|
||||
svstat.gnss = id.gnss;
|
||||
uint8_t pageno;
|
||||
auto cond = getCondensedBeidouMessage(makeVec((uint8_t*)nmm.bid1().contents().c_str(), nmm.bid1().contents().size()));
|
||||
auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bid1().contents().c_str(), nmm.bid1().contents().size()));
|
||||
auto& bm = svstat.beidoumsg;
|
||||
auto oldbm = bm;
|
||||
int fraid=bm.parse(cond, &pageno);
|
||||
|
|
@ -2916,7 +2916,7 @@ try
|
|||
}
|
||||
}
|
||||
else if(nmm.type()== NavMonMessage::BeidouInavTypeD2) {
|
||||
auto cond = getCondensedBeidouMessage(makeVec((uint8_t*)nmm.bid2().contents().c_str(), nmm.bid2().contents().size()));
|
||||
auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bid2().contents().c_str(), nmm.bid2().contents().size()));
|
||||
/*
|
||||
int fraid = getbitu(&cond[0], beidouBitconv(16), 3);
|
||||
int sow = getbitu(&cond[0], beidouBitconv(19), 20);
|
||||
|
|
@ -2938,7 +2938,7 @@ try
|
|||
svstat.gnss = id.gnss;
|
||||
auto& gm = svstat.glonassMessage;
|
||||
auto oldgm = gm;
|
||||
int strno = gm.parse(makeVec((uint8_t*)nmm.gloi().contents().c_str(), nmm.gloi().contents().size()));
|
||||
int strno = gm.parse(std::basic_string<uint8_t>((uint8_t*)nmm.gloi().contents().c_str(), nmm.gloi().contents().size()));
|
||||
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
||||
if(strno == 1 && gm.n4 != 0 && gm.NT !=0) {
|
||||
// uint32_t glotime = gm.getGloTime(); // this starts GLONASS time at 31st of december 1995, 00:00 UTC
|
||||
|
|
@ -3006,7 +3006,7 @@ try
|
|||
|
||||
sb.perrecv[nmm.sourceid()].last_seen = nmm.localutcseconds();
|
||||
|
||||
auto sbas = makeVec((uint8_t*)nmm.sbm().contents().c_str(), nmm.sbm().contents().length());
|
||||
basic_string<uint8_t> sbas((uint8_t*)nmm.sbm().contents().c_str(), nmm.sbm().contents().length());
|
||||
auto delta = sb.status.parse(sbas, nmm.localutcseconds());
|
||||
// fast correction - clogs the database, so dropping that for now
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -534,7 +534,7 @@ try
|
|||
|
||||
// cout<<" z-error: "<<dir.inner(v);
|
||||
|
||||
csv << e.first << " " << id.gnss <<" " << id.sv << " " << id.sigid <<" " << dir.inner(v) << " " << clkoffset << endl;
|
||||
csv << e.first << " " << id.gnss <<" " << id.sv << " " << id.sigid <<" " << dir.inner(v) << endl;
|
||||
idb.addValue({{"gnssid", id.gnss}, {"sv", id.sv}, {"sp3src", sp3src}},
|
||||
"sp3delta",
|
||||
{{"ecef-dx", v.x}, {"ecef-dy", v.y}, {"ecef-dz", v.z}, {"sv-dz", dir.inner(v)}, {"dclock", clkoffset},
|
||||
|
|
|
|||
20
sbas.cc
20
sbas.cc
|
|
@ -4,14 +4,14 @@ using namespace std;
|
|||
#include "bits.hh"
|
||||
#include <math.h>
|
||||
|
||||
void SBASState::parse0(const vector<uint8_t>& sbas, time_t now)
|
||||
void SBASState::parse0(const basic_string<uint8_t>& sbas, time_t now)
|
||||
{
|
||||
d_lastDNU = now;
|
||||
d_lastSeen = now;
|
||||
}
|
||||
|
||||
|
||||
void SBASState::parse1(const vector<uint8_t>& sbas, time_t now)
|
||||
void SBASState::parse1(const basic_string<uint8_t>& sbas, time_t now)
|
||||
{
|
||||
d_lastSeen = now;
|
||||
int slot=1;
|
||||
|
|
@ -25,7 +25,7 @@ void SBASState::parse1(const vector<uint8_t>& sbas, time_t now)
|
|||
}
|
||||
}
|
||||
|
||||
vector<SBASState::FastCorrection> SBASState::parse2_5(const vector<uint8_t>&sbas, time_t now)
|
||||
vector<SBASState::FastCorrection> SBASState::parse2_5(const basic_string<uint8_t>&sbas, time_t now)
|
||||
{
|
||||
d_lastSeen = now;
|
||||
int type = getbitu(&sbas[0], 8, 6);
|
||||
|
|
@ -47,7 +47,7 @@ vector<SBASState::FastCorrection> SBASState::parse2_5(const vector<uint8_t>&sbas
|
|||
return ret;
|
||||
}
|
||||
|
||||
vector<SBASState::FastCorrection> SBASState::parse6(const vector<uint8_t>&sbas, time_t now)
|
||||
vector<SBASState::FastCorrection> SBASState::parse6(const basic_string<uint8_t>&sbas, time_t now)
|
||||
{
|
||||
d_lastSeen = now;
|
||||
vector<SBASState::FastCorrection> ret;
|
||||
|
|
@ -68,7 +68,7 @@ vector<SBASState::FastCorrection> SBASState::parse6(const vector<uint8_t>&sbas,
|
|||
return ret;
|
||||
}
|
||||
|
||||
void SBASState::parse7(const vector<uint8_t>&sbas, time_t now)
|
||||
void SBASState::parse7(const basic_string<uint8_t>&sbas, time_t now)
|
||||
{
|
||||
d_lastSeen = now;
|
||||
d_latency = getbitu(&sbas[0], 14+4, 4);
|
||||
|
|
@ -101,7 +101,7 @@ SatID SBASState::getSBASSatID(int slot) const
|
|||
return ret;
|
||||
}
|
||||
|
||||
vector<SBASState::LongTermCorrection> SBASState::parse25(const vector<uint8_t>& sbas, time_t t)
|
||||
vector<SBASState::LongTermCorrection> SBASState::parse25(const basic_string<uint8_t>& sbas, time_t t)
|
||||
{
|
||||
d_lastSeen = t;
|
||||
vector<LongTermCorrection> ret;
|
||||
|
|
@ -111,7 +111,7 @@ vector<SBASState::LongTermCorrection> SBASState::parse25(const vector<uint8_t>&
|
|||
return ret;
|
||||
}
|
||||
|
||||
pair<vector<SBASState::FastCorrection>, vector<SBASState::LongTermCorrection>> SBASState::parse24(const vector<uint8_t>& sbas, time_t t)
|
||||
pair<vector<SBASState::FastCorrection>, vector<SBASState::LongTermCorrection>> SBASState::parse24(const basic_string<uint8_t>& sbas, time_t t)
|
||||
{
|
||||
d_lastSeen = t;
|
||||
pair<vector<FastCorrection>, vector<LongTermCorrection>> ret;
|
||||
|
|
@ -135,7 +135,7 @@ pair<vector<SBASState::FastCorrection>, vector<SBASState::LongTermCorrection>> S
|
|||
return ret;
|
||||
}
|
||||
|
||||
pair<vector<SBASState::FastCorrection>, vector<SBASState::LongTermCorrection>> SBASState::parse(const std::vector<uint8_t>& sbas, time_t now)
|
||||
pair<vector<SBASState::FastCorrection>, vector<SBASState::LongTermCorrection>> SBASState::parse(const std::basic_string<uint8_t>& sbas, time_t now)
|
||||
{
|
||||
pair<vector<SBASState::FastCorrection>, vector<SBASState::LongTermCorrection>> ret;
|
||||
int type = getbitu(&sbas[0], 8, 6);
|
||||
|
|
@ -164,7 +164,7 @@ pair<vector<SBASState::FastCorrection>, vector<SBASState::LongTermCorrection>> S
|
|||
return ret;
|
||||
}
|
||||
|
||||
void SBASState::parse25H(const vector<uint8_t>& sbas, time_t t, int offset, vector<LongTermCorrection>& ret)
|
||||
void SBASState::parse25H(const basic_string<uint8_t>& sbas, time_t t, int offset, vector<LongTermCorrection>& ret)
|
||||
{
|
||||
LongTermCorrection ltc;
|
||||
ltc.velocity = getbitu(&sbas[0], offset, 1);
|
||||
|
|
@ -217,7 +217,7 @@ void SBASState::parse25H(const vector<uint8_t>& sbas, time_t t, int offset, vect
|
|||
|
||||
// old version with ephemeris parsing
|
||||
#if 0
|
||||
void parseSBAS25H(int sv, const vector<uint8_t>& sbas, time_t t, ofstream& sbascsv, int offset, map<int, GPSState>* gpseph, const Point& src)
|
||||
void parseSBAS25H(int sv, const basic_string<uint8_t>& sbas, time_t t, ofstream& sbascsv, int offset, map<int, GPSState>* gpseph, const Point& src)
|
||||
{
|
||||
bool velocity = getbitu(&sbas[0], offset, 1);
|
||||
|
||||
|
|
|
|||
18
sbas.hh
18
sbas.hh
|
|
@ -45,22 +45,22 @@ struct SBASState
|
|||
time_t lastUpdate{-1};
|
||||
};
|
||||
|
||||
std::pair<std::vector<SBASState::FastCorrection>, std::vector<SBASState::LongTermCorrection>> parse(const std::vector<uint8_t>& sbas, time_t now);
|
||||
std::pair<std::vector<SBASState::FastCorrection>, std::vector<SBASState::LongTermCorrection>> parse(const std::basic_string<uint8_t>& sbas, time_t now);
|
||||
|
||||
void parse0(const std::vector<uint8_t>& message, time_t now);
|
||||
void parse0(const std::basic_string<uint8_t>& message, time_t now);
|
||||
|
||||
// updates slot2prn mapping
|
||||
void parse1(const std::vector<uint8_t>& message, time_t now);
|
||||
void parse1(const std::basic_string<uint8_t>& message, time_t now);
|
||||
|
||||
|
||||
std::vector<FastCorrection> parse2_5(const std::vector<uint8_t>& message, time_t now);
|
||||
std::vector<FastCorrection> parse2_5(const std::basic_string<uint8_t>& message, time_t now);
|
||||
|
||||
std::vector<FastCorrection> parse6(const std::vector<uint8_t>& message, time_t now);
|
||||
void parse7(const std::vector<uint8_t>& message, time_t now);
|
||||
std::vector<FastCorrection> parse6(const std::basic_string<uint8_t>& message, time_t now);
|
||||
void parse7(const std::basic_string<uint8_t>& message, time_t now);
|
||||
|
||||
std::pair<std::vector<FastCorrection>, std::vector<LongTermCorrection>> parse24(const std::vector<uint8_t>& message, time_t now);
|
||||
std::pair<std::vector<FastCorrection>, std::vector<LongTermCorrection>> parse24(const std::basic_string<uint8_t>& message, time_t now);
|
||||
|
||||
std::vector<LongTermCorrection> parse25(const std::vector<uint8_t>& message, time_t now);
|
||||
std::vector<LongTermCorrection> parse25(const std::basic_string<uint8_t>& message, time_t now);
|
||||
|
||||
int getSBASNumber(int slot) const;
|
||||
SatID getSBASSatID(int slot) const;
|
||||
|
|
@ -72,7 +72,7 @@ struct SBASState
|
|||
std::map<int,int> d_slot2prn;
|
||||
int d_latency = -1;
|
||||
time_t d_lastSeen{-1};
|
||||
void parse25H(const std::vector<uint8_t>& sbas, time_t t, int offset, std::vector<LongTermCorrection>& ret);
|
||||
void parse25H(const std::basic_string<uint8_t>& sbas, time_t t, int offset, std::vector<LongTermCorrection>& ret);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
91
septool.cc
91
septool.cc
|
|
@ -49,7 +49,7 @@ static int sepsig2ubx(int sep)
|
|||
|
||||
struct SEPMessage
|
||||
{
|
||||
SEPMessage(const std::vector<uint8_t>& str) : d_store(str) {}
|
||||
SEPMessage(const std::basic_string<uint8_t>& str) : d_store(str) {}
|
||||
|
||||
uint16_t getID() // includes revision
|
||||
{
|
||||
|
|
@ -62,11 +62,11 @@ struct SEPMessage
|
|||
}
|
||||
|
||||
|
||||
std::vector<uint8_t> getPayload()
|
||||
std::basic_string<uint8_t> getPayload()
|
||||
{
|
||||
return makeVec(&d_store[0], 8);
|
||||
return d_store.substr(8);
|
||||
}
|
||||
std::vector<uint8_t> d_store;
|
||||
std::basic_string<uint8_t> d_store;
|
||||
};
|
||||
|
||||
/* format:
|
||||
|
|
@ -98,12 +98,11 @@ std::pair<SEPMessage, struct timeval> getSEPMessage(int fd, double* timeout)
|
|||
}
|
||||
struct timeval tv;
|
||||
gettimeofday(&tv, 0);
|
||||
vector <uint8_t> msg= makeVec(marker, 2);
|
||||
|
||||
basic_string<uint8_t> msg;
|
||||
msg.append(marker, 2); // 0,1
|
||||
uint8_t b[6];
|
||||
readn2Timeout(fd, b, 6, timeout);
|
||||
for(int n=0; n < 6; ++n)
|
||||
msg.push_back(b[n]);
|
||||
msg.append(b, 6); // crc id len
|
||||
|
||||
// 0,1 = crc, 2-3 = marker, 4, 5
|
||||
// uint16_t blkid = htons(b[2] + 256*b[3]);
|
||||
|
|
@ -113,8 +112,8 @@ std::pair<SEPMessage, struct timeval> getSEPMessage(int fd, double* timeout)
|
|||
|
||||
uint8_t buffer[len-8];
|
||||
res=readn2Timeout(fd, buffer, len-8, timeout);
|
||||
for(int n=0; n < len-8; ++n)
|
||||
msg.push_back(buffer[n]);
|
||||
|
||||
msg.append(buffer, len - 8); // checksum
|
||||
return make_pair(SEPMessage(msg), tv);
|
||||
}
|
||||
else if(marker[1] != '$') {
|
||||
|
|
@ -208,7 +207,7 @@ try
|
|||
uint8_t navBits[32];
|
||||
} __attribute__((packed));
|
||||
SEPInav si;
|
||||
memcpy(&si, &str[0], sizeof(si));
|
||||
memcpy(&si, str.c_str(), sizeof(si));
|
||||
// cerr<<"tow "<<si.towMsec /1000<<" wn "<<si.wn <<" sv " << (int) si.sv - 70<<" ";
|
||||
int sigid = si.src & 31;
|
||||
int pbsigid=sepsig2ubx(sigid);
|
||||
|
|
@ -224,13 +223,13 @@ try
|
|||
|
||||
|
||||
// byte order adjustment
|
||||
std::vector<uint8_t> payload;
|
||||
std::basic_string<uint8_t> payload;
|
||||
|
||||
for(unsigned int i = 0 ; i < 8; ++i) {
|
||||
payload.push_back(si.navBits[4 * i + 3]);
|
||||
payload.push_back(si.navBits[4 * i + 2]);
|
||||
payload.push_back(si.navBits[4 * i + 1]);
|
||||
payload.push_back(si.navBits[4 * i + 0]);
|
||||
payload.append(1, si.navBits[4 * i + 3]);
|
||||
payload.append(1, si.navBits[4 * i + 2]);
|
||||
payload.append(1, si.navBits[4 * i + 1]);
|
||||
payload.append(1, si.navBits[4 * i + 0]);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -250,26 +249,26 @@ by the decoding software.
|
|||
|
||||
/* so we find EVEN_PAGE ODD_PAGE */
|
||||
|
||||
vector<uint8_t> inav2;
|
||||
basic_string<uint8_t> inav2;
|
||||
// copy in the payload bits of the even page
|
||||
for(int n = 0 ; n < 14; ++n)
|
||||
inav2.push_back(getbitu(&payload[0], 2 + n*8, 8));
|
||||
inav2.append(1, getbitu(payload.c_str(), 2 + n*8, 8));
|
||||
// odd page payload bits
|
||||
for(int n = 0 ; n < 2; ++n)
|
||||
inav2.push_back(getbitu(&payload[0], 116 + n*8, 8));
|
||||
inav2.append(1, getbitu(payload.c_str(), 116 + n*8, 8));
|
||||
// cerr<<makeHexDump(inav2) << endl;
|
||||
|
||||
|
||||
vector<uint8_t> reserved1;
|
||||
basic_string<uint8_t> reserved1;
|
||||
for(int n=0; n < 5 ; ++n)
|
||||
reserved1.push_back(getbitu(&payload[0], 116 + 16 + n*8, 8));
|
||||
reserved1.append(1, getbitu(payload.c_str(), 116 + 16 + n*8, 8));
|
||||
|
||||
vector<uint8_t> crc;
|
||||
basic_string<uint8_t> crc;
|
||||
for(int n=0; n < 3 ; ++n)
|
||||
crc.push_back(getbitu(&payload[0], 116 + 16 + 40 +22 + 2 + n*8, 8));
|
||||
crc.append(1, getbitu(payload.c_str(), 116 + 16 + 40 +22 + 2 + n*8, 8));
|
||||
|
||||
|
||||
uint8_t ssp = getbitu(&payload[0], 116 + 16 + 40 + 22 + 2 + 24, 8);
|
||||
uint8_t ssp = getbitu(payload.c_str(), 116 + 16 + 40 + 22 + 2 + 24, 8);
|
||||
|
||||
// xxx add reserved2
|
||||
// xxx add sar
|
||||
|
|
@ -316,7 +315,7 @@ by the decoding software.
|
|||
uint8_t synclevel;
|
||||
} __attribute__((packed));
|
||||
TimeMsg tmsg;
|
||||
memcpy(&tmsg, &str[0], sizeof(tmsg));
|
||||
memcpy(&tmsg, str.c_str(), sizeof(tmsg));
|
||||
if(!quiet)
|
||||
cerr<< fmt::sprintf("UTC Time: %04d%02d%02d %02d:%02d:%02d\n",
|
||||
2000+tmsg.utcyear,
|
||||
|
|
@ -345,7 +344,7 @@ by the decoding software.
|
|||
uint8_t navBits[40];
|
||||
} __attribute__((packed));
|
||||
GPSCA ga;
|
||||
memcpy(&ga, &str[0], sizeof(ga));
|
||||
memcpy(&ga, str.c_str(), sizeof(ga));
|
||||
int sigid = ga.src & 31;
|
||||
// cerr<<"tow "<<sf.towMsec /1000<<" wn "<<sf.wn <<" sv " << (int) sf.sv - 70<<" sigid " << sigid <<" ";
|
||||
if(!ga.crcPassed) {
|
||||
|
|
@ -366,7 +365,7 @@ by the decoding software.
|
|||
setbitu(tmp, i*32, 30, getbitu((uint8_t*)&rev, 0, 30));
|
||||
}
|
||||
|
||||
auto payload = makeVec(tmp, 40);
|
||||
std::basic_string<uint8_t> payload(tmp, 40);
|
||||
|
||||
auto cond = getCondensedGPSMessage(payload);
|
||||
// cerr<<makeHexDump(cond)<<" ";
|
||||
|
|
@ -390,7 +389,7 @@ by the decoding software.
|
|||
nmm.mutable_gpsi()->set_gnsstow(ga.towMsec/1000 - 6); // needs to be adjusted to beginning of message
|
||||
nmm.mutable_gpsi()->set_gnssid(0);
|
||||
nmm.mutable_gpsi()->set_gnsssv(ga.sv);
|
||||
nmm.mutable_gpsi()->set_contents(string((char*)&payload[0], payload.size()));
|
||||
nmm.mutable_gpsi()->set_contents(string((char*)payload.c_str(), payload.size()));
|
||||
ns.emitNMM( nmm);
|
||||
}
|
||||
else if(res.first.getID() == 4018) { // GPS-L2C
|
||||
|
|
@ -417,7 +416,7 @@ by the decoding software.
|
|||
uint8_t navBits[32];
|
||||
} __attribute__((packed));
|
||||
SEPFnav sf;
|
||||
memcpy(&sf, &str[0], sizeof(sf));
|
||||
memcpy(&sf, str.c_str(), sizeof(sf));
|
||||
int sigid = sf.src & 31;
|
||||
// cerr<<"tow "<<sf.towMsec /1000<<" wn "<<sf.wn <<" sv " << (int) sf.sv - 70<<" sigid " << sigid <<" ";
|
||||
if(!sf.crcPassed) {
|
||||
|
|
@ -427,13 +426,13 @@ by the decoding software.
|
|||
|
||||
std::string fnav((char*)sf.navBits, 32);
|
||||
// byte order adjustment
|
||||
std::vector<uint8_t> payload;
|
||||
std::basic_string<uint8_t> payload;
|
||||
|
||||
for(unsigned int i = 0 ; i < 8; ++i) {
|
||||
payload.push_back(sf.navBits[4 * i + 3]);
|
||||
payload.push_back(sf.navBits[4 * i + 2]);
|
||||
payload.push_back(sf.navBits[4 * i + 1]);
|
||||
payload.push_back(sf.navBits[4 * i + 0]);
|
||||
payload.append(1, sf.navBits[4 * i + 3]);
|
||||
payload.append(1, sf.navBits[4 * i + 2]);
|
||||
payload.append(1, sf.navBits[4 * i + 1]);
|
||||
payload.append(1, sf.navBits[4 * i + 0]);
|
||||
}
|
||||
|
||||
NavMonMessage nmm;
|
||||
|
|
@ -486,7 +485,7 @@ by the decoding software.
|
|||
uint8_t misc;
|
||||
} __attribute__((packed));
|
||||
PVTCartesian pc;
|
||||
memcpy(&pc, &str[0], sizeof(pc));
|
||||
memcpy(&pc, str.c_str(), sizeof(pc));
|
||||
|
||||
NavMonMessage nmm;
|
||||
nmm.set_type(NavMonMessage::ObserverPositionType);
|
||||
|
|
@ -545,7 +544,7 @@ by the decoding software.
|
|||
SEPCnav sc;
|
||||
|
||||
auto str = res.first.getPayload();
|
||||
memcpy(&sc, &str[0], sizeof(sc));
|
||||
memcpy(&sc, str.c_str(), sizeof(sc));
|
||||
int sigid = sc.src & 31;
|
||||
// cerr<<"C/NAV tow "<<sc.towMsec /1000<<" wn "<<sc.wn <<" sv " << (int) sc.sv - 70<<" sigid " << sigid <<" ";
|
||||
if(!sc.crcPassed) {
|
||||
|
|
@ -555,22 +554,22 @@ by the decoding software.
|
|||
|
||||
std::string cnav((char*)sc.navBits, 64);
|
||||
// byte order adjustment
|
||||
std::vector<uint8_t> payload;
|
||||
std::basic_string<uint8_t> payload;
|
||||
|
||||
for(unsigned int i = 0 ; i < 16; ++i) {
|
||||
payload.push_back( sc.navBits[4 * i + 3]);
|
||||
payload.push_back( sc.navBits[4 * i + 2]);
|
||||
payload.push_back( sc.navBits[4 * i + 1]);
|
||||
payload.push_back( sc.navBits[4 * i + 0]);
|
||||
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[0],i, 1));
|
||||
setbitu(crc_buff, 2+i, 1,getbitu(payload.c_str(),i, 1));
|
||||
|
||||
int calccrc=rtk_crc24q(crc_buff,58);
|
||||
int realcrc= getbitu(&payload[0], 14+448, 24);
|
||||
int realcrc= getbitu(payload.c_str(), 14+448, 24);
|
||||
if (calccrc != realcrc) {
|
||||
cerr << "CRC mismatch, " << calccrc << " != " << realcrc <<endl;
|
||||
}
|
||||
|
|
@ -611,7 +610,7 @@ by the decoding software.
|
|||
uint8_t res1;
|
||||
} __attribute__((packed));
|
||||
MeasEpoch me;
|
||||
memcpy(&me, &str[0], sizeof(me));
|
||||
memcpy(&me, str.c_str(), sizeof(me));
|
||||
// cerr<<"Got "<<(int)me.n1<<" signal statuses, block1 "<<(int)me.sb1len<<", block2 "<<(int)me.sb2len<<endl;
|
||||
|
||||
struct Block1
|
||||
|
|
@ -639,7 +638,7 @@ by the decoding software.
|
|||
int pos = sizeof(me);
|
||||
for(int n = 0 ; n < me.n1; ++n) {
|
||||
Block1 b1;
|
||||
memcpy(&b1, &str[0] + pos, sizeof(b1));
|
||||
memcpy(&b1, str.c_str() + pos, sizeof(b1));
|
||||
uint8_t sigid = b1.type & 31;
|
||||
// cerr<<"sv "<<(int)b1.sv<<" sigid "<< (int)sigid <<" cn0 ";
|
||||
double db;
|
||||
|
|
@ -684,7 +683,7 @@ by the decoding software.
|
|||
|
||||
for(int m = 0 ; m < b1.n2; ++m) {
|
||||
Block2 b2;
|
||||
memcpy(&b2, &str[0] + pos, sizeof(b2));
|
||||
memcpy(&b2, str.c_str() + pos, sizeof(b2));
|
||||
pos += me.sb2len;
|
||||
sigid = b2.type & 31;
|
||||
// cerr<<"\t sigid "<<(int)sigid<<" cn0 ";
|
||||
|
|
|
|||
|
|
@ -45,7 +45,7 @@ extern "C" {
|
|||
}
|
||||
}
|
||||
|
||||
vector<TrkSatStat> parseTrkMeas(const std::vector<uint8_t>& payload)
|
||||
vector<TrkSatStat> parseTrkMeas(std::basic_string_view<uint8_t> payload)
|
||||
{
|
||||
uint8_t plainchunk[16];
|
||||
std::basic_string<uint8_t> plaintext;
|
||||
|
|
|
|||
120
ubx.cc
120
ubx.cc
|
|
@ -6,7 +6,7 @@
|
|||
#include "navmon.hh"
|
||||
|
||||
using namespace std;
|
||||
uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, const std::vector<uint8_t>& str)
|
||||
uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, std::basic_string_view<uint8_t> str)
|
||||
{
|
||||
uint8_t CK_A = 0, CK_B = 0;
|
||||
|
||||
|
|
@ -25,33 +25,33 @@ uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, const std::vector<ui
|
|||
return (CK_B << 8) + CK_A;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, const std::initializer_list<uint8_t>& lst)
|
||||
std::basic_string<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, const std::initializer_list<uint8_t>& lst)
|
||||
{
|
||||
std::vector<uint8_t> str;
|
||||
std::basic_string<uint8_t> str;
|
||||
for(const auto& a : lst)
|
||||
str.push_back(a);
|
||||
str.append(1, a);
|
||||
return buildUbxMessage(ubxClass, ubxType, str);
|
||||
}
|
||||
|
||||
std::vector<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, const std::vector<uint8_t>& str)
|
||||
std::basic_string<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, std::basic_string_view<uint8_t> str)
|
||||
{
|
||||
// 0xb5 0x62 class id len1 len2 payload cka ckb
|
||||
|
||||
std::vector<uint8_t> msg;
|
||||
msg.push_back(0xb5);
|
||||
msg.push_back(0x62);
|
||||
msg.push_back(ubxClass); // CFG
|
||||
msg.push_back(ubxType); // MSG
|
||||
msg.push_back(str.size()); // len1
|
||||
msg.push_back(str.size()/256); // len2
|
||||
std::basic_string<uint8_t> msg;
|
||||
msg.append(1, 0xb5);
|
||||
msg.append(1, 0x62);
|
||||
msg.append(1, ubxClass); // CFG
|
||||
msg.append(1, ubxType); // MSG
|
||||
msg.append(1, str.size()); // len1
|
||||
msg.append(1, str.size()/256); // len2
|
||||
|
||||
for(unsigned int n= 0 ; n < str.size(); ++n)
|
||||
msg.push_back(str[n]);
|
||||
msg.append(1, str[n]);
|
||||
|
||||
uint16_t csum = calcUbxChecksum(ubxClass, ubxType, {msg.cbegin() + 6, msg.cend()});
|
||||
uint16_t csum = calcUbxChecksum(ubxClass, ubxType, msg.substr(6));
|
||||
|
||||
msg.push_back(csum&0xff);
|
||||
msg.push_back(csum>>8);
|
||||
msg.append(1, csum&0xff);
|
||||
msg.append(1, csum>>8);
|
||||
/*
|
||||
for(const auto& c : msg) {
|
||||
fmt::fprintf(stderr, "%02x ", (int)c);
|
||||
|
|
@ -61,71 +61,71 @@ std::vector<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, const st
|
|||
return msg;
|
||||
}
|
||||
|
||||
vector<uint8_t> getInavFromSFRBXMsg(const std::vector<uint8_t>& msg,
|
||||
vector<uint8_t>& reserved1,
|
||||
vector<uint8_t>& reserved2,
|
||||
vector<uint8_t>& sar,
|
||||
vector<uint8_t>& spare,
|
||||
vector<uint8_t>& crc, uint8_t* ssp)
|
||||
basic_string<uint8_t> getInavFromSFRBXMsg(std::basic_string_view<uint8_t> msg,
|
||||
basic_string<uint8_t>& reserved1,
|
||||
basic_string<uint8_t>& reserved2,
|
||||
basic_string<uint8_t>& sar,
|
||||
basic_string<uint8_t>& spare,
|
||||
basic_string<uint8_t>& crc, uint8_t* ssp)
|
||||
{
|
||||
// byte order adjustment
|
||||
std::vector<uint8_t> payload;
|
||||
std::basic_string<uint8_t> payload;
|
||||
for(unsigned int i = 0 ; i < (msg.size() - 8) / 4; ++i)
|
||||
for(int j=1; j <= 4; ++j)
|
||||
payload.push_back(msg[8 + (i+1) * 4 -j]);
|
||||
payload.append(1, msg[8 + (i+1) * 4 -j]);
|
||||
|
||||
/* test crc (4(pad) + 114 + 82 bits) */
|
||||
unsigned char crc_buff[26]={0};
|
||||
unsigned int i,j;
|
||||
for (i=0,j= 4;i<15;i++,j+=8) setbitu(crc_buff,j,8,getbitu(&*payload.cbegin() ,i*8,8));
|
||||
for (i=0,j=118;i<11;i++,j+=8) setbitu(crc_buff,j,8,getbitu(&payload[0] + 16,i*8,8));
|
||||
if (rtk_crc24q(crc_buff,25) != getbitu(&payload[0] +16,82,24)) {
|
||||
cerr << "CRC mismatch, " << rtk_crc24q(crc_buff, 25) << " != " << getbitu(&payload[0]+16,82,24) <<endl;
|
||||
for (i=0,j= 4;i<15;i++,j+=8) setbitu(crc_buff,j,8,getbitu(payload.c_str() ,i*8,8));
|
||||
for (i=0,j=118;i<11;i++,j+=8) setbitu(crc_buff,j,8,getbitu(payload.c_str()+16,i*8,8));
|
||||
if (rtk_crc24q(crc_buff,25) != getbitu(payload.c_str()+16,82,24)) {
|
||||
cerr << "CRC mismatch, " << rtk_crc24q(crc_buff, 25) << " != " << getbitu(payload.c_str()+16,82,24) <<endl;
|
||||
throw CRCMismatch();
|
||||
}
|
||||
|
||||
crc.clear();
|
||||
for(i=0; i < 3; ++i)
|
||||
crc.push_back(getbitu(&payload[0] +16,82+i*8,8));
|
||||
crc.append(1, getbitu(payload.c_str()+16,82+i*8,8));
|
||||
|
||||
if(ssp) {
|
||||
*ssp=getbitu(&payload[0]+16,82+24,8);
|
||||
*ssp=getbitu(payload.c_str()+16,82+24,8);
|
||||
}
|
||||
|
||||
|
||||
std::vector<uint8_t> inav;
|
||||
std::basic_string<uint8_t> inav;
|
||||
|
||||
for (i=0,j=2; i<14; i++, j+=8)
|
||||
inav.push_back((unsigned char)getbitu(&payload[0] ,j,8));
|
||||
inav.append(1, (unsigned char)getbitu(payload.c_str() ,j,8));
|
||||
for (i=0,j=2; i< 2; i++, j+=8)
|
||||
inav.push_back((unsigned char)getbitu(&payload[0]+16,j,8));
|
||||
inav.append(1, (unsigned char)getbitu(payload.c_str()+16,j,8));
|
||||
|
||||
reserved1.clear();
|
||||
for(i=0, j=18; i < 5 ; i++, j+=8)
|
||||
reserved1.push_back((unsigned char)getbitu(&payload[0] +16, j, 8));
|
||||
reserved1.append(1, (unsigned char)getbitu(payload.c_str()+16, j, 8));
|
||||
// cerr<<"reserved1: "<<makeHexDump(reserved1)<<endl;
|
||||
|
||||
sar.clear();
|
||||
for(i=0, j=58; i < 3 ; i++, j+=8) // you get 24 bits
|
||||
sar.push_back((unsigned char)getbitu(&payload[0] +16, j, 8));
|
||||
sar.append(1, (unsigned char)getbitu(payload.c_str()+16, j, 8));
|
||||
|
||||
spare.clear();
|
||||
spare.push_back((unsigned char)getbitu(&payload[0]+16, 80, 2));
|
||||
spare.append(1, (unsigned char)getbitu(payload.c_str()+16, 80, 2));
|
||||
|
||||
reserved2.clear();
|
||||
reserved2.push_back((unsigned char)getbitu(&payload[0] +16, 106, 8));
|
||||
reserved2.append(1, (unsigned char)getbitu(payload.c_str()+16, 106, 8));
|
||||
|
||||
return inav;
|
||||
}
|
||||
|
||||
vector<uint8_t> getFnavFromSFRBXMsg(const std::vector<uint8_t>& msg,
|
||||
vector<uint8_t>& crc)
|
||||
basic_string<uint8_t> getFnavFromSFRBXMsg(std::basic_string_view<uint8_t> msg,
|
||||
basic_string<uint8_t>& crc)
|
||||
{
|
||||
// byte order adjustment
|
||||
std::vector<uint8_t> payload;
|
||||
std::basic_string<uint8_t> payload;
|
||||
for(unsigned int i = 0 ; i < (msg.size() - 8) / 4; ++i)
|
||||
for(int j=1; j <= 4; ++j)
|
||||
payload.push_back(msg[8 + (i+1) * 4 -j]);
|
||||
payload.append(1, msg[8 + (i+1) * 4 -j]);
|
||||
|
||||
//
|
||||
|
||||
|
|
@ -134,24 +134,24 @@ vector<uint8_t> getFnavFromSFRBXMsg(const std::vector<uint8_t>& msg,
|
|||
// 216 bits -> 27 bytes
|
||||
unsigned char crc_buff[27]={0};
|
||||
unsigned int i,j;
|
||||
for (i=0,j= 2;i<27;i++,j+=8) setbitu(crc_buff,j,8,getbitu(&payload[0] ,i*8,8));
|
||||
for (i=0,j= 2;i<27;i++,j+=8) setbitu(crc_buff,j,8,getbitu(payload.c_str() ,i*8,8));
|
||||
|
||||
if (rtk_crc24q(crc_buff,27) != getbitu(&payload[0], 214,24)) {
|
||||
cerr << "CRC mismatch, " << rtk_crc24q(crc_buff, 27) << " != " << getbitu(&payload[0], 214,24) <<endl;
|
||||
cerr << makeHexDump(payload) << " " << (int) getbitu(&payload[0], 0, 6) << endl;
|
||||
if (rtk_crc24q(crc_buff,27) != getbitu(payload.c_str(), 214,24)) {
|
||||
cerr << "CRC mismatch, " << rtk_crc24q(crc_buff, 27) << " != " << getbitu(payload.c_str(), 214,24) <<endl;
|
||||
cerr << makeHexDump(payload) << " " << (int) getbitu(payload.c_str(), 0, 6) << endl;
|
||||
throw CRCMismatch();
|
||||
}
|
||||
// cerr << "F/NAV CRC MATCHED!!"<<endl;
|
||||
|
||||
crc.clear();
|
||||
for(i=0; i < 3; ++i)
|
||||
crc.push_back(getbitu(&payload[0], 214+i*8,8));
|
||||
crc.append(1, getbitu(payload.c_str(), 214+i*8,8));
|
||||
|
||||
|
||||
std::vector<uint8_t> fnav;
|
||||
std::basic_string<uint8_t> fnav;
|
||||
|
||||
for (i=0,j=0; i<27; i++, j+=8)
|
||||
fnav.push_back((unsigned char)getbitu(&payload[0] ,j,8));
|
||||
fnav.append(1, (unsigned char)getbitu(payload.c_str() ,j,8));
|
||||
|
||||
return fnav;
|
||||
}
|
||||
|
|
@ -159,51 +159,51 @@ vector<uint8_t> getFnavFromSFRBXMsg(const std::vector<uint8_t>& msg,
|
|||
|
||||
|
||||
// XXX this should do the parity check
|
||||
vector<uint8_t> getGPSFromSFRBXMsg(const std::vector<uint8_t>& msg)
|
||||
basic_string<uint8_t> getGPSFromSFRBXMsg(std::basic_string_view<uint8_t> msg)
|
||||
{
|
||||
// byte order adjustment
|
||||
std::vector<uint8_t> payload;
|
||||
std::basic_string<uint8_t> payload;
|
||||
for(unsigned int i = 0 ; i < (msg.size() - 8) / 4; ++i)
|
||||
for(int j=1; j <= 4; ++j)
|
||||
payload.push_back( msg[8 + (i+1) * 4 -j]);
|
||||
payload.append(1, msg[8 + (i+1) * 4 -j]);
|
||||
|
||||
|
||||
return payload;
|
||||
}
|
||||
|
||||
// note, this returns the fourth UBX specific word with derived data, feel free to ignore!
|
||||
vector<uint8_t> getGlonassFromSFRBXMsg(const std::vector<uint8_t>& msg)
|
||||
basic_string<uint8_t> getGlonassFromSFRBXMsg(std::basic_string_view<uint8_t> msg)
|
||||
{
|
||||
// byte order adjustment
|
||||
std::vector<uint8_t> payload;
|
||||
std::basic_string<uint8_t> payload;
|
||||
for(unsigned int i = 0 ; i < (msg.size() - 8) / 4; ++i)
|
||||
for(int j=1; j <= 4; ++j)
|
||||
payload.push_back( msg[8 + (i+1) * 4 -j]);
|
||||
payload.append(1, msg[8 + (i+1) * 4 -j]);
|
||||
|
||||
|
||||
return payload;
|
||||
}
|
||||
|
||||
// note, this returns the fourth UBX specific word with derived data, feel free to ignore!
|
||||
vector<uint8_t> getBeidouFromSFRBXMsg(const std::vector<uint8_t>& msg)
|
||||
basic_string<uint8_t> getBeidouFromSFRBXMsg(std::basic_string_view<uint8_t> msg)
|
||||
{
|
||||
// byte order adjustment
|
||||
std::vector<uint8_t> payload;
|
||||
std::basic_string<uint8_t> payload;
|
||||
for(unsigned int i = 0 ; i < (msg.size() - 8) / 4; ++i)
|
||||
for(int j=1; j <= 4; ++j)
|
||||
payload.push_back( msg[8 + (i+1) * 4 -j]);
|
||||
payload.append(1, msg[8 + (i+1) * 4 -j]);
|
||||
|
||||
|
||||
return payload;
|
||||
}
|
||||
|
||||
vector<uint8_t> getSBASFromSFRBXMsg(const std::vector<uint8_t>& msg)
|
||||
basic_string<uint8_t> getSBASFromSFRBXMsg(std::basic_string_view<uint8_t> msg)
|
||||
{
|
||||
// byte order adjustment
|
||||
std::vector<uint8_t> payload;
|
||||
std::basic_string<uint8_t> payload;
|
||||
for(unsigned int i = 0 ; i < (msg.size() - 8) / 4; ++i)
|
||||
for(int j=1; j <= 4; ++j)
|
||||
payload.push_back( msg[8 + (i+1) * 4 -j]);
|
||||
payload.append(1, msg[8 + (i+1) * 4 -j]);
|
||||
|
||||
|
||||
return payload;
|
||||
|
|
|
|||
32
ubx.hh
32
ubx.hh
|
|
@ -3,25 +3,25 @@
|
|||
#include <vector>
|
||||
#include <cstdint>
|
||||
|
||||
uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, const std::vector<uint8_t>& str);
|
||||
std::vector<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, const std::vector<uint8_t>& str);
|
||||
uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, std::basic_string_view<uint8_t> str);
|
||||
std::basic_string<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, std::basic_string_view<uint8_t> str);
|
||||
|
||||
std::vector<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, const std::initializer_list<uint8_t>& str);
|
||||
std::basic_string<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, const std::initializer_list<uint8_t>& str);
|
||||
|
||||
std::vector<uint8_t> getInavFromSFRBXMsg(const std::vector<uint8_t>& msg,
|
||||
std::vector<uint8_t>& reserved1,
|
||||
std::vector<uint8_t>& reserved2,
|
||||
std::vector<uint8_t>& sar,
|
||||
std::vector<uint8_t>& spare,
|
||||
std::vector<uint8_t>& crc, uint8_t* ssp=0);
|
||||
std::basic_string<uint8_t> getInavFromSFRBXMsg(std::basic_string_view<uint8_t> msg,
|
||||
std::basic_string<uint8_t>& reserved1,
|
||||
std::basic_string<uint8_t>& reserved2,
|
||||
std::basic_string<uint8_t>& sar,
|
||||
std::basic_string<uint8_t>& spare,
|
||||
std::basic_string<uint8_t>& crc, uint8_t* ssp=0);
|
||||
|
||||
std::vector<uint8_t> getFnavFromSFRBXMsg(const std::vector<uint8_t>& msg,
|
||||
std::vector<uint8_t>& crc);
|
||||
std::basic_string<uint8_t> getFnavFromSFRBXMsg(std::basic_string_view<uint8_t> msg,
|
||||
std::basic_string<uint8_t>& crc);
|
||||
|
||||
std::vector<uint8_t> getGPSFromSFRBXMsg(const std::vector<uint8_t>& msg);
|
||||
std::vector<uint8_t> getGlonassFromSFRBXMsg(const std::vector<uint8_t>& msg);
|
||||
std::vector<uint8_t> getBeidouFromSFRBXMsg(const std::vector<uint8_t>& msg);
|
||||
std::vector<uint8_t> getSBASFromSFRBXMsg(const std::vector<uint8_t>& msg);
|
||||
std::basic_string<uint8_t> getGPSFromSFRBXMsg(std::basic_string_view<uint8_t> msg);
|
||||
std::basic_string<uint8_t> getGlonassFromSFRBXMsg(std::basic_string_view<uint8_t> msg);
|
||||
std::basic_string<uint8_t> getBeidouFromSFRBXMsg(std::basic_string_view<uint8_t> msg);
|
||||
std::basic_string<uint8_t> getSBASFromSFRBXMsg(std::basic_string_view<uint8_t> msg);
|
||||
struct CRCMismatch{};
|
||||
|
||||
struct TrkSatStat
|
||||
|
|
@ -32,4 +32,4 @@ struct TrkSatStat
|
|||
uint64_t tr;
|
||||
};
|
||||
|
||||
std::vector<TrkSatStat> parseTrkMeas(const std::vector<uint8_t>& payload);
|
||||
std::vector<TrkSatStat> parseTrkMeas(std::basic_string_view<uint8_t> payload);
|
||||
|
|
|
|||
83
ubxtool.cc
83
ubxtool.cc
|
|
@ -135,13 +135,13 @@ class UBXMessage
|
|||
{
|
||||
public:
|
||||
struct BadChecksum{};
|
||||
explicit UBXMessage(const std::vector<uint8_t>& src)
|
||||
explicit UBXMessage(basic_string_view<uint8_t> src)
|
||||
{
|
||||
d_raw = src;
|
||||
if(d_raw.size() < 6)
|
||||
throw std::runtime_error("Partial UBX message");
|
||||
|
||||
uint16_t csum = calcUbxChecksum(getClass(), getType(), {d_raw.cbegin() + 6, d_raw.cend()-2});
|
||||
uint16_t csum = calcUbxChecksum(getClass(), getType(), d_raw.substr(6, d_raw.size()-8));
|
||||
if(csum != d_raw.at(d_raw.size()-2) + 256*d_raw.at(d_raw.size()-1))
|
||||
throw BadChecksum();
|
||||
|
||||
|
|
@ -154,11 +154,11 @@ public:
|
|||
{
|
||||
return d_raw.at(3);
|
||||
}
|
||||
std::vector<uint8_t> getPayload() const
|
||||
std::basic_string<uint8_t> getPayload() const
|
||||
{
|
||||
return {d_raw.cbegin() +6, d_raw.cend()-2};
|
||||
return d_raw.substr(6, d_raw.size()-8);
|
||||
}
|
||||
std::vector<uint8_t> d_raw;
|
||||
std::basic_string<uint8_t> d_raw;
|
||||
};
|
||||
|
||||
bool g_fromFile{false};
|
||||
|
|
@ -191,24 +191,21 @@ std::pair<UBXMessage, struct timeval> getUBXMessage(int fd, double* timeout)
|
|||
if(marker[0]==0xb5 && marker[1]==0x62) { // bingo
|
||||
struct timeval tv;
|
||||
gettimeofday(&tv, 0);
|
||||
vector<uint8_t> msg;
|
||||
msg.push_back(marker[0]);
|
||||
msg.push_back(marker[1]);
|
||||
basic_string<uint8_t> msg;
|
||||
msg.append(marker, 2); // 0,1
|
||||
uint8_t b[4];
|
||||
readn2Timeout(fd, b, 4, timeout);
|
||||
for(int n=0; n <4 ;++n)
|
||||
msg.push_back(b[n]);
|
||||
msg.append(b, 4); // class, type, len1, len2
|
||||
|
||||
uint16_t len = b[2] + 256*b[3];
|
||||
// if (doDEBUG) { cerr<<humanTimeNow()<<" Got class "<<(int)msg[2]<<" type "<<(int)msg[3]<<", len = "<<len<<endl; }
|
||||
uint8_t buffer[len+2];
|
||||
res=readn2Timeout(fd, buffer, len+2, timeout);
|
||||
|
||||
for(auto ptr = buffer ; ptr < buffer+len+2; ++ptr)
|
||||
msg.push_back(*ptr); // checksum
|
||||
msg.append(buffer, len+2); // checksum
|
||||
if (doLOGFILE) {
|
||||
if(!g_fromFile)
|
||||
writen2(logfile, &msg[0], msg.size());
|
||||
writen2(logfile, msg.c_str(), msg.size());
|
||||
}
|
||||
return make_pair(UBXMessage(msg), tv);
|
||||
}
|
||||
|
|
@ -231,7 +228,7 @@ UBXMessage waitForUBX(int fd, int seconds, uint8_t ubxClass, uint8_t ubxType)
|
|||
throw std::runtime_error("Did not get response on time");
|
||||
}
|
||||
|
||||
UBXMessage sendAndWaitForUBX(int fd, int seconds, const vector<uint8_t>& msg, uint8_t ubxClass, uint8_t ubxType)
|
||||
UBXMessage sendAndWaitForUBX(int fd, int seconds, basic_string_view<uint8_t> msg, uint8_t ubxClass, uint8_t ubxType)
|
||||
{
|
||||
for(int n=3; n; --n) {
|
||||
writen2(fd, &msg[0], msg.size());
|
||||
|
|
@ -279,7 +276,7 @@ bool waitForUBXAckNack(int fd, int seconds, int ubxClass, int ubxType)
|
|||
throw std::runtime_error("Did not get ACK/NACK response for class "+to_string(ubxClass)+" type "+to_string(ubxType)+" on time");
|
||||
}
|
||||
|
||||
bool sendAndWaitForUBXAckNack(int fd, int seconds, const vector<uint8_t>& msg, uint8_t ubxClass, uint8_t ubxType)
|
||||
bool sendAndWaitForUBXAckNack(int fd, int seconds, basic_string_view<uint8_t> msg, uint8_t ubxClass, uint8_t ubxType)
|
||||
{
|
||||
for(int n=3; n; --n) {
|
||||
writen2(fd, &msg[0], msg.size());
|
||||
|
|
@ -303,15 +300,15 @@ void enableUBXMessageOnPort(int fd, uint8_t ubxClass, uint8_t ubxType, uint8_t p
|
|||
{
|
||||
for(int n=0 ; n < 5; ++n) {
|
||||
try {
|
||||
vector<uint8_t> payload;
|
||||
basic_string<uint8_t> payload;
|
||||
if(version9) {
|
||||
payload= vector<uint8_t>({ubxClass, ubxType, rate});
|
||||
payload= basic_string<uint8_t>({ubxClass, ubxType, rate});
|
||||
}
|
||||
else {
|
||||
if(port > 6)
|
||||
throw std::runtime_error("Port number out of range (>6)");
|
||||
|
||||
payload = vector<uint8_t>({ubxClass, ubxType, 0, 0, 0, 0, 0, 0});
|
||||
payload.assign({ubxClass, ubxType, 0, 0, 0, 0, 0, 0});
|
||||
payload[2+ port]=rate;
|
||||
}
|
||||
|
||||
|
|
@ -357,7 +354,7 @@ void readSome(int fd)
|
|||
(void)timestamp;
|
||||
if (doDEBUG) { cerr<<humanTimeNow()<<" Read some init: "<<(int)msg.getClass() << " " <<(int)msg.getType() <<endl; }
|
||||
if(msg.getClass() == 0x4)
|
||||
if (doDEBUG) { cerr<<humanTimeNow()<<" "<<string((char*)&msg.getPayload()[0], msg.getPayload().size()) <<endl; }
|
||||
if (doDEBUG) { cerr<<humanTimeNow()<<" "<<string((char*)msg.getPayload().c_str(), msg.getPayload().size()) <<endl; }
|
||||
}
|
||||
catch(TimeoutError& te) {
|
||||
cerr<<"Timeout"<<endl;
|
||||
|
|
@ -445,7 +442,7 @@ int initFD(const char* fname, bool doRTSCTS)
|
|||
|
||||
}
|
||||
|
||||
static string format_serial(const vector<uint8_t>& payload)
|
||||
static string format_serial(basic_string<uint8_t> payload)
|
||||
{
|
||||
return fmt::sprintf("%02x%02x%02x%02x%02x",
|
||||
payload[4], payload[5],
|
||||
|
|
@ -615,13 +612,13 @@ int main(int argc, char** argv)
|
|||
auto um1=sendAndWaitForUBX(fd, 1, msg, 0x0a, 0x40);
|
||||
const auto rstat = um1.getPayload();
|
||||
|
||||
uint32_t brate =getbitu(&rstat[0], 40*8, 32);
|
||||
uint32_t brate =getbitu(rstat.c_str(), 40*8, 32);
|
||||
cerr<< "baud rate " << brate << " " << htonl(brate)<< " bitu " <<getbitu((uint8_t*)&brate, 31-19, 20)<<endl;
|
||||
|
||||
auto isOn = [&rstat](int byte, int bit, const char* name, int width=4) {
|
||||
int comp = 8*width - 1;
|
||||
cerr<<name<<": ";
|
||||
uint32_t val = getbitu((const unsigned char*)&rstat[0], byte*8, width*8);
|
||||
uint32_t val = getbitu((const unsigned char*)rstat.c_str(), byte*8, width*8);
|
||||
if(getbitu((const unsigned char*)&val, comp - bit, 1))
|
||||
cerr << "ON ";
|
||||
else
|
||||
|
|
@ -673,11 +670,11 @@ int main(int argc, char** argv)
|
|||
else
|
||||
readSome(fd);
|
||||
|
||||
std::vector<uint8_t> msg;
|
||||
std::basic_string<uint8_t> msg;
|
||||
if(doReset) {
|
||||
if (doDEBUG) { cerr<<humanTimeNow()<<" Sending a soft reset"<<endl; }
|
||||
msg = buildUbxMessage(0x06, 0x04, {0x00, 0x00, 0x01, 0x00}); // soft reset
|
||||
writen2(fd, &msg[0], msg.size());
|
||||
writen2(fd, msg.c_str(), msg.size());
|
||||
usleep(100000);
|
||||
close(fd);
|
||||
for(int n=0 ; n< 20; ++n) {
|
||||
|
|
@ -701,13 +698,13 @@ int main(int argc, char** argv)
|
|||
|
||||
if (doDEBUG) { cerr<<humanTimeNow()<<" Sending version query"<<endl; }
|
||||
UBXMessage um1=sendAndWaitForUBX(fd, 1, msg, 0x0a, 0x04); // ask for version
|
||||
swversion = (char*)&um1.getPayload()[0];
|
||||
hwversion = (char*)&um1.getPayload().at(30);
|
||||
swversion = (const char*)um1.getPayload().c_str();
|
||||
hwversion = (const char*)um1.getPayload().c_str()+30;
|
||||
cerr<<humanTimeNow()<<" swVersion: "<<swversion<<endl;
|
||||
cerr<<humanTimeNow()<<" hwVersion: "<<hwversion<<endl;
|
||||
|
||||
for(unsigned int n=0; 40+30*n < um1.getPayload().size(); ++n) {
|
||||
string line = (const char*)&um1.getPayload()[0] + 40 +30*n;
|
||||
string line = (const char*)um1.getPayload().c_str() + 40 +30*n;
|
||||
cerr<<humanTimeNow()<<" Extended info: "<<line <<endl;
|
||||
|
||||
if(line.find("F9") != string::npos)
|
||||
|
|
@ -935,7 +932,7 @@ int main(int argc, char** argv)
|
|||
minCentimetersVal = surveyMinCM * 100;
|
||||
uint8_t* ptrSeconds = (uint8_t*)&minSecondsVal, *ptrCent= (uint8_t*)&minCentimetersVal;
|
||||
uint8_t cmd;
|
||||
std::vector<uint8_t> msg;
|
||||
std::basic_string<uint8_t> msg;
|
||||
if(version9) {
|
||||
cmd = 0x8a;
|
||||
msg = buildUbxMessage(0x06, cmd, {0x00, 0x01, 0x00, 0x00,
|
||||
|
|
@ -1408,7 +1405,7 @@ int main(int argc, char** argv)
|
|||
uint32_t pAcc;
|
||||
};
|
||||
pos p;
|
||||
memcpy(&p, &payload[0], sizeof(pos));
|
||||
memcpy(&p, payload.c_str(), sizeof(pos));
|
||||
if(fuzzPositionMeters) {
|
||||
p.ecefX -= (p.ecefX % (fuzzPositionMeters*100));
|
||||
p.ecefY -= (p.ecefY % (fuzzPositionMeters*100));
|
||||
|
|
@ -1486,7 +1483,7 @@ int main(int argc, char** argv)
|
|||
nmm.mutable_gpsi()->set_gnsstow(tow); // "with 6 second increments" -- needs to be adjusted
|
||||
nmm.mutable_gpsi()->set_gnssid(id.first);
|
||||
nmm.mutable_gpsi()->set_gnsssv(id.second);
|
||||
nmm.mutable_gpsi()->set_contents(string((char*)&gpsframe[0], gpsframe.size()));
|
||||
nmm.mutable_gpsi()->set_contents(string((char*)gpsframe.c_str(), gpsframe.size()));
|
||||
ns.emitNMM( nmm);
|
||||
continue;
|
||||
}
|
||||
|
|
@ -1514,11 +1511,11 @@ int main(int argc, char** argv)
|
|||
nmm.mutable_gpsc()->set_gnsstow(tow); // "with 6 second increments" -- needs to be adjusted
|
||||
nmm.mutable_gpsc()->set_gnssid(id.first);
|
||||
nmm.mutable_gpsc()->set_gnsssv(id.second);
|
||||
nmm.mutable_gpsc()->set_contents(string((char*)&cnav[0], cnav.size()));
|
||||
nmm.mutable_gpsc()->set_contents(string((char*)cnav.c_str(), cnav.size()));
|
||||
ns.emitNMM( nmm);
|
||||
}
|
||||
else if(id.first ==2) { // GALILEO
|
||||
vector<uint8_t> reserved1, reserved2, sar, spare, crc;
|
||||
basic_string<uint8_t> reserved1, reserved2, sar, spare, crc;
|
||||
uint8_t ssp;
|
||||
|
||||
if(sigid == 3) { // F/NAV
|
||||
|
|
@ -1700,7 +1697,7 @@ int main(int argc, char** argv)
|
|||
nmm.mutable_bid1()->set_gnssid(id.first);
|
||||
nmm.mutable_bid1()->set_gnsssv(id.second);
|
||||
nmm.mutable_bid1()->set_sigid(sigid);
|
||||
nmm.mutable_bid1()->set_contents(string((char*)&gstr[0], gstr.size()));
|
||||
nmm.mutable_bid1()->set_contents(string((char*)gstr.c_str(), gstr.size()));
|
||||
ns.emitNMM( nmm);
|
||||
}
|
||||
else {
|
||||
|
|
@ -1740,7 +1737,7 @@ int main(int argc, char** argv)
|
|||
nmm.mutable_gloi()->set_gnssid(id.first);
|
||||
nmm.mutable_gloi()->set_gnsssv(id.second);
|
||||
nmm.mutable_gloi()->set_sigid(sigid);
|
||||
nmm.mutable_gloi()->set_contents(string((char*)&gstr[0], gstr.size()));
|
||||
nmm.mutable_gloi()->set_contents(string((char*)gstr.c_str(), gstr.size()));
|
||||
|
||||
ns.emitNMM( nmm);
|
||||
}
|
||||
|
|
@ -1760,7 +1757,7 @@ int main(int argc, char** argv)
|
|||
nmm.set_type(NavMonMessage::SBASMessageType);
|
||||
nmm.mutable_sbm()->set_gnssid(id.first);
|
||||
nmm.mutable_sbm()->set_gnsssv(id.second);
|
||||
nmm.mutable_sbm()->set_contents(string((char*)&sbas[0], sbas.size()));
|
||||
nmm.mutable_sbm()->set_contents(string((char*)sbas.c_str(), sbas.size()));
|
||||
|
||||
ns.emitNMM( nmm);
|
||||
}
|
||||
|
|
@ -1796,7 +1793,7 @@ int main(int argc, char** argv)
|
|||
nmm.mutable_rd()->set_db(db);
|
||||
nmm.mutable_rd()->set_el(el);
|
||||
nmm.mutable_rd()->set_azi(azi);
|
||||
nmm.mutable_rd()->set_prres(*((int16_t*)(&payload[0] + 14 +12*n)) *0.1);
|
||||
nmm.mutable_rd()->set_prres(*((int16_t*)(payload.c_str()+ 14 +12*n)) *0.1);
|
||||
|
||||
uint32_t status;
|
||||
memcpy(&status, &payload[16+12*n], 4);
|
||||
|
|
@ -1861,7 +1858,7 @@ int main(int argc, char** argv)
|
|||
nmm.mutable_rd()->set_gnssid(gnssid);
|
||||
nmm.mutable_rd()->set_gnsssv(sv);
|
||||
nmm.mutable_rd()->set_db(db);
|
||||
nmm.mutable_rd()->set_prres(*((int16_t*)(&payload[0] + 12 +16*n)) *0.1); // ENDIANISM
|
||||
nmm.mutable_rd()->set_prres(*((int16_t*)(payload.c_str()+ 12 +16*n)) *0.1); // ENDIANISM
|
||||
nmm.mutable_rd()->set_sigid(sigid);
|
||||
nmm.mutable_rd()->set_el(0);
|
||||
nmm.mutable_rd()->set_azi(0);
|
||||
|
|
@ -1951,14 +1948,14 @@ int main(int argc, char** argv)
|
|||
nmm.mutable_sr()->set_gnsssv(payload[2]);
|
||||
nmm.mutable_sr()->set_sigid(1); //
|
||||
nmm.mutable_sr()->set_type(payload[1]);
|
||||
nmm.mutable_sr()->set_identifier(string((char*)&payload[0] +4, 8));
|
||||
nmm.mutable_sr()->set_identifier(string((char*)payload.c_str()+4, 8));
|
||||
nmm.mutable_sr()->set_code(payload[12]);
|
||||
nmm.mutable_sr()->set_params(string((char*)&payload[0] + 13, payload.size()-14));
|
||||
nmm.mutable_sr()->set_params(string((char*)payload.c_str()+13, payload.size()-14));
|
||||
|
||||
|
||||
string hexstring;
|
||||
for(int n = 0; n < 15; ++n)
|
||||
hexstring+=fmt::sprintf("%x", (int)getbitu(&payload[0], 36 + 4*n, 4));
|
||||
hexstring+=fmt::sprintf("%x", (int)getbitu(payload.c_str(), 36 + 4*n, 4));
|
||||
|
||||
ns.emitNMM(nmm);
|
||||
}
|
||||
|
|
@ -1996,7 +1993,7 @@ int main(int argc, char** argv)
|
|||
cerr<<"Wrong NAV-SVIN message size, skipping"<<endl;
|
||||
continue;
|
||||
}
|
||||
memcpy(&NS, &payload[0], sizeof(NS));
|
||||
memcpy(&NS, payload.c_str(), sizeof(NS));
|
||||
NS.res[0] = NS.res[1] = NS.res[2] = 0;
|
||||
NS.res2=0;
|
||||
NS.res3[0] = NS.res3[1] = 0;
|
||||
|
|
@ -2030,7 +2027,7 @@ int main(int argc, char** argv)
|
|||
cerr<<"Wrong NAV-SVIN message size, skipping"<<endl;
|
||||
continue;
|
||||
}
|
||||
memcpy(&TS, &payload[0], sizeof(TS));
|
||||
memcpy(&TS, payload.c_str(), sizeof(TS));
|
||||
TS.res3[0] = TS.res3[1] = 0;
|
||||
if(memcmp(&TS, &lastTS, sizeof(TS))) {
|
||||
cerr<<humanTimeNow()<<" TIM-SVIN valid "<< (int)TS.valid<<" active " << (int)TS.active<<" duration "<<TS.dur<<"s meanAcc " <<sqrt(TS.meanVar)/10<< "cm obs "<<TS.obs<<" ";
|
||||
|
|
@ -2058,7 +2055,7 @@ int main(int argc, char** argv)
|
|||
uint32_t pinIrq, pullH, pullL;
|
||||
} __attribute__((packed));
|
||||
MonHW mhw;
|
||||
memcpy(&mhw, &payload[0], sizeof(MonHW));
|
||||
memcpy(&mhw, payload.c_str(), sizeof(MonHW));
|
||||
// cerr << "agcCnt "<< mhw.agcCnt <<" jamind " << (unsigned int) mhw.jamInd <<" flags "<< (unsigned int)mhw.flags << endl;
|
||||
NavMonMessage nmm;
|
||||
nmm.set_sourceid(g_srcid);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue