Rev 2 | Rev 4 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2 | Rev 3 | ||
|---|---|---|---|
| Line 19... | Line 19... | ||
| 19 | 19 | ||
| 20 | nmeaCallback rmcCallback = NULL; |
20 | nmeaCallback rmcCallback = NULL; |
| 21 | 21 | ||
| 22 | typedef enum |
22 | typedef enum |
| 23 | { |
23 | { |
| - | 24 | SEARCH, |
|
| 24 | SEARCH, READING |
25 | READING |
| 25 | } NmeaState_t; |
26 | } NmeaState_t; |
| 26 | 27 | ||
| 27 | NmeaState_t lineState = SEARCH; |
28 | NmeaState_t lineState = SEARCH; |
| 28 | 29 | ||
| 29 | static bool |
30 | static bool |
| 30 | decodePacket (char *linebuff, int linePos, Location *loc); |
31 | decodePacket(char *linebuff, int linePos, Location *loc); |
| 31 | 32 | ||
| 32 | bool |
- | |
| 33 | updateLocation (Location *loc, usart_ctl *uc) |
33 | bool updateLocation(Location *loc, usart_ctl *uc) |
| 34 | { |
34 | { |
| 35 | while (1) |
- | |
| 36 | { |
- | |
| 37 | if (!SerialCharsReceived (uc)) |
35 | unsigned chars = SerialCharsReceived(uc); |
| - | 36 | if (!chars) |
|
| 38 | return false; // nothing to read, return immediately |
37 | return false; // nothing to read, return immediately |
| 39 | 38 | ||
| - | 39 | for (int i = 0; i < chars; i++) |
|
| - | 40 | { |
|
| 40 | char c = GetCharSerial (uc); |
41 | char c = GetCharSerial(uc); |
| 41 | switch (lineState) |
42 | switch (lineState) |
| 42 | { |
43 | { |
| 43 | case SEARCH: |
44 | case SEARCH: |
| 44 | if (c != '$') |
45 | if (c != '$') |
| 45 | break; |
46 | break; |
| 46 | lineState = READING; |
47 | lineState = READING; |
| 47 | linePos = 0; |
48 | linePos = 0; |
| 48 | case READING: |
49 | case READING: |
| 49 | if (c == '\r') |
50 | if (c == '\r') |
| 50 | { |
51 | { |
| 51 | // handle the read code |
52 | // handle the read code |
| 52 | bool success = decodePacket (linebuff, linePos, loc); |
53 | bool success = decodePacket(linebuff, linePos, loc); |
| 53 | lineState = SEARCH; |
54 | lineState = SEARCH; |
| 54 | 55 | ||
| 55 | linePos = 0; |
56 | linePos = 0; |
| 56 | return success; |
57 | return success; |
| 57 | } |
58 | } |
| 58 | if (linePos < sizeof(linebuff)) |
59 | if (linePos < sizeof(linebuff)) |
| 59 | linebuff[linePos++] = c; |
60 | linebuff[linePos++] = c; |
| 60 | else |
61 | else |
| 61 | { |
62 | { |
| 62 | lineState = SEARCH; |
63 | lineState = SEARCH; |
| 63 | // search for the $ in any unread string |
64 | // search for the $ in any unread string |
| 64 | int i; |
65 | int i; |
| 65 | 66 | ||
| 66 | for (i = 0; i < linePos; i++) |
67 | for (i = 0; i < linePos; i++) |
| 67 | if (linebuff[i] == '$') |
68 | if (linebuff[i] == '$') |
| 68 | { |
69 | { |
| 69 | memcpy (linebuff, linebuff + i, linePos - i); |
70 | memcpy(linebuff, linebuff + i, linePos - i); |
| 70 | linePos = 1; |
71 | linePos = 1; |
| 71 | linebuff[linePos++] = c; |
72 | linebuff[linePos++] = c; |
| 72 | lineState = READING; |
73 | lineState = READING; |
| 73 | } |
74 | } |
| 74 | 75 | ||
| 75 | if (lineState == SEARCH) |
76 | if (lineState == SEARCH) |
| 76 | linePos = 0; |
77 | linePos = 0; |
| 77 | } |
78 | } |
| 78 | break; |
79 | break; |
| - | 80 | } |
|
| 79 | } |
81 | } |
| 80 | } |
- | |
| 81 | return false; |
82 | return false; |
| 82 | } |
83 | } |
| 83 | 84 | ||
| 84 | static int8_t |
85 | static int8_t |
| 85 | decodeDec (char c) |
86 | decodeDec(char c) |
| 86 | { |
87 | { |
| 87 | if (c > '9' && c < '0') |
88 | if (c > '9' && c < '0') |
| 88 | return -1; |
89 | return -1; |
| 89 | return c - '0'; |
90 | return c - '0'; |
| 90 | } |
91 | } |
| 91 | 92 | ||
| 92 | static uint8_t |
93 | static uint8_t |
| 93 | decodeHex (char c) |
94 | decodeHex(char c) |
| 94 | { |
95 | { |
| 95 | int8_t v = decodeDec (c); |
96 | int8_t v = decodeDec(c); |
| 96 | if (v >= 0) |
97 | if (v >= 0) |
| 97 | return v; |
98 | return v; |
| 98 | c = tolower (c); |
99 | c = tolower(c); |
| 99 | if (c >= 'a' && c <= 'f') |
100 | if (c >= 'a' && c <= 'f') |
| 100 | return c - 'a' + 10; |
101 | return c - 'a' + 10; |
| 101 | return 0; |
102 | return 0; |
| 102 | } |
103 | } |
| 103 | 104 | ||
| 104 | // lat/long decoder |
105 | // lat/long decoder |
| 105 | static float |
106 | static float |
| 106 | decodeLL (char *ptr, int msDigitWeight) |
107 | decodeLL(char *ptr, int msDigitWeight) |
| 107 | { |
108 | { |
| 108 | float digitWeight = msDigitWeight; |
109 | float digitWeight = msDigitWeight; |
| 109 | float result = 0; |
110 | float result = 0; |
| 110 | int i = 0; |
111 | int i = 0; |
| 111 | while (1) |
112 | while (1) |
| 112 | { |
- | |
| 113 | char c = ptr[i++]; |
- | |
| 114 | if (c == '.') |
- | |
| 115 | continue; |
- | |
| 116 | int8_t v = decodeDec (c); |
- | |
| 117 | if (v > 0) |
- | |
| 118 | { |
113 | { |
| - | 114 | char c = ptr[i++]; |
|
| - | 115 | if (c == '.') |
|
| - | 116 | continue; |
|
| - | 117 | int8_t v = decodeDec(c); |
|
| - | 118 | if (v > 0) |
|
| - | 119 | { |
|
| 119 | result += digitWeight * v; |
120 | result += digitWeight * v; |
| 120 | if (fabs (digitWeight - 1) < 0.01) |
121 | if (fabs(digitWeight - 1) < 0.01) |
| 121 | digitWeight = 1 / 6.0; |
122 | digitWeight = 1 / 6.0; |
| 122 | else |
123 | else |
| 123 | digitWeight = digitWeight / 10; |
124 | digitWeight = digitWeight / 10; |
| 124 | 125 | ||
| 125 | continue; |
126 | continue; |
| - | 127 | } |
|
| - | 128 | break; |
|
| 126 | } |
129 | } |
| 127 | break; |
- | |
| 128 | } |
- | |
| 129 | - | ||
| 130 | return result; |
- | |
| 131 | 130 | ||
| - | 131 | return result; |
|
| 132 | } |
132 | } |
| 133 | 133 | ||
| 134 | static float |
134 | static float |
| 135 | decodeFP (char *ptr) |
135 | decodeFP(char *ptr) |
| 136 | { |
136 | { |
| 137 | return strtof (ptr, NULL); |
137 | return strtof(ptr, NULL); |
| 138 | } |
138 | } |
| 139 | 139 | ||
| 140 | static int |
140 | static int |
| 141 | decodeDecimal (char *ptr) |
141 | decodeDecimal(char *ptr) |
| 142 | { |
142 | { |
| 143 | int i; |
143 | int i; |
| 144 | int res = 0; |
144 | int res = 0; |
| 145 | int const width = 2; |
145 | int const width = 2; |
| 146 | for (i = 0; i < width; i++) |
146 | for (i = 0; i < width; i++) |
| 147 | { |
- | |
| 148 | int8_t v = decodeDec (*ptr++); |
- | |
| 149 | if (v < 0) |
- | |
| 150 | return 0; |
- | |
| 151 | res *= 10; |
- | |
| 152 | res += v; |
- | |
| 153 | } |
- | |
| 154 | return res; |
- | |
| 155 | } |
- | |
| 156 | - | ||
| 157 | bool |
- | |
| 158 | decodePacket (char *linebuff, int linePos, Location *loc) |
- | |
| 159 | { |
- | |
| 160 | - | ||
| 161 | uint8_t checksum = 0; |
- | |
| 162 | for (int i = 1; i < linePos - 3; i++) |
- | |
| 163 | checksum ^= linebuff[i]; |
- | |
| 164 | uint8_t givenSum = (decodeHex (linebuff[linePos - 2]) << 4) |
- | |
| 165 | + decodeHex (linebuff[linePos - 1]); |
- | |
| 166 | if (givenSum != checksum) |
- | |
| 167 | return false; |
- | |
| 168 | - | ||
| 169 | char *fieldPos[20]; |
- | |
| 170 | int fieldCnt = 0; |
- | |
| 171 | // split fields |
- | |
| 172 | for (int i = 6; i < linePos - 3; i++) |
- | |
| 173 | { |
- | |
| 174 | if (linebuff[i] == ',') |
- | |
| 175 | { |
147 | { |
| 176 | fieldPos[fieldCnt++] = linebuff + i + 1; |
148 | int8_t v = decodeDec(*ptr++); |
| - | 149 | if (v < 0) |
|
| - | 150 | return 0; |
|
| - | 151 | res *= 10; |
|
| - | 152 | res += v; |
|
| 177 | } |
153 | } |
| - | 154 | return res; |
|
| - | 155 | } |
|
| 178 | 156 | ||
| 179 | } |
- | |
| 180 | - | ||
| 181 | // decode RMC |
- | |
| 182 | if (linebuff[3] == 'R' && linebuff[4] == 'M' && linebuff[5] == 'C') |
157 | bool decodePacket(char *linebuff, int linePos, Location *loc) |
| 183 | 158 | { |
|
| 184 | { |
- | |
| 185 | 159 | ||
| - | 160 | uint8_t checksum = 0; |
|
| - | 161 | for (int i = 1; i < linePos - 3; i++) |
|
| - | 162 | checksum ^= linebuff[i]; |
|
| - | 163 | uint8_t givenSum = (decodeHex(linebuff[linePos - 2]) << 4) + decodeHex(linebuff[linePos - 1]); |
|
| - | 164 | if (givenSum != checksum) |
|
| - | 165 | return false; |
|
| 186 | 166 | ||
| - | 167 | char *fieldPos[20]; |
|
| 187 | // decode the fields |
168 | int fieldCnt = 0; |
| 188 | loc->valid = *fieldPos[1]; |
169 | // split fields |
| 189 | if (loc->valid == 'A') |
170 | for (int i = 6; i < linePos - 3; i++) |
| 190 | { |
171 | { |
| 191 | memcpy (loc->time, fieldPos[0], 6); |
- | |
| 192 | loc->lat = decodeLL (fieldPos[2], 10); |
- | |
| 193 | loc->ns = *fieldPos[3]; |
172 | if (linebuff[i] == ',') |
| 194 | loc->lon = decodeLL (fieldPos[4], 100); |
- | |
| 195 | loc->ew = *fieldPos[5]; |
173 | { |
| 196 | loc->speed = decodeFP (fieldPos[6]); |
- | |
| 197 | loc->heading = decodeFP (fieldPos[7]); |
174 | fieldPos[fieldCnt++] = linebuff + i + 1; |
| 198 | memcpy (loc->date, fieldPos[1], 6); |
- | |
| 199 | 175 | } |
|
| 200 | loc->tv.tm_sec = decodeDecimal (&loc->time[4]); |
- | |
| 201 | loc->tv.tm_min = decodeDecimal (&loc->time[2]); |
- | |
| 202 | loc->tv.tm_hour = decodeDecimal (&loc->time[0]); |
- | |
| 203 | 176 | } |
|
| 204 | loc->tv.tm_mday = decodeDecimal (&loc->date[0]); |
- | |
| 205 | loc->tv.tm_mon = decodeDecimal (&loc->date[2]) - 1; |
- | |
| 206 | loc->tv.tm_year = decodeDecimal (&loc->date[4]) + 100; // |
- | |
| 207 | 177 | ||
| 208 | loc->tv.tm_isdst = 0; |
178 | // decode RMC |
| 209 | loc->utc = mktime (&loc->tv); |
179 | if (linebuff[3] == 'R' && linebuff[4] == 'M' && linebuff[5] == 'C') |
| 210 | 180 | ||
| 211 | loc->good = true; |
- | |
| 212 | } |
- | |
| 213 | if (rmcCallback) |
- | |
| 214 | { |
181 | { |
| - | 182 | ||
| - | 183 | // decode the fields |
|
| - | 184 | loc->valid = *fieldPos[1]; |
|
| - | 185 | if (loc->valid == 'A') |
|
| - | 186 | { |
|
| - | 187 | memcpy(loc->time, fieldPos[0], 6); |
|
| - | 188 | loc->lat = decodeLL(fieldPos[2], 10); |
|
| - | 189 | loc->ns = *fieldPos[3]; |
|
| - | 190 | loc->lon = decodeLL(fieldPos[4], 100); |
|
| - | 191 | loc->ew = *fieldPos[5]; |
|
| - | 192 | loc->speed = decodeFP(fieldPos[6]); |
|
| - | 193 | loc->heading = decodeFP(fieldPos[7]); |
|
| - | 194 | memcpy(loc->date, fieldPos[1], 6); |
|
| - | 195 | ||
| - | 196 | loc->tv.tm_sec = decodeDecimal(&loc->time[4]); |
|
| - | 197 | loc->tv.tm_min = decodeDecimal(&loc->time[2]); |
|
| - | 198 | loc->tv.tm_hour = decodeDecimal(&loc->time[0]); |
|
| - | 199 | ||
| - | 200 | loc->tv.tm_mday = decodeDecimal(&loc->date[0]); |
|
| - | 201 | loc->tv.tm_mon = decodeDecimal(&loc->date[2]) - 1; |
|
| - | 202 | loc->tv.tm_year = decodeDecimal(&loc->date[4]) + 100; // |
|
| - | 203 | ||
| - | 204 | loc->tv.tm_isdst = 0; |
|
| - | 205 | loc->utc = mktime(&loc->tv); |
|
| - | 206 | ||
| - | 207 | loc->good = true; |
|
| - | 208 | } |
|
| - | 209 | if (rmcCallback) |
|
| - | 210 | { |
|
| 215 | linebuff[linePos++]='\r'; |
211 | linebuff[linePos++] = '\r'; |
| 216 | linebuff[linePos++]='\n'; |
212 | linebuff[linePos++] = '\n'; |
| - | 213 | linebuff[linePos++] = 0; |
|
| 217 | rmcCallback ((uint8_t*) linebuff, linePos); |
214 | rmcCallback((uint8_t *)linebuff, linePos); |
| - | 215 | } |
|
| 218 | } |
216 | } |
| 219 | } |
- | |
| 220 | 217 | ||
| 221 | return true; |
218 | return true; |
| 222 | } |
219 | } |
| 223 | 220 | ||
| 224 | void |
- | |
| 225 | setRmcCallback (nmeaCallback callback) |
221 | void setRmcCallback(nmeaCallback callback) |
| 226 | { |
222 | { |
| 227 | rmcCallback = callback; |
223 | rmcCallback = callback; |
| 228 | } |
224 | } |