rtcm3.h

/*
 * Copyright (C) 2016 Wilco Vlenterie, Anand Sundaresan.
 * Contact: Anand Sundaresan <nomail@donotmailme.com>
 *
 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
 * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
 */

#ifndef LIBRTCM3_RTCM3_H
#define LIBRTCM3_RTCM3_H

/* Macros for RTCM message processing */
#define	READ_RESERVED   1
#define READ_LENGTH	    2
#define	READ_MESSAGE    3
#define	READ_CHECKSUM   4

#define RTCM3_PREAMBLE 0xD3
#define RTCM3_MSG_1005 0x69
#define RTCM3_MSG_1077 0xB1
#define RTCM3_MSG_1087 0xBB

/* Macros for UBX message processing */

#define UNINIT          0
#define	GOT_SYNC1       1
#define	GOT_SYNC2       2
#define	GOT_CLASS       3
#define GOT_ID          4
#define	GOT_LEN1        5
#define GOT_LEN2        6
#define GOT_PAYLOAD     7
#define GOT_CHECKSUM1   8

#define UBX_PREAMBLE1       0xB5                // Sync 1
#define UBX_PREAMBLE2       0x62                // Sync 2
#define UBX_NAV_SVIN        0x3B
#define GPS_UBX_MAX_PAYLOAD 255

#define GPS_UBX_ERR_NONE         0
#define GPS_UBX_ERR_OVERRUN      1
#define GPS_UBX_ERR_MSG_TOO_LONG 2
#define GPS_UBX_ERR_CHECKSUM     3
#define GPS_UBX_ERR_UNEXPECTED   4
#define GPS_UBX_ERR_OUT_OF_SYNC  5

#define NO_CLASS   0
#define RTCM_CLASS 1
#define UBX_CLASS  2


#define UBX_NAV_SVIN_VERSION(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+0))
#define UBX_NAV_SVIN_RES1(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+1))
#define UBX_NAV_SVIN_RES2(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+2)|*((uint8_t*)_ubx_payload+1+2)<<8)
#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)
#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)
#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)
#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)
#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)
#define UBX_NAV_SVIN_meanXHP(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+24))
#define UBX_NAV_SVIN_meanYHP(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+25))
#define UBX_NAV_SVIN_meanZHP(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+26))
#define UBX_NAV_SVIN_RES3(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+27))
#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)
#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)
#define UBX_NAV_SVIN_Valid(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+36))
#define UBX_NAV_SVIN_Active(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+37))
#define UBX_NAV_SVIN_RES4(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+38)|*((uint8_t*)_ubx_payload+1+38)<<8)

#define UBX_CFG_ID 0x06
#define UBX_CFG_TMODE3_ID 0x71
#define UBX_CFG_TMODE3_version(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+0))
#define UBX_CFG_TMODE3_RES1(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+1))
#define UBX_CFG_TMODE3_flags(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+2)|*((uint8_t*)_ubx_payload+1+2)<<8)
#define UBX_CFG_TMODE3_EcecfXOrLat(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+4)|*((uint8_t*)_ubx_payload+1+4)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+4))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+4))<<24)
#define UBX_CFG_TMODE3_EcecfYOrLon(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+8)|*((uint8_t*)_ubx_payload+1+8)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+8))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+8))<<24)
#define UBX_CFG_TMODE3_EcecfZOrAlt(_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)
#define UBX_CFG_TMODE3_EcecfXOrLatHP(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+16))
#define UBX_CFG_TMODE3_EcecfYOrLonHP(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+17))
#define UBX_CFG_TMODE3_EcecfZOrAltHP(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+18))
#define UBX_CFG_TMODE3_RES2(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+19))
#define UBX_CFG_TMODE3_FixedPosACC(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+20)|*((uint8_t*)_ubx_payload+1+20)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+20))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+20))<<24)
#define UBX_CFG_TMODE3_SvinMinDur(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+24)|*((uint8_t*)_ubx_payload+1+24)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+24))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+24))<<24)
#define UBX_CFG_TMODE3_SvinAcclimit(_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)
#define UBX_CFG_TMODE3_RES3(_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)
#define UBX_CFG_TMODE3_RES4(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+36)|*((uint8_t*)_ubx_payload+1+36)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+36))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+36))<<24)

#define UBX_ACK_ID 0x05

