J&W / Mbed 2 deprecated Rejestrator

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Rejestrator

Dependents:   Rejestrator

F401RE-USBHost/USBHostGPS/decodeNMEA.cpp@0:fa31f8461c63, 2015-04-18 (annotated)

Committer:
Waldek
Date:
Sat Apr 18 17:01:57 2015 +0000
Revision:
0:fa31f8461c63
working version, stop

Who changed what in which revision?

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