decodeNMEA.cpp

00001 // decodeNMEA.cpp 2012/12/13
00002 #include "mbed.h"
00003 #include "decodeNMEA.h"
00004 
00005 #define SEQ_INIT 0
00006 #define SEQ_DATA 1
00007 #define SEQ_SUM0 2
00008 #define SEQ_SUM1 3
00009 
00010 #define GPGGA 1
00011 #define GPGLL 2
00012 #define GPGSV 3
00013 #define GPRMC 4
00014 #define GPVTG 5
00015 #define GPZDA 6
00016 
00017 decodeNMEA::decodeNMEA():m_seq(0) {
00018     gprmc_t = 0;
00019     update_t = 0;
00020     m_status = false;
00021 }
00022 
00023 void decodeNMEA::inputNMEA(char* buf, int len) {
00024     for(int i = 0; i < len; i++) {
00025         inputNMEA(buf[i]);
00026     }
00027 }
00028 
00029 static int ctoh(char c) {
00030     if (c >= '0' && c <= '9') {
00031         return c-'0';
00032     }
00033     return c-'A'+10;
00034 }
00035      
00036 void decodeNMEA::inputNMEA(char c) {
00037     switch(m_seq) {
00038         case SEQ_INIT:
00039             if (c == '$') {
00040                 m_type = 0;
00041                 m_row = 0;
00042                 m_buf_pos = 0;
00043                 m_sum = 0x00;
00044                 m_seq = SEQ_DATA;
00045             }
00046             break;
00047         case SEQ_DATA:
00048             m_sum ^= c;
00049             if (c == ',' || c == '*') {
00050                 m_buf[m_buf_pos] = '\0';
00051                 parse(m_type, m_row, m_buf);
00052                 m_row++;
00053                 m_buf_pos =0;
00054                 if (c == '*') { // check sum ?
00055                     m_sum ^= c;
00056                     m_seq = SEQ_SUM0;
00057                 }
00058             } else {
00059                 if (m_buf_pos < sizeof(m_buf)-1) {
00060                     m_buf[m_buf_pos++] = c;
00061                 }
00062             }
00063             break;
00064         case SEQ_SUM0:
00065             if (ctoh(c) == (m_sum>>4)) {
00066                 m_seq = SEQ_SUM1;
00067             } else {
00068                 m_seq = SEQ_INIT;
00069             }
00070             break;
00071         case SEQ_SUM1:
00072             if (ctoh(c) == (m_sum&0x0f)) {
00073                 update(m_type, m_row);
00074             } else {
00075                 
00076                 m_seq = SEQ_INIT;
00077             }
00078             break;
00079         default:
00080             m_seq = SEQ_INIT;
00081             break;
00082     }
00083 }
00084 
00085 float DMMtoDegree(const char *s)
00086 {
00087     char *p = strchr(const_cast<char*>(s), '.');
00088     if (p == NULL) {
00089         return 0.0;
00090     }
00091     const uint32_t k[] = {10000,1000,100,10,1};
00092     uint32_t i3 = atoi(p+1) * k[strlen(p+1)];
00093     uint32_t i2 = atoi(p-2);
00094     uint32_t i1 = atoi(s) / 100;
00095 
00096     uint32_t i = i1*10000*60 + (i2*10000 + i3);
00097     return i / 10000.0 / 60.0;
00098 }
00099 
00100 void decodeNMEA::parse(int type, int row, char* buf) {
00101     if (row == 0) {
00102         if (strcmp(buf, "GPRMC") == 0) {
00103             m_type = GPRMC;
00104             m_status = false;
00105         } else {
00106             m_type = 0;
00107         }
00108         return;
00109     }
00110     if (type == GPRMC) {
00111         switch(row) {
00112             case 1:
00113                 tmp_timeinfo.tm_sec = atoi(buf+4);
00114                 buf[4] = '\0';
00115                 tmp_timeinfo.tm_min = atoi(buf+2);
00116                 buf[2] = '\0';
00117                 tmp_timeinfo.tm_hour = atoi(buf);
00118                 break;
00119             case 2:
00120                 if (buf[0] == 'A') {
00121                     m_status = true;
00122                 }
00123                 break;
00124             case 3:
00125                 tmp_lat = DMMtoDegree(buf);
00126                 break;
00127             case 4:
00128                 if (buf[0] == 'S') {
00129                     tmp_lat *= -1;
00130                 }
00131                 break;
00132             case 5:
00133                 tmp_lon = DMMtoDegree(buf);
00134                 break;
00135             case 6:
00136                 if (buf[0] == 'W') {
00137                     tmp_lon *= -1;
00138                 }
00139                 break;
00140             case 9:
00141                 tmp_timeinfo.tm_year = 2000 - 1900 + atoi(buf+4);
00142                 buf[4] = '\0';
00143                 tmp_timeinfo.tm_mon = atoi(buf+2) - 1;
00144                 buf[2] = '\0';
00145                 tmp_timeinfo.tm_mday = atoi(buf);
00146                 break;
00147         }
00148     }
00149 }
00150 
00151 void decodeNMEA::update(int type, int row) {
00152     if (type == GPRMC && m_status) {
00153         lat = tmp_lat;
00154         lon = tmp_lon;
00155         gprmc_t = mktime(&tmp_timeinfo);
00156         update_t = gprmc_t;
00157     }
00158 }
00159