#include "NMEA0183.h"
static short int from_hex(char a);
static unsigned char nmea_term_complete(gps_nmea* pnmea, gps_data* pdata);
static int sring_to_int(const char* pstr, unsigned char* b);
static int int_pow(int value, unsigned int count);
static double double_pow(double value, unsigned int count);
static unsigned char float_to_string(double value, char* pdest, unsigned int intgr, unsigned int dec);
static unsigned char int_to_string(int value, char* pdest, unsigned int intgr) ;
static double string_to_float(const char* pstr, unsigned char* b);
static int sring_to_int(const char* pstr, unsigned char* b);
static int make_date_time(gps_data* pdata, unsigned int date, unsigned int time);
/* GPS数据解析 */
unsigned char nmea_decode(NMEA0183* nmea, char c)
{
unsigned char valid_sentence = 0;
if((void*)0==nmea) return 0;
switch (c)
{
case ',': /* 字段域分割符号 */
nmea->gpsParse.parity ^= c;
/* no break */
case '\r':
case '\n':
case '*':
if (nmea->gpsParse.term_offset < sizeof(nmea->gpsParse.term))
{
nmea->gpsParse.term[nmea->gpsParse.term_offset] = 0;
valid_sentence = nmea_term_complete(&nmea->gpsParse, &nmea->gpsData);
}
++nmea->gpsParse.term_number;
nmea->gpsParse.term_offset = 0;
nmea->gpsParse.is_checksum_term = (c == '*'); /* 后面两个字节为校验字段 */
return valid_sentence;
case '$': /* 字段信息开始 */
nmea->gpsParse.term_number = nmea->gpsParse.term_offset = 0;
nmea->gpsParse.parity = 0;
nmea->gpsParse.sentence_type = GPS_SENTENCE_OTHER;
nmea->gpsParse.is_checksum_term = 0;
nmea->gpsParse.gps_data_good = 0;
return valid_sentence;
}
/* 保存字段域数据 */
if (nmea->gpsParse.term_offset < sizeof(nmea->gpsParse.term) - 1)
nmea->gpsParse.term[nmea->gpsParse.term_offset++] = c;
if (!nmea->gpsParse.is_checksum_term)
nmea->gpsParse.parity ^= c;
return valid_sentence;
}
/* 单个十六进制字符转换为十进制 */
static short int from_hex(char a)
{
if (a >= 'A' && a <= 'F')
return a - 'A' + 10;
else if (a >= 'a' && a <= 'f')
return a - 'a' + 10;
else
return a - '0';
}
/* 处理单个字段域数据 */
static unsigned char nmea_term_complete(gps_nmea* pnmea, gps_data* pdata)
{
if((void*)0==pnmea || (void*)0==pdata) return 0;
// handle the last term in a message
if (pnmea->is_checksum_term)
{
unsigned char checksum = 16 * from_hex(pnmea->term[0]) + from_hex(pnmea->term[1]);
/* 检验信息段是否完整无错，如果校验通过，则将解析得到的数据保存，并提供给用户调用*/
if (checksum == pnmea->parity)
{
if (pnmea->gps_data_good)
{
unsigned int now = current_time_ms();
switch (pnmea->sentence_type)
{
case GPS_SENTENCE_RMC:
pnmea->last_RMC_ms = now;
//time = _new_time;
//date = _new_date;
pdata->location.lat = pnmea->new_latitude;
pdata->location.lng = pnmea->new_longitude;
pdata->ground_speed = pnmea->new_speed;
pdata->ground_course = pnmea->new_course;
make_date_time(pdata, pnmea->new_date, pnmea->new_time);
pdata->last_gps_time_ms = now;
break;
case GPS_SENTENCE_GGA:
pnmea->last_GGA_ms = now;
pdata->location.alt = pnmea->new_altitude;
pdata->location.lat = pnmea->new_latitude;
pdata->location.lng = pnmea->new_longitude;
pdata->num_sats = pnmea->new_satellite_count;
pdata->hdop = pnmea->new_hdop;
switch(pnmea->new_quality_indicator) {
case 0: // Fix not available or invalid
pdata->status = NO_FIX;
break;
case 1: // GPS SPS Mode, fix valid
pdata->status = GPS_OK_FIX_3D;
break;
case 2: // Differential GPS, SPS Mode, fix valid
pdata->status = GPS_OK_FIX_3D_DGPS;
break;
case 3: // GPS PPS Mode, fix valid
pdata->status = GPS_OK_FIX_3D;
break;
case 4: // Real Time Kinematic. System used in RTK mode with fixed integers
pdata->status = GPS_OK_FIX_3D_RTK_FIXED;
break;
case 5: // Float RTK. Satellite system used in RTK mode, floating integers
pdata->status = GPS_OK_FIX_3D_RTK_FLOAT;
break;
case 6: // Estimated (dead reckoning) Mode
pdata->status = NO_FIX;
break;
default://to maintain compatibility with MAV_GPS_INPUT and others
pdata->status = GPS_OK_FIX_3D;
break;
}
break;
case GPS_SENTENCE_VTG:
pnmea->last_VTG_ms = now;
pdata->ground_speed = pnmea->new_speed;
pdata->ground_course = pnmea->new_course;
// VTG has no fix indicator, can't change fix status
break;
case GPS_SENTENCE_HDT:
pnmea->last_HDT_ms = now;
// pdata->gps_yaw = wrap_360(pnmea->new_gps_yaw*0.01f);
pdata->have_gps_yaw = 1;
break;
}
} else {
switch (pnmea->sentence_type) {
case GPS_SENTENCE_RMC:
case GPS_SENTENCE_GGA:
// Only these sentences give us information about
// fix status.
pdata->status = NO_FIX;
}
}
// see if we got a good message
//return _have_new_message();
return 1;
}
// we got a bad message, ignore it
return 0;
}
// the first term determines the sentence type
if (pnmea->term_number == 0) {
/*
The first two letters of the NMEA term are the talker
ID. The most common is 'GP' but there are a bunch of others
that are valid. We accept any two characters here.
*/
if (pnmea->term[0] < 'A' || pnmea->term[0] > 'Z' ||
pnmea->term[1] < 'A' || pnmea->term[1] > 'Z') {
pnmea->sentence_type = GPS_SENTENCE_OTHER;
return 0;
}
const char *term_type = &pnmea->term[2];
if (strcmp(term_type, "RMC") == 0) {
pnmea->sentence_type = GPS_SENTENCE_RMC;
} else if (strcmp(term_type, "GGA") == 0) {;
pnmea->sentence_type = GPS_SENTENCE_GGA;
}else if (strcmp(term_type, "HDT") == 0) {
pnmea->sentence_type = GPS_SENTENCE_HDT;
// HDT doesn't have a data qualifier
pnmea->gps_data_good = 1;
}else if (strcmp(term_type, "VTG") == 0) {
pnmea->sentence_type = GPS_SENTENCE_VTG;
// VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise.
pnmea->gps_data_good = 1;
} else {
pnmea->sentence_type = GPS_SENTENCE_OTHER;
}
return 0;
}
// 32 = RMC, 64 = GGA, 96 = VTG
if (pnmea->sentence_type != GPS_SENTENCE_OTHER && pnmea->term[0]) {
switch (pnmea->sentence_type + pnmea->term_number) {
// operational status
//
case GPS_SENTENCE_RMC + 2: // validity (RMC)
pnmea->gps_data_good = pnmea->term[0] == 'A';
break;
case GPS_SENTENCE_GGA + 6: // Fix data (GGA)
pnmea->gps_data_good = pnmea->term[0] > '0';