#define UBX_ACK_ACK_ID 0x01
#define UBX_ACK_ACK_ClsID(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+0))
#define UBX_ACK_ACK_MsgID(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+1))

#define UBX_ACK_NAK_ID 0x00
#define UBX_ACK_NAK_ClsID(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+0))
#define UBX_ACK_NAK_MsgID(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+1))


#include <errno.h>
#include "common.h"
//#include "ubx_protocol.h"

/** Return value indicating success. */
#define RTCM_OK              0
/** Return value indicating message decoded and callback executed by rtcm3_process. */
#define RTCM_OK_CALLBACK_EXECUTED 1
/** Return value indicating message decoded with no associated callback in rtcm3_process. */
#define RTCM_OK_CALLBACK_UNDEFINED 2
/** Return value indicating an error with the callback (function defined). */
#define RTCM_CALLBACK_ERROR -1
/** Return value indicating a CRC error. */
#define RTCM_CRC_ERROR      -2
/** Return value indicating an error occured whilst sending an RTCM3 message. */
#define RTCM_SEND_ERROR     -3
/** Return value indicating an error occured because an argument was NULL. */
#define RTCM_NULL_ERROR     -4

/** RTCM3 callback function prototype definition. */

typedef void (*msg_callback_t)( u8 len, u8 msg[]);

/** RTCM3 callback node.
 * Forms a linked list of callbacks.
 * \note Must be statically allocated for use with rtcm3_register_callback().
 */
typedef struct msg_callbacks_node {
  u16 msg_type;                        	/**< Message ID associated with callback. */
  msg_callback_t cb;               		/**< Pointer to callback function. */
  struct msg_callbacks_node *next; 		/**< Pointer to next node in list. */
} msg_callbacks_node_t;

/* structure for processing RTCM and UBX messages */
typedef struct {
	u8 state;
	u16 msg_type;
	u8 msg_class;
	u8 ubx_msg_class;
	u16 crc;
	u16 msg_len;
	u8 n_read;
	u8 msg_buff[1024+6+1];
	msg_callbacks_node_t* msg_callbacks_head;
	u8 status;
	u8 ck_a, ck_b;
	u8 error_cnt;
	u8 error_last;
} msg_state_t;


/* Function prototypes */

s8                          register_callback(msg_state_t* s, u16 msg_type, msg_callback_t cb, msg_callbacks_node_t *node);
void                        clear_callbacks(msg_state_t* s);
msg_callbacks_node_t* 		find_callback(msg_state_t* s, u16 msg_type);
void                        msg_state_init(msg_state_t *s);

s8                          rtcm3_process(msg_state_t *s, unsigned char buff);
s8 							ubx_process (msg_state_t *s, unsigned char buff);
unsigned int                RTCMgetbitu(unsigned char *, int, int);
int                         RTCMgetbits(unsigned char *, int , int );
static double               RTCMgetbits_38(unsigned char *, int );

inline void   		        UbxSend_CFG_TMODE3(uint8_t ubx_version, uint8_t ubx_res1, uint16_t ubx_flags, int32_t ubx_ececfxorlat, int32_t ubx_ececfyorlon, int32_t ubx_ececfzoralt, int8_t ubx_ececfxorlathp, int8_t ubx_ececfyorlonhp, int8_t ubx_ececfzoralthp, uint8_t ubx_res2, uint32_t ubx_fixedposacc, uint32_t ubx_svinmindur, uint32_t ubx_svinacclimit, uint32_t ubx_res3, uint32_t ubx_res4);
static inline void          UbxSend_CFG_MSG(uint8_t ubx_class, uint8_t ubx_msgid, uint8_t ubx_rate);
void                        ubx_header(uint8_t nav_id, uint8_t msg_id, uint16_t len);
void 						ubx_send_bytes(uint8_t len, uint8_t *bytes);
void 				        ubx_send_1byte(uint8_t byte);
void 						ubx_trailer(void);


uint8_t  					uart_write(uint8_t *buff);


/* Global variables used for RTCM processing */
int rd_msg_len      = 0;
int rd_msg_len1     = 0;
int byteIndex       = 0;
int checksumCounter = 0;
int rawIndex        = 0;

int bw 				= 0;
/* For sending ubx msg to ground station gps */
uint8_t send_ck_a, send_ck_b;


