GambitDev / F401RE-USBHost

USB Host Driver with Socket Modem support. Works with revision 323 of mbed-src but broken with any later version.

Dependencies:   FATFileSystem

Fork of F401RE-USBHost by Norimasa Okamoto

USBHostGPS/decodeNMEA.cpp@13:8774c07f12a5, 2014-06-10 (annotated)

Committer:
va009039
Date:
Tue Jun 10 13:38:41 2014 +0900
Revision:
13:8774c07f12a5
add USBHostC270

Who changed what in which revision?

UserRevisionLine numberNew contents of line
va009039 13:8774c07f12a5 1 // decodeNMEA.cpp 2012/12/13
va009039 13:8774c07f12a5 2 #include "mbed.h"
va009039 13:8774c07f12a5 3 #include "decodeNMEA.h"
va009039 13:8774c07f12a5 4
va009039 13:8774c07f12a5 5 #define SEQ_INIT 0
va009039 13:8774c07f12a5 6 #define SEQ_DATA 1
va009039 13:8774c07f12a5 7 #define SEQ_SUM0 2
va009039 13:8774c07f12a5 8 #define SEQ_SUM1 3
va009039 13:8774c07f12a5 9
va009039 13:8774c07f12a5 10 #define GPGGA 1
va009039 13:8774c07f12a5 11 #define GPGLL 2
va009039 13:8774c07f12a5 12 #define GPGSV 3
va009039 13:8774c07f12a5 13 #define GPRMC 4
va009039 13:8774c07f12a5 14 #define GPVTG 5
va009039 13:8774c07f12a5 15 #define GPZDA 6
va009039 13:8774c07f12a5 16
va009039 13:8774c07f12a5 17 decodeNMEA::decodeNMEA():m_seq(0) {
va009039 13:8774c07f12a5 18 gprmc_t = 0;
va009039 13:8774c07f12a5 19 update_t = 0;
va009039 13:8774c07f12a5 20 m_status = false;
va009039 13:8774c07f12a5 21 }
va009039 13:8774c07f12a5 22
va009039 13:8774c07f12a5 23 void decodeNMEA::inputNMEA(char* buf, int len) {
va009039 13:8774c07f12a5 24 for(int i = 0; i < len; i++) {
va009039 13:8774c07f12a5 25 inputNMEA(buf[i]);
va009039 13:8774c07f12a5 26 }
va009039 13:8774c07f12a5 27 }
va009039 13:8774c07f12a5 28
va009039 13:8774c07f12a5 29 static int ctoh(char c) {
va009039 13:8774c07f12a5 30 if (c >= '0' && c <= '9') {
va009039 13:8774c07f12a5 31 return c-'0';
va009039 13:8774c07f12a5 32 }
va009039 13:8774c07f12a5 33 return c-'A'+10;
va009039 13:8774c07f12a5 34 }
va009039 13:8774c07f12a5 35
va009039 13:8774c07f12a5 36 void decodeNMEA::inputNMEA(char c) {
va009039 13:8774c07f12a5 37 switch(m_seq) {
va009039 13:8774c07f12a5 38 case SEQ_INIT:
va009039 13:8774c07f12a5 39 if (c == '$') {
va009039 13:8774c07f12a5 40 m_type = 0;
va009039 13:8774c07f12a5 41 m_row = 0;
va009039 13:8774c07f12a5 42 m_buf_pos = 0;
va009039 13:8774c07f12a5 43 m_sum = 0x00;
va009039 13:8774c07f12a5 44 m_seq = SEQ_DATA;
va009039 13:8774c07f12a5 45 }
va009039 13:8774c07f12a5 46 break;
va009039 13:8774c07f12a5 47 case SEQ_DATA:
va009039 13:8774c07f12a5 48 m_sum ^= c;
va009039 13:8774c07f12a5 49 if (c == ',' || c == '*') {
va009039 13:8774c07f12a5 50 m_buf[m_buf_pos] = '\0';
va009039 13:8774c07f12a5 51 parse(m_type, m_row, m_buf);
va009039 13:8774c07f12a5 52 m_row++;
va009039 13:8774c07f12a5 53 m_buf_pos =0;
va009039 13:8774c07f12a5 54 if (c == '*') { // check sum ?
va009039 13:8774c07f12a5 55 m_sum ^= c;
va009039 13:8774c07f12a5 56 m_seq = SEQ_SUM0;
va009039 13:8774c07f12a5 57 }
va009039 13:8774c07f12a5 58 } else {
va009039 13:8774c07f12a5 59 if (m_buf_pos < sizeof(m_buf)-1) {
va009039 13:8774c07f12a5 60 m_buf[m_buf_pos++] = c;
va009039 13:8774c07f12a5 61 }
va009039 13:8774c07f12a5 62 }
va009039 13:8774c07f12a5 63 break;
va009039 13:8774c07f12a5 64 case SEQ_SUM0:
va009039 13:8774c07f12a5 65 if (ctoh(c) == (m_sum>>4)) {
va009039 13:8774c07f12a5 66 m_seq = SEQ_SUM1;
va009039 13:8774c07f12a5 67 } else {
va009039 13:8774c07f12a5 68 m_seq = SEQ_INIT;
va009039 13:8774c07f12a5 69 }
va009039 13:8774c07f12a5 70 break;
va009039 13:8774c07f12a5 71 case SEQ_SUM1:
va009039 13:8774c07f12a5 72 if (ctoh(c) == (m_sum&0x0f)) {
va009039 13:8774c07f12a5 73 update(m_type, m_row);
va009039 13:8774c07f12a5 74 } else {
va009039 13:8774c07f12a5 75
va009039 13:8774c07f12a5 76 m_seq = SEQ_INIT;
va009039 13:8774c07f12a5 77 }
va009039 13:8774c07f12a5 78 break;
va009039 13:8774c07f12a5 79 default:
va009039 13:8774c07f12a5 80 m_seq = SEQ_INIT;
va009039 13:8774c07f12a5 81 break;
va009039 13:8774c07f12a5 82 }
va009039 13:8774c07f12a5 83 }
va009039 13:8774c07f12a5 84
va009039 13:8774c07f12a5 85 float DMMtoDegree(const char *s)
va009039 13:8774c07f12a5 86 {
va009039 13:8774c07f12a5 87 char *p = strchr(const_cast<char*>(s), '.');
va009039 13:8774c07f12a5 88 if (p == NULL) {
va009039 13:8774c07f12a5 89 return 0.0;
va009039 13:8774c07f12a5 90 }
va009039 13:8774c07f12a5 91 const uint32_t k[] = {10000,1000,100,10,1};
va009039 13:8774c07f12a5 92 uint32_t i3 = atoi(p+1) * k[strlen(p+1)];
va009039 13:8774c07f12a5 93 uint32_t i2 = atoi(p-2);
va009039 13:8774c07f12a5 94 uint32_t i1 = atoi(s) / 100;
va009039 13:8774c07f12a5 95
va009039 13:8774c07f12a5 96 uint32_t i = i1*10000*60 + (i2*10000 + i3);
va009039 13:8774c07f12a5 97 return i / 10000.0 / 60.0;
va009039 13:8774c07f12a5 98 }
va009039 13:8774c07f12a5 99
va009039 13:8774c07f12a5 100 void decodeNMEA::parse(int type, int row, char* buf) {
va009039 13:8774c07f12a5 101 if (row == 0) {
va009039 13:8774c07f12a5 102 if (strcmp(buf, "GPRMC") == 0) {
va009039 13:8774c07f12a5 103 m_type = GPRMC;
va009039 13:8774c07f12a5 104 m_status = false;
va009039 13:8774c07f12a5 105 } else {
va009039 13:8774c07f12a5 106 m_type = 0;
va009039 13:8774c07f12a5 107 }
va009039 13:8774c07f12a5 108 return;
va009039 13:8774c07f12a5 109 }
va009039 13:8774c07f12a5 110 if (type == GPRMC) {
va009039 13:8774c07f12a5 111 switch(row) {
va009039 13:8774c07f12a5 112 case 1:
va009039 13:8774c07f12a5 113 tmp_timeinfo.tm_sec = atoi(buf+4);
va009039 13:8774c07f12a5 114 buf[4] = '\0';
va009039 13:8774c07f12a5 115 tmp_timeinfo.tm_min = atoi(buf+2);
va009039 13:8774c07f12a5 116 buf[2] = '\0';
va009039 13:8774c07f12a5 117 tmp_timeinfo.tm_hour = atoi(buf);
va009039 13:8774c07f12a5 118 break;
va009039 13:8774c07f12a5 119 case 2:
va009039 13:8774c07f12a5 120 if (buf[0] == 'A') {
va009039 13:8774c07f12a5 121 m_status = true;
va009039 13:8774c07f12a5 122 }
va009039 13:8774c07f12a5 123 break;
va009039 13:8774c07f12a5 124 case 3:
va009039 13:8774c07f12a5 125 tmp_lat = DMMtoDegree(buf);
va009039 13:8774c07f12a5 126 break;
va009039 13:8774c07f12a5 127 case 4:
va009039 13:8774c07f12a5 128 if (buf[0] == 'S') {
va009039 13:8774c07f12a5 129 tmp_lat *= -1;
va009039 13:8774c07f12a5 130 }
va009039 13:8774c07f12a5 131 break;
va009039 13:8774c07f12a5 132 case 5:
va009039 13:8774c07f12a5 133 tmp_lon = DMMtoDegree(buf);
va009039 13:8774c07f12a5 134 break;
va009039 13:8774c07f12a5 135 case 6:
va009039 13:8774c07f12a5 136 if (buf[0] == 'W') {
va009039 13:8774c07f12a5 137 tmp_lon *= -1;
va009039 13:8774c07f12a5 138 }
va009039 13:8774c07f12a5 139 break;
va009039 13:8774c07f12a5 140 case 9:
va009039 13:8774c07f12a5 141 tmp_timeinfo.tm_year = 2000 - 1900 + atoi(buf+4);
va009039 13:8774c07f12a5 142 buf[4] = '\0';
va009039 13:8774c07f12a5 143 tmp_timeinfo.tm_mon = atoi(buf+2) - 1;
va009039 13:8774c07f12a5 144 buf[2] = '\0';
va009039 13:8774c07f12a5 145 tmp_timeinfo.tm_mday = atoi(buf);
va009039 13:8774c07f12a5 146 break;
va009039 13:8774c07f12a5 147 }
va009039 13:8774c07f12a5 148 }
va009039 13:8774c07f12a5 149 }
va009039 13:8774c07f12a5 150
va009039 13:8774c07f12a5 151 void decodeNMEA::update(int type, int row) {
va009039 13:8774c07f12a5 152 if (type == GPRMC && m_status) {
va009039 13:8774c07f12a5 153 lat = tmp_lat;
va009039 13:8774c07f12a5 154 lon = tmp_lon;
va009039 13:8774c07f12a5 155 gprmc_t = mktime(&tmp_timeinfo);
va009039 13:8774c07f12a5 156 update_t = gprmc_t;
va009039 13:8774c07f12a5 157 }
va009039 13:8774c07f12a5 158 }
va009039 13:8774c07f12a5 159