| Line | Branch | Decision | Exec | Source |
|---|---|---|---|---|
| 1 | /** | |||
| 2 | * @date Dec 20, 2013 | |||
| 3 | * | |||
| 4 | * @author Andrey Belomutskiy, (c) 2012-2020 | |||
| 5 | * @author Kot_dnz | |||
| 6 | * | |||
| 7 | * This file is part of rusEfi - see http://rusefi.com | |||
| 8 | * | |||
| 9 | * rusEfi is free software; you can redistribute it and/or modify it under the terms of | |||
| 10 | * the GNU General Public License as published by the Free Software Foundation; either | |||
| 11 | * version 3 of the License, or (at your option) any later version. | |||
| 12 | * | |||
| 13 | * rusEfi is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without | |||
| 14 | * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |||
| 15 | * GNU General Public License for more details. | |||
| 16 | * | |||
| 17 | * You should have received a copy of the GNU General Public License along with this program. | |||
| 18 | * If not, see <http://www.gnu.org/licenses/>. | |||
| 19 | * | |||
| 20 | * see #testGpsParser | |||
| 21 | */ | |||
| 22 | #include "pch.h" | |||
| 23 | #include "nmea.h" | |||
| 24 | #include "rtc_helper.h" | |||
| 25 | ||||
| 26 | using namespace rusefi::stringutil; | |||
| 27 | ||||
| 28 | 4 | static long hex2int(const char * a, const int len) { | ||
| 29 | int i; | |||
| 30 | 4 | long val = 0; | ||
| 31 | ||||
| 32 |
2/2✓ Branch 0 taken 8 times.
✓ Branch 1 taken 4 times.
|
2/2✓ Decision 'true' taken 8 times.
✓ Decision 'false' taken 4 times.
|
12 | for (i = 0; i < len; i++) |
| 33 |
1/2✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
|
1/2✓ Decision 'true' taken 8 times.
✗ Decision 'false' not taken.
|
8 | if (a[i] <= 57) |
| 34 | 8 | val += (a[i] - 48) * (1 << (4 * (len - 1 - i))); // it's decimal number | ||
| 35 | else | |||
| 36 | ✗ | val += (a[i] - 87) * (1 << (4 * (len - 1 - i))); // it's a-f -> work only with low case hex | ||
| 37 | 4 | return val; | ||
| 38 | } | |||
| 39 | ||||
| 40 | 2 | static float gps_deg_dec(float deg_point) { | ||
| 41 | 2 | float ddeg; | ||
| 42 | 2 | float sec = modff(deg_point, &ddeg) * 60; | ||
| 43 | 2 | int deg = (int) (ddeg / 100); | ||
| 44 | 2 | int min = (int) (deg_point - (deg * 100)); | ||
| 45 | ||||
| 46 | 2 | float absdlat = round(deg * 1000000.); | ||
| 47 | 2 | float absmlat = round(min * 1000000.); | ||
| 48 | 2 | float absslat = round(sec * 1000000.); | ||
| 49 | ||||
| 50 | 2 | return round(absdlat + (absmlat / 60) + (absslat / 3600)) / 1000000; | ||
| 51 | } | |||
| 52 | ||||
| 53 | // Convert lat e lon to decimals (from deg) | |||
| 54 | 1 | static void gps_convert_deg_to_dec(float *latitude, char ns, float *longitude, char we) { | ||
| 55 |
1/2✓ Branch 0 taken 1 time.
✗ Branch 1 not taken.
|
1 | float lat = (ns == 'N') ? *latitude : -1 * (*latitude); | |
| 56 |
1/2✓ Branch 0 taken 1 time.
✗ Branch 1 not taken.
|
1 | float lon = (we == 'E') ? *longitude : -1 * (*longitude); | |
| 57 | ||||
| 58 | 1 | *latitude = gps_deg_dec(lat); | ||
| 59 | 1 | *longitude = gps_deg_dec(lon); | ||
| 60 | 1 | } | ||
| 61 | ||||
| 62 | // in string collect all char till comma and convert to float | |||
| 63 | 26 | static int str_till_comma(const char * const a, char * const dStr) { | ||
| 64 | ||||
| 65 | 26 | int i = 0, sLen = strlen(a); | ||
| 66 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 26 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 26 times.
|
26 | if (sLen > GPS_MAX_STRING) |
| 67 | ✗ | sLen = GPS_MAX_STRING; | ||
| 68 | ||||
| 69 |
3/4✓ Branch 0 taken 164 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 138 times.
✓ Branch 3 taken 26 times.
|
0/1? Decision couldn't be analyzed.
|
164 | while (i < sLen && a[i] != 44) { // while not comma or end |
| 70 | 138 | dStr[i] = a[i]; | ||
| 71 | 138 | i++; | ||
| 72 | } | |||
| 73 | 26 | dStr[i] = '\0'; | ||
| 74 | 26 | return i; | ||
| 75 | } | |||
| 76 | ||||
| 77 | /* | |||
| 78 | GxGGA - name code | |||
| 79 | Parameter Value Unit Description | |||
| 80 | UTC hhmmss.sss Universal time coordinated | |||
| 81 | Lat ddmm.mmmm Latitude | |||
| 82 | Northing Indicator N=North, S=South | |||
| 83 | Lon dddmm.mmmm Longitude | |||
| 84 | Easting Indicator E=East, W=West | |||
| 85 | Status 0 0=Invalid, 1=2D/3D, 2=DGPS, 6=Dead Reckoning | |||
| 86 | SVs Used 00 Number of SVs used for Navigation | |||
| 87 | HDOP 99.99 Horizontal Dilution of Precision | |||
| 88 | Alt (MSL) m Altitude (above means sea level) | |||
| 89 | Unit M=Meters | |||
| 90 | Geoid Sep. m Geoid Separation = Alt(HAE) - Alt(MSL) | |||
| 91 | Unit M=Meters | |||
| 92 | Age of DGPS Corr s Age of Differential Corrections | |||
| 93 | DGPS Ref Station ID of DGPS Reference Station | |||
| 94 | */ | |||
| 95 | 1 | static void nmea_parse_gpgga(char const * const nmea, loc_t *loc) { | ||
| 96 | 1 | char const * p = (char *)nmea; | ||
| 97 | 1 | char dStr[GPS_MAX_STRING]; | ||
| 98 | ||||
| 99 | 1 | p = strchr(p, ',') + 1; //skip time - we read date&time if Valid in GxRMC | ||
| 100 | ||||
| 101 | 1 | p = strchr(p, ',') + 1; // in p string started with searching address | ||
| 102 | 1 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
| 103 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 time.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 1 time.
|
1 | if (strlen(p) == 0) { |
| 104 | ✗ | return; // if no data in field - empty data - we return | ||
| 105 | } | |||
| 106 | ||||
| 107 |
1/1✓ Branch 1 taken 1 time.
|
1 | loc->latitude = atoff(dStr); // fulfil data | |
| 108 | ||||
| 109 | 1 | p = strchr(p, ',') + 1; // see above | ||
| 110 |
1/4✓ Branch 0 taken 1 time.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
1 | switch (p[0]) { | |
| 111 |
1/1✓ Decision 'true' taken 1 time.
|
1 | case 'N': | |
| 112 | 1 | loc->lat = 'N'; | ||
| 113 | 1 | break; | ||
| 114 | ✗ | case 'S': | ||
| 115 | ✗ | loc->lat = 'S'; | ||
| 116 | ✗ | break; | ||
| 117 | ✗ | case ',': | ||
| 118 | ✗ | loc->lat = '\0'; | ||
| 119 | ✗ | break; | ||
| 120 | } | |||
| 121 | ||||
| 122 | 1 | p = strchr(p, ',') + 1; | ||
| 123 | 1 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
| 124 |
1/1✓ Branch 1 taken 1 time.
|
1 | loc->longitude = atoff(dStr); | |
| 125 | ||||
| 126 | 1 | p = strchr(p, ',') + 1; | ||
| 127 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 1 time.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
1 | switch (p[0]) { | |
| 128 | ✗ | case 'W': | ||
| 129 | ✗ | loc->lon = 'W'; | ||
| 130 | ✗ | break; | ||
| 131 |
1/1✓ Decision 'true' taken 1 time.
|
1 | case 'E': | |
| 132 | 1 | loc->lon = 'E'; | ||
| 133 | 1 | break; | ||
| 134 | ✗ | case ',': | ||
| 135 | ✗ | loc->lon = '\0'; | ||
| 136 | ✗ | break; | ||
| 137 | } | |||
| 138 | ||||
| 139 | 1 | p = strchr(p, ',') + 1; | ||
| 140 | 1 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
| 141 | 1 | loc->quality = atoi(dStr); | ||
| 142 | ||||
| 143 | 1 | p = strchr(p, ',') + 1; | ||
| 144 | 1 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
| 145 | 1 | loc->satellites = atoi(dStr); | ||
| 146 | ||||
| 147 | 1 | p = strchr(p, ',') + 1; | ||
| 148 | ||||
| 149 | 1 | p = strchr(p, ',') + 1; | ||
| 150 | 1 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
| 151 |
1/1✓ Branch 1 taken 1 time.
|
1 | loc->altitude = atoff(dStr); | |
| 152 | } | |||
| 153 | ||||
| 154 | /* | |||
| 155 | GxRMC - nmea code | |||
| 156 | Parameter Value Unit Description | |||
| 157 | UTC hhmmss.sss Universal time coordinated | |||
| 158 | Status V A=Valid, V=Invalid | |||
| 159 | Lat ddmm.mmmm Latitude | |||
| 160 | Northing Indicator N=North, S=South | |||
| 161 | Lon dddmm.mmmm Longitude | |||
| 162 | Easting Indicator E=East, W=West | |||
| 163 | SOG nots Speed Over Ground | |||
| 164 | COG (true) ° Course Over Ground (true) | |||
| 165 | Date ddmmyy Universal time coordinated | |||
| 166 | Magnetic Variation ° Magnetic Variation | |||
| 167 | Magnetic Variation E=East,W=West | |||
| 168 | Mode Indicator N A=Autonomous, D=Differential, E=Dead Reckoning, N=None | |||
| 169 | Navigational Status S=Safe C=Caution U=Unsafe V=Not valid | |||
| 170 | */ | |||
| 171 | ||||
| 172 | 3 | static void nmea_parse_gprmc(char const * const nmea, loc_t *loc) { | ||
| 173 | 3 | char const * p = (char *)nmea; | ||
| 174 | 3 | char dStr[GPS_MAX_STRING]; | ||
| 175 | 3 | efidatetime_t dt; | ||
| 176 | ||||
| 177 | 3 | p = strchr(p, ',') + 1; // read time | ||
| 178 | 3 | str_till_comma(p, dStr); | ||
| 179 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
1/2✓ Decision 'true' taken 3 times.
✗ Decision 'false' not taken.
|
3 | if (strlen(dStr) > 5) { |
| 180 | 3 | dt.hour = str2int(dStr, 2); | ||
| 181 | 3 | dt.minute = str2int(dStr + 2, 2); | ||
| 182 | 3 | dt.second = str2int(dStr + 4, 2); | ||
| 183 | } | |||
| 184 | ||||
| 185 | 3 | p = strchr(p, ',') + 1; // read field Valid status | ||
| 186 | 3 | str_till_comma(p, dStr); | ||
| 187 | ||||
| 188 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 3 times.
|
3 | if (dStr[0] == 'V') { // if field is invalid |
| 189 | ✗ | loc->quality = 0; | ||
| 190 | ✗ | return; | ||
| 191 | } | |||
| 192 | ||||
| 193 | 3 | loc->quality = 4; // this is declaration that last receive field VALID | ||
| 194 | ||||
| 195 | 3 | p = strchr(p, ',') + 1; // latitude | ||
| 196 | 3 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
| 197 |
1/1✓ Branch 1 taken 3 times.
|
3 | loc->latitude = atoff(dStr); | |
| 198 | ||||
| 199 | 3 | p = strchr(p, ',') + 1; | ||
| 200 |
1/4✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
3 | switch (p[0]) { | |
| 201 |
1/1✓ Decision 'true' taken 3 times.
|
3 | case 'N': | |
| 202 | 3 | loc->lat = 'N'; | ||
| 203 | 3 | break; | ||
| 204 | ✗ | case 'S': | ||
| 205 | ✗ | loc->lat = 'S'; | ||
| 206 | ✗ | break; | ||
| 207 | ✗ | case ',': | ||
| 208 | ✗ | loc->lat = '\0'; | ||
| 209 | ✗ | break; | ||
| 210 | } | |||
| 211 | ||||
| 212 | 3 | p = strchr(p, ',') + 1; // longitude | ||
| 213 | 3 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
| 214 |
1/1✓ Branch 1 taken 3 times.
|
3 | loc->longitude = atoff(dStr); | |
| 215 | ||||
| 216 | 3 | p = strchr(p, ',') + 1; | ||
| 217 |
2/4✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 time.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
3 | switch (p[0]) { | |
| 218 |
1/1✓ Decision 'true' taken 2 times.
|
2 | case 'W': | |
| 219 | 2 | loc->lon = 'W'; | ||
| 220 | 2 | break; | ||
| 221 |
1/1✓ Decision 'true' taken 1 time.
|
1 | case 'E': | |
| 222 | 1 | loc->lon = 'E'; | ||
| 223 | 1 | break; | ||
| 224 | ✗ | case ',': | ||
| 225 | ✗ | loc->lon = '\0'; | ||
| 226 | ✗ | break; | ||
| 227 | } | |||
| 228 | ||||
| 229 | 3 | p = strchr(p, ',') + 1; | ||
| 230 | 3 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
| 231 |
1/1✓ Branch 1 taken 3 times.
|
3 | loc->speed = atoff(dStr); | |
| 232 | ||||
| 233 | 3 | p = strchr(p, ',') + 1; | ||
| 234 | 3 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
| 235 |
1/1✓ Branch 1 taken 3 times.
|
3 | loc->course = atoff(dStr); | |
| 236 | ||||
| 237 | 3 | p = strchr(p, ',') + 1; // read date | ||
| 238 | 3 | str_till_comma(p, dStr); | ||
| 239 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
1/2✓ Decision 'true' taken 3 times.
✗ Decision 'false' not taken.
|
3 | if (strlen(dStr) > 5) { |
| 240 | 3 | dt.day = str2int(dStr, 2); | ||
| 241 | 3 | dt.month = str2int(dStr + 2, 2); | ||
| 242 | 3 | dt.year = 100 + // we receive -200, but standard wait -1900 = add correction | ||
| 243 | 3 | str2int(dStr + 4, 2); | ||
| 244 | } | |||
| 245 | ||||
| 246 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
1/2✓ Decision 'true' taken 3 times.
✗ Decision 'false' not taken.
|
3 | if (dt.year > 0 ) { // check if date field is valid |
| 247 | 3 | memcpy(&loc->time, &dt, sizeof(dt)); | ||
| 248 | } | |||
| 249 | } | |||
| 250 | ||||
| 251 | /** | |||
| 252 | * Get the message type (GPGGA, GPRMC, etc..) | |||
| 253 | * | |||
| 254 | * This function filters out also wrong packages (invalid checksum) | |||
| 255 | * | |||
| 256 | * @param message The NMEA message | |||
| 257 | * @return The type of message if it is valid | |||
| 258 | */ | |||
| 259 | 5 | nmea_message_type nmea_get_message_type(const char *message) { | ||
| 260 | 5 | int checksum = nmea_valid_checksum(message); | ||
| 261 |
2/2✓ Branch 0 taken 1 time.
✓ Branch 1 taken 4 times.
|
2/2✓ Decision 'true' taken 1 time.
✓ Decision 'false' taken 4 times.
|
5 | if (checksum != _EMPTY) { |
| 262 | 1 | return static_cast<nmea_message_type>(checksum); | ||
| 263 | } | |||
| 264 | ||||
| 265 |
2/2✓ Branch 0 taken 1 time.
✓ Branch 1 taken 3 times.
|
2/2✓ Decision 'true' taken 1 time.
✓ Decision 'false' taken 3 times.
|
4 | if (strstr(message, NMEA_GPGGA_STR) != NULL) { |
| 266 | 1 | return NMEA_GPGGA; | ||
| 267 | } | |||
| 268 | ||||
| 269 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
1/2✓ Decision 'true' taken 3 times.
✗ Decision 'false' not taken.
|
3 | if (strstr(message, NMEA_GPRMC_STR) != NULL) { |
| 270 | 3 | return NMEA_GPRMC; | ||
| 271 | } | |||
| 272 | ||||
| 273 | ✗ | return NMEA_UNKNOWN; | ||
| 274 | } | |||
| 275 | ||||
| 276 | 5 | int nmea_valid_checksum(char const * message) { | ||
| 277 | char p; | |||
| 278 | 5 | int sum = 0; | ||
| 279 | 5 | const char* starPtr = strrchr(message, '*'); | ||
| 280 |
2/2✓ Branch 0 taken 1 time.
✓ Branch 1 taken 4 times.
|
2/2✓ Decision 'true' taken 1 time.
✓ Decision 'false' taken 4 times.
|
5 | if (!starPtr) { |
| 281 | 1 | return NMEA_CHECKSUM_ERR; | ||
| 282 | } | |||
| 283 | 4 | const char* int_message = starPtr + 1; | ||
| 284 | 4 | long checksum = hex2int(int_message, 2); | ||
| 285 | ||||
| 286 | 4 | ++message; | ||
| 287 |
2/2✓ Branch 0 taken 253 times.
✓ Branch 1 taken 4 times.
|
2/2✓ Decision 'true' taken 253 times.
✓ Decision 'false' taken 4 times.
|
257 | while ((p = *message++) != '*') { |
| 288 | 253 | sum ^= p; | ||
| 289 | } | |||
| 290 | ||||
| 291 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 4 times.
|
4 | if (sum != checksum) { |
| 292 | ✗ | return NMEA_CHECKSUM_ERR; | ||
| 293 | } | |||
| 294 | 4 | return _EMPTY; | ||
| 295 | } | |||
| 296 | ||||
| 297 | // Compute the GPS location using decimal scale | |||
| 298 | 5 | void gps_location(loc_t *coord, char const * const buffer) { | ||
| 299 | 5 | coord->type = nmea_get_message_type(buffer); | ||
| 300 | ||||
| 301 |
3/4✓ Branch 0 taken 1 time.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 time.
|
5 | switch (coord->type) { | |
| 302 |
1/1✓ Decision 'true' taken 1 time.
|
1 | case NMEA_GPGGA: | |
| 303 | 1 | nmea_parse_gpgga(buffer, coord); | ||
| 304 | 1 | gps_convert_deg_to_dec(&(coord->latitude), coord->lat, &(coord->longitude), coord->lon); | ||
| 305 | 1 | break; | ||
| 306 |
1/1✓ Decision 'true' taken 3 times.
|
3 | case NMEA_GPRMC: | |
| 307 | 3 | nmea_parse_gprmc(buffer, coord); | ||
| 308 | 3 | break; | ||
| 309 | ✗ | case NMEA_UNKNOWN: | ||
| 310 | // unknown message type | |||
| 311 | ✗ | break; | ||
| 312 | } | |||
| 313 | 5 | } | ||
| 314 |