/** Register a callback for a message type.
 * Register a callback that is called when a message
 * with type msg_type is received.
 *
 * \param msg_type Message type associated with callback
 * \param cb       Pointer to message callback function
 * \param context  Pointer to context for callback function
 * \param node     Statically allocated #msg_callbacks_node_t struct
 * \return `RTCM_OK` (0) if successful, `RTCM_CALLBACK_ERROR` if callback was
 *         already registered for that message type.
 */
s8 register_callback(msg_state_t *s, u16 msg_type, msg_callback_t cb,
                         msg_callbacks_node_t *node)
{
  /* Check our callback function pointer isn't NULL. */
  if (cb == 0)
    return RTCM_NULL_ERROR;

  /* Check our callback node pointer isn't NULL. */
  if (node == 0)
    return RTCM_NULL_ERROR;

  /* Check if callback was already registered for this type. */
  if (find_callback(s, msg_type) != 0)
    return RTCM_CALLBACK_ERROR;

  /* Fill in our new rtcm3_msg_callback_node_t. */
  node->msg_type = msg_type;
  node->cb = cb;
  //node->context = context;
  /* The next pointer is set to NULL, i.e. this
   * will be the new end of the linked list.
   */
  node->next = 0;

  /* If our linked list is empty then just
   * add the new node to the start.
   */
  if (s->msg_callbacks_head == 0) {
    s->msg_callbacks_head = node;
    return RTCM_OK;
  }

  /* Find the tail of our linked list and
   * add our new node to the end.
   */
  msg_callbacks_node_t *p = s->msg_callbacks_head;
  while (p->next)
    p = p->next;

  p->next = node;

  return RTCM_OK;
}

/** Clear all registered callbacks.
 * This is probably only useful for testing but who knows!
 */
void clear_callbacks(msg_state_t *s)
{
  /* Reset the head of the callbacks list to NULL. */
  s->msg_callbacks_head = 0;
}

/** Find the callback function associated with a message type.
 * Searches through the list of registered callbacks to find the callback
 * associated with the passed message type.
 *
 * \param msg_type Message type to find callback for
 * \return Pointer to callback node (#msg_callbacks_node_t) or `NULL` if
 *         callback not found for that message type.
 */
msg_callbacks_node_t* find_callback(msg_state_t *s, u16 msg_type)
{
  /* If our list is empty, return NULL. */
  if (!s->msg_callbacks_head)
    return 0;

  /* Traverse the linked list and return the callback
   * function pointer if we find a node with a matching
   * message id.
   */
  msg_callbacks_node_t *p = s->msg_callbacks_head;
  do
    if (p->msg_type == msg_type)
      return p;

  while ((p = p->next));

  /* Didn't find a matching callback, return NULL. */
  return 0;
}

/** Initialize an #msg_state_t struct before use.
 * This resets the entire state, including all callbacks.
 * Remember to use this function to initialize the state before calling
 * rtcm3_process() for the first time.
 *
 * \param s State structure
 */
void msg_state_init(msg_state_t *s)
{
  s->state = UNINIT;
  s->msg_class = NO_CLASS;

  /* Set the IO context pointer, passed to read and write functions, to NULL. */
  // s->io_context = 0;

  /* Clear the callbacks, if any, currently in s */
  clear_callbacks(s);
}




/** Read and process RTCM3 messages.
 * Reads bytes from an input source using the provided `read` function, decodes
 * the RTCM3.
 *
 * When an RTCM3 message is successfully received then the list of callbacks is
 * searched for a callback corresponding to the received message type. If a
 * callback is found then it is called with the ID of the sender, the message
 * length and the message payload data buffer as arguments.
 *
 * \note rtcm3_process will always call `read` with n > 0
 *       (aka it will attempt to always read something)
 *
 * The supplied `read` function must have the prototype:
 *
 * ~~~
 * u32 read(u8 *buff, u32 n, void* context)
 * ~~~
 *
 * where `n` is the number of bytes requested and `buff` is the buffer into
 * which to write the received data, and `context` is the arbitrary pointer
 * set by `rtcm3_state_set_io_context`.
 * The function should return the number of
 * bytes successfully written into `buff` which may be between 0 and `n`
 * inclusive, but must never be greater than `n`.
 *
 * Note that `rtcm3_process` may not read all available bytes from the `read`
 * function so the caller should loop until all bytes available from the input
 * source have been consumed.
 *
 * \param s State structure
 * \param read Function pointer to a function that reads `n` bytes from the
 *             input source into `buff` and returns the number of bytes
 *             successfully read.
 * \return `RTCM_OK` (0) if successful but no complete message yet,
 *         `RTCM_OK_CALLBACK_EXECUTED` (1) if message decoded and callback executed,
 *         `RTCM_OK_CALLBACK_UNDEFINED` (2) if message decoded with no associated
 *         callback, and `RTCM_CRC_ERROR` (-2) if a CRC error
 *         has occurred. Thus can check for >0 to ensure good processing.
 */
