int
MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
{
int ret = 0;
if (_decode_state == MTK_DECODE_UNINIT) {
if (b == MTK_SYNC1_V16) {
_decode_state = MTK_DECODE_GOT_CK_A;
_mtk_revision = 16;
} else if (b == MTK_SYNC1_V19) {
_decode_state = MTK_DECODE_GOT_CK_A;
_mtk_revision = 19;
}
} else if (_decode_state == MTK_DECODE_GOT_CK_A) {
if (b == MTK_SYNC2) {
_decode_state = MTK_DECODE_GOT_CK_B;
} else {
// Second start symbol was wrong, reset state machine
decode_init();
}
} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
// Add to checksum
if (_rx_count < 33)
add_byte_to_checksum(b);
// Fill packet buffer
((uint8_t *)(&packet))[_rx_count] = b;
_rx_count++;
/* Packet size minus checksum, XXX ? */
if (_rx_count >= sizeof(packet)) {
/* Compare checksum */
if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
ret = 1;
} else {
ret = -1;
}
// Reset state machine to decode next packet
decode_init();
}
}
return ret;
}
int
UBX::parse_char(uint8_t b)
{
switch (_decode_state) {
/* First, look for sync1 */
case UBX_DECODE_UNINIT:
if (b == UBX_SYNC1) {
_decode_state = UBX_DECODE_GOT_SYNC1;
}
break;
/* Second, look for sync2 */
case UBX_DECODE_GOT_SYNC1:
if (b == UBX_SYNC2) {
_decode_state = UBX_DECODE_GOT_SYNC2;
} else {
/* Second start symbol was wrong, reset state machine */
decode_init();
/* don't return error, it can be just false sync1 */
}
break;
/* Now look for class */
case UBX_DECODE_GOT_SYNC2:
/* everything except sync1 and sync2 needs to be added to the checksum */
add_byte_to_checksum(b);
_message_class = b;
_decode_state = UBX_DECODE_GOT_CLASS;
break;
case UBX_DECODE_GOT_CLASS:
add_byte_to_checksum(b);
_message_id = b;
_decode_state = UBX_DECODE_GOT_MESSAGEID;
break;
case UBX_DECODE_GOT_MESSAGEID:
add_byte_to_checksum(b);
_payload_size = b; //this is the first length byte
_decode_state = UBX_DECODE_GOT_LENGTH1;
break;
case UBX_DECODE_GOT_LENGTH1:
add_byte_to_checksum(b);
_payload_size += b << 8; // here comes the second byte of length
_decode_state = UBX_DECODE_GOT_LENGTH2;
break;
case UBX_DECODE_GOT_LENGTH2:
/* Add to checksum if not yet at checksum byte */
if (_rx_count < _payload_size)
add_byte_to_checksum(b);
_rx_buffer[_rx_count] = b;
/* once the payload has arrived, we can process the information */
if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
/* compare checksum */
if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) {
decode_init();
return 1; // message received successfully
} else {
warnx("checksum wrong");
decode_init();
return -1;
}
} else if (_rx_count < RECV_BUFFER_SIZE) {
_rx_count++;
} else {
warnx("buffer full");
decode_init();
return -1;
}
break;
default:
break;
}
return 0; // message decoding in progress
}
int
UBX::parse_char(uint8_t b)
{
switch (_decode_state) {
/* First, look for sync1 */
case UBX_DECODE_UNINIT:
if (b == UBX_SYNC1) {
_decode_state = UBX_DECODE_GOT_SYNC1;
}
break;
/* Second, look for sync2 */
case UBX_DECODE_GOT_SYNC1:
if (b == UBX_SYNC2) {
_decode_state = UBX_DECODE_GOT_SYNC2;
} else {
/* Second start symbol was wrong, reset state machine */
decode_init();
}
break;
/* Now look for class */
case UBX_DECODE_GOT_SYNC2:
/* everything except sync1 and sync2 needs to be added to the checksum */
add_byte_to_checksum(b);
/* check for known class */
switch (b) {
case UBX_CLASS_ACK:
_decode_state = UBX_DECODE_GOT_CLASS;
_message_class = ACK;
break;
case UBX_CLASS_NAV:
_decode_state = UBX_DECODE_GOT_CLASS;
_message_class = NAV;
break;
// case UBX_CLASS_RXM:
// _decode_state = UBX_DECODE_GOT_CLASS;
// _message_class = RXM;
// break;
case UBX_CLASS_CFG:
_decode_state = UBX_DECODE_GOT_CLASS;
_message_class = CFG;
break;
default: //unknown class: reset state machine
decode_init();
break;
}
break;
case UBX_DECODE_GOT_CLASS:
add_byte_to_checksum(b);
switch (_message_class) {
case NAV:
switch (b) {
case UBX_MESSAGE_NAV_POSLLH:
_decode_state = UBX_DECODE_GOT_MESSAGEID;
_message_id = NAV_POSLLH;
break;
case UBX_MESSAGE_NAV_SOL:
_decode_state = UBX_DECODE_GOT_MESSAGEID;
_message_id = NAV_SOL;
break;
case UBX_MESSAGE_NAV_TIMEUTC:
_decode_state = UBX_DECODE_GOT_MESSAGEID;
_message_id = NAV_TIMEUTC;
break;
// case UBX_MESSAGE_NAV_DOP:
// _decode_state = UBX_DECODE_GOT_MESSAGEID;
// _message_id = NAV_DOP;
// break;
case UBX_MESSAGE_NAV_SVINFO:
_decode_state = UBX_DECODE_GOT_MESSAGEID;
_message_id = NAV_SVINFO;
break;
case UBX_MESSAGE_NAV_VELNED:
_decode_state = UBX_DECODE_GOT_MESSAGEID;
_message_id = NAV_VELNED;
break;
default: //unknown class: reset state machine, should not happen
decode_init();
break;
}
break;
// case RXM:
// switch (b) {
// case UBX_MESSAGE_RXM_SVSI:
// _decode_state = UBX_DECODE_GOT_MESSAGEID;
// _message_id = RXM_SVSI;
// break;
//
// default: //unknown class: reset state machine, should not happen
// decode_init();
// break;
// }
// break;
case CFG:
switch (b) {
case UBX_MESSAGE_CFG_NAV5:
_decode_state = UBX_DECODE_GOT_MESSAGEID;
_message_id = CFG_NAV5;
break;
default: //unknown class: reset state machine, should not happen
decode_init();
break;
}
break;
case ACK:
switch (b) {
case UBX_MESSAGE_ACK_ACK:
_decode_state = UBX_DECODE_GOT_MESSAGEID;
_message_id = ACK_ACK;
break;
case UBX_MESSAGE_ACK_NAK:
_decode_state = UBX_DECODE_GOT_MESSAGEID;
_message_id = ACK_NAK;
break;
default: //unknown class: reset state machine, should not happen
decode_init();
break;
}
break;
default: //should not happen because we set the class
warnx("UBX Error, we set a class that we don't know");
decode_init();
// config_needed = true;
break;
}
break;
case UBX_DECODE_GOT_MESSAGEID:
add_byte_to_checksum(b);
_payload_size = b; //this is the first length byte
_decode_state = UBX_DECODE_GOT_LENGTH1;
break;
case UBX_DECODE_GOT_LENGTH1:
add_byte_to_checksum(b);
_payload_size += b << 8; // here comes the second byte of length
_decode_state = UBX_DECODE_GOT_LENGTH2;
break;
case UBX_DECODE_GOT_LENGTH2:
/* Add to checksum if not yet at checksum byte */
if (_rx_count < _payload_size)
add_byte_to_checksum(b);
_rx_buffer[_rx_count] = b;
/* once the payload has arrived, we can process the information */
if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
/* compare checksum */
if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) {
return 1;
} else {
decode_init();
return -1;
warnx("ubx: Checksum wrong");
}
return 1;
} else if (_rx_count < RECV_BUFFER_SIZE) {
_rx_count++;
} else {
warnx("ubx: buffer full");
decode_init();
return -1;
}
break;
default:
break;
}
return 0; //XXX ?
}
int // 0 = decoding, 1 = message handled, 2 = sat info message handled
UBX::parse_char(const uint8_t b)
{
int ret = 0;
switch (_decode_state) {
/* Expecting Sync1 */
case UBX_DECODE_SYNC1:
if (b == UBX_SYNC1) { // Sync1 found --> expecting Sync2
UBX_TRACE_PARSER("\nA");
_decode_state = UBX_DECODE_SYNC2;
}
break;
/* Expecting Sync2 */
case UBX_DECODE_SYNC2:
if (b == UBX_SYNC2) { // Sync2 found --> expecting Class
UBX_TRACE_PARSER("B");
_decode_state = UBX_DECODE_CLASS;
} else { // Sync1 not followed by Sync2: reset parser
decode_init();
}
break;
/* Expecting Class */
case UBX_DECODE_CLASS:
UBX_TRACE_PARSER("C");
add_byte_to_checksum(b); // checksum is calculated for everything except Sync and Checksum bytes
_rx_msg = b;
_decode_state = UBX_DECODE_ID;
break;
/* Expecting ID */
case UBX_DECODE_ID:
UBX_TRACE_PARSER("D");
add_byte_to_checksum(b);
_rx_msg |= b << 8;
_decode_state = UBX_DECODE_LENGTH1;
break;
/* Expecting first length byte */
case UBX_DECODE_LENGTH1:
UBX_TRACE_PARSER("E");
add_byte_to_checksum(b);
_rx_payload_length = b;
_decode_state = UBX_DECODE_LENGTH2;
break;
/* Expecting second length byte */
case UBX_DECODE_LENGTH2:
UBX_TRACE_PARSER("F");
add_byte_to_checksum(b);
_rx_payload_length |= b << 8; // calculate payload size
if (payload_rx_init() != 0) { // start payload reception
// payload will not be handled, discard message
decode_init();
} else {
_decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1;
}
break;
/* Expecting payload */
case UBX_DECODE_PAYLOAD:
UBX_TRACE_PARSER(".");
add_byte_to_checksum(b);
switch (_rx_msg) {
case UBX_MSG_NAV_SVINFO:
ret = payload_rx_add_nav_svinfo(b); // add a NAV-SVINFO payload byte
break;
case UBX_MSG_MON_VER:
ret = payload_rx_add_mon_ver(b); // add a MON-VER payload byte
break;
default:
ret = payload_rx_add(b); // add a payload byte
break;
}
if (ret < 0) {
// payload not handled, discard message
decode_init();
} else if (ret > 0) {
// payload complete, expecting checksum
_decode_state = UBX_DECODE_CHKSUM1;
} else {
// expecting more payload, stay in state UBX_DECODE_PAYLOAD
}
ret = 0;
break;
/* Expecting first checksum byte */
case UBX_DECODE_CHKSUM1:
if (_rx_ck_a != b) {
UBX_WARN("ubx checksum err");
decode_init();
} else {
_decode_state = UBX_DECODE_CHKSUM2;
}
break;
/* Expecting second checksum byte */
case UBX_DECODE_CHKSUM2:
if (_rx_ck_b != b) {
UBX_WARN("ubx checksum err");
} else {
ret = payload_rx_done(); // finish payload processing
}
decode_init();
break;
default:
break;
}
return ret;
}
