141 lines
4.0 KiB
C++
141 lines
4.0 KiB
C++
#include <iostream>
|
|
#include "ubx.hh"
|
|
#include "fmt/format.h"
|
|
#include "fmt/printf.h"
|
|
#include "bits.hh"
|
|
|
|
using namespace std;
|
|
uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, std::basic_string_view<uint8_t> str)
|
|
{
|
|
uint8_t CK_A = 0, CK_B = 0;
|
|
|
|
auto update = [&CK_A, &CK_B](uint8_t c) {
|
|
CK_A = CK_A + c;
|
|
CK_B = CK_B + CK_A;
|
|
};
|
|
update(ubxClass);
|
|
update(ubxType);
|
|
uint16_t len = str.size();
|
|
update(((uint8_t*)&len)[0]);
|
|
update(((uint8_t*)&len)[1]);
|
|
for(unsigned int I=0; I < str.size(); I++) {
|
|
update(str[I]);
|
|
}
|
|
return (CK_B << 8) + CK_A;
|
|
}
|
|
|
|
std::basic_string<uint8_t> buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, const std::initializer_list<uint8_t>& lst)
|
|
{
|
|
std::basic_string<uint8_t> str;
|
|
for(const auto& a : lst)
|
|
str.append(1, a);
|
|
return buildUbxMessage(ubxClass, ubxType, 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::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.append(1, str[n]);
|
|
|
|
uint16_t csum = calcUbxChecksum(ubxClass, ubxType, msg.substr(6));
|
|
|
|
msg.append(1, csum&0xff);
|
|
msg.append(1, csum>>8);
|
|
/*
|
|
for(const auto& c : msg) {
|
|
fmt::fprintf(stderr, "%02x ", (int)c);
|
|
}
|
|
fmt::fprintf(stderr,"\n");
|
|
*/
|
|
return msg;
|
|
}
|
|
|
|
basic_string<uint8_t> getInavFromSFRBXMsg(std::basic_string_view<uint8_t> msg)
|
|
{
|
|
// byte order adjustment
|
|
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.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.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)) {
|
|
cout << "CRC mismatch, " << rtk_crc24q(crc_buff, 25) << " != " << getbitu(payload.c_str()+16,82,24) <<endl;
|
|
throw CRCMismatch();
|
|
}
|
|
|
|
std::basic_string<uint8_t> inav;
|
|
|
|
for (i=0,j=2; i<14; i++, j+=8)
|
|
inav.append(1, (unsigned char)getbitu(payload.c_str() ,j,8));
|
|
for (i=0,j=2; i< 2; i++, j+=8)
|
|
inav.append(1, (unsigned char)getbitu(payload.c_str()+16,j,8));
|
|
|
|
return inav;
|
|
}
|
|
|
|
// XXX this should do the parity check
|
|
basic_string<uint8_t> getGPSFromSFRBXMsg(std::basic_string_view<uint8_t> msg)
|
|
{
|
|
// byte order adjustment
|
|
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.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!
|
|
basic_string<uint8_t> getGlonassFromSFRBXMsg(std::basic_string_view<uint8_t> msg)
|
|
{
|
|
// byte order adjustment
|
|
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.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!
|
|
basic_string<uint8_t> getBeidouFromSFRBXMsg(std::basic_string_view<uint8_t> msg)
|
|
{
|
|
// byte order adjustment
|
|
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.append(1, msg[8 + (i+1) * 4 -j]);
|
|
|
|
|
|
return payload;
|
|
}
|
|
|
|
basic_string<uint8_t> getSBASFromSFRBXMsg(std::basic_string_view<uint8_t> msg)
|
|
{
|
|
// byte order adjustment
|
|
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.append(1, msg[8 + (i+1) * 4 -j]);
|
|
|
|
|
|
return payload;
|
|
}
|