s8 rtcm3_process(msg_state_t *s, unsigned char buff)
{
/*	FAKE MESSAGE
	buff[s->n_read] = s->n_read;
	s->n_read++;
	int fakeMsgLen = 200;
	if(s->n_read == fakeMsgLen)
	{
		s->n_read = 0;
		msg_callbacks_node_t* node = find_callback(s, RTCM3_MSG_1077);
		(*node->cb)(s->sender_id, fakeMsgLen, buff, node->context);
		return RTCM_OK_CALLBACK_EXECUTED;
	}else{
		return RTCM_OK;
	}
 */
	if(s->n_read == (1024 + 6) && s->state != UNINIT)
	{
		// 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!
		s->state = UNINIT;
		s->msg_class = NO_CLASS;
	}
	// Suppose we get more bytes than requested, lets still process them all
#ifdef DEBUG_PRINT_PACKAGE
	printf("0x%x ", buff);
#endif
	if(s->state != UNINIT && s->msg_class == RTCM_CLASS) s->msg_buff[s->n_read] = buff;
	switch (s->state){
	case UNINIT:
		s->n_read = 0;
		if (((int) buff) == RTCM3_PREAMBLE){
			s->msg_class = RTCM_CLASS;
			s->state = READ_RESERVED;
			rawIndex        = 0;
			checksumCounter = 0;
			byteIndex       = 0;
			s->msg_buff[s->n_read] = buff;
		}
		break;
	case READ_RESERVED:
		rd_msg_len1 = ((int) buff) & 0b00000011;
		s->state    = READ_LENGTH;
		break;
	case READ_LENGTH:
		rd_msg_len  = (rd_msg_len1 << 8) + ((int) buff) ;
		s->state    = READ_MESSAGE;
		break;
	case READ_MESSAGE:
		if (byteIndex == (rd_msg_len - 1)) s->state = READ_CHECKSUM;
		byteIndex++;
		break;
	case READ_CHECKSUM:
		checksumCounter++;
		if(checksumCounter == 3)
		{
#ifdef DEBUG_PRINT_PACKAGE
			printf("\n\n");
#endif
			s->state = UNINIT;
			// Check what message type it is
			switch(RTCMgetbitu(s->msg_buff, 24 + 0, 12))
			{
			case 1005: s->msg_type = RTCM3_MSG_1005; break;
			case 1077: s->msg_type = RTCM3_MSG_1077; break;
			case 1087: s->msg_type = RTCM3_MSG_1087; break;
			default  : printf("Unknown message type\n"); return RTCM_OK_CALLBACK_UNDEFINED;
			}
			s->n_read++;
			s->msg_len   = s->n_read;
			s->msg_class = NO_CLASS;
#ifdef NO_CALLBACK
			return RTCM_OK_CALLBACK_EXECUTED;
#else
			/* Message complete, process its callback. */
			msg_callbacks_node_t* node = find_callback(s, s->msg_type);
			if (node) {
				(*node->cb)( s->msg_len, s->msg_buff);
				return RTCM_OK_CALLBACK_EXECUTED;
			} else {
				return RTCM_OK_CALLBACK_UNDEFINED;
			}
#endif
		}
		break;
	}
	s->n_read++;
	return RTCM_OK;
}


// UBX message decoding

