From 9267d8f45f046da5c51947cb051862fc963370c9 Mon Sep 17 00:00:00 2001 From: José Bollo Date: Thu, 7 Jul 2016 18:57:37 +0200 Subject: initial commit MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Change-Id: I11c093ad112268a35317f44a5c2500c3978f38dd Signed-off-by: José Bollo --- .gitignore | 1 + CMakeLists.txt | 34 ++++ README.md | 6 + src/CMakeLists.txt | 66 ++++++++ src/af-gps-binding.c | 467 +++++++++++++++++++++++++++++++++++++++++++++++++++ src/export.map | 1 + 6 files changed, 575 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 src/CMakeLists.txt create mode 100644 src/af-gps-binding.c create mode 100644 src/export.map diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..567609b --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..016b497 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,34 @@ +########################################################################### +# Copyright 2016 IoT.bzh +# +# author: José Bollo +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +########################################################################### + +project(af-gps-binding C) + +cmake_minimum_required(VERSION 3.4) + +include(GNUInstallDirs) + +set(PROJECT_NAME "af-gps-binding") +set(PROJECT_PRETTY_NAME "af binding for GPS") +set(PROJECT_DESCRIPTION "Binding for handling NMEA signalisation of GPS") +set(PROJECT_VERSION "1.0") + +set(CMAKE_BUILD_TYPE Debug) + +add_subdirectory(src) + + diff --git a/README.md b/README.md new file mode 100644 index 0000000..85e7529 --- /dev/null +++ b/README.md @@ -0,0 +1,6 @@ +mkdir build +cd build +cmake -DCMAKE_INSTALL_PREFIX=.... .. +make +make install + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..38e82ed --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1,66 @@ +########################################################################### +# Copyright 2016 IoT.bzh +# +# author: José Bollo +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +########################################################################### + +cmake_minimum_required(VERSION 3.4) + +########################################################################### + +link_libraries(-Wl,--as-needed -Wl,--gc-sections) + +add_compile_options(-Wall -Wextra -Wconversion) +add_compile_options(-Wno-unused-parameter) # frankly not using a parameter does it care? +add_compile_options(-Werror=maybe-uninitialized) +add_compile_options(-Werror=implicit-function-declaration) +add_compile_options(-ffunction-sections -fdata-sections) +add_compile_options(-Wl,--as-needed -Wl,--gc-sections) +add_compile_options(-fPIC) + +set(CMAKE_C_FLAGS_PROFILING "-g -O0 -pg -Wp,-U_FORTIFY_SOURCE") +set(CMAKE_C_FLAGS_DEBUG "-g -O0 -ggdb -Wp,-U_FORTIFY_SOURCE") +set(CMAKE_C_FLAGS_RELEASE "-g -O2") +set(CMAKE_C_FLAGS_CCOV "-g -O2 --coverage") + +########################################################################### + +include(FindPkgConfig) + +pkg_check_modules(EXTRAS REQUIRED json-c libsystemd afb-daemon) +add_compile_options(${EXTRAS_CFLAGS}) +include_directories(${EXTRAS_INCLUDE_DIRS}) +link_libraries(${EXTRAS_LIBRARIES}) + +########################################################################### +# the binding for afb + +message(STATUS "Creation af-gps-binding for AFB-DAEMON") + +############################################################### +pkg_get_variable(afb_binding_install_dir afb-daemon binding_install_dir) +#execute_process( +# COMMAND pkg-config --variable binding_install_dir afb-daemon +# OUTPUT_VARIABLE afb_binding_install_dir OUTPUT_STRIP_TRAILING_WHITESPACE +#) + +############################################################### +add_library(af-gps-binding MODULE af-gps-binding.c) +set_target_properties(af-gps-binding PROPERTIES + PREFIX "" + LINK_FLAGS "-Wl,--version-script=${CMAKE_CURRENT_SOURCE_DIR}/export.map" +) +install(TARGETS af-gps-binding LIBRARY DESTINATION ${afb_binding_install_dir}) + diff --git a/src/af-gps-binding.c b/src/af-gps-binding.c new file mode 100644 index 0000000..a5b0c94 --- /dev/null +++ b/src/af-gps-binding.c @@ -0,0 +1,467 @@ +/* + * Copyright (C) 2016 "IoT.bzh" + * Author José Bollo + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#define _GNU_SOURCE +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include + +#define NAUTICAL_MILE_IN_METER 1852 +#define KNOT_TO_METER_PER_SECOND 0.5144444444444444 + +/* + * references: + * + * https://www.w3.org/TR/geolocation-API/ + * http://www.gpsinformation.org/dale/nmea.htm + */ + +/* declare the connection routine */ +static int nmea_connect(); + +/* + * the interface to afb-daemon + */ +const struct afb_binding_interface *afbitf; + +struct gps { + uint32_t time; + double latitude; + double longitude; +} gps; + +static struct json_object *position() +{ + return NULL; +} + +static int nmea_time(const char *text, uint32_t *result) +{ + uint32_t x; + + if (text[0] < '0' || text[0] > '2' + || text[1] < '0' || text[1] > (text[0] == '2' ? '3' : '9') + || text[2] < '0' || text[2] > '5' + || text[3] < '0' || text[3] > '9' + || text[4] < '0' || text[4] > '5' + || text[5] < '0' || text[5] > '9' + || (text[6] != 0 && text[6] != '.')) + return 0; + + x = (uint32_t)(text[0] - '0'); + x = x * 10 + (uint32_t)(text[1]-'0'); + x = x * 6 + (uint32_t)(text[2]-'0'); + x = x * 10 + (uint32_t)(text[3]-'0'); + x = x * 6 + (uint32_t)(text[4]-'0'); + x = x * 10 + (uint32_t)(text[5]-'0'); + x = x * 1000; + if (text[6] == '.') { + if (text[7] != 0) { + if (text[7] < '0' || text[7] > '9') return 0; + x += (uint32_t)(text[7]-'0') * 100; + if (text[8] != 0) { + if (text[8] < '0' || text[8] > '9') return 0; + x += (uint32_t)(text[8]-'0') * 10; + if (text[9] != 0) { + if (text[9] < '0' || text[9] > '9') return 0; + x += (uint32_t)(text[9]-'0'); + if (text[10] != 0) { + if (text[10] < '0' || text[10] > '9') return 0; + x += text[10] > '5'; + } + } + } + } + } + + *result = x; + return 1; +} + +static int nmea_angle(const char *text, double *result) +{ + uint32_t x = 0; + int dotidx = (int)(strchrnul(text, '.') - text); + + switch(dotidx) { + case 5: + if (text[dotidx - 5] < '0' || text[dotidx - 5] > '9') + return 0; + x = x * 10 + (uint32_t)(text[dotidx - 5] - '0'); + case 4: + if (text[dotidx - 4] < '0' || text[dotidx - 4] > '9') + return 0; + x = x * 10 + (uint32_t)(text[dotidx - 4] - '0'); + case 3: + if (text[dotidx - 3] < '0' || text[dotidx - 3] > '9') + return 0; + x = x * 10 + (uint32_t)(text[dotidx - 3] - '0'); + case 2: + if (text[dotidx - 2] < '0' || text[dotidx - 2] > '9') + return 0; + x = x * 6 + (uint32_t)(text[dotidx - 3] - '0'); + case 1: + if (text[dotidx - 1] < '0' || text[dotidx - 1] > '9') + return 0; + x = x * 10 + (uint32_t)(text[dotidx - 3] - '0'); + case 0: + break; + default: + return 0; + } + + if (text[dotidx] == '.') + *result = atof(&text[dotidx]) + x; + else + *result = x; + + return 1; +} + +static int nmea_set( + const char *tim, + const char *lat, const char *latu, + const char *lon, const char *lonu, + const char *alt, const char *altu, + const char *spe, + const char *tra, + const char *dat +) +{ + uint32_t time; + double latitude, longitude, altitude, speed, track; + struct gps *gps; + + DEBUG(afbitf, "time=%s latitude=%s%s longitude=%s%s altitude=%s%s speed=%s track=%s date=%s", + tim, lat, latu, lon, lonu, alt, altu, spe, tra, dat); + + /* get the time in milliseconds */ + if (tim != NULL) { + if (!nmea_time(tim, &time)) + return 0; + } + + /* get the latitude */ + if (lat != NULL && latu != NULL) { + if ((latu[0] != 'N' && latu[0] != 'S') || latu[1] != 0) + return 0; + if (!nmea_angle(lat, &latitude)) + return 0; + if (latu[0] == 'S') + latitude = -latitude; + } + + /* get the longitude */ + if (lon != NULL && lonu != NULL) { + if ((lonu[0] != 'E' && lonu[0] != 'W') || lonu[1] != 0) + return 0; + if (!nmea_angle(lon, &longitude)) + return 0; + if (lonu[0] == 'W') + longitude = 360.0 - longitude; + } + + /* get the altitude */ + if (alt != NULL && altu != NULL) { + if (altu[0] != 'M' || altu[1] != 0) + return 0; + altitude = atof(alt); + } + + /* get the speed */ + if (spe != NULL) { + speed = atof(spe) * KNOT_TO_METER_PER_SECOND; + } + + /* get the track */ + if (tra != NULL) { + track = atof(tra); + } + + return 1; +} + +static int nmea_split(char *s, char *fields[], int count) +{ + int index = 0; + while (*s && index < count) { + fields[index++] = s; + while (*s && *s != ',') + s++; + if (*s == ',') + *s++ = 0; + } + return !*s && index == count; +} + +/* + * interprete one sentence GGA - Fix information + */ +static int nmea_gga(char *s) +{ + char *f[14]; + + return nmea_split(s, f, (int)(sizeof f / sizeof *f)) + && *f[5] != '0' + && nmea_set(f[0], f[1], f[2], f[3], f[4], f[6], f[7], NULL, NULL, NULL); +} + +/* + * interprete one sentence RMC - Recommended Minimum + */ +static int nmea_rmc(char *s) +{ + char *f[12]; + + return nmea_split(s, f, (int)(sizeof f / sizeof *f)) + && *f[1] == 'A' + && nmea_set(f[0], f[2], f[3], f[4], f[5], NULL, NULL, f[6], f[7], f[8]); +} + + +/* + * interprete one NMEA sentence + */ +static int nmea_sentence(char *s) +{ + if (!s[0] || !s[1]) + return 0; + + if (s[2] == 'G' && s[3] == 'G' && s[4] == 'A' && s[5] == ',') + return nmea_gga(&s[6]); + + if (s[2] == 'R' && s[3] == 'M' && s[4] == 'C' && s[5] == ',') + return nmea_rmc(&s[6]); + + return 0; +} + +/* + * reads the NMEA stream + */ +static int nmea_read(int fd) +{ + static char buffer[160]; + static int pos = 0; + static int overflow = 0; + + int rc; + + for(;;) { + rc = (int)read(fd, &buffer[pos], sizeof buffer - (size_t)pos); + if (rc < 0) { + /* its an error if not interrupted */ + if (errno != EINTR) + return rc; + } else if (rc == 0) { + /* nothing more to be read */ + return 0; + } else { + /* scan the buffer */ + while (pos != rc) { + if (buffer[pos] != '\n') { + pos++; + if (pos == rc) { + if (pos == (int)(sizeof buffer)) { + overflow = 1; + pos = 0; + rc = 0; + } + } + } else { + if (buffer[0] == '$' && pos > 0 && buffer[pos-1] == '\r' && !overflow) { + if (pos > 3 && buffer[pos-4] == '*') { + /* TODO: check the cheksum */ + buffer[pos-4] = 0; + } else { + buffer[pos-1] = 0; + } + nmea_sentence(&buffer[1]); + } + pos++; + rc -= pos; + if (rc > 0) + memmove(buffer, buffer+pos, (size_t)rc); + pos = 0; + overflow = 0; + } + } + } + } +} + +/* + * called on an event on the NMEA stream + */ +static int nmea_on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata) +{ + /* read available data */ + if ((revents & EPOLLIN) != 0) + nmea_read(fd); + + /* check if error or hangup */ + if ((revents & (EPOLLERR|EPOLLRDHUP|EPOLLHUP)) != 0) { + sd_event_source_unref(s); + close(fd); + nmea_connect(fd); + } + return 0; +} + +/* + * opens a socket to a host and a service (or port) + */ +static int open_socket_to(const char *host, const char *service) +{ + int rc, fd; + struct addrinfo hint, *rai, *iai; + char xhost[32]; + + /* get addr */ + memset(&hint, 0, sizeof hint); + hint.ai_family = AF_INET; + hint.ai_socktype = SOCK_STREAM; + rc = getaddrinfo(host, service, &hint, &rai); + if (rc != 0) + return -1; + + /* get the socket */ + iai = rai; + while (iai != NULL) { + struct sockaddr_in *a = (struct sockaddr_in*)(iai->ai_addr); + unsigned char *ipv4 = (unsigned char*)&(a->sin_addr.s_addr); + unsigned char *port = (unsigned char*)&(a->sin_port); + sprintf(xhost, "%d.%d.%d.%d:%d", + (int)ipv4[0], (int)ipv4[1], (int)ipv4[2], (int)ipv4[3], + (((int)port[0]) << 8)|(int)port[1]); + fd = socket(iai->ai_family, iai->ai_socktype, iai->ai_protocol); + if (fd >= 0) { + rc = connect(fd, iai->ai_addr, iai->ai_addrlen); + if (rc == 0) { + fcntl(fd, F_SETFL, O_NONBLOCK); + freeaddrinfo(rai); + return fd; + } + close(fd); + } + iai = iai->ai_next; + } + freeaddrinfo(rai); + return -1; +} + +/* + * connection to nmea stream + */ +static int nmea_connect() +{ + sd_event_source *source; + int rc, fd; + const char *host; + const char *service; + + /* TODO connect to somewhere else */ + host = "sinagot.net"; + service = "5001"; + fd = open_socket_to(host, service); + if (fd < 0) { + ERROR(afbitf, "can't connect to host %s, service %s", host, service); + return fd; + } + + /* adds to the event loop */ + rc = sd_event_add_io(afb_daemon_get_event_loop(afbitf->daemon), &source, fd, EPOLLIN, nmea_on_event, NULL); + if (rc < 0) { + close(fd); + ERROR(afbitf, "can't coonect host %s, service %s to the event loop", host, service); + } + return rc; +} + +/* + * Get the last known position + */ +static void get(struct afb_req req) +{ + afb_req_success(req, position(), NULL); +} + +/* + * subscribe to notification of position + */ +static void subscribe(struct afb_req req) +{ + afb_req_success(req, position(), NULL); +} + +/* + * unsubscribe a previous subscription + */ +static void unsubscribe(struct afb_req req) +{ + afb_req_success(req, position(), NULL); +} + +/* + * array of the verbs exported to afb-daemon + */ +static const struct afb_verb_desc_v1 binding_verbs[] = { + /* VERB'S NAME SESSION MANAGEMENT FUNCTION TO CALL SHORT DESCRIPTION */ + { .name= "get", .session= AFB_SESSION_NONE, .callback= get, .info= "get the last known data" }, + { .name= "subscribe", .session= AFB_SESSION_NONE, .callback= subscribe, .info= "subscribe to notification of position" }, + { .name= "unsubscribe", .session= AFB_SESSION_NONE, .callback= unsubscribe, .info= "unsubscribe a previous subscription" }, + { .name= NULL } /* marker for end of the array */ +}; + +/* + * description of the binding for afb-daemon + */ +static const struct afb_binding binding_description = +{ + /* description conforms to VERSION 1 */ + .type= AFB_BINDING_VERSION_1, + .v1= { /* fills the v1 field of the union when AFB_BINDING_VERSION_1 */ + .prefix= "gps", /* the API name (or binding name or prefix) */ + .info= "Access to the GPS data", /* short description of of the binding */ + .verbs = binding_verbs /* the array describing the verbs of the API */ + } +}; + +/* + * activation function for registering the binding called by afb-daemon + */ +const struct afb_binding *afbBindingV1Register(const struct afb_binding_interface *itf) +{ + afbitf = itf; /* records the interface for accessing afb-daemon */ + + nmea_connect(); + + return &binding_description; /* returns the description of the binding */ +} + + diff --git a/src/export.map b/src/export.map new file mode 100644 index 0000000..0ef1ac7 --- /dev/null +++ b/src/export.map @@ -0,0 +1 @@ +{ global: afbBindingV1Register; local: *; }; -- cgit 1.2.3-korg