From 6361460615e5cd8ddfe63c2daa378446d56ee128 Mon Sep 17 00:00:00 2001 From: Paul Kocialkowski Date: Wed, 23 Apr 2014 16:43:07 +0200 Subject: GTA04 GPS Signed-off-by: Paul Kocialkowski --- gps/Android.mk | 28 ++ gps/gta04_gps.c | 839 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ gps/gta04_gps.h | 124 +++++++++ gps/nmea.c | 564 +++++++++++++++++++++++++++++++++++++ gps/rfkill.h | 109 ++++++++ 5 files changed, 1664 insertions(+) create mode 100644 gps/Android.mk create mode 100644 gps/gta04_gps.c create mode 100644 gps/gta04_gps.h create mode 100644 gps/nmea.c create mode 100644 gps/rfkill.h (limited to 'gps') diff --git a/gps/Android.mk b/gps/Android.mk new file mode 100644 index 0000000..65b4fff --- /dev/null +++ b/gps/Android.mk @@ -0,0 +1,28 @@ +# Copyright (C) 2014 Paul Kocialkowski +# +# 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 . + +LOCAL_PATH:= $(call my-dir) +include $(CLEAR_VARS) + +LOCAL_SRC_FILES := gta04_gps.c nmea.c + +LOCAL_SHARED_LIBRARIES := liblog + +LOCAL_PRELINK_MODULE := false +LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/hw +LOCAL_MODULE_TAGS := optional + +LOCAL_MODULE := gps.gta04 +include $(BUILD_SHARED_LIBRARY) diff --git a/gps/gta04_gps.c b/gps/gta04_gps.c new file mode 100644 index 0000000..21a8195 --- /dev/null +++ b/gps/gta04_gps.c @@ -0,0 +1,839 @@ +/* + * Copyright (C) 2014 Paul Kocialkowski + * + * 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 . + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define LOG_TAG "gta04_gps" +#include + +#include + +#include "gta04_gps.h" +#include "rfkill.h" + +/* + * Globals + */ + +const char antenna_state_path[] = "/sys/class/switch/gps_antenna/state"; + +const char serial_path[] = "/dev/ttyO1"; +const speed_t serial_speed = B9600; + +const int channel_count = 12; + +struct gta04_gps *gta04_gps = NULL; + +/* + * Callbacks + */ + +void gta04_gps_location_callback(void) +{ + GpsLocationFlags single_flags = GPS_LOCATION_HAS_LAT_LONG | GPS_LOCATION_HAS_ALTITUDE | GPS_LOCATION_HAS_ACCURACY; + + if (gta04_gps == NULL || gta04_gps->callbacks == NULL || gta04_gps->callbacks->location_cb == NULL) + return; + + if (gta04_gps->recurrence == GPS_POSITION_RECURRENCE_SINGLE) { + if ((gta04_gps->location.flags & single_flags) != single_flags) + return; + else + gta04_gps_event_write(GTA04_GPS_EVENT_STOP); + } + + gta04_gps->callbacks->location_cb(>a04_gps->location); +} + +void gta04_gps_status_callback(void) +{ + if (gta04_gps == NULL || gta04_gps->callbacks == NULL || gta04_gps->callbacks->status_cb == NULL) + return; + + gta04_gps->callbacks->status_cb(>a04_gps->status); +} + +void gta04_gps_sv_status_callback(void) +{ + if (gta04_gps == NULL || gta04_gps->callbacks == NULL || gta04_gps->callbacks->sv_status_cb == NULL) + return; + + if (gta04_gps->recurrence == GPS_POSITION_RECURRENCE_SINGLE) + return; + + gta04_gps->callbacks->sv_status_cb(>a04_gps->sv_status); +} + +void gta04_gps_set_capabilities_callback(void) +{ + if (gta04_gps == NULL || gta04_gps->callbacks == NULL || gta04_gps->callbacks->set_capabilities_cb == NULL) + return; + + gta04_gps->callbacks->set_capabilities_cb(gta04_gps->capabilities); +} + +void gta04_gps_acquire_wakelock_callback(void) +{ + if (gta04_gps == NULL || gta04_gps->callbacks == NULL || gta04_gps->callbacks->acquire_wakelock_cb == NULL) + return; + + gta04_gps->callbacks->acquire_wakelock_cb(); +} + +void gta04_gps_release_wakelock_callback(void) +{ + if (gta04_gps == NULL || gta04_gps->callbacks == NULL || gta04_gps->callbacks->release_wakelock_cb == NULL) + return; + + gta04_gps->callbacks->release_wakelock_cb(); +} + +/* + * GTA04 GPS + */ + +int gta04_gps_antenna_state(void) +{ + char state[9] = { 0 }; + int fd; + + fd = open(antenna_state_path, O_RDONLY); + if (fd < 0) + return -1; + + read(fd, &state, sizeof(state)); + close(fd); + + state[8] = '\0'; + + ALOGD("GPS antenna state is: %s", state); + + return 0; +} + +// Rfkill + +int gta04_gps_rfkill_change(unsigned char type, unsigned char soft) +{ + struct rfkill_event event; + int fd = -1; + int rc; + + fd = open("/dev/rfkill", O_WRONLY); + if (fd < 0) { + ALOGE("Opening rfkill device failed"); + return -1; + } + + memset(&event, 0, sizeof(event)); + event.type = type; + event.op = RFKILL_OP_CHANGE_ALL; + event.soft = soft; + + rc = write(fd, &event, sizeof(event)); + if (rc < (int) sizeof(struct rfkill_event)) { + ALOGE("Writing rfkill event failed"); + goto error; + } + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (fd >= 0) + close(fd); + + return rc; +} + +// Serial + +int gta04_gps_serial_open(void) +{ + struct termios termios; + int serial_fd = -1; + int rc; + + if (gta04_gps == NULL) + return -1; + + pthread_mutex_lock(>a04_gps->mutex); + + if (gta04_gps->serial_fd >= 0) + close(gta04_gps->serial_fd); + + serial_fd = open(serial_path, O_RDWR | O_NONBLOCK); + if (serial_fd < 0) { + ALOGE("Opening serial failed"); + goto error; + } + + rc = tcgetattr(serial_fd, &termios); + if (rc < 0) + goto error; + + if (cfgetispeed(&termios) != serial_speed) { + cfsetispeed(&termios, serial_speed); + + rc = tcsetattr(serial_fd, 0, &termios); + if (rc < 0) + goto error; + } + + if (cfgetospeed(&termios) != serial_speed) { + cfsetospeed(&termios, serial_speed); + + rc = tcsetattr(serial_fd, 0, &termios); + if (rc < 0) + goto error; + } + + gta04_gps->serial_fd = serial_fd; + + rc = 0; + goto complete; + +error: + if (serial_fd >= 0) + close(serial_fd); + + gta04_gps->serial_fd = -1; + + rc = -1; + +complete: + pthread_mutex_unlock(>a04_gps->mutex); + + return rc; +} + +int gta04_gps_serial_close(void) +{ + if (gta04_gps == NULL) + return -1; + + if (gta04_gps->serial_fd < 0) + return 0; + + pthread_mutex_lock(>a04_gps->mutex); + + close(gta04_gps->serial_fd); + gta04_gps->serial_fd = -1; + + pthread_mutex_unlock(>a04_gps->mutex); + + return 0; +} + +int gta04_gps_serial_read(void *buffer, size_t length) +{ + int rc; + + if (buffer == NULL || length == 0) + return -EINVAL; + + if (gta04_gps == NULL || gta04_gps->serial_fd < 0) + return -1; + + pthread_mutex_lock(>a04_gps->mutex); + gta04_gps_acquire_wakelock_callback(); + + rc = read(gta04_gps->serial_fd, buffer, length); + if (rc <= 0) + goto error; + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + gta04_gps_release_wakelock_callback(); + pthread_mutex_unlock(>a04_gps->mutex); + + return rc; +} + +int gta04_gps_serial_write(void *buffer, size_t length) +{ + int rc; + + if (buffer == NULL || length == 0) + return -EINVAL; + + if (gta04_gps == NULL || gta04_gps->serial_fd < 0) + return -1; + + pthread_mutex_lock(>a04_gps->mutex); + gta04_gps_acquire_wakelock_callback(); + + rc = write(gta04_gps->serial_fd, buffer, length); + if (rc < (int) length) + goto error; + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + gta04_gps_release_wakelock_callback(); + pthread_mutex_unlock(>a04_gps->mutex); + + return rc; +} + +// Event + +int gta04_gps_event_read(eventfd_t *event) +{ + int rc; + + if (event == NULL) + return -EINVAL; + + if (gta04_gps == NULL || gta04_gps->event_fd < 0) + return -1; + + rc = eventfd_read(gta04_gps->event_fd, event); + if (rc < 0) + return -1; + + return 0; +} + +int gta04_gps_event_write(eventfd_t event) +{ + eventfd_t flush; + int rc; + + if (gta04_gps == NULL || gta04_gps->event_fd < 0) + return -1; + + eventfd_read(gta04_gps->event_fd, &flush); + + rc = eventfd_write(gta04_gps->event_fd, event); + if (rc < 0) + return -1; + + return 0; +} + +// Thread + +int gta04_gps_serial_handle(void) +{ + char buffer[80] = { 0 }; + char *nmea = NULL; + char *address; + int interval; + int rc; + + if (gta04_gps == NULL) + return -1; + + rc = gta04_gps_serial_read(&buffer, sizeof(buffer)); + if (rc < 0) { + ALOGE("Reading from serial failed"); + return -1; + } + + nmea = gta04_gps_nmea_extract(buffer, sizeof(buffer)); + if (nmea == NULL) { + rc = 0; + goto complete; + } + + if (gta04_gps->status.status != GPS_STATUS_SESSION_BEGIN) { + // Now is a good time to setup the interval of location messages + + if (gta04_gps->recurrence == GPS_POSITION_RECURRENCE_SINGLE) + gta04_gps->interval = 1000; + + interval = gta04_gps->interval / 1000; + + // Location is reported from GPRMC message + gta04_gps_nmea_psrf103(4, interval); + + gta04_gps->status.status = GPS_STATUS_SESSION_BEGIN; + gta04_gps_status_callback(); + } + + // ALOGD("NMEA: %s", nmea); + + address = gta04_gps_nmea_parse(nmea); + if (address == NULL) { + ALOGE("Parsing NMEA failed"); + goto error; + } + + if (strcmp("GPGGA", address) == 0) + rc = gta04_gps_nmea_gpgga(nmea); + else if (strcmp("GPGLL", address) == 0) + rc = gta04_gps_nmea_gpgll(nmea); + else if (strcmp("GPGSA", address) == 0) + rc = gta04_gps_nmea_gpgsa(nmea); + else if (strcmp("GPGSV", address) == 0) + rc = gta04_gps_nmea_gpgsv(nmea); + else if (strcmp("GPRMC", address) == 0) + rc = gta04_gps_nmea_gprmc(nmea); + else + rc = 0; + + gta04_gps_nmea_parse(NULL); + + goto complete; + +error: + rc = -1; + +complete: + if (nmea != NULL) + free(nmea); + + return rc; +} + +int gta04_gps_event_handle(void) +{ + eventfd_t event = 0; + int rc; + + if (gta04_gps == NULL) + return -1; + + rc = gta04_gps_event_read(&event); + if (rc < 0) { + ALOGE("Reading event failed"); + return -1; + } + + switch (event) { + case GTA04_GPS_EVENT_TERMINATE: + return 1; + case GTA04_GPS_EVENT_START: + ALOGD("Starting the GPS"); + + rc = gta04_gps_rfkill_change(RFKILL_TYPE_GPS, 0); + if (rc < 0) + return -1; + + // Give it some time to power up the antenna + usleep(20000); + + gta04_gps_antenna_state(); + + rc = gta04_gps_serial_open(); + if (rc < 0) + return -1; + + gta04_gps->status.status = GPS_STATUS_ENGINE_ON; + gta04_gps_status_callback(); + break; + case GTA04_GPS_EVENT_STOP: + ALOGD("Stopping the GPS"); + + gta04_gps->status.status = GPS_STATUS_SESSION_END; + gta04_gps_status_callback(); + + rc = gta04_gps_serial_close(); + if (rc < 0) + return -1; + + rc = gta04_gps_rfkill_change(RFKILL_TYPE_GPS, 1); + if (rc < 0) + return -1; + + gta04_gps->status.status = GPS_STATUS_ENGINE_OFF; + gta04_gps_status_callback(); + break; + case GTA04_GPS_EVENT_RESTART: + ALOGD("Restarting the GPS"); + + gta04_gps->status.status = GPS_STATUS_SESSION_END; + gta04_gps_status_callback(); + + rc = gta04_gps_serial_close(); + if (rc < 0) + return -1; + + gta04_gps->status.status = GPS_STATUS_ENGINE_OFF; + gta04_gps_status_callback(); + + // Small delay between off and on + usleep(20000); + + rc = gta04_gps_serial_open(); + if (rc < 0) + return -1; + + gta04_gps->status.status = GPS_STATUS_ENGINE_ON; + gta04_gps_status_callback(); + break; + case GTA04_GPS_EVENT_INJECT_TIME: + break; + case GTA04_GPS_EVENT_INJECT_LOCATION: + break; + case GTA04_GPS_EVENT_SET_POSITION_MODE: + // Position mode is set later on + break; + } + + return 0; +} + +void gta04_gps_thread(void *data) +{ + struct timeval time; + struct timeval *timeout; + fd_set fds; + int fd_max; + int failures; + int rc; + + ALOGD("%s(%p)", __func__, data); + + if (gta04_gps == NULL || gta04_gps->event_fd < 0) + return; + + failures = 0; + + while (1) { + timeout = NULL; + + FD_ZERO(&fds); + FD_SET(gta04_gps->event_fd, &fds); + + if (gta04_gps->serial_fd >= 0) { + FD_SET(gta04_gps->serial_fd, &fds); + + time.tv_sec = 2; + time.tv_usec = 0; + + timeout = &time; + } + + fd_max = gta04_gps->event_fd > gta04_gps->serial_fd ? gta04_gps->event_fd : gta04_gps->serial_fd; + + rc = select(fd_max + 1, &fds, NULL, NULL, timeout); + if (rc < 0) { + ALOGE("Polling failed"); + break; + } else if (rc == 0 && gta04_gps->status.status == GPS_STATUS_ENGINE_ON) { + ALOGE("Not receiving anything from the GPS"); + gta04_gps_event_write(GTA04_GPS_EVENT_RESTART); + continue; + } + + rc = 0; + + if (FD_ISSET(gta04_gps->serial_fd, &fds)) + rc |= gta04_gps_serial_handle(); + + if (FD_ISSET(gta04_gps->event_fd, &fds)) + rc |= gta04_gps_event_handle(); + + if (rc < 0) + failures++; + else if (rc == 1) + break; + else if (failures) + failures = 0; + + if (failures > 10) { + ALOGE("Too many failures, terminating thread"); + break; + } + } +} + +/* + * GPS Interface + */ + +int gta04_gps_init(GpsCallbacks *callbacks) +{ + int event_fd; + int rc; + + ALOGD("%s(%p)", __func__, callbacks); + + if (callbacks == NULL || callbacks->create_thread_cb == NULL) + return -EINVAL; + + if (gta04_gps != NULL) + free(gta04_gps); + + gta04_gps = (struct gta04_gps *) calloc(1, sizeof(struct gta04_gps)); + gta04_gps->callbacks = callbacks; + gta04_gps->serial_fd = -1; + + gta04_gps->location.size = sizeof(GpsLocation); + gta04_gps->status.size = sizeof(GpsStatus); + gta04_gps->sv_status.size = sizeof(GpsSvStatus); + + gta04_gps->capabilities = GPS_CAPABILITY_SCHEDULING | GPS_CAPABILITY_SINGLE_SHOT; + + pthread_mutex_init(>a04_gps->mutex, NULL); + + event_fd = eventfd(0, EFD_NONBLOCK); + if (event_fd < 0) { + ALOGE("Opening eventfd failed"); + goto error; + } + + gta04_gps->event_fd = event_fd; + + gta04_gps->thread = callbacks->create_thread_cb("GTA04 GPS", >a04_gps_thread, NULL); + + rc = gta04_gps_rfkill_change(RFKILL_TYPE_GPS, 1); + if (rc < 0) + goto error; + + gta04_gps_set_capabilities_callback(); + + gta04_gps->status.status = GPS_STATUS_ENGINE_OFF; + gta04_gps_status_callback(); + + rc = 0; + goto complete; + +error: + pthread_mutex_destroy(>a04_gps->mutex); + + if (event_fd >= 0) { + gta04_gps_event_write(GTA04_GPS_EVENT_TERMINATE); + + close(gta04_gps->event_fd); + gta04_gps->event_fd = -1; + } + + if (gta04_gps != NULL) { + free(gta04_gps); + gta04_gps = NULL; + } + + rc = -1; + +complete: + return rc; +} + +void gta04_gps_cleanup(void) +{ + ALOGD("%s()", __func__); + + if (gta04_gps == NULL) + return; + + pthread_mutex_destroy(>a04_gps->mutex); + + if (gta04_gps->event_fd >= 0) { + gta04_gps_event_write(GTA04_GPS_EVENT_TERMINATE); + + close(gta04_gps->event_fd); + gta04_gps->event_fd = -1; + } + + free(gta04_gps); + gta04_gps = NULL; +} + +int gta04_gps_start(void) +{ + int rc; + + ALOGD("%s()", __func__); + + rc = gta04_gps_event_write(GTA04_GPS_EVENT_START); + if (rc < 0) { + ALOGE("Writing event failed"); + return -1; + } + + return 0; +} + +int gta04_gps_stop(void) +{ + int rc; + + ALOGD("%s()", __func__); + + rc = gta04_gps_event_write(GTA04_GPS_EVENT_STOP); + if (rc < 0) { + ALOGE("Writing event failed"); + return -1; + } + + return 0; +} + +int gta04_gps_inject_time(GpsUtcTime time, int64_t reference, + int uncertainty) +{ + int rc; + + ALOGD("%s(%ld, %ld, %d)", __func__, (long int) time, (long int) reference, uncertainty); + + if (gta04_gps == NULL) + return -1; + + rc = gta04_gps_event_write(GTA04_GPS_EVENT_INJECT_TIME); + if (rc < 0) { + ALOGE("Writing event failed"); + return -1; + } + + return 0; +} + +int gta04_gps_inject_location(double latitude, double longitude, float accuracy) +{ + int rc; + + ALOGD("%s(%f, %f, %f)", __func__, latitude, longitude, accuracy); + + if (gta04_gps == NULL) + return -1; + + rc = gta04_gps_event_write(GTA04_GPS_EVENT_INJECT_LOCATION); + if (rc < 0) { + ALOGE("Writing event failed"); + return -1; + } + + return 0; +} + +void gta04_gps_delete_aiding_data(GpsAidingData flags) +{ + return; +} + +int gta04_gps_set_position_mode(GpsPositionMode mode, + GpsPositionRecurrence recurrence, uint32_t interval, + uint32_t preferred_accuracy, uint32_t preferred_time) +{ + int rc; + + ALOGD("%s(%d, %d, %d, %d, %d)", __func__, mode, recurrence, interval, preferred_accuracy, preferred_time); + + if (gta04_gps == NULL) + return -1; + + gta04_gps->recurrence = recurrence; + gta04_gps->interval = interval; + + rc = gta04_gps_event_write(GTA04_GPS_EVENT_SET_POSITION_MODE); + if (rc < 0) { + ALOGE("Writing event failed"); + return -1; + } + + return 0; +} + +const void *gta04_gps_get_extension(const char *name) +{ + ALOGD("%s(%s)", __func__, name); + + return NULL; +} + +/* + * Interface + */ + +const GpsInterface gta04_gps_interface = { + .size = sizeof(GpsInterface), + .init = gta04_gps_init, + .cleanup = gta04_gps_cleanup, + .start = gta04_gps_start, + .stop = gta04_gps_stop, + .inject_time = gta04_gps_inject_time, + .inject_location = gta04_gps_inject_location, + .delete_aiding_data = gta04_gps_delete_aiding_data, + .set_position_mode = gta04_gps_set_position_mode, + .get_extension = gta04_gps_get_extension, +}; + +const GpsInterface *gta04_gps_get_gps_interface(struct gps_device_t *device) +{ + ALOGD("%s(%p)", __func__, device); + + return >a04_gps_interface; +} + +int gta04_gps_close(struct hw_device_t *device) +{ + ALOGD("%s(%p)", __func__, device); + + if (device != NULL) + free(device); + + return 0; +} + +int gta04_gps_open(const struct hw_module_t *module, const char *id, + struct hw_device_t **device) +{ + struct gps_device_t *gps_device; + + ALOGD("%s(%p, %s, %p)", __func__, module, id, device); + + if (module == NULL || id == NULL || device == NULL) + return -EINVAL; + + gps_device = calloc(1, sizeof(struct gps_device_t)); + gps_device->common.tag = HARDWARE_DEVICE_TAG; + gps_device->common.version = 0; + gps_device->common.module = (struct hw_module_t *) module; + gps_device->common.close = (int (*)(struct hw_device_t *)) gta04_gps_close; + gps_device->get_gps_interface = gta04_gps_get_gps_interface; + + *device = (struct hw_device_t *) gps_device; + + return 0; +} + +struct hw_module_methods_t gta04_gps_module_methods = { + .open = gta04_gps_open, +}; + +struct hw_module_t HAL_MODULE_INFO_SYM = { + .tag = HARDWARE_MODULE_TAG, + .version_major = 1, + .version_minor = 0, + .id = GPS_HARDWARE_MODULE_ID, + .name = "GTA04 GPS", + .author = "Paul Kocialkowski", + .methods = >a04_gps_module_methods, +}; diff --git a/gps/gta04_gps.h b/gps/gta04_gps.h new file mode 100644 index 0000000..fcdf3df --- /dev/null +++ b/gps/gta04_gps.h @@ -0,0 +1,124 @@ +/* + * Copyright (C) 2014 Paul Kocialkowski + * + * 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 . + */ + +#include +#include +#include + +#include + +#ifndef _GTA04_GPS_H_ +#define _GTA04_GPS_H_ + +/* + * Structures + */ + +struct gta04_gps { + GpsCallbacks *callbacks; + + GpsLocation location; + GpsStatus status; + GpsSvStatus sv_status; + uint32_t capabilities; + + int sv_index; + unsigned char accurate; + + int year; + int month; + int day; + + GpsPositionRecurrence recurrence; + uint32_t interval; + + pthread_mutex_t mutex; + pthread_t thread; + + int serial_fd; + int event_fd; +}; + +/* + * Values + */ + +enum { + GTA04_GPS_EVENT_NONE, + GTA04_GPS_EVENT_TERMINATE, + GTA04_GPS_EVENT_START, + GTA04_GPS_EVENT_STOP, + GTA04_GPS_EVENT_RESTART, + GTA04_GPS_EVENT_INJECT_TIME, + GTA04_GPS_EVENT_INJECT_LOCATION, + GTA04_GPS_EVENT_SET_POSITION_MODE, +}; + +/* + * Globals + */ + +extern const char antenna_state_path[]; +extern const char serial_path[]; +extern const speed_t serial_speed; + +extern const int channel_count; + +extern struct gta04_gps *gta04_gps; + +/* + * Declarations + */ + +// GTA04 GPS + +void gta04_gps_location_callback(void); +void gta04_gps_status_callback(void); +void gta04_gps_sv_status_callback(void); +void gta04_gps_set_capabilities_callback(void); +void gta04_gps_acquire_wakelock_callback(void); +void gta04_gps_release_wakelock_callback(void); + +int gta04_gps_serial_open(void); +int gta04_gps_serial_close(void); +int gta04_gps_serial_read(void *buffer, size_t length); +int gta04_gps_serial_write(void *buffer, size_t length); + +int gta04_gps_event_read(eventfd_t *event); +int gta04_gps_event_write(eventfd_t event); + +// NMEA + +char *gta04_gps_nmea_prepare(char *nmea); +char *gta04_gps_nmea_extract(char *buffer, size_t length); +char *gta04_gps_nmea_parse(char *nmea); +int gta04_gps_nmea_parse_int(char *string, size_t offset, size_t length); +double gta04_gps_nmea_parse_float(char *string, size_t offset, size_t length); + +int gta04_gps_nmea_time(char *nmea); +int gta04_gps_nmea_date(char *nmea); +int gta04_gps_nmea_coordinates(char *nmea); + +int gta04_gps_nmea_gpgga(char *nmea); +int gta04_gps_nmea_gpgll(char *nmea); +int gta04_gps_nmea_gpgsa(char *nmea); +int gta04_gps_nmea_gpgsv(char *nmea); +int gta04_gps_nmea_gprmc(char *nmea); + +int gta04_gps_nmea_psrf103(unsigned char message, int interval); + +#endif 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 + * + * 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 . + */ + +#include +#include +#include +#include + +#define LOG_TAG "gta04_gps" +#include + +#include + +#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; +} diff --git a/gps/rfkill.h b/gps/rfkill.h new file mode 100644 index 0000000..058757f --- /dev/null +++ b/gps/rfkill.h @@ -0,0 +1,109 @@ +/* + * Copyright (C) 2006 - 2007 Ivo van Doorn + * Copyright (C) 2007 Dmitry Torokhov + * Copyright 2009 Johannes Berg + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ +#ifndef _UAPI__RFKILL_H +#define _UAPI__RFKILL_H + + +#include + +/* define userspace visible states */ +#define RFKILL_STATE_SOFT_BLOCKED 0 +#define RFKILL_STATE_UNBLOCKED 1 +#define RFKILL_STATE_HARD_BLOCKED 2 + +/** + * enum rfkill_type - type of rfkill switch. + * + * @RFKILL_TYPE_ALL: toggles all switches (requests only - not a switch type) + * @RFKILL_TYPE_WLAN: switch is on a 802.11 wireless network device. + * @RFKILL_TYPE_BLUETOOTH: switch is on a bluetooth device. + * @RFKILL_TYPE_UWB: switch is on a ultra wideband device. + * @RFKILL_TYPE_WIMAX: switch is on a WiMAX device. + * @RFKILL_TYPE_WWAN: switch is on a wireless WAN device. + * @RFKILL_TYPE_GPS: switch is on a GPS device. + * @RFKILL_TYPE_FM: switch is on a FM radio device. + * @RFKILL_TYPE_NFC: switch is on an NFC device. + * @NUM_RFKILL_TYPES: number of defined rfkill types + */ +enum rfkill_type { + RFKILL_TYPE_ALL = 0, + RFKILL_TYPE_WLAN, + RFKILL_TYPE_BLUETOOTH, + RFKILL_TYPE_UWB, + RFKILL_TYPE_WIMAX, + RFKILL_TYPE_WWAN, + RFKILL_TYPE_GPS, + RFKILL_TYPE_FM, + RFKILL_TYPE_NFC, + NUM_RFKILL_TYPES, +}; + +/** + * enum rfkill_operation - operation types + * @RFKILL_OP_ADD: a device was added + * @RFKILL_OP_DEL: a device was removed + * @RFKILL_OP_CHANGE: a device's state changed -- userspace changes one device + * @RFKILL_OP_CHANGE_ALL: userspace changes all devices (of a type, or all) + */ +enum rfkill_operation { + RFKILL_OP_ADD = 0, + RFKILL_OP_DEL, + RFKILL_OP_CHANGE, + RFKILL_OP_CHANGE_ALL, +}; + +/** + * struct rfkill_event - events for userspace on /dev/rfkill + * @idx: index of dev rfkill + * @type: type of the rfkill struct + * @op: operation code + * @hard: hard state (0/1) + * @soft: soft state (0/1) + * + * Structure used for userspace communication on /dev/rfkill, + * used for events from the kernel and control to the kernel. + */ +struct rfkill_event { + __u32 idx; + __u8 type; + __u8 op; + __u8 soft, hard; +} __attribute__((packed)); + +/* + * We are planning to be backward and forward compatible with changes + * to the event struct, by adding new, optional, members at the end. + * When reading an event (whether the kernel from userspace or vice + * versa) we need to accept anything that's at least as large as the + * version 1 event size, but might be able to accept other sizes in + * the future. + * + * One exception is the kernel -- we already have two event sizes in + * that we've made the 'hard' member optional since our only option + * is to ignore it anyway. + */ +#define RFKILL_EVENT_SIZE_V1 8 + +/* ioctl for turning off rfkill-input (if present) */ +#define RFKILL_IOC_MAGIC 'R' +#define RFKILL_IOC_NOINPUT 1 +#define RFKILL_IOCTL_NOINPUT _IO(RFKILL_IOC_MAGIC, RFKILL_IOC_NOINPUT) + +/* and that's all userspace gets */ + +#endif /* _UAPI__RFKILL_H */ -- cgit v1.2.3