Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
rtcm3.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 Wilco Vlenterie, Anand Sundaresan.
3  * Contact: Anand Sundaresan <nomail@donotmailme.com>
4  *
5  * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
6  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
7  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
8  */
9 
10 #ifndef LIBRTCM3_RTCM3_H
11 #define LIBRTCM3_RTCM3_H
12 
13 /* Macros for RTCM message processing */
14 #define READ_RESERVED 1
15 #define READ_LENGTH 2
16 #define READ_MESSAGE 3
17 #define READ_CHECKSUM 4
18 
19 #define RTCM3_PREAMBLE 0xD3
20 #define RTCM3_MSG_1005 0x69
21 #define RTCM3_MSG_1077 0xB1
22 #define RTCM3_MSG_1087 0xBB
23 
24 /* Macros for UBX message processing */
25 
26 #define UNINIT 0
27 #define GOT_SYNC1 1
28 #define GOT_SYNC2 2
29 #define GOT_CLASS 3
30 #define GOT_ID 4
31 #define GOT_LEN1 5
32 #define GOT_LEN2 6
33 #define GOT_PAYLOAD 7
34 #define GOT_CHECKSUM1 8
35 
36 #define UBX_PREAMBLE1 0xB5 // Sync 1
37 #define UBX_PREAMBLE2 0x62 // Sync 2
38 #define UBX_NAV_SVIN 0x3B
39 #define GPS_UBX_MAX_PAYLOAD 255
40 
41 #define GPS_UBX_ERR_NONE 0
42 #define GPS_UBX_ERR_OVERRUN 1
43 #define GPS_UBX_ERR_MSG_TOO_LONG 2
44 #define GPS_UBX_ERR_CHECKSUM 3
45 #define GPS_UBX_ERR_UNEXPECTED 4
46 #define GPS_UBX_ERR_OUT_OF_SYNC 5
47 
48 #define NO_CLASS 0
49 #define RTCM_CLASS 1
50 #define UBX_CLASS 2
51 
52 
53 #define UBX_NAV_SVIN_VERSION(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+0))
54 #define UBX_NAV_SVIN_RES1(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+1))
55 #define UBX_NAV_SVIN_RES2(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+2)|*((uint8_t*)_ubx_payload+1+2)<<8)
56 #define UBX_NAV_SVIN_ITOW(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+4)|*((uint8_t*)_ubx_payload+1+4)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+4))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+4))<<24)
57 #define UBX_NAV_SVIN_dur(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+8)|*((uint8_t*)_ubx_payload+1+8)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+8))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+8))<<24)
58 #define UBX_NAV_SVIN_meanX(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+12)|*((uint8_t*)_ubx_payload+1+12)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+12))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+12))<<24)
59 #define UBX_NAV_SVIN_meanY(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+16)|*((uint8_t*)_ubx_payload+1+16)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+16))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+16))<<24)
60 #define UBX_NAV_SVIN_meanZ(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+20)|*((uint8_t*)_ubx_payload+1+20)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+20))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+20))<<24)
61 #define UBX_NAV_SVIN_meanXHP(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+24))
62 #define UBX_NAV_SVIN_meanYHP(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+25))
63 #define UBX_NAV_SVIN_meanZHP(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+26))
64 #define UBX_NAV_SVIN_RES3(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+27))
65 #define UBX_NAV_SVIN_meanACC(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+28)|*((uint8_t*)_ubx_payload+1+28)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+28))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+28))<<24)
66 #define UBX_NAV_SVIN_OBS(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+32)|*((uint8_t*)_ubx_payload+1+32)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+32))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+32))<<24)
67 #define UBX_NAV_SVIN_Valid(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+36))
68 #define UBX_NAV_SVIN_Active(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+37))
69 #define UBX_NAV_SVIN_RES4(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+38)|*((uint8_t*)_ubx_payload+1+38)<<8)
70 
71 
72 #include <errno.h>
73 #include "common.h"
74 //#include "ubx_protocol.h"
75 
77 #define RTCM_OK 0
78 
79 #define RTCM_OK_CALLBACK_EXECUTED 1
80 
81 #define RTCM_OK_CALLBACK_UNDEFINED 2
82 
83 #define RTCM_CALLBACK_ERROR -1
84 
85 #define RTCM_CRC_ERROR -2
86 
87 #define RTCM_SEND_ERROR -3
88 
89 #define RTCM_NULL_ERROR -4
90 
93 typedef void (*rtcm3_msg_callback_t)(u8 len, u8 msg[]);
94 
99 typedef struct rtcm3_msg_callbacks_node {
102  //void *context; /**< Pointer to a context */
105 
106 /* structure for processing RTCM and UBX messages */
107 typedef struct {
108  // For RTCM processing
115  u8 msg_buff[1024 + 6 + 1];
121 } msg_state_t;
122 
123 
124 /* Function prototypes */
125 
130 void msg_state_init(msg_state_t *s);
131 
132 s8 rtcm3_process(msg_state_t *s, unsigned char buff);
133 s8 ubx_process(msg_state_t *s, unsigned char buff);
134 unsigned int RTCMgetbitu(unsigned char *, int, int);
135 int RTCMgetbits(unsigned char *, int , int);
136 static double RTCMgetbits_38(unsigned char *, int);
137 
138 
139 /* Global variables used for RTCM processing */
140 int rd_msg_len = 0;
141 int rd_msg_len1 = 0;
142 int byteIndex = 0;
144 int rawIndex = 0;
145 
159 {
160  /* Check our callback function pointer isn't NULL. */
161  if (cb == 0) {
162  return RTCM_NULL_ERROR;
163  }
164 
165  /* Check our callback node pointer isn't NULL. */
166  if (node == 0) {
167  return RTCM_NULL_ERROR;
168  }
169 
170  /* Check if callback was already registered for this type. */
171  if (rtcm3_find_callback(s, msg_type) != 0) {
172  return RTCM_CALLBACK_ERROR;
173  }
174 
175  /* Fill in our new rtcm3_msg_callback_node_t. */
176  node->msg_type = msg_type;
177  node->cb = cb;
178  //node->context = context;
179  /* The next pointer is set to NULL, i.e. this
180  * will be the new end of the linked list.
181  */
182  node->next = 0;
183 
184  /* If our linked list is empty then just
185  * add the new node to the start.
186  */
187  if (s->rtcm3_msg_callbacks_head == 0) {
188  s->rtcm3_msg_callbacks_head = node;
189  return RTCM_OK;
190  }
191 
192  /* Find the tail of our linked list and
193  * add our new node to the end.
194  */
196  while (p->next) {
197  p = p->next;
198  }
199 
200  p->next = node;
201 
202  return RTCM_OK;
203 }
204 
209 {
210  /* Reset the head of the callbacks list to NULL. */
212 }
213 
223 {
224  /* If our list is empty, return NULL. */
225  if (!s->rtcm3_msg_callbacks_head) {
226  return 0;
227  }
228 
229  /* Traverse the linked list and return the callback
230  * function pointer if we find a node with a matching
231  * message id.
232  */
234  do
235  if (p->msg_type == msg_type) {
236  return p;
237  }
238 
239  while ((p = p->next));
240 
241  /* Didn't find a matching callback, return NULL. */
242  return 0;
243 }
244 
253 {
254  s->state = UNINIT;
255  s->msg_class = NO_CLASS;
256 
257  /* Set the IO context pointer, passed to read and write functions, to NULL. */
258  // s->io_context = 0;
259 
260  /* Clear the callbacks, if any, currently in s */
262 }
263 
264 
265 
266 
306 s8 rtcm3_process(msg_state_t *s, unsigned char buff)
307 {
308  /* FAKE MESSAGE
309  buff[s->n_read] = s->n_read;
310  s->n_read++;
311  int fakeMsgLen = 200;
312  if(s->n_read == fakeMsgLen)
313  {
314  s->n_read = 0;
315  rtcm3_msg_callbacks_node_t* node = rtcm3_find_callback(s, RTCM3_MSG_1077);
316  (*node->cb)(s->sender_id, fakeMsgLen, buff, node->context);
317  return RTCM_OK_CALLBACK_EXECUTED;
318  }else{
319  return RTCM_OK;
320  }
321  */
322  if (s->n_read == (1024 + 6) && s->state != UNINIT) {
323  // We have exceeded the maximum message length (10bit) + 3 opening and 3 closing bytes. And we are not at UNINIT, this is not a proper message, reset!
324  s->state = UNINIT;
325  s->msg_class = NO_CLASS;
326  }
327  // Suppose we get more bytes than requested, lets still process them all
328 #ifdef DEBUG_PRINT_PACKAGE
329  printf("0x%x ", buff);
330 #endif
331  if (s->state != UNINIT && s->msg_class == RTCM_CLASS) { s->msg_buff[s->n_read] = buff; }
332  switch (s->state) {
333  case UNINIT:
334  s->n_read = 0;
335  if (((int) buff) == RTCM3_PREAMBLE) {
336  s->msg_class = RTCM_CLASS;
337  s->state = READ_RESERVED;
338  rawIndex = 0;
339  checksumCounter = 0;
340  byteIndex = 0;
341  s->msg_buff[s->n_read] = buff;
342  }
343  break;
344  case READ_RESERVED:
345  rd_msg_len1 = ((int) buff) & 0b00000011;
346  s->state = READ_LENGTH;
347  break;
348  case READ_LENGTH:
349  rd_msg_len = (rd_msg_len1 << 8) + ((int) buff) ;
350  s->state = READ_MESSAGE;
351  break;
352  case READ_MESSAGE:
353  if (byteIndex == (rd_msg_len - 1)) { s->state = READ_CHECKSUM; }
354  byteIndex++;
355  break;
356  case READ_CHECKSUM:
357  checksumCounter++;
358  if (checksumCounter == 3) {
359 #ifdef DEBUG_PRINT_PACKAGE
360  printf("\n\n");
361 #endif
362  s->state = UNINIT;
363  // Check what message type it is
364  switch (RTCMgetbitu(s->msg_buff, 24 + 0, 12)) {
365  case 1005: s->msg_type = RTCM3_MSG_1005; break;
366  case 1077: s->msg_type = RTCM3_MSG_1077; break;
367  case 1087: s->msg_type = RTCM3_MSG_1087; break;
368  default : printf("Unknown message type\n"); return RTCM_OK_CALLBACK_UNDEFINED;
369  }
370  s->n_read++;
371  s->msg_len = s->n_read;
372  s->msg_class = NO_CLASS;
373 #ifdef NO_CALLBACK
375 #else
376  /* Message complete, process its callback. */
378  if (node) {
379  (*node->cb)(s->msg_len, s->msg_buff);
381  } else {
383  }
384 #endif
385  }
386  break;
387  }
388  s->n_read++;
389  return RTCM_OK;
390 }
391 
392 
393 // UBX message decoding
394 
395 s8 ubx_process(msg_state_t *s, unsigned char buff)
396 {
397  if (s->state < GOT_PAYLOAD && s->msg_class == UBX_CLASS) {
398  s->ck_a += buff;
399  s->ck_b += s->ck_a;
400  }
401  switch (s->state) {
402  case UNINIT:
403  if (buff == UBX_PREAMBLE1) {
404  s->state++;
405  s->msg_class = UBX_CLASS;
406  } else {
407  s->n_read = 0;
408  }
409  break;
410  case GOT_SYNC1:
411  if (buff != UBX_PREAMBLE2 && s->msg_class == UBX_CLASS) {
413  goto error;
414  }
415  s->ck_a = 0;
416  s->ck_b = 0;
417  s->state ++;
418  break;
419  case GOT_SYNC2:
420  //s->msg_class = buff;
421  s->state++;
422  break;
423  case GOT_CLASS:
424  s->msg_type = buff;
425  s->state++;
426  break;
427  case GOT_ID:
428  s->msg_len = buff;
429  s->state++;
430  break;
431  case GOT_LEN1:
432  s->msg_len |= (buff << 8);
433  if (s->msg_len > GPS_UBX_MAX_PAYLOAD) {
435  goto error;
436  }
437  s->n_read = 0;
438  s->state++;
439  break;
440  case GOT_LEN2:
441  s->msg_buff[s->n_read] = buff;
442  s->n_read++;
443  if (s->n_read >= s->msg_len) {
444  s->state++;
445  }
446  break;
447  case GOT_PAYLOAD:
448  if (buff != s->ck_a) {
450  goto error;
451  }
452  s->state++;
453  break;
454  case GOT_CHECKSUM1:
455  if (buff != s->ck_b) {
457  goto error;
458  }
459  s->msg_class = NO_CLASS;
460  s->n_read = 0;
461  s->state = UNINIT;
462 #ifdef NO_CALLBACK
464 #else
465  /* Message complete, process its callback. */
467  if (node) {
468  (*node->cb)(s->msg_len, s->msg_buff);
470  } else {
472  }
473 #endif
474  break;
475  default:
477  goto error;
478  }
479  return RTCM_OK;
480 error:
481  s->error_cnt++;
482  s->state = UNINIT;
483  s->msg_class = NO_CLASS;
484  return s->error_last;
485 }
486 
487 unsigned int RTCMgetbitu(unsigned char *buff, int pos, int lenb)
488 {
489  unsigned int bits = 0;
490  int i;
491  for (i = pos; i < pos + lenb; i++) { bits = (bits << 1) + ((buff[i / 8] >> (7 - i % 8)) & 1u); }
492  return bits;
493 }
494 
495 int RTCMgetbits(unsigned char *buff, int pos, int lenb)
496 {
497  unsigned int bits = RTCMgetbitu(buff, pos, lenb);
498  if (lenb <= 0 || 32 <= lenb || !(bits & (1u << (lenb - 1)))) { return (int)bits; }
499  return (int)(bits | (~0u << lenb)); /* extend sign */
500 }
501 
502 static double RTCMgetbits_38(unsigned char *buff, int pos)
503 {
504  return (double)RTCMgetbits(buff, pos, 32) * 64.0 + RTCMgetbitu(buff, pos + 32, 6);
505 }
506 
507 #endif /* LIBRTCM3_RTCM3_H */
#define UBX_PREAMBLE2
Definition: rtcm3.h:37
s8 ubx_process(msg_state_t *s, unsigned char buff)
Definition: rtcm3.h:395
u8 msg_class
Definition: rtcm3.h:111
int byteIndex
Definition: rtcm3.h:142
void msg_state_init(msg_state_t *s)
Initialize an msg_state_t struct before use.
Definition: rtcm3.h:252
#define READ_RESERVED
Definition: rtcm3.h:14
u8 error_last
Definition: rtcm3.h:120
void rtcm3_clear_callbacks(msg_state_t *s)
Clear all registered callbacks.
Definition: rtcm3.h:208
int rd_msg_len1
Definition: rtcm3.h:141
#define RTCM3_MSG_1077
Definition: rtcm3.h:21
#define NO_CLASS
Definition: rtcm3.h:48
#define READ_LENGTH
Definition: rtcm3.h:15
u8 ck_a
Definition: rtcm3.h:118
u8 msg_buff[1024+6+1]
Definition: rtcm3.h:115
#define RTCM3_PREAMBLE
Definition: rtcm3.h:19
int8_t s8
Signed 8-bit integer.
Definition: common.h:33
static double RTCMgetbits_38(unsigned char *, int)
Definition: rtcm3.h:502
uint8_t ck_a
Definition: w5100.c:91
#define RTCM_OK
Return value indicating success.
Definition: rtcm3.h:77
int rawIndex
Definition: rtcm3.h:144
#define GPS_UBX_ERR_OUT_OF_SYNC
Definition: rtcm3.h:46
u16 msg_len
Definition: rtcm3.h:113
u16 msg_type
Definition: rtcm3.h:110
RTCM3 callback node.
Definition: rtcm3.h:99
s8 rtcm3_process(msg_state_t *s, unsigned char buff)
Read and process RTCM3 messages.
Definition: rtcm3.h:306
unsigned int RTCMgetbitu(unsigned char *, int, int)
Definition: rtcm3.h:487
rtcm3_msg_callbacks_node_t * rtcm3_msg_callbacks_head
Definition: rtcm3.h:116
s8 rtcm3_register_callback(msg_state_t *s, u16 msg_type, rtcm3_msg_callback_t cb, rtcm3_msg_callbacks_node_t *node)
Register a callback for a message type.
Definition: rtcm3.h:157
uint8_t u8
Unsigned 8-bit integer.
Definition: common.h:41
#define GPS_UBX_ERR_MSG_TOO_LONG
Definition: rtcm3.h:43
#define GOT_PAYLOAD
Definition: rtcm3.h:33
u8 error_cnt
Definition: rtcm3.h:119
struct rtcm3_msg_callbacks_node * next
Pointer to next node in list.
Definition: rtcm3.h:103
#define READ_MESSAGE
Definition: rtcm3.h:16
#define RTCM_CALLBACK_ERROR
Return value indicating an error with the callback (function defined).
Definition: rtcm3.h:83
u8 state
Definition: rtcm3.h:109
#define UBX_CLASS
Definition: rtcm3.h:50
u8 n_read
Definition: rtcm3.h:114
rtcm3_msg_callbacks_node_t * rtcm3_find_callback(msg_state_t *s, u16 msg_type)
Find the callback function associated with a message type.
Definition: rtcm3.h:222
#define RTCM3_MSG_1087
Definition: rtcm3.h:22
#define RTCM_CLASS
Definition: rtcm3.h:49
u8 ck_b
Definition: rtcm3.h:118
#define RTCM_OK_CALLBACK_EXECUTED
Return value indicating message decoded and callback executed by rtcm3_process.
Definition: rtcm3.h:79
int RTCMgetbits(unsigned char *, int, int)
Definition: rtcm3.h:495
uint16_t u16
Unsigned 16-bit integer.
Definition: common.h:43
#define RTCM_NULL_ERROR
Return value indicating an error occured because an argument was NULL.
Definition: rtcm3.h:89
int checksumCounter
Definition: rtcm3.h:143
#define GOT_SYNC1
Definition: rtcm3.h:27
void(* rtcm3_msg_callback_t)(u8 len, u8 msg[])
RTCM3 callback function prototype definition.
Definition: rtcm3.h:93
u16 msg_type
Message ID associated with callback.
Definition: rtcm3.h:100
#define GPS_UBX_MAX_PAYLOAD
Definition: rtcm3.h:39
#define RTCM_OK_CALLBACK_UNDEFINED
Return value indicating message decoded with no associated callback in rtcm3_process.
Definition: rtcm3.h:81
#define GOT_CLASS
Definition: rtcm3.h:29
static float p[2][2]
#define GOT_ID
Definition: rtcm3.h:30
u8 status
Definition: rtcm3.h:117
#define RTCM3_MSG_1005
Definition: rtcm3.h:20
#define GOT_SYNC2
Definition: rtcm3.h:28
#define GOT_CHECKSUM1
Definition: rtcm3.h:34
#define GPS_UBX_ERR_UNEXPECTED
Definition: rtcm3.h:45
struct rtcm3_msg_callbacks_node rtcm3_msg_callbacks_node_t
RTCM3 callback node.
rtcm3_msg_callback_t cb
Pointer to callback function.
Definition: rtcm3.h:101
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
#define UNINIT
Definition: rtcm3.h:26
uint8_t buff[25]
Buffer used for general comunication over SPI (in buffer)
int rd_msg_len
Definition: rtcm3.h:140
#define GOT_LEN2
Definition: rtcm3.h:32
#define GOT_LEN1
Definition: rtcm3.h:31
#define READ_CHECKSUM
Definition: rtcm3.h:17
u16 crc
Definition: rtcm3.h:112
#define UBX_PREAMBLE1
Definition: rtcm3.h:36
#define GPS_UBX_ERR_CHECKSUM
Definition: rtcm3.h:44