Char_type hex_str_to_char( Iter_type& begin )
{
const Char_type c1( *( ++begin ) );
const Char_type c2( *( ++begin ) );
return ( hex_to_num( c1 ) << 4 ) + hex_to_num( c2 );
}
Char_type unicode_str_to_char(Iter_type& begin )
{
const Char_type c1( *( ++begin ) );
const Char_type c2( *( ++begin ) );
const Char_type c3( *( ++begin ) );
const Char_type c4( *( ++begin ) );
return ( hex_to_num( c1 ) << 12 ) +
( hex_to_num( c2 ) << 8 ) +
( hex_to_num( c3 ) << 4 ) +
hex_to_num( c4 );
}
void gps_parse(Usart * c_uart)
{
uint8_t c = c_uart->Read();
// DEBUG("%c", c);
switch (gps_parser_state)
{
case(GPS_IDLE):
if (c == '$')
{
gps_parser_buffer_index = 0;
gps_checksum = 0;
gps_parser_state = GPS_DATA;
}
// else
// {
// gps_parser_buffer_index++;
// if (gps_parser_buffer_index > 10)
// {
// gps_set_baudrate();
// gps_change_uart_baudrate();
// gps_parser_buffer_index = 0;
// }
// }
break;
case(GPS_DATA):
if (c == '*')
{
gps_parser_buffer[gps_parser_buffer_index] = c;
gps_parser_buffer_index++;
gps_parser_state = GPS_CRC;
}
else
{
gps_checksum ^= c;
gps_parser_buffer[gps_parser_buffer_index] = c;
gps_parser_buffer_index++;
}
if (gps_parser_buffer_index >= NMEA_MAX_LEN)
{
assert(0);
gps_parser_buffer_index = 0;
gps_parser_state = GPS_IDLE;
}
break;
case(GPS_CRC):
gps_rx_checksum = hex_to_num(c) << 4;
gps_parser_buffer[gps_parser_buffer_index] = c;
gps_parser_buffer_index++;
gps_parser_state = GPS_END;
break;
case(GPS_END):
gps_rx_checksum += hex_to_num(c);
gps_parser_buffer[gps_parser_buffer_index] = c;
gps_parser_buffer[gps_parser_buffer_index + 1] = '\n';
gps_parser_buffer[gps_parser_buffer_index + 2] = '\0';
gps_parser_buffer_index = 0;
gps_parser_state = GPS_IDLE;
// DEBUG(">>%s<<\n", gps_parser_buffer);
if (gps_rx_checksum == gps_checksum)
{
if (cmpn_p(gps_parser_buffer, PSTR("GP"), 2))
{
gps_parser_ptr = gps_parser_buffer + 2;
if (cmpn_p(gps_parser_ptr, PSTR("RMC"), 3))
gps_parse_rmc();
if (cmpn_p(gps_parser_ptr, PSTR("GGA"), 3))
gps_parse_gga();
if (cmpn_p(gps_parser_ptr, PSTR("GSA"), 3))
gps_parse_gsa();
if (cmpn_p(gps_parser_ptr, PSTR("GSV"), 3))
gps_parse_gsv();
}
else if (cmpn_p(gps_parser_buffer, PSTR("PMTK"), 4))
{
gps_parser_ptr = gps_parser_buffer + 4;
if (cmpn_p(gps_parser_ptr, PSTR("011"), 3))
gps_parse_hello();
if (cmpn_p(gps_parser_ptr, PSTR("010"), 3))
gps_parse_sys();
}
}
else
{
DEBUG(">>%s<<\n", gps_parser_buffer);
DEBUG("GPS CHECKSUM IS WRONG! %02X %02X\n", gps_rx_checksum, gps_checksum);
}
break;
}
}