Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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_4072 0x72
22 #define RTCM3_MSG_1077 0xB1
23 #define RTCM3_MSG_1087 0xBB
24 #define RTCM3_MSG_1230 0xE6
25 
26 /* Macros for UBX message processing */
27 
28 #define UNINIT 0
29 #define GOT_SYNC1 1
30 #define GOT_SYNC2 2
31 #define GOT_CLASS 3
32 #define GOT_ID 4
33 #define GOT_LEN1 5
34 #define GOT_LEN2 6
35 #define GOT_PAYLOAD 7
36 #define GOT_CHECKSUM1 8
37 
38 #define UBX_PREAMBLE1 0xB5 // Sync 1
39 #define UBX_PREAMBLE2 0x62 // Sync 2
40 #define UBX_NAV_SVIN 0x3B
41 #define GPS_UBX_MAX_PAYLOAD 255
42 
43 #define GPS_UBX_ERR_NONE 0
44 #define GPS_UBX_ERR_OVERRUN 1
45 #define GPS_UBX_ERR_MSG_TOO_LONG 2
46 #define GPS_UBX_ERR_CHECKSUM 3
47 #define GPS_UBX_ERR_UNEXPECTED 4
48 #define GPS_UBX_ERR_OUT_OF_SYNC 5
49 
50 #define NO_CLASS 0
51 #define RTCM_CLASS 1
52 #define UBX_CLASS 2
53 
54 
55 #define UBX_NAV_SVIN_VERSION(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+0))
56 #define UBX_NAV_SVIN_RES1(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+1))
57 #define UBX_NAV_SVIN_RES2(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+2)|*((uint8_t*)_ubx_payload+1+2)<<8)
58 #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)
59 #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)
60 #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)
61 #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)
62 #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)
63 #define UBX_NAV_SVIN_meanXHP(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+24))
64 #define UBX_NAV_SVIN_meanYHP(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+25))
65 #define UBX_NAV_SVIN_meanZHP(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+26))
66 #define UBX_NAV_SVIN_RES3(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+27))
67 #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)
68 #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)
69 #define UBX_NAV_SVIN_Valid(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+36))
70 #define UBX_NAV_SVIN_Active(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+37))
71 #define UBX_NAV_SVIN_RES4(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+38)|*((uint8_t*)_ubx_payload+1+38)<<8)
72 
73 
74 #include <errno.h>
75 #include "common.h"
76 //#include "ubx_protocol.h"
77 
79 #define RTCM_OK 0
81 #define RTCM_OK_CALLBACK_EXECUTED 1
83 #define RTCM_OK_CALLBACK_UNDEFINED 2
85 #define RTCM_CALLBACK_ERROR -1
87 #define RTCM_CRC_ERROR -2
89 #define RTCM_SEND_ERROR -3
91 #define RTCM_NULL_ERROR -4
92 
95 typedef void (*rtcm3_msg_callback_t)(u8 len, u8 msg[]);
96 
101 typedef struct rtcm3_msg_callbacks_node {
104  //void *context; /**< Pointer to a context */
107 
108 /* structure for processing RTCM and UBX messages */
109 typedef struct {
110  // For RTCM processing
117  u8 msg_buff[1024 + 6 + 1];
123 } msg_state_t;
124 
125 
126 /* Function prototypes */
127 
133 
134 s8 rtcm3_process(msg_state_t *s, unsigned char buff);
135 s8 ubx_process(msg_state_t *s, unsigned char buff);
136 unsigned int RTCMgetbitu(unsigned char *, int, int);
137 int RTCMgetbits(unsigned char *, int , int);
138 static double RTCMgetbits_38(unsigned char *, int);
139 
140 
141 /* Global variables used for RTCM processing */
142 int rd_msg_len = 0;
143 int rd_msg_len1 = 0;
144 int byteIndex = 0;
146 int rawIndex = 0;
147 
161 {
162  /* Check our callback function pointer isn't NULL. */
163  if (cb == 0) {
164  return RTCM_NULL_ERROR;
165  }
166 
167  /* Check our callback node pointer isn't NULL. */
168  if (node == 0) {
169  return RTCM_NULL_ERROR;
170  }
171 
172  /* Check if callback was already registered for this type. */
173  if (rtcm3_find_callback(s, msg_type) != 0) {
174  return RTCM_CALLBACK_ERROR;
175  }
176 
177  /* Fill in our new rtcm3_msg_callback_node_t. */
178  node->msg_type = msg_type;
179  node->cb = cb;
180  //node->context = context;
181  /* The next pointer is set to NULL, i.e. this
182  * will be the new end of the linked list.
183  */
184  node->next = 0;
185 
186  /* If our linked list is empty then just
187  * add the new node to the start.
188  */
189  if (s->rtcm3_msg_callbacks_head == 0) {
190  s->rtcm3_msg_callbacks_head = node;
191  return RTCM_OK;
192  }
193 
194  /* Find the tail of our linked list and
195  * add our new node to the end.
196  */
197  rtcm3_msg_callbacks_node_t *p = s->rtcm3_msg_callbacks_head;
198  while (p->next) {
199  p = p->next;
200  }
201 
202  p->next = node;
203 
204  return RTCM_OK;
205 }
206 
211 {
212  /* Reset the head of the callbacks list to NULL. */
213  s->rtcm3_msg_callbacks_head = 0;
214 }
215 
225 {
226  /* If our list is empty, return NULL. */
227  if (!s->rtcm3_msg_callbacks_head) {
228  return 0;
229  }
230 
231  /* Traverse the linked list and return the callback
232  * function pointer if we find a node with a matching
233  * message id.
234  */
235  rtcm3_msg_callbacks_node_t *p = s->rtcm3_msg_callbacks_head;
236  do
237  if (p->msg_type == msg_type) {
238  return p;
239  }
240 
241  while ((p = p->next));
242 
243  /* Didn't find a matching callback, return NULL. */
244  return 0;
245 }
246 
255 {
256  s->state = UNINIT;
257  s->msg_class = NO_CLASS;
258 
259  /* Set the IO context pointer, passed to read and write functions, to NULL. */
260  // s->io_context = 0;
261 
262  /* Clear the callbacks, if any, currently in s */
264 }
265 
266 
267 
268 
309 {
310  /* FAKE MESSAGE
311  buff[s->n_read] = s->n_read;
312  s->n_read++;
313  int fakeMsgLen = 200;
314  if(s->n_read == fakeMsgLen)
315  {
316  s->n_read = 0;
317  rtcm3_msg_callbacks_node_t* node = rtcm3_find_callback(s, RTCM3_MSG_1077);
318  (*node->cb)(s->sender_id, fakeMsgLen, buff, node->context);
319  return RTCM_OK_CALLBACK_EXECUTED;
320  }else{
321  return RTCM_OK;
322  }
323  */
324  if (s->n_read == (1024 + 6) && s->state != UNINIT) {
325  // 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!
326  s->state = UNINIT;
327  s->msg_class = NO_CLASS;
328  }
329  // Suppose we get more bytes than requested, lets still process them all
330 #ifdef DEBUG_PRINT_PACKAGE
331  printf("0x%x ", buff);
332 #endif
333  if (s->state != UNINIT && s->msg_class == RTCM_CLASS) { s->msg_buff[s->n_read] = buff; }
334  switch (s->state) {
335  case UNINIT:
336  s->n_read = 0;
337  if (((int) buff) == RTCM3_PREAMBLE) {
338  s->msg_class = RTCM_CLASS;
339  s->state = READ_RESERVED;
340  rawIndex = 0;
341  checksumCounter = 0;
342  byteIndex = 0;
343  s->msg_buff[s->n_read] = buff;
344  }
345  break;
346  case READ_RESERVED:
347  rd_msg_len1 = ((int) buff) & 0b00000011;
348  s->state = READ_LENGTH;
349  break;
350  case READ_LENGTH:
351  rd_msg_len = (rd_msg_len1 << 8) + ((int) buff) ;
352  s->state = READ_MESSAGE;
353  break;
354  case READ_MESSAGE:
355  if (byteIndex >= (rd_msg_len - 1)) { s->state = READ_CHECKSUM; }
356  byteIndex++;
357  break;
358  case READ_CHECKSUM:
359  checksumCounter++;
360  if (checksumCounter == 3) {
361 #ifdef DEBUG_PRINT_PACKAGE
362  printf("\n\n");
363 #endif
364  s->state = UNINIT;
365  // Check what message type it is
366  switch (RTCMgetbitu(s->msg_buff, 24 + 0, 12)) {
367  case 1005: s->msg_type = RTCM3_MSG_1005; break;
368  case 4072: s->msg_type = RTCM3_MSG_4072; break;
369  case 1077: s->msg_type = RTCM3_MSG_1077; break;
370  case 1087: s->msg_type = RTCM3_MSG_1087; break;
371  case 1230: s->msg_type = RTCM3_MSG_1230; break;
372  default:
373 #ifdef DEBUG_PRINT_PACKAGE
374  printf("Unknown message type %u\n", RTCMgetbitu(s->msg_buff, 24 + 0, 12));
375 #endif
377  }
378  s->n_read++;
379  s->msg_len = s->n_read;
380  s->msg_class = NO_CLASS;
381 #ifdef NO_CALLBACK
383 #else
384  /* Message complete, process its callback. */
386  if (node) {
387  (*node->cb)(s->msg_len, s->msg_buff);
389  } else {
391  }
392 #endif
393  }
394  break;
395  }
396  s->n_read++;
397  return RTCM_OK;
398 }
399 
400 
401 // UBX message decoding
402 
403 s8 ubx_process(msg_state_t *s, unsigned char buff)
404 {
405  if (s->state < GOT_PAYLOAD && s->msg_class == UBX_CLASS) {
406  s->ck_a += buff;
407  s->ck_b += s->ck_a;
408  }
409  switch (s->state) {
410  case UNINIT:
411  if (buff == UBX_PREAMBLE1) {
412  s->state++;
413  s->msg_class = UBX_CLASS;
414  } else {
415  s->n_read = 0;
416  }
417  break;
418  case GOT_SYNC1:
419  if (buff != UBX_PREAMBLE2 && s->msg_class == UBX_CLASS) {
420  s->error_last = GPS_UBX_ERR_OUT_OF_SYNC;
421  goto error;
422  }
423  s->ck_a = 0;
424  s->ck_b = 0;
425  s->state ++;
426  break;
427  case GOT_SYNC2:
428  //s->msg_class = buff;
429  s->state++;
430  break;
431  case GOT_CLASS:
432  s->msg_type = buff;
433  s->state++;
434  break;
435  case GOT_ID:
436  s->msg_len = buff;
437  s->state++;
438  break;
439  case GOT_LEN1:
440  s->msg_len |= (buff << 8);
441  if (s->msg_len > GPS_UBX_MAX_PAYLOAD) {
442  s->error_last = GPS_UBX_ERR_MSG_TOO_LONG;
443  goto error;
444  }
445  s->n_read = 0;
446  s->state++;
447  break;
448  case GOT_LEN2:
449  s->msg_buff[s->n_read] = buff;
450  s->n_read++;
451  if (s->n_read >= s->msg_len) {
452  s->state++;
453  }
454  break;
455  case GOT_PAYLOAD:
456  if (buff != s->ck_a) {
457  s->error_last = GPS_UBX_ERR_CHECKSUM;
458  goto error;
459  }
460  s->state++;
461  break;
462  case GOT_CHECKSUM1:
463  if (buff != s->ck_b) {
464  s->error_last = GPS_UBX_ERR_CHECKSUM;
465  goto error;
466  }
467  s->msg_class = NO_CLASS;
468  s->n_read = 0;
469  s->state = UNINIT;
470 #ifdef NO_CALLBACK
472 #else
473  /* Message complete, process its callback. */
475  if (node) {
476  (*node->cb)(s->msg_len, s->msg_buff);
478  } else {
480  }
481 #endif
482  break;
483  default:
484  s->error_last = GPS_UBX_ERR_UNEXPECTED;
485  goto error;
486  }
487  return RTCM_OK;
488 error:
489  s->error_cnt++;
490  s->state = UNINIT;
491  s->msg_class = NO_CLASS;
492  return s->error_last;
493 }
494 
495 unsigned int RTCMgetbitu(unsigned char *buff, int pos, int lenb)
496 {
497  unsigned int bits = 0;
498  int i;
499  for (i = pos; i < pos + lenb; i++) { bits = (bits << 1) + ((buff[i / 8] >> (7 - i % 8)) & 1u); }
500  return bits;
501 }
502 
503 int RTCMgetbits(unsigned char *buff, int pos, int lenb)
504 {
505  unsigned int bits = RTCMgetbitu(buff, pos, lenb);
506  if (lenb <= 0 || 32 <= lenb || !(bits & (1u << (lenb - 1)))) { return (int)bits; }
507  return (int)(bits | (~0u << lenb)); /* extend sign */
508 }
509 
510 static double RTCMgetbits_38(unsigned char *buff, int pos)
511 {
512  return (double)RTCMgetbits(buff, pos, 32) * 64.0 + RTCMgetbitu(buff, pos + 32, 6);
513 }
514 
515 #endif /* LIBRTCM3_RTCM3_H */
uint8_t u8
Unsigned 8-bit integer.
Definition: common.h:41
int8_t s8
Signed 8-bit integer.
Definition: common.h:33
uint16_t u16
Unsigned 16-bit integer.
Definition: common.h:43
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
uint8_t buff[25]
Buffer used for general comunication over SPI (in buffer)
static float p[2][2]
static uint32_t s
#define GOT_CLASS
Definition: rtcm3.h:31
u8 state
Definition: rtcm3.h:111
#define RTCM3_MSG_1005
Definition: rtcm3.h:20
u16 msg_len
Definition: rtcm3.h:115
#define GPS_UBX_ERR_UNEXPECTED
Definition: rtcm3.h:47
struct rtcm3_msg_callbacks_node rtcm3_msg_callbacks_node_t
RTCM3 callback node.
int checksumCounter
Definition: rtcm3.h:145
void(* rtcm3_msg_callback_t)(u8 len, u8 msg[])
RTCM3 callback function prototype definition.
Definition: rtcm3.h:95
#define READ_MESSAGE
Definition: rtcm3.h:16
#define GOT_SYNC2
Definition: rtcm3.h:30
#define UBX_PREAMBLE1
Definition: rtcm3.h:38
#define RTCM3_MSG_1077
Definition: rtcm3.h:22
#define RTCM3_MSG_4072
Definition: rtcm3.h:21
#define UBX_CLASS
Definition: rtcm3.h:52
s8 ubx_process(msg_state_t *s, unsigned char buff)
Definition: rtcm3.h:403
int rd_msg_len
Definition: rtcm3.h:142
#define RTCM_OK
Return value indicating success.
Definition: rtcm3.h:79
#define RTCM3_MSG_1230
Definition: rtcm3.h:24
#define GOT_LEN1
Definition: rtcm3.h:33
#define RTCM_OK_CALLBACK_UNDEFINED
Return value indicating message decoded with no associated callback in rtcm3_process.
Definition: rtcm3.h:83
void msg_state_init(msg_state_t *s)
Initialize an msg_state_t struct before use.
Definition: rtcm3.h:254
#define GPS_UBX_ERR_MSG_TOO_LONG
Definition: rtcm3.h:45
#define NO_CLASS
Definition: rtcm3.h:50
#define RTCM3_MSG_1087
Definition: rtcm3.h:23
#define GOT_PAYLOAD
Definition: rtcm3.h:35
#define GOT_SYNC1
Definition: rtcm3.h:29
#define UNINIT
Definition: rtcm3.h:28
#define GOT_CHECKSUM1
Definition: rtcm3.h:36
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:224
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:159
#define RTCM_OK_CALLBACK_EXECUTED
Return value indicating message decoded and callback executed by rtcm3_process.
Definition: rtcm3.h:81
u8 n_read
Definition: rtcm3.h:116
#define READ_CHECKSUM
Definition: rtcm3.h:17
#define GOT_ID
Definition: rtcm3.h:32
#define GPS_UBX_ERR_CHECKSUM
Definition: rtcm3.h:46
unsigned int RTCMgetbitu(unsigned char *, int, int)
Definition: rtcm3.h:495
#define RTCM_NULL_ERROR
Return value indicating an error occured because an argument was NULL.
Definition: rtcm3.h:91
#define READ_RESERVED
Definition: rtcm3.h:14
#define RTCM_CALLBACK_ERROR
Return value indicating an error with the callback (function defined).
Definition: rtcm3.h:85
struct rtcm3_msg_callbacks_node * next
Pointer to next node in list.
Definition: rtcm3.h:105
u8 ck_a
Definition: rtcm3.h:120
u8 error_cnt
Definition: rtcm3.h:121
u8 error_last
Definition: rtcm3.h:122
#define READ_LENGTH
Definition: rtcm3.h:15
s8 rtcm3_process(msg_state_t *s, unsigned char buff)
Read and process RTCM3 messages.
Definition: rtcm3.h:308
#define RTCM_CLASS
Definition: rtcm3.h:51
u8 status
Definition: rtcm3.h:119
int rd_msg_len1
Definition: rtcm3.h:143
u16 msg_type
Message ID associated with callback.
Definition: rtcm3.h:102
#define RTCM3_PREAMBLE
Definition: rtcm3.h:19
#define UBX_PREAMBLE2
Definition: rtcm3.h:39
rtcm3_msg_callback_t cb
Pointer to callback function.
Definition: rtcm3.h:103
int rawIndex
Definition: rtcm3.h:146
int byteIndex
Definition: rtcm3.h:144
#define GPS_UBX_MAX_PAYLOAD
Definition: rtcm3.h:41
rtcm3_msg_callbacks_node_t * rtcm3_msg_callbacks_head
Definition: rtcm3.h:118
void rtcm3_clear_callbacks(msg_state_t *s)
Clear all registered callbacks.
Definition: rtcm3.h:210
u16 crc
Definition: rtcm3.h:114
#define GOT_LEN2
Definition: rtcm3.h:34
#define GPS_UBX_ERR_OUT_OF_SYNC
Definition: rtcm3.h:48
static double RTCMgetbits_38(unsigned char *, int)
Definition: rtcm3.h:510
u8 msg_class
Definition: rtcm3.h:113
int RTCMgetbits(unsigned char *, int, int)
Definition: rtcm3.h:503
u16 msg_type
Definition: rtcm3.h:112
RTCM3 callback node.
Definition: rtcm3.h:101
uint8_t ck_b
Definition: w5100.c:91