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(&gta04_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;
+}