diff options
author | zheng_wenlong <wenlong_zheng@nexty-ele.com> | 2019-02-06 17:12:28 +0900 |
---|---|---|
committer | zheng_wenlong <wenlong_zheng@nexty-ele.com> | 2019-02-06 17:13:49 +0900 |
commit | 1fed2c5a38fed8bc91bb6b8a08982303eb9c2e82 (patch) | |
tree | 502b5c63a0570d78863311fbd930f1c96b8a2990 /src | |
parent | a98fcb4b99e5a7b14b671e6014ea0b1cc1ea199d (diff) |
Change steering to systemd service for ces2019halibut_7.90.0halibut/7.90.07.90.0
Change steering to systemd service for ces2019
Change-Id: I319fb56a302f4f980b43a1b90dee1f36820355fb
Signed-off-by: zheng_wenlong <wenlong_zheng@nexty-ele.com>
Diffstat (limited to 'src')
-rw-r--r-- | src/CMakeLists.txt | 61 | ||||
-rw-r--r-- | src/af-steering-wheel-binding.c | 426 | ||||
-rw-r--r-- | src/bind_event.c | 114 | ||||
-rw-r--r-- | src/bind_event.h | 33 | ||||
-rw-r--r-- | src/can_encoder.c | 344 | ||||
-rw-r--r-- | src/can_encoder.h | 55 | ||||
-rw-r--r-- | src/config.xml | 21 | ||||
-rw-r--r-- | src/debug_msg.h | 77 | ||||
-rw-r--r-- | src/export.map | 1 | ||||
-rw-r--r-- | src/gear_shift_para.json | 32 | ||||
-rw-r--r-- | src/icon.png | bin | 10651 -> 0 bytes | |||
-rw-r--r-- | src/js_raw.c | 77 | ||||
-rw-r--r-- | src/js_raw.h | 3 | ||||
-rw-r--r-- | src/js_signal_event.c | 259 | ||||
-rw-r--r-- | src/js_signal_event.h | 9 | ||||
-rw-r--r-- | src/prop_info.c | 6 | ||||
-rw-r--r-- | src/prop_info.h | 6 | ||||
-rw-r--r-- | src/prop_search.c | 8 | ||||
-rw-r--r-- | src/prop_search.h | 2 | ||||
-rw-r--r-- | src/steering_wheel.json | 5 | ||||
-rw-r--r-- | src/steering_wheel_json.c | 62 | ||||
-rw-r--r-- | src/steering_wheel_json.h | 2 | ||||
-rw-r--r-- | src/steering_wheel_map.json | 36 | ||||
-rw-r--r-- | src/wheel-service.c | 420 | ||||
-rw-r--r-- | src/wheel-service.h (renamed from src/af-steering-wheel-binding.h) | 12 |
25 files changed, 1214 insertions, 857 deletions
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 73377ca..bd80cec 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,5 +1,5 @@ # -# Copyright (c) 2017 TOYOTA MOTOR CORPORATION +# Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -26,20 +26,19 @@ 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) +#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") -set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/package/root/lib) ########################################################################### include(FindPkgConfig) #*<NESSARY> pkg_check_modules(EXTRAS REQUIRED json-c libsystemd afb-daemon) -#pkg_check_modules(EXTRAS REQUIRED json-c) +pkg_check_modules(EXTRAS REQUIRED json-c libsystemd) add_compile_options(${EXTRAS_CFLAGS}) include_directories(${EXTRAS_INCLUDE_DIRS}) link_libraries(${EXTRAS_LIBRARIES}) @@ -54,54 +53,8 @@ include_directories(${GIO_UNIX_INCLUDE_DIRS}) link_libraries(${GIO_UNIX_LIBRARIES}) message(STATUS "${GIO_UNIX_INCLUDE_DIRS}") -#set(VAR "") -#get_target_property(VAR target INCLUDE_DIRECTORIES) -#message(STATUS "${VAR}") - -########################################################################### -# the binding for afb - -message(STATUS "Creation af-steering-wheel-binding for AFB-DAEMON") - -############################################################### -#pkg_get_variable(afb_binding_install_dir afb-daemon binding_install_dir) - -#*<NESSARY> -#* execute_process( -#* COMMAND pkg-config --variable binding_install_dir afb-daemon -#* OUTPUT_VARIABLE afb_binding_install_dir OUTPUT_STRIP_TRAILING_WHITESPACE -#* ) - ############################################################### -add_library(af-steering-wheel-binding MODULE af-steering-wheel-binding.c bind_event.c steering_wheel_json.c prop_info.c prop_search.c js_signal_event.c js_raw.c) -set_target_properties(af-steering-wheel-binding PROPERTIES - PREFIX "" - LINK_FLAGS "-Wl,--version-script=${CMAKE_CURRENT_SOURCE_DIR}/export.map" -) - -add_custom_command(TARGET af-steering-wheel-binding POST_BUILD - COMMAND cp -f ${CMAKE_CURRENT_SOURCE_DIR}/config.xml ${CMAKE_CURRENT_SOURCE_DIR}/steering_wheel.json ${CMAKE_CURRENT_SOURCE_DIR}/icon.png ${PROJECT_BINARY_DIR}/package/root - COMMAND cp -f ${CMAKE_CURRENT_SOURCE_DIR}/steering_wheel_map.json ${CMAKE_CURRENT_SOURCE_DIR}/gear_shift_para.json ${PROJECT_BINARY_DIR}/package/root) - -add_custom_target(package DEPENDS ${PROJECT_BINARY_DIR}/package/root - COMMAND wgtpkg-pack -f -o ${PROJECT_BINARY_DIR}/package/${PROJECT_NAME}.wgt ${PROJECT_BINARY_DIR}/package/root) - -#*<NESSARY> install(TARGETS af-canivi-binding LIBRARY DESTINATION ${afb_binding_install_dir}) -# <OR following packaging> -############################################################## -# WGT packaging - -#message(STATUS "Creation of af-canivi.wgt package for AppFW") -############################################################## -#configure_file(config.xml.in config.xml) -#add_custom_command( -# OUTPUT af-canivi.wgt -# COMMAND rm -rf package -# COMMAND mkdir -p package/libs package/htdocs -# COMMAND cp config.xml package/ -# COMMAND cp ${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_ICON} package/icon.png -# COMMAND cp af-canivi-binding.so package -# COMMAND wgtpkg-pack -f -o af-canivi.wgt package -#) -#add_custom_target(widget ALL DEPENDS af-canivi.wgt) - +SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin) +AUX_SOURCE_DIRECTORY(. SRC_LST) +ADD_EXECUTABLE( wheel-service ${SRC_LST}) +TARGET_LINK_LIBRARIES( wheel-service pthread) diff --git a/src/af-steering-wheel-binding.c b/src/af-steering-wheel-binding.c deleted file mode 100644 index 908bf71..0000000 --- a/src/af-steering-wheel-binding.c +++ /dev/null @@ -1,426 +0,0 @@ -/* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION - * - * 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 <stdio.h> -#include <string.h> -#include <unistd.h> -#include <stdlib.h> -#include <errno.h> -#include <netdb.h> -#include <fcntl.h> -#include <math.h> -#include <sys/time.h> -#include <sys/types.h> -#include <sys/socket.h> -#include <json-c/json.h> - -#include <systemd/sd-event.h> - -#include "af-steering-wheel-binding.h" -#include "bind_event.h" -#include "steering_wheel_json.h" -#include "prop_search.h" -#include "js_raw.h" - -#define ENABLE_EVENT_DROP - -struct wheel_conf -{ - char *devname; -}; - -int notify_property_changed(struct prop_info_t *property_info) -{ - struct bind_event_t *event; - - //NOTICEMSG("notify_property_changed %s", property_info->name); - event = get_event(property_info->name); - if (event == NULL) - { - DBGMSG("<IGNORE event>: event(\"%s\") is not subscribed", property_info->name); - return 1; - } - - if (afb_event_is_valid(event->event)) - { - int recved_client; - - recved_client = afb_event_push(event->event, property2json(property_info)); - if(recved_client < 1) - { - if(recved_client == 0) - { - DBGMSG("event(\"%s\") is no client.", property_info->name); -#ifdef ENABLE_EVENT_DROP - /* Drop event */ - remove_event(property_info->name); -#endif - } - else - { - ERRMSG("count of clients that received the event : %d",recved_client); - } - - return 1; - } /* else (1 <= recved_lient) - * NORMAL - */ - } - else - { - ERRMSG("event(\"%s\") is invalid. don't adb_event_push()", property_info->name); - return 1; - } - - return 0; -} - -static int connection(struct wheel_conf *conf) -{ - int js; - int ret; - sd_event_source *source; - - js = js_open(conf->devname); - if (js < 0) - { - js_close(js); - ERRMSG("can't connect to joy stick, the event loop failed"); - return 0; - } - - ret = sd_event_add_io( - afb_daemon_get_event_loop(), - &source, - js, - EPOLLIN, - on_event, NULL); - if (ret < 0) - { - js_close(js); - ERRMSG("can't add event, the event loop failed"); - return 0; - } - - NOTICEMSG("connect to JS(%s), event loop started",conf->devname); - - return 0; -} - -/***************************************************************************************/ -/** **/ -/** **/ -/** SECTION: BINDING VERBS IMPLEMENTATION **/ -/** **/ -/** **/ -/***************************************************************************************/ -/** - * get method. - */ -static void get(struct afb_req req) -{ - -} - -///** -// * set method. -// */ -//static void set(struct afb_req req) -//{ -// -//} - -/** - * list method. - */ -static struct json_object *tmplists; - -static void _listup_properties(struct prop_info_t *property_info) -{ - if (property_info->name == NULL) - { - return; - } - else - { - json_object_array_add(tmplists, json_object_new_string(property_info->name)); - } -} - -static void list(struct afb_req req) -{ - if (tmplists != NULL) - { - json_object_put(tmplists); - } - tmplists = json_object_new_array(); - if (tmplists == NULL) - { - ERRMSG("not enough memory"); - afb_req_fail(req,"not enough memory", NULL); - return; - } - (void)canid_walker(_listup_properties); - afb_req_success(req, tmplists, NULL); -} - -/** - * subscribe method. - */ -static int subscribe_all(struct afb_req req, struct json_object *apply); - -static int subscribe_signal(struct afb_req req, const char *name, struct json_object *apply) -{ - struct bind_event_t *event; - - if(strcmp(name, "*") == 0) - { - return subscribe_all(req, apply); - } - - event = get_event(name); - if (event == NULL) - { - event = register_event(name); - if (event == NULL) - { - ERRMSG("register event failed: %s", name); - return 1; - } - } - else - { - DBGMSG("subscribe event: \"%s\" alrady exist", name); - } - - if (afb_req_subscribe(req, event->event) != 0) - { - ERRMSG("afb_req_subscrive() is failed."); - return 1; - } - - json_object_object_add(apply, event->name, property2json(event->raw_info)); - - return 0; -} - -static int subscribe_all(struct afb_req req, struct json_object *apply) -{ - int i; - int nProp; - const char **list = getSupportPropertiesList(&nProp); - int err = 0; - for(i = 0; i < nProp; i++) - err += subscribe_signal(req, list[i], apply); - - return err; -} - -static void subscribe(struct afb_req req) -{ - struct json_object *apply; - int err = 0; - const char *event; - - event = afb_req_value(req, "event"); - if (event == NULL) - { - ERRMSG("Unknwon subscrive event args"); - afb_req_fail(req, "error", NULL); - return; - } - apply = json_object_new_object(); - err = subscribe_signal(req, event, apply); - - if (!err) - { - afb_req_success(req, apply, NULL); - } - else - { - json_object_put(apply); - afb_req_fail(req, "error", NULL); - } -} - -/** - * unsubscribe method. - */ - -static int unsubscribe_all(struct afb_req req); - -static int unsubscribe_signal(struct afb_req req, const char *name) -{ - struct bind_event_t *event; - if(strcmp(name, "*") == 0) - { - return unsubscribe_all(req); - } - - event = get_event(name); - if (event == NULL) - { - ERRMSG("not subscribed event \"%s\"", name); - return 0; /* Alrady unsubscribed */ - } - if (afb_req_unsubscribe(req, event->event) != 0) - { - ERRMSG("afb_req_subscrive() is failed."); - return 1; - } - - return 0; -} - -static int unsubscribe_all(struct afb_req req) -{ - int i; - int nProp; - const char **list = getSupportPropertiesList(&nProp); - int err = 0; - for(i = 0; i < nProp; i++) - { - err += unsubscribe_signal(req, list[i]); - } - - return err; -} - -static void unsubscribe(struct afb_req req) -{ - struct json_object *args, *val; - int n,i; - int err = 0; - - args = afb_req_json(req); - if ((args == NULL) || !json_object_object_get_ex(args, "event", &val)) - { - err = unsubscribe_all(req); - } - else if (json_object_get_type(val) != json_type_array) - { - err = unsubscribe_signal(req, json_object_get_string(val)); - } - else - { - struct json_object *ent; - n = json_object_array_length(val); - for (i = 0; i < n; i++) - { - ent = json_object_array_get_idx(val,i); - err += unsubscribe_signal(req, json_object_get_string(ent)); - } - } - - if (!err) - afb_req_success(req, NULL, NULL); - else - afb_req_fail(req, "error", NULL); -} - -/* - * parse configuration file (steering_wheel.json) - */ -static int init_conf(int fd_conf, struct wheel_conf *conf) -{ -#define CONF_FILE_MAX (1024) - char filebuf[CONF_FILE_MAX]; - json_object *jobj; - - memset(filebuf,0, sizeof(filebuf)); - FILE *fp = fdopen(fd_conf,"r"); - if (fp == NULL) - { - ERRMSG("canno read configuration file(steering_wheel.json)"); - return -1; - } - - fread(filebuf, 1, sizeof(filebuf), fp); - fclose(fp); - - jobj = json_tokener_parse(filebuf); - if (jobj == NULL) - { - ERRMSG("json: Invalid steering_wheel.json format"); - return -1; - } - json_object_object_foreach(jobj, key, val) - { - if (strcmp(key,"dev_name") == 0) - { - conf->devname = strdup(json_object_get_string(val)); - } - else if (strcmp(key,"wheel_map") == 0) - { - wheel_define_init(json_object_get_string(val)); - } - else if (strcmp(key,"gear_para") == 0) - { - wheel_gear_para_init(json_object_get_string(val)); - } - } - json_object_put(jobj); - - return 0; -} - -static int init() -{ - NOTICEMSG("init"); - - int fd_conf; - struct wheel_conf conf; - fd_conf = afb_daemon_rootdir_open_locale("steering_wheel.json", O_RDONLY, NULL); - if (fd_conf < 0) - { - ERRMSG("wheel configuration (steering_wheel.json) is not access"); - return -1; - } - if (init_conf(fd_conf, &conf)) - { - return -1; - } - return connection(&conf); -} - -/* - * array of the verbs exported to afb-daemon - */ -static const struct afb_verb_v2 binding_verbs[] = -{ - /* VERB'S NAME SESSION MANAGEMENT FUNCTION TO CALL Authorization */ - { .verb= "get", .session= AFB_SESSION_NONE, .callback= get, .info = "get", .auth = NULL }, -// { .name= "set", .session= AFB_SESSION_NONE, .callback= set, .auth = NULL }, - { .verb= "list", .session= AFB_SESSION_NONE, .callback= list, .info = "list", .auth = NULL }, - { .verb= "subscribe", .session= AFB_SESSION_NONE, .callback= subscribe, .info = "subscribe", .auth = NULL }, - { .verb= "unsubscribe", .session= AFB_SESSION_NONE, .callback= unsubscribe, .info = "unsubscribe", .auth = NULL }, - { .verb= NULL } /* marker for end of the array */ -}; - -/* - * description of the binding for afb-daemon - */ -const struct afb_binding_v2 afbBindingV2 = -{ - .api = "steering-wheel", - .specification = NULL, - .verbs = binding_verbs, /* the array describing the verbs of the API */ - .preinit = NULL, - .init = init, - .onevent = NULL, - .noconcurrency = 1 -}; diff --git a/src/bind_event.c b/src/bind_event.c deleted file mode 100644 index 01887cd..0000000 --- a/src/bind_event.c +++ /dev/null @@ -1,114 +0,0 @@ -/* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION - * - * 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 <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <search.h> - -#include "af-steering-wheel-binding.h" -#include "bind_event.h" -#include "prop_search.h" - -static void *bind_event_root = NULL; - -static int compare(const void *pa, const void *pb) -{ - struct bind_event_t *a = (struct bind_event_t *)pa; - struct bind_event_t *b = (struct bind_event_t *)pb; - return strcmp(a->name, b->name); -} - -struct bind_event_t *get_event(const char *name) -{ - struct bind_event_t key; - void *ent; - - /* - * search subscribed event. - */ - key.name = name; - ent = tfind(&key, &bind_event_root, compare); - if (ent != NULL) - return *(struct bind_event_t **)ent; /* found */ - return NULL; /* not found */ -} - -struct bind_event_t *register_event(const char *name) -{ - struct bind_event_t *evt; - struct prop_info_t *property_info; - void *ent; - - property_info = getProperty_dict(name); - if (property_info == NULL) - { - ERRMSG("NOT Supported property:\"%s\".", name); - return NULL; - } - evt = calloc(1, sizeof(*evt)); - if (evt == NULL) - { - ERRMSG("not enough memory"); - return NULL; - } - - evt->event = afb_daemon_make_event(property_info->name); - if (!afb_event_is_valid(evt->event)) - { - free(evt); - ERRMSG("afb_daemon_make_event failed"); - return NULL; - } - - evt->name = property_info->name; - evt->raw_info = property_info; - ent = tsearch(evt, &bind_event_root, compare); - if (ent == NULL) - { - ERRMSG("not enough memory"); - afb_event_drop(evt->event); - free(evt); - return NULL; - } -#ifdef DEBUG /* double check */ - if ((*(struct bind_event_t **)ent) != evt) - ERRMSG("event \"%s\" is duplicate?", property_info->name); -#endif - - return evt; -} - -int remove_event(const char *name) -{ - struct bind_event_t *dnode; - struct bind_event_t key; - void *p; - - key.name = name; - p = tfind(&key, &bind_event_root, compare); - if (p == NULL) - { - return 0; - } - dnode = *(struct bind_event_t **)p; - afb_event_drop(dnode->event); - tdelete(&key, &bind_event_root, compare); - free(dnode); - - return 0; -} diff --git a/src/bind_event.h b/src/bind_event.h deleted file mode 100644 index 5e44a5b..0000000 --- a/src/bind_event.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION - * - * 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. - */ - -#ifndef _BIND_EVENT_H_ -#define _BIND_EVENT_H_ - -#include "prop_info.h" - -struct bind_event_t { - const char *name; /* name of the event : IvI Property-Name */ - struct afb_event event; /* the event for the binder */ - struct prop_info_t *raw_info; -}; - -extern struct bind_event_t *get_event(const char *name); -extern struct bind_event_t *register_event(const char *name); -extern int remove_event(const char *name); - -#endif - diff --git a/src/can_encoder.c b/src/can_encoder.c new file mode 100644 index 0000000..672bfd3 --- /dev/null +++ b/src/can_encoder.c @@ -0,0 +1,344 @@ +/* + * Copyright (c) 2017-2018 TOYOTA MOTOR CORPORATION + * + * 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 <stdio.h> +#include <string.h> +#include <unistd.h> +#include <stdlib.h> +#include <errno.h> +#include <linux/can.h> +#include <linux/can/error.h> +#include <pthread.h> +#include <search.h> + +#include "can_encoder.h" +#include "debug_msg.h" +#include "wheel-service.h" + + +#define CANID_DELIM '#' +#define DATA_SEPERATOR '.' + + +static struct can_data_t *phead = NULL, *ptail = NULL; +static pthread_mutex_t lock; +static char buf[MAX_CANDATA_SIZE+1] = {0}; +static char str[MAX_LENGTH+1] = {0}; +static void *canmsg_root = NULL; + + +/* CAN DLC to real data length conversion helpers */ +static const unsigned char dlc2len[] = {0, 1, 2, 3, 4, 5, 6, 7, + 8, 12, 16, 20, 24, 32, 48, 64}; + +/* + * get data length from can_dlc with sanitized can_dlc + */ +unsigned char can_dlc2len(unsigned char can_dlc) +{ + return dlc2len[can_dlc & 0x0F]; +} + +static const unsigned char len2dlc[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, /* 0 - 8 */ + 9, 9, 9, 9, /* 9 - 12 */ + 10, 10, 10, 10, /* 13 - 16 */ + 11, 11, 11, 11, /* 17 - 20 */ + 12, 12, 12, 12, /* 21 - 24 */ + 13, 13, 13, 13, 13, 13, 13, 13, /* 25 - 32 */ + 14, 14, 14, 14, 14, 14, 14, 14, /* 33 - 40 */ + 14, 14, 14, 14, 14, 14, 14, 14, /* 41 - 48 */ + 15, 15, 15, 15, 15, 15, 15, 15, /* 49 - 56 */ + 15, 15, 15, 15, 15, 15, 15, 15}; /* 57 - 64 */ + +/* + * map the sanitized data length to an appropriate data length code + */ +unsigned char can_len2dlc(unsigned char len) +{ + if (len > 64) + return 0xF; + + return len2dlc[len]; +} + +/* + * asc to nibble + */ +unsigned char asc2nibble(char c) { + + if ((c >= '0') && (c <= '9')) + return c - '0'; + + if ((c >= 'A') && (c <= 'F')) + return c - 'A' + 10; + + if ((c >= 'a') && (c <= 'f')) + return c - 'a' + 10; + + return 16; /* error */ +} + +static int canmsg_compare(const void *pa, const void *pb) +{ + struct canmsg_info_t *a =(struct canmsg_info_t *)pa; + struct canmsg_info_t *b =(struct canmsg_info_t *)pb; + return strcmp(a->canid,b->canid); +} + +/* + * init + */ +void init_can_encoder(void) +{ + pthread_mutex_init(&lock, NULL); + phead = NULL; + ptail = NULL; +} + +/* + * push can msg to queue + */ +int push(char *dat) +{ + if (dat == NULL) + { + DBG_ERROR("push data is NULL"); + return -1; + } + + struct can_data_t *p = malloc(sizeof(struct can_data_t)); + strncpy(p->dat, dat, MAX_CANDATA_SIZE); + p->dat[MAX_CANDATA_SIZE] = '\0'; + p->next = NULL; + + pthread_mutex_lock(&lock); + if (ptail == NULL) + { + ptail = p; + phead = p; + } + else + { + ptail->next = p; + ptail = p; + } + pthread_mutex_unlock(&lock); + return 0; +} + +/* + * pop can msg from queue + */ +struct can_data_t* pop(void) +{ + struct can_data_t *p = NULL; + pthread_mutex_lock(&lock); + if (phead != NULL) + { + p = phead; + if (phead->next != NULL) + { + phead = p->next; + } + else + { + phead = NULL; + ptail = NULL; + } + } + pthread_mutex_unlock(&lock); + return p; +} + +/* + * clear transmission msg queue + */ +void clear(void) +{ + struct can_data_t *p = NULL; + pthread_mutex_lock(&lock); + while (phead != NULL) + { + p = phead; + phead = phead->next; + free(p); + } + ptail = NULL; + pthread_mutex_unlock(&lock); +} + +/* + * make "0" string + */ +static char *makeZeroString(uint8_t dlc) +{ + int len = dlc * 2; + + if (len > MAX_LENGTH) + { + DBG_ERROR("makeZeroString input dlc error; dlc=%d",dlc); + return NULL; + } + + for (int i = 0; i < len; i++) { + str[i] = '0'; + } + + str[len + 1] = '\0'; + + return str; +} + +static struct canmsg_info_t * getCanMsg_dict(const char * can_id) +{ + struct canmsg_info_t info; + strncpy(info.canid, can_id, sizeof(info.canid)); + info.value = 0; + + void *v; + v = tfind((void *)&info, &canmsg_root, canmsg_compare); + if (v == NULL) + { + /* new msg, add node */ + struct canmsg_info_t * p = (struct canmsg_info_t *)malloc(sizeof(struct canmsg_info_t)); + strncpy(p->canid, can_id, sizeof(p->canid)); + p->value = 0; + v = tsearch((void *)p, &canmsg_root, canmsg_compare); + if (v == NULL) + { + DBG_ERROR("add canmsg failed: not enogh memory?"); + } + } + + return (*(struct canmsg_info_t **)v); +} + +/* + * make can frame data + */ +char * makeCanData(struct prop_info_t *property_info) +{ + char tmp[MAX_LENGTH+1] = {0}; + u_int64_t val = 0, mask = 0; + struct canmsg_info_t * p = getCanMsg_dict(property_info->can_id); + if (p == NULL) + { + return NULL; + } + + memset(buf, 0, sizeof(buf)); + sprintf(buf, "%s\#%s", property_info->can_id, makeZeroString(property_info->dlc)); + mask = (1 << (property_info->bit_size)) - 1; + val = mask & property_info->curValue.uint32_val; + val = val << ((property_info->dlc * 8) - property_info->bit_size - property_info->bit_pos); + mask = mask << ((property_info->dlc * 8) - property_info->bit_size - property_info->bit_pos); + mask = ~mask; + p->value = p->value & mask; + p->value = p->value | val; + + sprintf(tmp, "%lx", p->value); + strncpy((buf + 4 + property_info->dlc * 2 -strlen(tmp)), tmp ,strlen(tmp)); + +// DBG_ERROR("makeCanData buf is [%s]", buf); + + return buf; +} + +/* + * convert to canframe format + */ +int parse_canframe(char *cs, struct canfd_frame *cf) { + /* documentation see lib.h */ + + int i, idx, dlen, len; + int maxdlen = CAN_MAX_DLEN; + int ret = CAN_MTU; + unsigned char tmp; + + len = strlen(cs); + + memset(cf, 0, sizeof(*cf)); /* init CAN FD frame, e.g. LEN = 0 */ + + if (len < 4) + return 0; + + if (cs[3] == CANID_DELIM) { /* 3 digits */ + + idx = 4; + for (i=0; i<3; i++){ + if ((tmp = asc2nibble(cs[i])) > 0x0F) + return 0; + cf->can_id |= (tmp << (2-i)*4); + } + + } else if (cs[8] == CANID_DELIM) { /* 8 digits */ + + idx = 9; + for (i=0; i<8; i++){ + if ((tmp = asc2nibble(cs[i])) > 0x0F) + return 0; + cf->can_id |= (tmp << (7-i)*4); + } + if (!(cf->can_id & CAN_ERR_FLAG)) /* 8 digits but no errorframe? */ + cf->can_id |= CAN_EFF_FLAG; /* then it is an extended frame */ + + } else + return 0; + + if((cs[idx] == 'R') || (cs[idx] == 'r')){ /* RTR frame */ + cf->can_id |= CAN_RTR_FLAG; + + /* check for optional DLC value for CAN 2.0B frames */ + if(cs[++idx] && (tmp = asc2nibble(cs[idx])) <= CAN_MAX_DLC) + cf->len = tmp; + + return ret; + } + + if (cs[idx] == CANID_DELIM) { /* CAN FD frame escape char '##' */ + + maxdlen = CANFD_MAX_DLEN; + ret = CANFD_MTU; + + /* CAN FD frame <canid>##<flags><data>* */ + if ((tmp = asc2nibble(cs[idx+1])) > 0x0F) + return 0; + + cf->flags = tmp; + idx += 2; + } + + for (i=0, dlen=0; i < maxdlen; i++){ + + if(cs[idx] == DATA_SEPERATOR) /* skip (optional) separator */ + idx++; + + if(idx >= len) /* end of string => end of data */ + break; + + if ((tmp = asc2nibble(cs[idx++])) > 0x0F) + return 0; + cf->data[i] = (tmp << 4); + if ((tmp = asc2nibble(cs[idx++])) > 0x0F) + return 0; + cf->data[i] |= tmp; + dlen++; + } + cf->len = dlen; + + return ret; +} diff --git a/src/can_encoder.h b/src/can_encoder.h new file mode 100644 index 0000000..82b5ea0 --- /dev/null +++ b/src/can_encoder.h @@ -0,0 +1,55 @@ +/* + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION + * + * 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. + */ + +#ifndef _CAN_ENCODER_H_ +#define _CAN_ENCODER_H_ + +#include <string.h> +#include <stdint.h> +#include <sys/types.h> + +#include <sys/socket.h> /* for sa_family_t */ +#include <linux/can.h> +#include <linux/can/error.h> + + +#include "prop_info.h" + +#define MAX_LENGTH 16 +#define MAX_CANDATA_SIZE 20 + +struct can_data_t +{ + char dat[MAX_CANDATA_SIZE+1]; + struct can_data_t * next; +}; + +struct canmsg_info_t { + char canid[4]; + u_int64_t value; +}; + + +extern void init_can_encoder(void); +extern int push(char *dat); +extern struct can_data_t* pop(void); +extern void clear(void); +extern char * makeCanData(struct prop_info_t *property_info); +extern unsigned char can_dlc2len(unsigned char can_dlc); +extern unsigned char can_len2dlc(unsigned char len); +extern int parse_canframe(char *cs, struct canfd_frame *cf); + +#endif /* _CAN_ENCODER_H_ */ diff --git a/src/config.xml b/src/config.xml deleted file mode 100644 index 58b1ffb..0000000 --- a/src/config.xml +++ /dev/null @@ -1,21 +0,0 @@ -<?xml version="1.0" encoding="UTF-8"?> -<widget xmlns="http://www.w3.org/ns/widgets" id="agl-service-steering-wheel" version="1.0"> - <name>steering-wheel-service</name> - <icon src="icon.png"/> - <content src="config.xml" type="application/vnd.agl.service"/> - <description>The name says it all!</description> - <author>TOYOTA</author> - <license>APL2.0</license> - <feature name="urn:AGL:widget:provided-api"> - <param name="steering-wheel" value="ws" /> - </feature> - <feature name="urn:AGL:widget:required-binding"> - <param name="lib/af-steering-wheel-binding.so" value="local" /> - </feature> - <feature name="urn:AGL:widget:required-permission"> - <param name="#target" value="main" /> - <param name="urn:AGL:permission::public:hidden" value="required" /> - </feature> -</widget> - - diff --git a/src/debug_msg.h b/src/debug_msg.h new file mode 100644 index 0000000..47e6ea2 --- /dev/null +++ b/src/debug_msg.h @@ -0,0 +1,77 @@ +/* + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION + * + * 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. + */ + +#ifndef __DBG_DEBUG_H__ +#define __DBG_DEBUG_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include <time.h> +#include <stdio.h> +#include <stdarg.h> +#include <string.h> +#include <stdlib.h> + +enum LOG_LEVEL{ + LOG_LEVEL_NONE = 0, + LOG_LEVEL_ERROR, + LOG_LEVEL_WARNING, + LOG_LEVEL_NOTICE, + LOG_LEVEL_INFO, + LOG_LEVEL_DEBUG, + LOG_LEVEL_MAX = LOG_LEVEL_DEBUG +}; + +#define __FILENAME__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__) + +#define DBG_ERROR(args,...) _DBG_LOG(LOG_LEVEL_ERROR, __FILENAME__, __FUNCTION__, __LINE__, args, ##__VA_ARGS__) +#define DBG_WARNING(args,...) _DBG_LOG(LOG_LEVEL_WARNING, __FILENAME__, __FUNCTION__,__LINE__, args,##__VA_ARGS__) +#define DBG_NOTICE(args,...) _DBG_LOG(LOG_LEVEL_NOTICE, __FILENAME__, __FUNCTION__,__LINE__, args,##__VA_ARGS__) +#define DBG_INFO(args,...) _DBG_LOG(LOG_LEVEL_INFO, __FILENAME__, __FUNCTION__,__LINE__, args,##__VA_ARGS__) +#define DBG_DEBUG(args,...) _DBG_LOG(LOG_LEVEL_DEBUG, __FILENAME__, __FUNCTION__,__LINE__, args,##__VA_ARGS__) + +static char ERROR_FLAG[6][20] = {"NONE", "ERROR", "WARNING", "NOTICE", "INFO", "DEBUG"}; + +static void _DBG_LOG(enum LOG_LEVEL level, const char* file, const char* func, const int line, const char* log, ...) +{ + const int log_level = (getenv("USE_WHEEL_DEBUG") == NULL)?LOG_LEVEL_ERROR:atoi(getenv("USE_WHEEL_DEBUG")); + if(log_level < level) + { + return; + } + + char *message; + struct timespec tp; + unsigned int time; + + clock_gettime(CLOCK_REALTIME, &tp); + time = (tp.tv_sec * 1000000L) + (tp.tv_nsec / 1000); + + va_list args; + va_start(args, log); + if (log == NULL || vasprintf(&message, log, args) < 0) + message = NULL; + fprintf(stderr, "[%10.3f] [%s %s] [%s, %s(), Line:%d] >>> %s \n", time / 1000.0, "wheel", ERROR_FLAG[level], file, func, line, message); + va_end(args); + free(message); +} + +#ifdef __cplusplus +} +#endif +#endif //__DBG_DEBUG_H__ diff --git a/src/export.map b/src/export.map deleted file mode 100644 index ee2f413..0000000 --- a/src/export.map +++ /dev/null @@ -1 +0,0 @@ -{ global: afbBindingV*; local: *; }; diff --git a/src/gear_shift_para.json b/src/gear_shift_para.json deleted file mode 100644 index 3b58da7..0000000 --- a/src/gear_shift_para.json +++ /dev/null @@ -1,32 +0,0 @@ -{ - "GEAR_PARA" : [ - { - "POS" : "First", - "VAL" : "4.12", - }, - { - "POS" : "Second", - "VAL" : "2.84", - }, - { - "POS" : "Third", - "VAL" : "2.28", - }, - { - "POS" : "Fourth", - "VAL" : "1.45", - }, - { - "POS" : "Fifth", - "VAL" : "1.0", - }, - { - "POS" : "Sixth", - "VAL" : "0.69", - }, - { - "POS" : "Reverse", - "VAL" : "3.21", - }, - ] -}
\ No newline at end of file diff --git a/src/icon.png b/src/icon.png Binary files differdeleted file mode 100644 index def888b..0000000 --- a/src/icon.png +++ /dev/null diff --git a/src/js_raw.c b/src/js_raw.c index 5541162..9cdc168 100644 --- a/src/js_raw.c +++ b/src/js_raw.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -26,7 +26,7 @@ #include <linux/input.h> #include <linux/joystick.h> -#include "af-steering-wheel-binding.h" +#include "wheel-service.h" #include "js_raw.h" #include "js_signal_event.h" @@ -34,6 +34,8 @@ static int *axis; static char *button; +static timer_t tid; +static struct sigaction oldact; int js_signal_read(int fd) { @@ -44,16 +46,22 @@ int js_signal_read(int fd) size = read(fd, &jsEvent, sizeof(struct js_event)); if(size != 0) { - switch (jsEvent.type & ~JS_EVENT_INIT) + switch (jsEvent.type) //& ~JS_EVENT_INIT don't deal with init state. { + case JS_EVENT_BUTTON | JS_EVENT_INIT: + DBG_NOTICE("This is JS_EVENT_BUTTON init state [%d] : %d.", jsEvent.number, button[jsEvent.number]); + break; + case JS_EVENT_AXIS | JS_EVENT_INIT: + DBG_NOTICE("This is JS_EVENT_AXIS init state [%d] : %d.", jsEvent.number, axis[jsEvent.number]); + break; case JS_EVENT_BUTTON: button[jsEvent.number] = (char)jsEvent.value; - //NOTICEMSG("JS_EVENT_BUTTON %d\n", button[jsEvent.number]); + DBG_NOTICE("JS_EVENT_BUTTON [%d] : %d.", jsEvent.number, button[jsEvent.number]); newButtonValue(jsEvent.number, jsEvent.value); break; case JS_EVENT_AXIS: axis[jsEvent.number] = jsEvent.value; - //NOTICEMSG("JS_EVENT_AXIS %d\n", axis[jsEvent.number]); + DBG_NOTICE("JS_EVENT_AXIS [%d] : %d.", jsEvent.number, axis[jsEvent.number]); newAxisValue(jsEvent.number, jsEvent.value); break; default: @@ -80,7 +88,7 @@ int on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata) rc = js_signal_read(fd); if(rc == -1) { - ERRMSG("JS Frame Read failed"); + DBG_ERROR("JS Frame Read failed"); return -1; } @@ -89,19 +97,20 @@ int on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata) { /* T.B.D * if error or hungup */ - ERRMSG("Error or Hunup: rvent=%08x", revents); + DBG_ERROR("Error or Hunup: rvent=%08x", revents); } return 0; } -int js_open(const char *devname) +int js_open(const char *devname) { unsigned char numAxes = 0; unsigned char numButtons = 0; int version = 0; int fd; char name[JSNAMELEN] = "Unknown"; + char steering_name[JSNAMELEN] = "Logitech G29 Driving Force Racing Wheel"; struct js_corr cal[6]; int i, j; @@ -125,6 +134,17 @@ int js_open(const char *devname) ioctl(fd, JSIOCGBUTTONS, &numButtons); ioctl(fd, JSIOCGNAME(JSNAMELEN), name); + if (strcmp(name, steering_name) == 0) + { + setJsType(JS_TYPE_STEERING); + DBG_NOTICE("JS_TYPE_STEERING type!"); + } + else + { + setJsType(JS_TYPE_GAME_CTL); + DBG_NOTICE("JS_TYPE_GAME_CTL type!"); + } + for (i = 0; i < 6; i++) { int k = 0; @@ -151,7 +171,7 @@ int js_open(const char *devname) gis = g_unix_input_stream_new(fd, TRUE); if(gis == NULL) { - ERRMSG("g_unix_input_stream_new() failed!"); + DBG_ERROR("g_unix_input_stream_new() failed!"); } else { @@ -172,3 +192,42 @@ void js_close(int js) close(js); } + +int init_timer() +{ + struct sigaction act; + struct itimerspec itval; + + memset(&act, 0, sizeof(struct sigaction)); + memset(&oldact, 0, sizeof(struct sigaction)); + + act.sa_handler = updateTimerHandler; + act.sa_flags = SA_RESTART; + if(sigaction(SIGALRM, &act, &oldact) < 0) { + DBG_ERROR("sigaction failed."); + return -1; + } + + itval.it_value.tv_sec = 0; + itval.it_value.tv_nsec = 200000000; + itval.it_interval.tv_sec = 0; + itval.it_interval.tv_nsec = 200000000; + + if(timer_create(CLOCK_REALTIME, NULL, &tid) < 0) { + DBG_ERROR("timer_create failed."); + return -1; + } + + if(timer_settime(tid, 0, &itval, NULL) < 0) { + DBG_ERROR("timer_settime failed."); + return -1; + } + + return 0; +} + +void deinit_timer() +{ + timer_delete(tid); + sigaction(SIGALRM, &oldact, NULL); +}
\ No newline at end of file diff --git a/src/js_raw.h b/src/js_raw.h index 79db219..833a5e1 100644 --- a/src/js_raw.h +++ b/src/js_raw.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -23,5 +23,6 @@ extern int on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata); extern int js_open(const char *devname); extern void js_close(int js); +extern int init_timer(); #endif diff --git a/src/js_signal_event.c b/src/js_signal_event.c index 712d51c..ad3035a 100644 --- a/src/js_signal_event.c +++ b/src/js_signal_event.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -22,20 +22,13 @@ #include <string.h> #include <sys/ioctl.h> #include <alloca.h> +#include <math.h> +#include <time.h> +#include <signal.h> -#include "af-steering-wheel-binding.h" +#include "wheel-service.h" #include "js_signal_event.h" -// Axis -#define JS_STEERING 0 -#define JS_THROTTLE 2 -#define JS_BRAKE 3 -// Button -#define JS_LEFT_PADDLE_SHIFTER 4 -#define JS_RIGHT_PADDLE_SHIFTER 5 -#define JS_TURN_SIGNAL_RIGHT 6 -#define JS_TURN_SIGNAL_LEFT 7 - // Property string #define VEHICLE_SPEED "VehicleSpeed" #define ENGINE_SPEED "EngineSpeed" @@ -45,56 +38,89 @@ #define STEERING_WHEEL_ANGLE "SteeringWheelAngle" #define TURN_SIGNAL_STATUS "TurnSignalStatus" #define LIGHT_STATUS_BRAKE "LightStatusBrake" +#define PARKING_STATUS_BRAKE "ParkingStatusBrake" +#define FUEL_LEVEL "FuelLevel" #define MAX_TRANSMISSION_GEAR_INFO 6 #define MIN_TRANSMISSION_GEAR_INFO 0 -enum eTransmissionGearInfo -{ +enum eJsButtonEvent { + JS_BUTTON_X = 0, + JS_BUTTON_R = 1, + JS_BUTTON_O = 2, + JS_BUTTON_T = 3, + JS_BUTTON_R1 = 4, + JS_BUTTON_L1 = 5, + JS_BUTTON_R2 = 6, + JS_BUTTON_L2 = 7, + JS_BUTTON_SHARE = 8, + JS_BUTTON_OPTION = 9, + JS_BUTTON_R3 = 10, + JS_BUTTON_L3 = 11, + JS_BUTTON_PLUS = 19, + JS_BUTTON_MINUS = 20, + JS_BUTTON_SCROLL_R = 21, + JS_BUTTON_SCROLL_L = 22, + JS_BUTTON_ENTER = 23, + JS_BUTTON_PS = 24 +}; + +enum eJsAxisEvent { + JS_AXIS_STEERING = 0, + JS_AXIS_CLUTCH = 1, + JS_AXIS_ACCEL = 2, + JS_AXIS_BRAKE = 3, +}; + +enum eTransmissionGearInfo { eTransmissionGearInfoLeft = 0, eTransmissionGearInfoRight = 1, }; -enum eTurnSignalStatus -{ +enum eTurnSignalStatus { eTurnSignalStatusOff = 0, - eTurnSignalStatusRight = 1, + eTurnSignalStatusRight = 4, eTurnSignalStatusLeft = 2, - eTurnSignalStatusHazard = 3 + eTurnSignalStatusHazard = 1 }; -double gearRatio[8] = -{ - 0.0, //Neutral - 1.0/4.12, //First - 1.0/2.84, //Second - 1.0/2.28, //Third - 1.0/1.45, //Fourth - 1.0/1.0, //Fifth - 1.0/0.69, //Sixth - 1.0/3.21 //Reverse +double gearRatio[8] = { + 0.0, //Neutral + 1.0/4.12, //First + 1.0/2.84, //Second + 1.0/2.28, //Third + 1.0/1.45, //Fourth + 1.0/1.0, //Fifth + 1.0/0.69, //Sixth + 1.0/3.21 //Reverse }; -static int gAcceleratorPedalPosition = 0; +static int gJsType=JS_TYPE_STEERING; +static int gCurAccPedalPos=0, gAccPedalPos=0; +static int gEngineSpeed=0; +static int gVehicleSpeed=0; +static float gBaseValue = 0.0; + static int gLightStatusBrake = 0; -static int gTransmissionGearInfo = 1; +static int gTransmissionGearInfo = 0; static int gSteeringWheelAngle = 0; +static int gParkingStatusBrake = 0; +static int gFuelLevel = 0; static enum eTurnSignalStatus gTurnSignalStatus = eTurnSignalStatusOff; -// Method to set value -static void setAcceleratorPedalPosition(int val) +static void setLightStatusBrake(int val) { - if(gAcceleratorPedalPosition != val) + if(gLightStatusBrake != val) { - gAcceleratorPedalPosition = val; + gLightStatusBrake = val; } } -static void setLightStatusBrake(int val) +static void setFuelLevel(int val) { - if(gLightStatusBrake != val) + if(gFuelLevel != val) { - gLightStatusBrake = val; + gFuelLevel = val; } } @@ -136,6 +162,10 @@ static void setTurnSignalStatus(int val, enum eTurnSignalStatus turn) { gTurnSignalStatus = eTurnSignalStatusLeft; } + else if(turn == eTurnSignalStatusHazard) + { + gTurnSignalStatus = eTurnSignalStatusHazard; + } } else { @@ -143,27 +173,12 @@ static void setTurnSignalStatus(int val, enum eTurnSignalStatus turn) } } -// Method to calculate property -static int calcAcceleratorPedalPosition() +static void setParkingStatusBrake(int val) { - return (int)(((double)(gAcceleratorPedalPosition - 32767)/(double)-65534.0)*(double)100.0); -} - -static int calcEngineSpeed() -{ - int acceleratorPedalPosition = calcAcceleratorPedalPosition(); - int engineSpeed = (int)acceleratorPedalPosition * 100; - - return engineSpeed; -} - -static int calcVehicleSpeed() -{ - int engineSpeed = calcEngineSpeed(); - double transmissionGearInfoRatio = gearRatio[(gTransmissionGearInfo == 128 ? 7 : gTransmissionGearInfo)]; - int vehicleSpeed = (int)(engineSpeed * transmissionGearInfoRatio / 100); - - return vehicleSpeed; + if(gParkingStatusBrake != val) + { + gParkingStatusBrake = val; + } } static int calcSteeringWheelAngle() @@ -174,7 +189,7 @@ static int calcSteeringWheelAngle() } // Method to update property -static void updateValue(char *prop, int val) +static void setCanValue(char *prop, int val) { unsigned int nProp = wheel_info->nData; for(unsigned int i = 0; i < nProp; i++) @@ -193,38 +208,36 @@ static void updateValue(char *prop, int val) void updateTransmissionGearInfo() { - updateValue(TRANSMISSION_GEAR_INFO, gTransmissionGearInfo); + setCanValue(TRANSMISSION_GEAR_INFO, 5); } void updateTransmissionMode() { - updateValue(TRANSMISSION_MODE, gTransmissionGearInfo); + setCanValue(TRANSMISSION_MODE, 0); } static void updateAcceleratorPedalPosition() { - int acceleratorPedalPosition; - - acceleratorPedalPosition = calcAcceleratorPedalPosition(); - updateValue(ACCELERATOR_PEDAL_POSITION, acceleratorPedalPosition); + setCanValue(ACCELERATOR_PEDAL_POSITION, gAccPedalPos); + DBG_NOTICE("acceleratorPedalPosition : %d\n", gAccPedalPos); } static void updateEngineSpeed() { // Update EngineSpeed - int engineSpeed; - - engineSpeed = calcEngineSpeed(); - updateValue(ENGINE_SPEED, engineSpeed); + gEngineSpeed = gBaseValue*3000+1000; + setCanValue(ENGINE_SPEED, gEngineSpeed); + DBG_DEBUG("engineSpeed : %d\n", gEngineSpeed); } static void updateVehicleSpeed() { // Update VehicleSpeed - int vehicleSpeed; - - vehicleSpeed = calcVehicleSpeed(); - updateValue(VEHICLE_SPEED, vehicleSpeed); + if(gBaseValue < 0) + gBaseValue = 0; + gVehicleSpeed = (int)(gBaseValue*9550/100)*100; + setCanValue(VEHICLE_SPEED, gVehicleSpeed); + DBG_DEBUG("vehicleSpeed : %d\n", gVehicleSpeed); } static void updateLightStatusBrake() @@ -232,18 +245,30 @@ static void updateLightStatusBrake() int lightStatusBrake; lightStatusBrake = (gLightStatusBrake < 20000); - updateValue(LIGHT_STATUS_BRAKE, lightStatusBrake); + setCanValue(LIGHT_STATUS_BRAKE, lightStatusBrake); +} + +static void updateFuelLevel() +{ + int fuelLevel = (32511 - gFuelLevel)*256.0/65278.0; + DBG_DEBUG("fuelLevel: %d, gFuelLevel: %d \n", fuelLevel, gFuelLevel); + setCanValue(FUEL_LEVEL, fuelLevel); +} + +static void updateParkingStatusBrake() +{ + setCanValue(PARKING_STATUS_BRAKE, gParkingStatusBrake); } static void updateSteeringWheelAngle() { int steering = calcSteeringWheelAngle(); - updateValue(STEERING_WHEEL_ANGLE, steering); + setCanValue(STEERING_WHEEL_ANGLE, steering); } static void updateTurnSignalStatus() { - updateValue(TURN_SIGNAL_STATUS, gTurnSignalStatus); + setCanValue(TURN_SIGNAL_STATUS, gTurnSignalStatus); } // Method to handle joy stick event @@ -251,7 +276,7 @@ void newButtonValue(char number, int val) { switch (number) { - case JS_LEFT_PADDLE_SHIFTER: //Left paddle shifter + case JS_BUTTON_R1: //Left paddle shifter // Set gear position setTransmissionGearInfo(val, eTransmissionGearInfoLeft); @@ -261,7 +286,7 @@ void newButtonValue(char number, int val) updateVehicleSpeed(); break; - case JS_RIGHT_PADDLE_SHIFTER: //Right paddle shifter + case JS_BUTTON_L1: //Right paddle shifter // Set gear position setTransmissionGearInfo(val, eTransmissionGearInfoRight); @@ -271,7 +296,7 @@ void newButtonValue(char number, int val) updateVehicleSpeed(); break; - case JS_TURN_SIGNAL_RIGHT: //Right upper wheel button + case JS_BUTTON_R2: //Right upper wheel button // Set value setTurnSignalStatus(val, eTurnSignalStatusRight); @@ -279,14 +304,27 @@ void newButtonValue(char number, int val) updateTurnSignalStatus(); break; - case JS_TURN_SIGNAL_LEFT: //Left upper wheel button + case JS_BUTTON_L2: //Left upper wheel button // Set value setTurnSignalStatus(val, eTurnSignalStatusLeft); // Update property updateTurnSignalStatus(); break; + case JS_BUTTON_R3: + // Set value + setTurnSignalStatus(val, eTurnSignalStatusHazard); + + // Update property + updateTurnSignalStatus(); + break; + case JS_BUTTON_L3: + // Set value + setParkingStatusBrake(val); + // Update property + updateParkingStatusBrake(); + break; default: break; } @@ -294,9 +332,24 @@ void newButtonValue(char number, int val) void newAxisValue(char number, int val) { +// DBG_ERROR("#################number: %d, %d. \n", number, val); +// DBG_ERROR("#################gJsType: %d. \n", gJsType); + if ((JS_TYPE_GAME_CTL == gJsType) && (number == JS_AXIS_BRAKE)) + { + number = JS_AXIS_ACCEL; + } + else if ((JS_TYPE_GAME_CTL == gJsType) && (number == JS_AXIS_ACCEL)) + { + number = JS_AXIS_BRAKE; + } + else + { + //do nothing + } + switch (number) { - case JS_STEERING: //Wheel angle, -32767 - 32767 + case JS_AXIS_STEERING: //Wheel angle, -32767 - 32767 // Set value setSteeringWheelAngle(val); @@ -304,17 +357,29 @@ void newAxisValue(char number, int val) updateSteeringWheelAngle(); break; - case JS_THROTTLE: //Throttle, -32767 (depressed) - 32767 (undepressed) + case JS_AXIS_ACCEL: //Throttle, -32767 (depressed) - 32767 (undepressed) // Set origin value - setAcceleratorPedalPosition(val); + if (JS_TYPE_GAME_CTL == gJsType) + { + gAccPedalPos = (int)(((double)val/(double)-32767.0)*(double)100.0); + } + else + { + gAccPedalPos = (int)(((double)(val - 32767)/(double)-65534.0)*(double)100.0); + } // Update property updateAcceleratorPedalPosition(); - updateEngineSpeed(); - updateVehicleSpeed(); + if (gAccPedalPos > gCurAccPedalPos){ + gCurAccPedalPos = gAccPedalPos; + gBaseValue = sin((float)gCurAccPedalPos/100.0); + + updateEngineSpeed(); + updateVehicleSpeed(); + } break; - case JS_BRAKE: + case JS_AXIS_BRAKE: // Set value setLightStatusBrake(val); @@ -322,7 +387,31 @@ void newAxisValue(char number, int val) updateLightStatusBrake(); break; //Brake, -32767 (depressed) - 32767 (undepressed) + case JS_AXIS_CLUTCH: + setFuelLevel(val); + updateFuelLevel(); + break; default: + DBG_DEBUG("number: %d, %d. \n", number, val); break; } } + +void updateTimerHandler(int signum) +{ + if (gCurAccPedalPos == gAccPedalPos){ + gCurAccPedalPos = gCurAccPedalPos + rand()%2; + }else if (gAccPedalPos < (gCurAccPedalPos - 5)){ + gCurAccPedalPos = gCurAccPedalPos - 5; + }else{ + gCurAccPedalPos = gAccPedalPos; + } + gBaseValue = sin((float)gCurAccPedalPos/100.0); + updateEngineSpeed(); + updateVehicleSpeed(); +} + +void setJsType(int jstype) +{ + gJsType = jstype; +}
\ No newline at end of file diff --git a/src/js_signal_event.h b/src/js_signal_event.h index 3cff704..3dca3a9 100644 --- a/src/js_signal_event.h +++ b/src/js_signal_event.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -21,5 +21,12 @@ extern struct wheel_info_t *wheel_info; extern void newButtonValue(char number, int val); extern void newAxisValue(char number, int val); +extern void updateTimerHandler(int signum); +extern void setJsType(int jstype); + +enum eJsType { + JS_TYPE_STEERING = 0, + JS_TYPE_GAME_CTL = 1, +}; #endif diff --git a/src/prop_info.c b/src/prop_info.c index 95d4f58..e8ab461 100644 --- a/src/prop_info.c +++ b/src/prop_info.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -20,7 +20,7 @@ #include <stdint.h> #include "prop_info.h" -#include "af-steering-wheel-binding.h" +#include "wheel-service.h" #define ENABLE1_TYPENAME "ENABLE-1" /* spec. type1.json original type name */ @@ -96,7 +96,7 @@ int propertyValue_int(struct prop_info_t *property_info) case ARRAY_T: case UINT64_T: case INT64_T: - ERRMSG("Getting property Value:NOT SUPPORT vartype contents:%s %d", property_info->name, property_info->var_type); + DBG_ERROR("Getting property Value:NOT SUPPORT vartype contents:%s %d", property_info->name, property_info->var_type); break; } return x; diff --git a/src/prop_info.h b/src/prop_info.h index 3d6bb5c..dac63eb 100644 --- a/src/prop_info.h +++ b/src/prop_info.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -52,6 +52,10 @@ union data_content_t { struct prop_info_t { const char * name; unsigned char var_type; + const char * can_id; + uint8_t bit_pos; + uint8_t bit_size; + uint8_t dlc; union data_content_t curValue; struct prop_info_t *next; diff --git a/src/prop_search.c b/src/prop_search.c index f88de8a..fce139e 100644 --- a/src/prop_search.c +++ b/src/prop_search.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -21,7 +21,7 @@ #include <search.h> #include <string.h> -#include "af-steering-wheel-binding.h" +#include "wheel-service.h" #include "prop_search.h" #define SUPPORT_PROPERTY_LIMIT 512 @@ -47,12 +47,12 @@ static int addProperty_dict(struct prop_info_t *prop_info) v = tsearch((void *)prop_info, &property_root, property_compare); if (v == NULL) { - ERRMSG("add property failed: not enogh memory?"); + DBG_ERROR("add property failed: not enogh memory?"); return -1; } if (nProperties >= SUPPORT_PROPERTY_LIMIT) { - ERRMSG("Reach properties limit"); + DBG_ERROR("Reach properties limit"); return -1; } support_property_list[nProperties] = prop_info->name; diff --git a/src/prop_search.h b/src/prop_search.h index 85791e5..85c505c 100644 --- a/src/prop_search.h +++ b/src/prop_search.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/src/steering_wheel.json b/src/steering_wheel.json deleted file mode 100644 index 1a5e889..0000000 --- a/src/steering_wheel.json +++ /dev/null @@ -1,5 +0,0 @@ -{ - "dev_name": "/dev/input/js0", - "wheel_map": "steering_wheel_map.json", - "gear_para": "gear_shift_para.json" -} diff --git a/src/steering_wheel_json.c b/src/steering_wheel_json.c index 9d72284..5831983 100644 --- a/src/steering_wheel_json.c +++ b/src/steering_wheel_json.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -20,7 +20,7 @@ #include <stdlib.h> #include <string.h> #include <sys/stat.h> -#include "af-steering-wheel-binding.h" +#include "wheel-service.h" #include "steering_wheel_json.h" #include "prop_info.h" #include "prop_search.h" @@ -33,6 +33,7 @@ static int parse_property(struct wheel_info_t *wheel_info, int idx, json_object { int var_type = 0; char *name = NULL; + char *canid = NULL; if(obj_property) { @@ -47,10 +48,30 @@ static int parse_property(struct wheel_info_t *wheel_info, int idx, json_object const char *_varname = json_object_get_string(val); var_type = string2vartype(_varname); } + else if(strcmp("CANID", key) == 0) + { + canid = (char *)json_object_get_string(val); + } + else if(strcmp("BIT_POSITION", key) == 0) + { + const char * tmp = json_object_get_string(val); + wheel_info->property[idx].bit_pos = (uint8_t)strtoul(tmp, 0, 0); + } + else if(strcmp("BIT_SIZE", key) == 0) + { + const char * tmp = json_object_get_string(val); + wheel_info->property[idx].bit_size = (uint8_t)strtoul(tmp, 0, 0); + } + else if(strcmp("DLC", key) == 0) + { + const char * tmp = json_object_get_string(val); + wheel_info->property[idx].dlc = (uint8_t)strtoul(tmp, 0, 0); + } } wheel_info->property[idx].name = strdup(name); wheel_info->property[idx].var_type = (unsigned char)var_type; + wheel_info->property[idx].can_id = strdup(canid); } return 0; @@ -68,7 +89,7 @@ static int parse_propertys(json_object *obj_propertys) wheel_info = malloc(ms); if (wheel_info == NULL) { - ERRMSG("Not enogh memory"); + DBG_ERROR("Not enogh memory"); return 1; } memset(wheel_info, 0 , ms); @@ -82,7 +103,7 @@ static int parse_propertys(json_object *obj_propertys) wheel_info = realloc(wheel_info, sizeof(struct wheel_info_t) + ((size_t)array_len * sizeof(struct prop_info_t))); if (wheel_info == NULL) { - ERRMSG("not enogh memory"); + DBG_ERROR("not enogh memory"); exit(1); } @@ -115,7 +136,7 @@ static int parse_json(json_object *obj) else { ++err; - ERRMSG("json: Unknown key \"%s\"", key); + DBG_ERROR("json: Unknown key \"%s\"", key); } } return err; @@ -128,23 +149,23 @@ int wheel_define_init(const char *fname) struct stat stbuf; char *filebuf; - fd_wheel_map = afb_daemon_rootdir_open_locale(fname, O_RDONLY, NULL); + fd_wheel_map = open(fname, O_RDONLY); if (fd_wheel_map < 0) { - ERRMSG("wheel map is not access"); + DBG_ERROR("wheel map is not access"); return -1; } FILE *fp = fdopen(fd_wheel_map,"r"); if (fp == NULL) { - ERRMSG("canno read wheel map file"); + DBG_ERROR("cannot read wheel map file"); return -1; } if (fstat(fd_wheel_map, &stbuf) == -1) { - ERRMSG("cant get file state"); + DBG_ERROR("cant get file state"); return -1; } @@ -155,7 +176,7 @@ int wheel_define_init(const char *fname) jobj = json_tokener_parse(filebuf); if (jobj == NULL) { - ERRMSG("cannot data from file \"%s\"",fname); + DBG_ERROR("cannot read data from file \"%s\"",fname); free(filebuf); return 1; } @@ -183,7 +204,7 @@ static int parse_gear_para_json(json_object *obj) if (type == json_type_array) { int array_len = json_object_array_length(val); - NOTICEMSG("array_len:%d!", array_len); + DBG_NOTICE("array_len:%d!", array_len); for (int i = 0; i < array_len; i++) { obj_speed_para = json_object_array_get_idx(val, i); @@ -236,12 +257,12 @@ static int parse_gear_para_json(json_object *obj) } else { - ERRMSG("json: Need array \"%s\"", key); + DBG_ERROR("json: Need array \"%s\"", key); } } else { - ERRMSG("json: Unknown key \"%s\"", key); + DBG_ERROR("json: Unknown key \"%s\"", key); } } return err; @@ -254,23 +275,24 @@ int wheel_gear_para_init(const char *fname) struct stat stbuf; char *filebuf; - fd_gear_para = afb_daemon_rootdir_open_locale(fname, O_RDONLY, NULL); +// fd_gear_para = afb_daemon_rootdir_open_locale(fname, O_RDONLY, NULL); + fd_gear_para = open(fname, O_RDONLY); if (fd_gear_para < 0) { - ERRMSG("gear para is not access"); + DBG_ERROR("gear para is not access"); return -1; } FILE *fp = fdopen(fd_gear_para,"r"); if (fp == NULL) { - ERRMSG("canno read gear para file"); + DBG_ERROR("cannot read gear para file"); return -1; } if (fstat(fd_gear_para, &stbuf) == -1) { - ERRMSG("cant get file state.\n"); + DBG_ERROR("cant get file state.\n"); return -1; } @@ -281,7 +303,7 @@ int wheel_gear_para_init(const char *fname) jobj = json_tokener_parse(filebuf); if (jobj == NULL) { - ERRMSG("cannot data from file \"%s\"",fname); + DBG_ERROR("cannot data from file \"%s\"",fname); free(filebuf); return 1; } @@ -326,13 +348,13 @@ struct json_object *property2json(struct prop_info_t *property) break; default: - ERRMSG("Unknown value type:%d", property->var_type); + DBG_ERROR("Unknown value type:%d", property->var_type); break; } if (valueObject == NULL) { - ERRMSG("fail json ValueObject"); + DBG_ERROR("fail json ValueObject"); json_object_put(jobj); return NULL; } diff --git a/src/steering_wheel_json.h b/src/steering_wheel_json.h index 3c2c8c9..e9d2d05 100644 --- a/src/steering_wheel_json.h +++ b/src/steering_wheel_json.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/src/steering_wheel_map.json b/src/steering_wheel_map.json deleted file mode 100644 index 9e580d9..0000000 --- a/src/steering_wheel_map.json +++ /dev/null @@ -1,36 +0,0 @@ -{
- "PROPERTYS" : [
- {
- "PROPERTY" : "VehicleSpeed",
- "TYPE" : "uint16_t",
- },
- {
- "PROPERTY" : "EngineSpeed",
- "TYPE" : "uint16_t",
- },
- {
- "PROPERTY" : "AcceleratorPedalPosition",
- "TYPE" : "uint16_t",
- },
- {
- "PROPERTY" : "TransmissionGearInfo",
- "TYPE" : "uint16_t",
- },
- {
- "PROPERTY" : "TransmissionMode",
- "TYPE" : "uint16_t",
- },
- {
- "PROPERTY" : "SteeringWheelAngle",
- "TYPE" : "uint16_t",
- },
- {
- "PROPERTY" : "TurnSignalStatus",
- "TYPE" : "uint16_t",
- },
- {
- "PROPERTY" : "LightStatusBrake",
- "TYPE" : "bool",
- },
- ]
-}
diff --git a/src/wheel-service.c b/src/wheel-service.c new file mode 100644 index 0000000..38303e8 --- /dev/null +++ b/src/wheel-service.c @@ -0,0 +1,420 @@ +/* + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION + * + * 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 <stdio.h> +#include <string.h> +#include <unistd.h> +#include <stdlib.h> +#include <errno.h> +#include <netdb.h> +#include <fcntl.h> +#include <math.h> +#include <sys/time.h> +#include <sys/types.h> +#include <sys/socket.h> +#include <json-c/json.h> +#include <sys/stat.h> +#include <pthread.h> +#include <systemd/sd-event.h> +#include <net/if.h> +#include <sys/ioctl.h> +#include <sys/socket.h> +#include <linux/can.h> +#include <linux/can/raw.h> + + +#include "wheel-service.h" +#include "steering_wheel_json.h" +#include "prop_search.h" +#include "js_raw.h" +#include "can_encoder.h" + +#define ENABLE_EVENT_DROP +#define STEERING_WHEEL_JSON "/etc/steering_wheel.json" +#define BUS_MAP_CONF "/etc/dev-mapping.conf" + +struct wheel_conf +{ + char *devname; +}; + +struct transmission_bus_conf +{ + char *hs; + char *ls; +}; +static struct transmission_bus_conf trans_conf; + +/* + * notify function + */ +int notify_property_changed(struct prop_info_t *property_info) +{ + DBG_NOTICE("notify_property_changed name=%s,value=%d", property_info->name, property_info->curValue); + + int rc = push(makeCanData(property_info)); + if (rc < 0) + { + DBG_ERROR("push failed"); + return -1; + } + + return 0; +} + +/* + * transmission loop + */ +static void *transmission_event_loop(void *args) +{ + int s; /* can raw socket */ + int required_mtu; + int mtu; + int enable_canfd = 1; + struct sockaddr_can addr; + struct canfd_frame frame; + struct ifreq ifr; +// int retry = 0; + + /* open socket */ + if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + perror("open socket failed"); + return 0; + } + + addr.can_family = AF_CAN; + strcpy(ifr.ifr_name, trans_conf.hs); + /* wait until hs device start */ + while(1) { + if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) { + clear(); /* clear transmission msg queue */ + perror("SIOCGIFINDEX"); + sleep(2); + } + else + { + break; + } + } + + addr.can_ifindex = ifr.ifr_ifindex; + + /* disable default receive filter on this RAW socket */ + /* This is obsolete as we do not read from the socket at all, but for */ + /* this reason we can remove the receive list in the Kernel to save a */ + /* little (really a very little!) CPU usage. */ + setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0); + + if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind"); + return 0; + } + + while(1) + { + struct can_data_t* p = pop(); + if(p == NULL) + { + /* sleep 150ms */ + usleep(150000); + continue; + } + + /* parse CAN frame */ + required_mtu = parse_canframe(p->dat, &frame); + free(p); + if (!required_mtu){ + DBG_ERROR("\nWrong CAN-frame format! Try:\n\n"); + DBG_ERROR(" <can_id>#{R|data} for CAN 2.0 frames\n"); + DBG_ERROR(" <can_id>##<flags>{data} for CAN FD frames\n\n"); + DBG_ERROR("<can_id> can have 3 (SFF) or 8 (EFF) hex chars\n"); + DBG_ERROR("{data} has 0..8 (0..64 CAN FD) ASCII hex-values (optionally"); + DBG_ERROR(" separated by '.')\n"); + DBG_ERROR("<flags> a single ASCII Hex value (0 .. F) which defines"); + DBG_ERROR(" canfd_frame.flags\n\n"); + DBG_ERROR("e.g. 5A1#11.2233.44556677.88 / 123#DEADBEEF / 5AA# / "); + DBG_ERROR("123##1 / 213##311\n 1F334455#1122334455667788 / 123#R "); + DBG_ERROR("for remote transmission request.\n\n"); + continue; + } + + if (required_mtu > CAN_MTU) { + + /* check if the frame fits into the CAN netdevice */ + if (ioctl(s, SIOCGIFMTU, &ifr) < 0) { + perror("SIOCGIFMTU"); + continue; + } + mtu = ifr.ifr_mtu; + + if (mtu != CANFD_MTU) { + DBG_ERROR("CAN interface ist not CAN FD capable - sorry.\n"); + continue; + } + + /* interface is ok - try to switch the socket into CAN FD mode */ + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, + &enable_canfd, sizeof(enable_canfd))){ + DBG_ERROR("error when enabling CAN FD support\n"); + continue; + } + + /* ensure discrete CAN FD length values 0..8, 12, 16, 20, 24, 32, 64 */ + frame.len = can_dlc2len(can_len2dlc(frame.len)); + } + + /* send frame */ + if (write(s, &frame, required_mtu) != required_mtu) { + perror("write"); + } + } +} +/* + * transmission thread main function + */ +static pthread_t runTransmissionLoop(void) +{ + pthread_t thread_id = 0; + int ret = pthread_create(&thread_id, NULL, transmission_event_loop, NULL); + if(ret != 0) { + DBG_ERROR( "Cannot run eventloop due to error:%d", errno); + return -1; + } + + return thread_id; +} + +/* + * connect to js device + */ +static int connection(struct wheel_conf *conf) +{ + int js; + int ret; + int rc; + sd_event_source *source; + sd_event *loop = NULL; + + js = js_open(conf->devname); + if (js < 0) + { + js_close(js); + DBG_ERROR("can't connect to joy stick, the event loop failed"); + return 0; + } + + if(init_timer() < 0) + { + js_close(js); + DBG_ERROR("can't start update timer"); + return 0; + } + + /* get the default event loop */ + rc = sd_event_default(&loop); + if (rc < 0) { + DBG_ERROR("connection to default event loop failed: %s\n", strerror(-rc)); + loop = NULL; + return 0; + } + + ret = sd_event_add_io( + loop, + &source, + js, + EPOLLIN, + on_event, NULL); + if (ret < 0) + { + js_close(js); + DBG_ERROR("can't add event, the event loop failed"); + return 0; + } + + DBG_NOTICE("connect to JS(%s), event loop started",conf->devname); + + return 0; +} + +/* + * read /etc/dev-mapping.conf file + */ +static int readTransBus(void) +{ + char *line = NULL; + char bus_name[4], bus_val[10]; + size_t len = 0; + ssize_t read; + + FILE *fp = fopen(BUS_MAP_CONF,"r"); + if (fp == NULL) + { + DBG_ERROR("cannot read /etc/dev-mapping.conf"); + return -1; + } + + while(( read = getline(&line, &len, fp)) != -1) { + if(line == NULL || line[0] == '[') + continue; + + memset(bus_name, 0, sizeof(bus_name)); + memset(bus_val, 0, sizeof(bus_val)); + + sscanf(line, "%2s=\"%s", bus_name, bus_val); + bus_val[strlen(bus_val)-1] = '\0'; + if (strcmp(bus_name, "hs") == 0) + { + trans_conf.hs = strdup(bus_val); + } + else if (strcmp(bus_name, "ls") == 0) + { + trans_conf.ls = strdup(bus_val); + } + } + + if(line != NULL) + free(line); + + fclose(fp); +// DBG_ERROR("readTransBus end, hs:%s,ls:%s", trans_conf.hs, trans_conf.ls); + return 0; +} + +/* + * parse configuration file (steering_wheel.json) + */ +static int init_conf(int fd_conf, struct wheel_conf *conf) +{ + + char *filebuf = NULL; + json_object *jobj = NULL; + struct stat stbuf; + + FILE *fp = fdopen(fd_conf,"r"); + if (fp == NULL) + { + DBG_ERROR("cannot read configuration file(steering_wheel.json)"); + return -1; + } + + if (fstat(fd_conf, &stbuf) == -1) + { + DBG_ERROR("can't get file state"); + return -1; + } + +// fseek(fp, 0, SEEK_SET); + filebuf = (char *)malloc(stbuf.st_size); + fread(filebuf, 1, stbuf.st_size, fp); + fclose(fp); + + jobj = json_tokener_parse(filebuf); + if (jobj == NULL) +// if (is_error(jobj)) + { + DBG_ERROR("json: Invalid steering_wheel.json format"); + return -1; + } + json_object_object_foreach(jobj, key, val) + { + if (strcmp(key,"dev_name") == 0) + { + conf->devname = strdup(json_object_get_string(val)); + } + else if (strcmp(key,"wheel_map") == 0) + { + wheel_define_init(json_object_get_string(val)); + } + else if (strcmp(key,"gear_para") == 0) + { + wheel_gear_para_init(json_object_get_string(val)); + } + } + json_object_put(jobj); + free(filebuf); + + return 0; +} + +/* + * init js device + */ +static int init() +{ + DBG_NOTICE("init"); + + int fd_conf; + static struct wheel_conf conf; + + init_can_encoder(); + + if (readTransBus()) + { + DBG_ERROR("read file (/etc/dev-mapping.conf) failed"); + return -1; + } + + fd_conf = open(STEERING_WHEEL_JSON, O_RDONLY); + if (fd_conf < 0) + { + DBG_ERROR("wheel configuration (steering_wheel.json) is not access"); + return -1; + } + if (init_conf(fd_conf, &conf)) + { + DBG_ERROR("wheel json file (steering_wheel_map.json) is not access"); + return -1; + } + + return connection(&conf); +} + +/* + * entry function + */ +int main(void) +{ + int rc; + pthread_t thread_id; + sd_event *loop = NULL; + + /* device init */ + rc = init(); + if (rc < 0) { + DBG_ERROR("js device init failed: %s\n", strerror(-rc)); + return 1; + } + + /* start post thread */ + thread_id = runTransmissionLoop(); + if (thread_id < 0) { + DBG_ERROR("run post thread failed: %s\n", strerror(-thread_id)); + } + + /* get the default event loop */ + rc = sd_event_default(&loop); + if (rc < 0) { + DBG_ERROR("connection to default event loop failed: %s\n", strerror(-rc)); + return 1; + } + + /* loop until end */ + for(;;) + sd_event_run(loop, 30000000); + return 0; +} + diff --git a/src/af-steering-wheel-binding.h b/src/wheel-service.h index 1b7323c..f588599 100644 --- a/src/af-steering-wheel-binding.h +++ b/src/wheel-service.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2017 TOYOTA MOTOR CORPORATION + * Copyright (c) 2017-2019 TOYOTA MOTOR CORPORATION * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -19,16 +19,10 @@ #define AFB_BINDING_VERSION 2 #include <afb/afb-binding.h> - #include "prop_info.h" +#include "debug_msg.h" -#if !defined(NO_BINDING_VERBOSE_MACRO) -#define ERRMSG(msg, ...) AFB_ERROR(msg,##__VA_ARGS__) -#define WARNMSG(msg, ...) AFB_WARNING(msg,##__VA_ARGS__) -#define DBGMSG(msg, ...) AFB_DEBUG(msg,##__VA_ARGS__) -#define INFOMSG(msg, ...) AFB_INFO(msg,##__VA_ARGS__) -#define NOTICEMSG(msg, ...) AFB_NOTICE(msg,##__VA_ARGS__) -#endif +#define LOG_PREFIX "wheel-service" extern int notify_property_changed(struct prop_info_t *property_info); |