s8 ubx_process (msg_state_t *s, unsigned char buff)
{
	if (s->state < GOT_PAYLOAD && s->msg_class == UBX_CLASS) {
		s->ck_a += buff;
		s->ck_b += s->ck_a;
	}
	switch (s->state) {
	case UNINIT:
		if (buff == UBX_PREAMBLE1) {
			s->state++;
			s->msg_class = UBX_CLASS;
		}else{
			s->n_read = 0;
		}
		break;
	case GOT_SYNC1:
		if (buff != UBX_PREAMBLE2 && s->msg_class == UBX_CLASS) {
			s->error_last = GPS_UBX_ERR_OUT_OF_SYNC;
			goto error;
		}
		s->ck_a = 0;
		s->ck_b = 0;
		s->state ++;
		break;
	case GOT_SYNC2:
		s->ubx_msg_class = buff;
		if (s->ubx_msg_class == 5){
		printf("Ubx msg class: 0x%x \n", s->ubx_msg_class);
		}
		s->state++;
		break;
	case GOT_CLASS:
		s->msg_type = buff;
		s->state++;
		break;
	case GOT_ID:
		s->msg_len = buff;
		s->state++;
		break;
	case GOT_LEN1:
		s->msg_len |= (buff << 8);
		if (s->msg_len > GPS_UBX_MAX_PAYLOAD) {
			s->error_last = GPS_UBX_ERR_MSG_TOO_LONG;
			goto error;
		}
		s->n_read = 0;
		s->state++;
		break;
	case GOT_LEN2:
		s->msg_buff[s->n_read] = buff;
		s->n_read++;
		if (s->n_read >= s->msg_len) {
			s->state++;
		}
		break;
	case GOT_PAYLOAD:
		if (buff != s->ck_a) {
			s->error_last = GPS_UBX_ERR_CHECKSUM;
			goto error;
		}
		s->state++;
		break;
	case GOT_CHECKSUM1:
		if (buff != s->ck_b) {
			s->error_last = GPS_UBX_ERR_CHECKSUM;
			goto error;
		}
		s->msg_class = NO_CLASS;
		s->n_read = 0;
		s->state = UNINIT;
#ifdef NO_CALLBACK
					return RTCM_OK_CALLBACK_EXECUTED;
#else
					/* Message complete, process its callback. */
					msg_callbacks_node_t* node = find_callback(s, s->msg_type);
					if (node) {
						(*node->cb)( s->msg_len, s->msg_buff);
						return RTCM_OK_CALLBACK_EXECUTED;
					} else {
						return RTCM_OK_CALLBACK_UNDEFINED;
					}
#endif
		break;
	default:
		s->error_last = GPS_UBX_ERR_UNEXPECTED;
		goto error;
	}
	return RTCM_OK;
	error:
	s->error_cnt++;
	s->state = UNINIT;
	s->msg_class = NO_CLASS;
	return s->error_last;
}

unsigned int RTCMgetbitu(unsigned char *buff, int pos, int lenb)
{
	unsigned int bits=0;
	int i;
	for (i=pos;i<pos+lenb;i++) bits=(bits<<1)+((buff[i/8]>>(7-i%8))&1u);
	return bits;
}

int RTCMgetbits(unsigned char *buff, int pos, int lenb)
{
	unsigned int bits=RTCMgetbitu(buff,pos,lenb);
	if (lenb<=0||32<=lenb||!(bits&(1u<<(lenb-1)))) return (int)bits;
	return (int)(bits|(~0u<<lenb)); /* extend sign */
}

static double RTCMgetbits_38(unsigned char *buff, int pos)
{
	return (double)RTCMgetbits(buff,pos,32)*64.0+RTCMgetbitu(buff,pos+32,6);
}

