/* check Galileo E1B CRC -------------------------------------------------------
* compute and check CRC of Galileo E1B page data
* args : uint8_t *data1 I E1B page part 1 (15 bytes (120 bits))
* uint8_t *data2 I E1B page part 2 (15 bytes (120 bits))
* return : int 1:okay 0: wrong parity
*-----------------------------------------------------------------------------*/
extern int checkcrc_e1b(uint8_t *data1, uint8_t *data2)
{
uint8_t crcbins[25]={0};
int i,j,crcbits[196],crc,crcmsg;
/* page part 1 */
for (i=0;i<15;i++) {
for (j=0;j<8;j++) {
if (8*i+j==114) break;
crcbits[8*i+j]=-2*(((data1[i]<<j)&0x80)>>7)+1;
}
}
/* page part 2 */
for (i=0;i<11;i++) {
for (j=0;j<8;j++) {
if (8*i+j==82) break;
crcbits[114+8*i+j]=-2*(((data2[i]<<j)&0x80)>>7)+1;
}
}
bits2byte(crcbits,196,25,1,crcbins); /* right alignment for crc */
crc=crc24q(crcbins,25); /* compute crc24 */
crcmsg=getbitu(data2,82,24); /* crc in message */
/* crc matching */
if (crc==crcmsg) return 0;
else return -1;
}
/* convert lex type 12 to rtcm ssr message -----------------------------------*/
static int lex2rtcm(const unsigned char *msg, int i, unsigned char *buff)
{
unsigned int crc;
int j,ns,type,n=0;
if (i+12>=LEXFRMLEN-LEXRSLEN) return 0;
switch ((type=getbitu(msg,i,12))) {
case 1057: ns=getbitu(msg,i+62,6); n=68+ns*135; break; /* gps */
case 1058: ns=getbitu(msg,i+61,6); n=67+ns* 76; break;
case 1059: ns=getbitu(msg,i+61,6); n=67+ns* 11;
for (j=0;j<ns;j++) n+=getbitu(msg,i+73+j*11,5)*19; break;
case 1060: ns=getbitu(msg,i+62,6); n=68+ns*205; break;
case 1061: ns=getbitu(msg,i+61,6); n=67+ns* 12; break;
case 1062: ns=getbitu(msg,i+61,6); n=67+ns* 28; break;
case 1063: ns=getbitu(msg,i+59,6); n=65+ns*134; break; /* glonass */
case 1064: ns=getbitu(msg,i+58,6); n=64+ns* 75; break;
case 1065: ns=getbitu(msg,i+58,6); n=64+ns* 10;
for (j=0;j<ns;j++) n+=getbitu(msg,i+69+j*10,5)*19; break;
case 1066: ns=getbitu(msg,i+59,6); n=65+ns*204; break;
case 1067: ns=getbitu(msg,i+58,6); n=64+ns* 11; break;
case 1068: ns=getbitu(msg,i+58,6); n=64+ns* 27; break;
case 1240: ns=getbitu(msg,i+62,6); n=68+ns*135; break; /* galileo */
case 1241: ns=getbitu(msg,i+61,6); n=67+ns* 76; break;
case 1242: ns=getbitu(msg,i+61,6); n=67+ns* 11;
for (j=0;j<ns;j++) n+=getbitu(msg,i+73+j*11,5)*19; break;
case 1243: ns=getbitu(msg,i+62,6); n=68+ns*205; break;
case 1244: ns=getbitu(msg,i+61,6); n=67+ns* 12; break;
case 1245: ns=getbitu(msg,i+61,6); n=67+ns* 28; break;
case 1246: ns=getbitu(msg,i+62,4); n=66+ns*133; break; /* qzss */
case 1247: ns=getbitu(msg,i+61,4); n=65+ns* 74; break;
case 1248: ns=getbitu(msg,i+61,4); n=65+ns* 9;
for (j=0;j<ns;j++) n+=getbitu(msg,i+69+j*9,5)*19; break;
case 1249: ns=getbitu(msg,i+62,4); n=66+ns*203; break;
case 1250: ns=getbitu(msg,i+61,4); n=65+ns* 10; break;
case 1251: ns=getbitu(msg,i+61,4); n=65+ns* 26; break;
default:
if (type) trace(2,"lex 12: unsupported type=%4d\n",type);
return 0;
}
n=(n+7)/8; /* message length (bytes) */
if (i+n*8>LEXFRMLEN-LEXRSLEN) {
trace(2,"lex 12: invalid ssr size: len=%4d\n",n);
return 0;
}
/* save rtcm message to buffer */
setbitu(buff, 0, 8,RTCM3PREAMB);
setbitu(buff, 8, 6,0);
setbitu(buff,14,10,n);
for (j=0;j<n;j++) {
buff[j+3]=getbitu(msg,i+j*8,8);
}
crc=crc24q(buff,3+n);
setbitu(buff,24+n*8,24,crc);
return n;
}
/** Write RTCM frame header and CRC into a buffer.
*
* The buffer should already contain the data message starting at the
* 4th byte of the buffer, i.e.
*
* data_message = &buff[3]
*
* The buffer must have 3 bytes free at the start to contain the header and
* must leave 3 bytes past the end free to contain the CRC. The total length of
* the buffer should be `len+6` bytes.
*
* `len` should be in the range 0-1023 as per the RTCM v3 standard.
*
* \param len The length of the data message contained in the buffer.
* \param buff A pointer to the RTCM message buffer.
* \return Zero on success, -1 if `len` is too large.
*/
s8 rtcm3_write_frame(u16 len, u8 *buff)
{
if (len > 1023)
return -1;
/* Set preamble, reserved and length bits. */
setbitu(buff, 0, 8, RTCM3_PREAMBLE);
setbitu(buff, 8, 6, 0);
setbitu(buff, 14, 10, len);
/* Calculate CRC of frame header and data message. */
u32 crc = crc24q(buff, len + 3, 0);
/* Write CRC to end of frame. */
setbitu(buff, (len+3)*8, 24, crc);
return 0;
}
/** Check RTCM frame header and CRC valid.
*
* \param buff A pointer to the RTCM message buffer to check.
* \return If valid then return length of the data message in range 0-1023.
* Returns a negative number if the frame is invalid:
* - `-1` : Preamble not valid
* - `-2` : CRC mismatch
*/
s16 rtcm3_check_frame(u8 *buff)
{
/* Check preamble byte. */
u8 preamble = getbitu(buff, 0, 8);
if (preamble != RTCM3_PREAMBLE)
return -1;
s16 len = getbitu(buff, 14, 10);
/* Calculate CRC of frame header and data message. */
u32 crc_calc = crc24q(buff, len + 3, 0);
/* Check CRC in frame. */
u32 crc_frame = getbitu(buff, (len+3)*8, 24);
if (crc_calc != crc_frame)
return -2;
return len;
}
/* decode sbas message ---------------------------------------------------------
* decode sbas message frame words and check crc
* args : gtime_t time I reception time
* int prn I sbas satellite prn number
* unsigned int *word I message frame words (24bit x 10)
* sbsmsg_t *sbsmsg O sbas message
* return : status (1:ok,0:crc error)
*-----------------------------------------------------------------------------*/
extern int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words,
sbsmsg_t *sbsmsg)
{
int i,j;
unsigned char f[29];
double tow;
trace(5,"sbsdecodemsg: prn=%d\n",prn);
if (time.time==0) return 0;
tow=time2gpst(time,&sbsmsg->week);
sbsmsg->tow=(int)(tow+DTTOL);
sbsmsg->prn=prn;
for (i=0;i<7;i++) for (j=0;j<4;j++) {
sbsmsg->msg[i*4+j]=(unsigned char)(words[i]>>((3-j)*8));
}
sbsmsg->msg[28]=(unsigned char)(words[7]>>18)&0xC0;
for (i=28;i>0;i--) f[i]=(sbsmsg->msg[i]>>6)+(sbsmsg->msg[i-1]<<2);
f[0]=sbsmsg->msg[0]>>6;
return crc24q(f,29)==(words[7]&0xFFFFFF); /* check crc */
}
/* generate rtcm 3 message -----------------------------------------------------
* generate rtcm 3 message
* args : rtcm_t *rtcm IO rtcm control struct
* int type I message type
* int sync I sync flag (1:another message follows)
* return : status (1:ok,0:error)
*-----------------------------------------------------------------------------*/
int gen_rtcm3(rtcm_t *rtcm, int type, int sync)
{
unsigned int crc;
int i = 0;
rtcm->nbit = rtcm->len = rtcm->nbyte = 0;
/* set preamble and reserved */
setbitu(rtcm->buff, i, 8, RTCM3PREAMB); i += 8;
setbitu(rtcm->buff, i, 6, 0 ); i += 6;
setbitu(rtcm->buff, i, 10, 0 ); i += 10;
/* encode rtcm 3 message body */
if (!encode_rtcm3(rtcm, type, sync)) return 0;
/* padding to align 8 bit boundary */
for (i = rtcm->nbit; i % 8; i++)
setbitu(rtcm->buff, i, 1, 0);
/* message length (header+data) (bytes) */
if ((rtcm->len = i / 8) >= 3 + 1024) {
/*trace(2,"generate rtcm 3 message length error len=%d\n",rtcm->len-3);*/
rtcm->nbit = rtcm->len = 0;
return 0;
}
/* message length without header and parity */
setbitu(rtcm->buff, 14, 10, rtcm->len - 3);
/* crc-24q */
crc = crc24q(rtcm->buff, rtcm->len, 0);
setbitu(rtcm->buff, i, 24, crc);
/* length total (bytes) */
rtcm->nbyte = rtcm->len + 3;
return 1;
}
void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data)
{
uint8_t i;
// nothing to do
if (length == 0) {
return;
}
#ifdef GPS_UBX_UCENTER
// not ready
if (gps_ubx_ucenter_get_status() != 0) {
return;
}
#endif
// go through buffer
for (i = 0; i < length; i++) {
if (rtcm.nbyte == 0) {
// wait for frame start byte
if (data[i] == RTCM3_PREAMBLE) {
rtcm.buff[rtcm.nbyte++] = data[i];
}
} else {
// fill buffer
if (rtcm.nbyte < INJECT_BUFF_SIZE) {
rtcm.buff[rtcm.nbyte++] = data[i];
if (rtcm.nbyte == 3) {
// extract length
rtcm.len = RTCMgetbitu(rtcm.buff, 14, 10) + 3;
} else {
// wait complete frame
if (rtcm.nbyte == rtcm.len + 3) {
// check CRC
unsigned int crc1 = crc24q(rtcm.buff, rtcm.len);
unsigned int crc2 = RTCMgetbitu(rtcm.buff, rtcm.len * 8, 24);
if (crc1 == crc2) {
// write to GPS
gps_ublox_write(&(UBX_GPS_LINK).device, rtcm.buff, rtcm.len + 3);
switch (packet_id) {
case RTCM3_MSG_1005 : break;
case RTCM3_MSG_1077 : break;
case RTCM3_MSG_1087 : break;
default: DEBUG_PRINT("Unknown type: %i", packet_id); break;
}
} else {
DEBUG_PRINT("Skipping message %i (CRC failed) - %d", packet_id, rtcm.buff[0]);
unsigned int j;
for (j = 1; j < rtcm.len; j++) {
DEBUG_PRINT(",%d", rtcm.buff[j]);
}
DEBUG_PRINT("\n");
}
// reset index
rtcm.nbyte = 0;
}
}
} else {
// reset index
rtcm.nbyte = 0;
}
}
}
}
