first release for keyboard
Fork of F401RE-USBHost by
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Tue Jul 12 2022 21:43:28 by 1.7.2