#define UBX_CFG_MSG_ID 0x01
static inline void UbxSend_CFG_MSG(uint8_t ubx_class, uint8_t ubx_msgid, uint8_t ubx_rate) {
  ubx_header(UBX_CFG_ID, UBX_CFG_MSG_ID, 3);
  uint8_t _class = ubx_class;
  ubx_send_bytes(1, (uint8_t*)&_class);
  uint8_t _msgid = ubx_msgid;
  ubx_send_bytes( 1, (uint8_t*)&_msgid);
  uint8_t _rate = ubx_rate;
  ubx_send_bytes( 1, (uint8_t*)&_rate);
  ubx_trailer();
}
// Configuring the ground station survery in parameters.
inline void UbxSend_CFG_TMODE3(uint8_t ubx_version, uint8_t ubx_res1, uint16_t ubx_flags, int32_t ubx_ececfxorlat, int32_t ubx_ececfyorlon, int32_t ubx_ececfzoralt, int8_t ubx_ececfxorlathp, int8_t ubx_ececfyorlonhp, int8_t ubx_ececfzoralthp, uint8_t ubx_res2, uint32_t ubx_fixedposacc, uint32_t ubx_svinmindur, uint32_t ubx_svinacclimit, uint32_t ubx_res3, uint32_t ubx_res4) {

   ubx_header(UBX_CFG_ID, UBX_CFG_TMODE3_ID, 40);
   uint8_t _version = ubx_version;				ubx_send_bytes(1, (uint8_t*)&_version);
   uint8_t _res1 = ubx_res1; 					ubx_send_bytes(1, (uint8_t*)&_res1);
   uint16_t _flags = ubx_flags;					ubx_send_bytes(2, (uint8_t*)&_flags);
   int32_t _ececfxorlat = ubx_ececfxorlat; 		ubx_send_bytes(4, (uint8_t*)&_ececfxorlat);
   int32_t _ececfyorlon = ubx_ececfyorlon; 		ubx_send_bytes(4, (uint8_t*)&_ececfyorlon);
   int32_t _ececfzoralt = ubx_ececfzoralt; 		ubx_send_bytes(4, (uint8_t*)&_ececfzoralt);
   int8_t _ececfxorlathp = ubx_ececfxorlathp; 	ubx_send_bytes(1, (uint8_t*)&_ececfxorlathp);
   int8_t _ececfyorlonhp = ubx_ececfyorlonhp; 	ubx_send_bytes(1, (uint8_t*)&_ececfyorlonhp);
   int8_t _ececfzoralthp = ubx_ececfzoralthp; 	ubx_send_bytes(1, (uint8_t*)&_ececfzoralthp);
   uint8_t _res2 = ubx_res2; 					ubx_send_bytes(1, (uint8_t*)&_res2);
   uint32_t _fixedposacc = ubx_fixedposacc; 	ubx_send_bytes(4, (uint8_t*)&_fixedposacc);
   uint32_t _svinmindur = ubx_svinmindur; 		ubx_send_bytes(4, (uint8_t*)&_svinmindur);
   uint32_t _svinacclimit = ubx_svinacclimit; 	ubx_send_bytes(4, (uint8_t*)&_svinacclimit);
   uint32_t _res3 = ubx_res3; 					ubx_send_bytes(4, (uint8_t*)&_res3);
   uint32_t _res4 = ubx_res4; 					ubx_send_bytes(4, (uint8_t*)&_res4); // 8 seperate bytes
   ubx_trailer();
}

void ubx_header(uint8_t nav_id, uint8_t msg_id, uint16_t len) // writes the first six bytes of ubx msg package
{
	uint8_t sync1 = UBX_PREAMBLE1;
	uint8_t sync2 = UBX_PREAMBLE2;
	uart_write(&sync1);
	uart_write(&sync2);
	send_ck_a = 0;
	send_ck_b = 0;
	ubx_send_1byte(nav_id);
	ubx_send_1byte(msg_id);
	ubx_send_1byte((uint8_t)(len&0xFF));
	ubx_send_1byte((uint8_t)(len>>8));
}

void ubx_trailer(void)
{
	uart_write(&send_ck_a);
	uart_write(&send_ck_b);
}

void ubx_send_1byte(uint8_t byte)
{
	uart_write(&byte);
	send_ck_a = send_ck_a + byte;
	send_ck_b = send_ck_b + send_ck_a;
}

void ubx_send_bytes(uint8_t len, uint8_t *bytes)
{
	int i;
	for (i = 0; i < len; i++) {
		ubx_send_1byte(bytes[i]);
	}
}

uint8_t uart_write(uint8_t *p)
{
//	printf("Size of the buffer: %i, Buffer content:%0x \n",sizeof(*p),*p);
//    unsigned char buff1 = *p;
//    printf("Content in buffer: %0x \n", buff1);
	int ret = 0;
	do{
		ret = write( serial_port->fd, p, 1);
//		serial_port_flush_output(serial_port->fd);
	  } while(ret < 1 && errno == EAGAIN); //FIXME: max retry

	if(ret > 0){
		printf ("Byte content: 0x%X \n", *p);
		return ret;
	}
	else
		return 0;
}
#endif /* LIBRTCM3_RTCM3_H */