diff options
Diffstat (limited to 'gps/nmea.c')
-rw-r--r-- | gps/nmea.c | 564 |
1 files changed, 564 insertions, 0 deletions
diff --git a/gps/nmea.c b/gps/nmea.c new file mode 100644 index 0000000..b8d6338 --- /dev/null +++ b/gps/nmea.c @@ -0,0 +1,564 @@ +/* + * Copyright (C) 2014 Paul Kocialkowski <contact@paulk.fr> + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <stdlib.h> +#include <string.h> +#include <errno.h> +#include <math.h> + +#define LOG_TAG "gta04_gps" +#include <cutils/log.h> + +#include <hardware/gps.h> + +#include "gta04_gps.h" + +unsigned char gta04_gps_nmea_checksum(void *data, size_t size) +{ + unsigned char checksum = 0; + unsigned char *p; + size_t c; + + if (data == NULL || size == 0) + return 0; + + p = (unsigned char *) data; + + for (c = 0; c < size; c++) + checksum ^= *p++; + + return checksum; +} + +char *gta04_gps_nmea_prepare(char *nmea) +{ + static char buffer[80] = { 0 }; + unsigned char checksum; + size_t length; + + if (nmea == NULL) + return NULL; + + length = strlen(nmea); + + checksum = gta04_gps_nmea_checksum(nmea, length); + + memset(&buffer, 0, sizeof(buffer)); + snprintf((char *) &buffer, sizeof(buffer), "$%s*%02X\r\n", nmea, checksum); + + return buffer; +} + +char *gta04_gps_nmea_extract(char *buffer, size_t length) +{ + unsigned char checksum[2]; + char *nmea = NULL; + char *start = NULL; + char *end = NULL; + size_t c; + + if (buffer == NULL || length == 0) + return NULL; + + for (c = 0; c < length; c++) { + if (buffer[c] == '$' && c < (length -1)) + start = &buffer[c + 1]; + else if (buffer[c] == '*' && c < (length - 2)) + end = &buffer[c]; + } + + if (start == NULL || end == NULL || start == end) + goto error; + + *end++ = '\0'; + nmea = strdup(start); + + checksum[0] = gta04_gps_nmea_checksum(nmea, strlen(nmea)); + checksum[1] = strtol(end, NULL, 16); + + if (checksum[0] != checksum[1]) { + ALOGE("Checksum mismatch: %02X != %02X", checksum[0], checksum[1]); + goto error; + } + + goto complete; + +error: + if (nmea != NULL) { + free(nmea); + nmea = NULL; + } + +complete: + return nmea; +} + +char *gta04_gps_nmea_parse(char *nmea) +{ + static char *reference = NULL; + static char *start = NULL; + char *buffer; + char *p; + size_t c; + + if (nmea != reference) { + reference = nmea; + start = nmea; + } + + if (nmea == NULL) + return NULL; + + buffer = start; + p = start; + + while (*p != '\0' && *p != ',') + p++; + + if (p == start) + buffer = NULL; + + if (*p == ',') + *p++ = '\0'; + + start = p; + + return buffer; +} + +int gta04_gps_nmea_parse_int(char *string, size_t offset, size_t length) +{ + char *buffer; + int value; + + if (string == NULL || length == 0) + return -EINVAL; + + buffer = (char *) calloc(1, length + 1); + + strncpy(buffer, string + offset, length); + value = atoi(buffer); + + free(buffer); + + return value; +} + +double gta04_gps_nmea_parse_float(char *string, size_t offset, size_t length) +{ + char *buffer; + double value; + + if (string == NULL || length == 0) + return -EINVAL; + + buffer = (char *) calloc(1, length + 1); + + strncpy(buffer, string + offset, length); + value = atof(buffer); + + free(buffer); + + return value; +} + +int gta04_gps_nmea_time(char *nmea) +{ + struct tm tm; + struct tm tm_utc; + struct tm tm_local; + time_t timestamp; + time_t time_now; + int hour; + int minute; + double second; + char *buffer; + + if (nmea == NULL) + return -EINVAL; + + if (gta04_gps == NULL) + return -1; + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL) { + hour = gta04_gps_nmea_parse_int(buffer, 0, 2); + minute = gta04_gps_nmea_parse_int(buffer, 2, 2); + second = gta04_gps_nmea_parse_float(buffer, 4, strlen(buffer) - 4); + + memset(&tm, 0, sizeof(tm)); + + tm.tm_year = gta04_gps->year + 100; + tm.tm_mon = gta04_gps->month - 1; + tm.tm_mday = gta04_gps->day; + + tm.tm_hour = hour; + tm.tm_min = minute; + tm.tm_sec = (int) second; + + // In the end, we need UTC, so never mind DST so that we only have timezone diff from mktime + tm.tm_isdst = 0; + + // Offset between UTC and local time due to timezone + time_now = time(NULL); + gmtime_r(&time_now, &tm_utc); + localtime_r(&time_now, &tm_local); + + // Time with correct offset (timezone-independent) + timestamp = mktime(&tm) - (mktime(&tm_utc) - mktime(&tm_local)); + + gta04_gps->location.timestamp = (GpsUtcTime) timestamp * 1000 + (GpsUtcTime) ((second - tm.tm_sec) * 1000); + } + + return 0; +} + +int gta04_gps_nmea_date(char *nmea) +{ + int date; + char *buffer; + + if (nmea == NULL) + return -EINVAL; + + if (gta04_gps == NULL) + return -1; + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL) { + gta04_gps->day = gta04_gps_nmea_parse_int(buffer, 0, 2); + gta04_gps->month = gta04_gps_nmea_parse_int(buffer, 2, 2); + gta04_gps->year = gta04_gps_nmea_parse_int(buffer, 4, strlen(buffer) - 4); + } + + return 0; +} + +int gta04_gps_nmea_coordinates(char *nmea) +{ + double latitude = 0; + double longitude = 0; + double minutes; + int degrees; + char *buffer; + + if (nmea == NULL) + return -EINVAL; + + if (gta04_gps == NULL) + return -1; + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL) { + latitude = gta04_gps_nmea_parse_float(buffer, 0, strlen(buffer)); + degrees = (int) floor(latitude / 100); + minutes = latitude - degrees * 100.0f; + latitude = degrees + minutes / 60.0f; + } + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL && buffer[0] == 'S') + latitude *= -1.0f; + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL) { + longitude = gta04_gps_nmea_parse_float(buffer, 0, strlen(buffer)); + degrees = (int) floor(longitude / 100); + minutes = longitude - degrees * 100.0f; + longitude = degrees + minutes / 60.0f; + } + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL && buffer[0] == 'W') + longitude *= -1.0f; + + gta04_gps->location.latitude = latitude; + gta04_gps->location.longitude = longitude; + + if (latitude != 0 && longitude != 0) + gta04_gps->location.flags |= GPS_LOCATION_HAS_LAT_LONG; + + return 0; +} + +int gta04_gps_nmea_accurate(char *nmea) +{ + unsigned char accurate; + char *buffer; + + if (gta04_gps == NULL) + return -1; + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer == NULL) + return -1; + + if (buffer[0] == 'A') + accurate = 1; + else + accurate = 0; + + if (gta04_gps->accurate == 1 && accurate == 0) { + memset(>a04_gps->location, 0, sizeof(GpsLocation)); + gta04_gps->location.size = sizeof(GpsLocation); + } + + gta04_gps->accurate = accurate; + + return 0; +} + +int gta04_gps_nmea_gpgga(char *nmea) +{ + double altitude = 0; + char *buffer; + int i; + + if (nmea == NULL) + return -EINVAL; + + if (gta04_gps == NULL) + return -1; + + gta04_gps_nmea_time(nmea); + gta04_gps_nmea_coordinates(nmea); + + // Skip the following 3 arguments + for (i = 6; i < 9; i++) + gta04_gps_nmea_parse(nmea); + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL) + altitude = gta04_gps_nmea_parse_float(buffer, 0, strlen(buffer)); + + gta04_gps->location.altitude = altitude; + + if (altitude != 0) + gta04_gps->location.flags |= GPS_LOCATION_HAS_ALTITUDE; + + return 0; +} + +int gta04_gps_nmea_gpgll(char *nmea) +{ + if (nmea == NULL) + return -EINVAL; + + if (gta04_gps == NULL) + return -1; + + gta04_gps_nmea_coordinates(nmea); + gta04_gps_nmea_time(nmea); + gta04_gps_nmea_accurate(nmea); + + if (gta04_gps->accurate) + gta04_gps_location_callback(); + + return 0; +} + +int gta04_gps_nmea_gpgsa(char *nmea) +{ + char *buffer; + int prn; + float accuracy = 0; + int i; + + if (nmea == NULL) + return -EINVAL; + + if (gta04_gps == NULL) + return -1; + + // Skip the first 2 arguments + for (i = 0; i < 2; i++) + gta04_gps_nmea_parse(nmea); + + gta04_gps->sv_status.used_in_fix_mask = 0; + + for (i = 0; i < channel_count; i++) { + buffer = gta04_gps_nmea_parse(nmea); + if (buffer == NULL) + continue; + + prn = gta04_gps_nmea_parse_int(buffer, 0, strlen(buffer)); + if (prn != 0) + gta04_gps->sv_status.used_in_fix_mask |= 1 << (prn - 1); + } + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL) { + accuracy = gta04_gps_nmea_parse_float(buffer, 0, strlen(buffer)); + + gta04_gps->location.accuracy = accuracy; + gta04_gps->location.flags |= GPS_LOCATION_HAS_ACCURACY; + } + + return 0; +} + +int gta04_gps_nmea_gpgsv(char *nmea) +{ + int sv_count = 0; + int sv_index; + int count; + int index; + int prn; + float elevation; + float azimuth; + float snr; + char *buffer; + int i; + + if (nmea == NULL) + return -EINVAL; + + if (gta04_gps == NULL) + return -1; + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer == NULL) + return -1; + + count = gta04_gps_nmea_parse_int(buffer, 0, strlen(buffer)); + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer == NULL) + return -1; + + index = gta04_gps_nmea_parse_int(buffer, 0, strlen(buffer)); + + if (index == 1) + gta04_gps->sv_index = 0; + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL) + sv_count = gta04_gps_nmea_parse_int(buffer, 0, strlen(buffer)); + + if (sv_count > GPS_MAX_SVS) + sv_count = GPS_MAX_SVS; + + for (sv_index = gta04_gps->sv_index; sv_index < sv_count; sv_index++) { + buffer = gta04_gps_nmea_parse(nmea); + if (buffer == NULL) + break; + + prn = gta04_gps_nmea_parse_int(buffer, 0, strlen(buffer)); + + snr = 0; + elevation = 0; + azimuth = 0; + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL) + elevation = gta04_gps_nmea_parse_float(buffer, 0, strlen(buffer)); + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL) + azimuth = gta04_gps_nmea_parse_float(buffer, 0, strlen(buffer)); + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL) + snr = gta04_gps_nmea_parse_float(buffer, 0, strlen(buffer)); + + gta04_gps->sv_status.sv_list[sv_index].size = sizeof(GpsSvInfo); + gta04_gps->sv_status.sv_list[sv_index].prn = prn; + gta04_gps->sv_status.sv_list[sv_index].snr = snr; + gta04_gps->sv_status.sv_list[sv_index].elevation = elevation; + gta04_gps->sv_status.sv_list[sv_index].azimuth = azimuth; + } + + gta04_gps->sv_index = sv_index; + + if (index == count) { + gta04_gps->sv_status.num_svs = sv_count; + + gta04_gps_sv_status_callback(); + } + + return 0; +} + +int gta04_gps_nmea_gprmc(char *nmea) +{ + float speed = 0; + float bearing = 0; + char *buffer; + + if (nmea == NULL) + return -EINVAL; + + if (gta04_gps == NULL) + return -1; + + gta04_gps_nmea_time(nmea); + gta04_gps_nmea_accurate(nmea); + gta04_gps_nmea_coordinates(nmea); + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL) + speed = gta04_gps_nmea_parse_float(buffer, 0, strlen(buffer)); + + buffer = gta04_gps_nmea_parse(nmea); + if (buffer != NULL) + bearing = gta04_gps_nmea_parse_float(buffer, 0, strlen(buffer)); + + gta04_gps->location.speed = speed; + gta04_gps->location.bearing = bearing; + + if (speed != 0) + gta04_gps->location.flags |= GPS_LOCATION_HAS_SPEED; + + if (bearing != 0) + gta04_gps->location.flags |= GPS_LOCATION_HAS_BEARING; + + gta04_gps_nmea_date(nmea); + + if (gta04_gps->accurate) + gta04_gps_location_callback(); + + return 0; +} + +int gta04_gps_nmea_psrf103(unsigned char message, int interval) +{ + char nmea[74] = { 0 }; + char *buffer; + int rc; + + if (gta04_gps == NULL) + return -1; + + if (interval > 255) + interval = 255; + + snprintf((char *) &nmea, sizeof(nmea), "PSRF103,%02d,00,%02d,01", message, interval); + + buffer = gta04_gps_nmea_prepare(nmea); + if (buffer == NULL) { + ALOGE("Preparing NMEA failed"); + return -1; + } + + rc = gta04_gps_serial_write(&buffer, strlen(buffer) + 1); + if (rc < 0) { + ALOGE("Writing to serial failed"); + return -1; + } + + return 0; +} |