From: kj7.sung Date: Thu, 22 Jun 2017 23:34:25 +0000 (+0900) Subject: sync : lbs-server integration version, incomplete X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=refs%2Fheads%2Ffused;p=platform%2Fcore%2Flocation%2Flbs-server.git sync : lbs-server integration version, incomplete Change-Id: Ie9a1601724dd220b8b49855dab9cf8d71bc3a0e1 Signed-off-by: kj7.sung --- diff --git a/CMakeLists.txt b/CMakeLists.txt index 418b610..99360da 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,9 +7,9 @@ SET(PREFIX ${CMAKE_INSTALL_PREFIX}) SET(BIN_DIR "${PREFIX}/bin") #Dependencies -SET(common_dp "glib-2.0 lbs-dbus dlog gio-2.0") +SET(common_dp "glib-2.0 lbs-dbus dlog gio-2.0 lbs-location") SET(server_dp "${common_dp} network tapi vconf vconf-internal-keys gthread-2.0 gio-unix-2.0 capi-network-wifi") -SET(module_dp "${common_dp} gmodule-2.0 lbs-location") +SET(module_dp "${common_dp} gmodule-2.0") # Set required packages INCLUDE(FindPkgConfig) @@ -27,9 +27,9 @@ FOREACH(flag ${module_pkgs_CFLAGS}) ENDFOREACH(flag) SET(EXTRA_CFLAGS "${EXTRA_CFLAGS} -Wl,-zdefs -fvisibility=hidden ") - SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${EXTRA_CFLAGS}") SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Werror -Wextra -fvisibility=hidden -fPIC") + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-unused-parameter -Wno-missing-field-initializers -Wno-missing-declarations -Wall -Wcast-align -Wno-sign-compare") ADD_DEFINITIONS("-DPREFIX=\"${CMAKE_INSTALL_PREFIX}\"") diff --git a/lbs-server/CMakeLists.txt b/lbs-server/CMakeLists.txt index dd2ce64..7dd4a35 100644 --- a/lbs-server/CMakeLists.txt +++ b/lbs-server/CMakeLists.txt @@ -15,8 +15,11 @@ SET(SERVER_SRCS ${SERVER_SRCS_DIR}/setting.c ${SERVER_SRCS_DIR}/dump_log.c ${SERVER_SRCS_DIR}/nps_plugin_module.c + ${SERVER_SRCS_DIR}/location-fused.c ) +FILE(GLOB_RECURSE FUSED_SRCS ${SERVER_SRCS_DIR}/location-fused/*.c) + INCLUDE_DIRECTORIES( src include @@ -31,7 +34,7 @@ CONFIGURE_FILE(lbs-server-plugin.pc.in lbs-server-plugin.pc @ONLY) INSTALL(FILES lbs-server-plugin.pc DESTINATION ${LIB_DIR}/pkgconfig) INSTALL(FILES config/lbs-server.conf DESTINATION ${SYSCONF_DIR}/dbus-1/system.d) -ADD_EXECUTABLE(${PROJECT_NAME} ${SERVER_SRCS}) +ADD_EXECUTABLE(${PROJECT_NAME} ${SERVER_SRCS} ${FUSED_SRCS}) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${server_pkgs_LDFLAGS} -lm) SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS ${SERVER_EXTRA_CFLAGS}) SET(CMAKE_EXE_LINKER_FLAGS "-Wl,--as-needed -pie") diff --git a/lbs-server/src/data_connection.c b/lbs-server/src/data_connection.c index 7daf204..6440642 100644 --- a/lbs-server/src/data_connection.c +++ b/lbs-server/src/data_connection.c @@ -292,6 +292,7 @@ unsigned int query_dns(char *pdns_lookup_addr, unsigned int *ipaddr, int *port) pdns_lookup_addr = ptr; ptr = (char *)strtok_r(NULL, ":", &last); + if (ptr) *port = atoi(ptr); } diff --git a/lbs-server/src/debug_util.h b/lbs-server/src/debug_util.h index 1a75355..2924bba 100644 --- a/lbs-server/src/debug_util.h +++ b/lbs-server/src/debug_util.h @@ -1,7 +1,7 @@ /* * lbs-server * - * Copyright (c) 2011-2013 Samsung Electronics Co., Ltd. All rights reserved. + * Copyright (c) 2011-2017 Samsung Electronics Co., Ltd. All rights reserved. * * Contact: Youngae Kang , Minjune Kim * Genie Kim @@ -24,29 +24,48 @@ #include #include +#include "location-fused/parameters.h" #ifdef __cplusplus extern "C" { #endif #include -#define TAG_GPS_MANAGER "LOCATION_SERVER_GPS" -#define TAG_NPS_MANAGER "LOCATION_SERVER_WPS" -#define TAG_MOCK_MANAGER "LOCATION_SERVER_MOCK" +#define TAG_GPS_MANAGER "LBS_SERVER_GPS" +#define TAG_NPS_MANAGER "LBS_SERVER_NPS" +#define TAG_MOCK_MANAGER "LBS_SERVER_MOCK" +#define TAG_FUSED "LBS_SERVER_FUSED" #define DBG_LOW LOG_DEBUG #define DBG_INFO LOG_INFO #define DBG_WARN LOG_WARN #define DBG_ERR LOG_ERROR -#define LOG_GPS(dbg_lvl, fmt, args...) SLOG(dbg_lvl, TAG_GPS_MANAGER, fmt, ##args) -#define LOG_NPS(dbg_lvl, fmt, args...) SLOG(dbg_lvl, TAG_NPS_MANAGER, fmt, ##args) -#define LOG_MOCK(dbg_lvl, fmt, args...) SLOG(dbg_lvl, TAG_MOCK_MANAGER, fmt, ##args) -#define LOG_SEC(fmt, args...) SECURE_SLOG(LOG_DEBUG, TAG_MOCK_MANAGER, fmt, ##args) +/* Do not remove this macro: In case of loggers alternative to system dlog it + * is substituted with a non-trivial one in order to prefix the message with + * code location info. + */ +#define FUNCTION_PREFIX(format) format +#define LOG_INDENT_PREFIX "> " +#define LOG_UNINDENT_PREFIX "< " +#define ENTER_FUNCTION_PREFIX(format) LOG_INDENT_PREFIX FUNCTION_PREFIX(format) +#define LEAVE_FUNCTION_PREFIX(format) LOG_UNINDENT_PREFIX FUNCTION_PREFIX(format) +#define ENTER_FUNCTION_THIS_PREFIX(format) LOG_INDENT_PREFIX FUNCTION_THIS_PREFIX(format) +#define LEAVE_FUNCTION_THIS_PREFIX(format) LOG_UNINDENT_PREFIX FUNCTION_THIS_PREFIX(format) + +#define LOG_GPS(dbg_lvl, fmt, args...) SLOG(dbg_lvl, TAG_GPS_MANAGER, fmt, ##args) +#define LOG_NPS(dbg_lvl, fmt, args...) SLOG(dbg_lvl, TAG_NPS_MANAGER, fmt, ##args) +#define LOG_MOCK(dbg_lvl, fmt, args...) SLOG(dbg_lvl, TAG_MOCK_MANAGER, fmt, ##args) +#if(FL_ENABLE_DEVEL_LOG) +#define LOG_FUSED(dbg_lvl, fmt, args...) SLOG(dbg_lvl, TAG_FUSED, fmt, ##args) +#else +#define LOG_FUSED(dbg_lvl, fmt, args...) +#endif +#define LOG_SEC(fmt, args...) SECURE_SLOG(LOG_DEBUG, TAG_MOCK_MANAGER, fmt, ##args) -#define FUNC_ENTRANCE_SERVER LOG_GPS(DBG_LOW, "[%s] Entered!!", __func__); +#define FUNC_ENTRANCE_SERVER LOG_GPS(DBG_LOW, "[%s] Entered!!", __func__); #ifdef __cplusplus } #endif -#endif /* _DEBUG_UTIL_H_ */ +#endif /* _DEBUG_UTIL_H_ */ diff --git a/lbs-server/src/gps_plugin_module.c b/lbs-server/src/gps_plugin_module.c index 5339444..e3821a2 100644 --- a/lbs-server/src/gps_plugin_module.c +++ b/lbs-server/src/gps_plugin_module.c @@ -59,7 +59,7 @@ int load_plugin_module(char *specific_name, void **plugin_handle) *plugin_handle = dlopen(plugin_path, RTLD_NOW); if (!*plugin_handle) { LOG_GPS(DBG_ERR, "Failed to load plugin module: %s", plugin_path); - /* LOG_GPS(DBG_ERR, "%s", dlerror()); */ + LOG_GPS(DBG_ERR, "%s", dlerror()); return FALSE; } diff --git a/lbs-server/src/last_position.h b/lbs-server/src/last_position.h index 010f5f8..918f072 100644 --- a/lbs-server/src/last_position.h +++ b/lbs-server/src/last_position.h @@ -32,4 +32,4 @@ void gps_get_last_position(pos_data_t *last_pos); void gps_set_last_mock(int timestamp, double lat, double lon, double alt, double spd, double dir, double h_acc); -#endif /* _LAST_POSITON_H_ */ +#endif /* _LAST_POSITON_H_ */ diff --git a/lbs-server/src/lbs_server.c b/lbs-server/src/lbs_server.c old mode 100755 new mode 100644 index be1fa54..09e7351 --- a/lbs-server/src/lbs_server.c +++ b/lbs-server/src/lbs_server.c @@ -1,7 +1,7 @@ /* * lbs-server * - * Copyright (c) 2011-2013 Samsung Electronics Co., Ltd. All rights reserved. + * Copyright (c) 2011-2017 Samsung Electronics Co., Ltd. All rights reserved. * * Contact: Youngae Kang , Minjune Kim * Genie Kim @@ -41,8 +41,14 @@ #include "debug_util.h" #include "dump_log.h" +#include "location-types.h" +#include "location-fused.h" + #define MOCK_LOCATION_CLEAR_VALUE 999 #define MOCK_RUNNING_OFF LBS_SERVER_METHOD_SIZE +#define MAX_INTERVAL 120 +#define MAX_BATCH_INTERVAL 255 +#define MAX_BATCH_PERIOD 60000 typedef struct { /* gps variables */ @@ -54,7 +60,7 @@ typedef struct { gboolean nmea_used; gboolean is_gps_running; - gint gps_client_count; + guint gps_client_count; /* nps variables */ NpsManagerHandle handle; @@ -64,7 +70,7 @@ typedef struct { Plugin_LocationInfo location; gboolean is_nps_running; - gint nps_client_count; + guint nps_client_count; unsigned char *token; /* shared variables */ @@ -107,12 +113,23 @@ typedef enum { } lbs_batch_interval; #endif +typedef enum { + LBS_FUSED_STATUS_NONE = 0, + LBS_FUSED_STATUS_USED, +} lbs_fused_status; + +typedef enum { + _LBS_CLIENT_ADD = 0, + _LBS_CLIENT_REMOVE, + _LBS_CLIENT_REMOVE_ALL, +} lbs_client_count; + static gboolean gps_remove_all_clients(lbs_server_s *lbs_server); static NpsManagerPositionExt g_mock_position; static void set_mock_location_cb(gint method, gdouble latitude, gdouble longitude, gdouble altitude, gdouble speed, gdouble direction, gdouble accuracy, gpointer userdata); static void mock_start_tracking(lbs_server_s *lbs_server); static void mock_stop_tracking(lbs_server_s *lbs_server); - +static void __client_count_update(lbs_server_s *lbs_server, lbs_server_method_e method, lbs_client_count operation); static void __setting_gps_cb(keynode_t *key, gpointer user_data) { @@ -237,10 +254,12 @@ static void nps_update_position(lbs_server_s *lbs_server_nps, NpsManagerPosition LOG_NPS(DBG_LOW, "time:%d", lbs_server_nps->pos.timestamp); + location_fused_on_wps_position(pos.timestamp, pos.latitude, pos.longitude, pos.hor_accuracy, pos.ver_accuracy); + lbs_server_emit_position_changed(lbs_server_nps->lbs_dbus_server, LBS_SERVER_METHOD_NPS, - lbs_server_nps->pos.fields, lbs_server_nps->pos.timestamp, lbs_server_nps->pos.latitude, - lbs_server_nps->pos.longitude, lbs_server_nps->pos.altitude, lbs_server_nps->pos.speed, - lbs_server_nps->pos.direction, 0.0, accuracy); + lbs_server_nps->pos.fields, lbs_server_nps->pos.timestamp, lbs_server_nps->pos.latitude, + lbs_server_nps->pos.longitude, lbs_server_nps->pos.altitude, lbs_server_nps->pos.speed, + lbs_server_nps->pos.direction, 0.0, accuracy); g_variant_unref(accuracy); } @@ -427,9 +446,7 @@ static void __nps_cancel_callback(void *arg) static void start_batch_tracking(lbs_server_s *lbs_server, int batch_interval, int batch_period) { LOG_GPS(DBG_LOW, "start_batch_tracking"); - g_mutex_lock(&lbs_server->mutex); - lbs_server->gps_client_count++; - g_mutex_unlock(&lbs_server->mutex); + __client_count_update(lbs_server, LBS_SERVER_METHOD_GPS, _LBS_CLIENT_ADD); if (lbs_server->is_gps_running == FALSE) { LOG_GPS(DBG_LOW, "Batch: start tracking GPS"); @@ -450,10 +467,7 @@ static void start_batch_tracking(lbs_server_s *lbs_server, int batch_interval, i static void stop_batch_tracking(lbs_server_s *lbs_server, int batch_interval, int batch_period) { LOG_GPS(DBG_LOW, "Batch: stop_tracking GPS"); - - g_mutex_lock(&lbs_server->mutex); - lbs_server->gps_client_count--; - g_mutex_unlock(&lbs_server->mutex); + __client_count_update(lbs_server, LBS_SERVER_METHOD_GPS, _LBS_CLIENT_REMOVE); if (lbs_server->is_gps_running == FALSE) { LOG_GPS(DBG_LOW, "Batch: gps- is already stopped"); @@ -461,14 +475,10 @@ static void stop_batch_tracking(lbs_server_s *lbs_server, int batch_interval, in } int session_status = 1; /* Keep current status */ - if (lbs_server->gps_client_count <= 0) { - g_mutex_lock(&lbs_server->mutex); - lbs_server->gps_client_count = 0; - g_mutex_unlock(&lbs_server->mutex); + if (lbs_server->gps_client_count == 0) session_status = 0; /* stop */ - } - /* TURE: All clients stopped, FALSE: Some clients are running with GPS or BATCH */ + /* TRUE: All clients stopped, FALSE: Some clients are running with GPS or BATCH */ if (request_stop_batch_session(batch_interval, batch_period, session_status) == TRUE) { g_mutex_lock(&lbs_server->mutex); lbs_server->is_gps_running = FALSE; @@ -483,24 +493,22 @@ static void stop_batch_tracking(lbs_server_s *lbs_server, int batch_interval, in } #endif -static void start_tracking(lbs_server_s *lbs_server, lbs_server_method_e method) +static void start_tracking(gpointer lbs_server_p, lbs_server_method_e method) { LOG_GPS(DBG_LOW, ">>> start_tracking"); + lbs_server_s *lbs_server = (lbs_server_s *)lbs_server_p; int ret = 0; switch (method) { - case LBS_SERVER_METHOD_GPS: - - g_mutex_lock(&lbs_server->mutex); - lbs_server->gps_client_count++; - g_mutex_unlock(&lbs_server->mutex); + case LBS_SERVER_METHOD_GPS: { + __client_count_update(lbs_server, LBS_SERVER_METHOD_GPS, _LBS_CLIENT_ADD); if (lbs_server->is_mock_running != MOCK_RUNNING_OFF && lbs_server->is_gps_running == FALSE && lbs_server->is_nps_running == FALSE) mock_start_tracking(lbs_server); if (lbs_server->is_gps_running == TRUE) { LOG_GPS(DBG_LOW, "gps is already running"); - return; + break; //return; } LOG_GPS(DBG_LOW, "start tracking GPS"); lbs_server->status = LBS_STATUS_ACQUIRING; @@ -517,18 +525,16 @@ static void start_tracking(lbs_server_s *lbs_server, lbs_server_method_e method) } break; - - case LBS_SERVER_METHOD_NPS: - g_mutex_lock(&lbs_server->mutex); - lbs_server->nps_client_count++; - g_mutex_unlock(&lbs_server->mutex); + } + case LBS_SERVER_METHOD_NPS: { + __client_count_update(lbs_server, LBS_SERVER_METHOD_NPS, _LBS_CLIENT_ADD); if (lbs_server->is_mock_running != MOCK_RUNNING_OFF && lbs_server->is_gps_running == FALSE && lbs_server->is_nps_running == FALSE) mock_start_tracking(lbs_server); if (lbs_server->is_nps_running == TRUE) { LOG_NPS(DBG_LOW, "nps is already running"); - return; + break; //return; } LOG_NPS(DBG_LOW, "start_tracking - LBS_SERVER_METHOD_NPS"); @@ -539,6 +545,7 @@ static void start_tracking(lbs_server_s *lbs_server, lbs_server_method_e method) LOG_NPS(DBG_LOW, "after get_nps_plugin_module()->location"); if (ret) { + LOG_NPS(DBG_LOW, "get_nps_plugin_module()->start() success"); lbs_server->handle = handle_str; g_mutex_lock(&lbs_server->mutex); lbs_server->is_nps_running = TRUE; @@ -546,21 +553,22 @@ static void start_tracking(lbs_server_s *lbs_server, lbs_server_method_e method) /* calling key_changed API was comment out. vconf_ignore_key_changed(VCONFKEY_LOCATION_NETWORK_ENABLED, _network_enabled_cb); */ - return; + break; //return; } if (nps_is_dummy_plugin_module() != TRUE) nps_offline_location(lbs_server); - LOG_NPS(DBG_ERR, "Filed to start NPS"); + LOG_NPS(DBG_ERR, "Failed to start NPS"); nps_set_status(lbs_server, LBS_STATUS_ERROR); break; - + } + case LBS_SERVER_METHOD_PASSIVE: + break; default: LOG_GPS(DBG_LOW, "start_tracking Invalid"); break; } - } static gboolean nps_stop(lbs_server_s *lbs_server_nps) @@ -596,62 +604,55 @@ static gboolean nps_stop(lbs_server_s *lbs_server_nps) return TRUE; } -static void stop_tracking(lbs_server_s *lbs_server, lbs_server_method_e method) +static void stop_tracking(gpointer lbs_server_p, lbs_server_method_e method) { + lbs_server_s *lbs_server = (lbs_server_s *)lbs_server_p; + switch (method) { - case LBS_SERVER_METHOD_GPS: - LOG_GPS(DBG_LOW, "stop_tracking GPS"); + case LBS_SERVER_METHOD_GPS: { + LOG_GPS(DBG_LOW, "stop_tracking GPS [client: %d]", lbs_server->gps_client_count); if (lbs_server->is_mock_running == MOCK_RUNNING_OFF) gps_set_last_position(&lbs_server->position); - g_mutex_lock(&lbs_server->mutex); - lbs_server->gps_client_count--; - g_mutex_unlock(&lbs_server->mutex); + __client_count_update(lbs_server, LBS_SERVER_METHOD_GPS, _LBS_CLIENT_REMOVE); if (lbs_server->is_gps_running == FALSE) { LOG_GPS(DBG_LOW, "gps is already stopped"); - return; + break; //return; } - if (lbs_server->gps_client_count <= 0) { + if (lbs_server->gps_client_count == 0 && request_stop_session() == TRUE) { g_mutex_lock(&lbs_server->mutex); - lbs_server->gps_client_count = 0; - - if (request_stop_session() == TRUE) { - lbs_server->is_gps_running = FALSE; - lbs_server->sv_used = FALSE; - /* remove notify */ - setting_ignore_key_changed(VCONFKEY_LOCATION_ENABLED, __setting_gps_cb); - } + lbs_server->is_gps_running = FALSE; + lbs_server->sv_used = FALSE; g_mutex_unlock(&lbs_server->mutex); + /* remove notify */ + setting_ignore_key_changed(VCONFKEY_LOCATION_ENABLED, __setting_gps_cb); } lbs_server->status = LBS_STATUS_UNAVAILABLE; lbs_server_emit_status_changed(lbs_server->lbs_dbus_server, LBS_SERVER_METHOD_GPS, LBS_STATUS_UNAVAILABLE); break; - case LBS_SERVER_METHOD_NPS: + } + case LBS_SERVER_METHOD_NPS: { LOG_NPS(DBG_LOW, "stop_tracking NPS"); - g_mutex_lock(&lbs_server->mutex); - lbs_server->nps_client_count--; - g_mutex_unlock(&lbs_server->mutex); + __client_count_update(lbs_server, LBS_SERVER_METHOD_NPS, _LBS_CLIENT_REMOVE); if (lbs_server->is_nps_running == FALSE) { LOG_NPS(DBG_LOW, "nps is already stopped"); - return; + break; //return; } - if (lbs_server->nps_client_count <= 0) { - g_mutex_lock(&lbs_server->mutex); - lbs_server->nps_client_count = 0; - g_mutex_unlock(&lbs_server->mutex); - + if (lbs_server->nps_client_count == 0) { LOG_NPS(DBG_LOW, "lbs_server_nps Normal stop"); nps_stop(lbs_server); } - + break; + } + case LBS_SERVER_METHOD_PASSIVE: break; default: LOG_GPS(DBG_LOW, "stop_tracking Invalid"); @@ -659,102 +660,150 @@ static void stop_tracking(lbs_server_s *lbs_server, lbs_server_method_e method) } } +/* +* manipulate the dynamic_interval_table. +*/ + +static void find_min_interval_foreach_cb(gpointer key, gpointer value, gpointer userdata) +{ + lbs_server_s *lbs_server = (lbs_server_s *)userdata; + guint interval = GPOINTER_TO_UINT(key); + LOG_GPS(DBG_LOW, "list : interval [%u], count [%u]", interval, GPOINTER_TO_UINT(value)); + if (lbs_server->temp_minimum_interval > interval) + lbs_server->temp_minimum_interval = interval; +} + static void update_dynamic_interval_table_foreach_cb(gpointer key, gpointer value, gpointer userdata) { - guint *interval_array = (guint *)value; + GHashTable *interval_table = (GHashTable *) value; dynamic_interval_updator_user_data *updator_ud = (dynamic_interval_updator_user_data *)userdata; lbs_server_s *lbs_server = updator_ud->lbs_server; int method = updator_ud->method; - LOG_GPS(DBG_LOW, "method:[%d], key:[%s] interval:[%u], current optimized interval [%u]", method, (char *)key, interval_array[method], lbs_server->optimized_interval_array[method]); - if ((lbs_server->temp_minimum_interval > interval_array[method])) - lbs_server->temp_minimum_interval = interval_array[method]; + g_hash_table_foreach(interval_table, (GHFunc) find_min_interval_foreach_cb, (gpointer) lbs_server); + LOG_GPS(DBG_LOW, "foreach_cb, method[%d], client[%s] temp_min[%u], hash_size[%u]", method, (char *)key, lbs_server->temp_minimum_interval, g_hash_table_size(interval_table)); } -static gboolean update_pos_tracking_interval(lbs_server_interval_manipulation_type type, const gchar *client, int method, guint interval, gpointer userdata) +static void __add_interval_table(GHashTable *interval_table, guint interval) { - LOG_GPS(DBG_LOW, "ENTER >>>"); - if (userdata == NULL) return FALSE; + gpointer *value = (gpointer *) g_hash_table_lookup(interval_table, GUINT_TO_POINTER(interval)); + if (value) { + guint count = GPOINTER_TO_UINT(value); + g_hash_table_insert(interval_table, GUINT_TO_POINTER(interval), GUINT_TO_POINTER(count + 1)); + } else { + g_hash_table_insert(interval_table, GUINT_TO_POINTER(interval), GUINT_TO_POINTER(1)); + } +} + +static void __remove_interval_table(GHashTable *interval_table, guint interval) +{ + gpointer *value = (gpointer *) g_hash_table_lookup(interval_table, GUINT_TO_POINTER(interval)); + if (value) { + guint count = GPOINTER_TO_UINT(value); + if (count == 1) { + if (g_hash_table_remove(interval_table, GUINT_TO_POINTER(interval)) != TRUE) + LOG_GPS(DBG_LOW, "Remove interval[%u] failed from g_hash_table", interval); + } else { + g_hash_table_insert(interval_table, GUINT_TO_POINTER(interval), GUINT_TO_POINTER(count - 1)); + } + } else { + LOG_GPS(DBG_LOW, "Remove interval : lookup result is null"); + } +} + +static void __update_interval_table(GHashTable *interval_table, guint interval, guint prev_interval) +{ + LOG_GPS(DBG_LOW, "Update interval [%u -> %u]", prev_interval, interval); + __remove_interval_table(interval_table, prev_interval); + __add_interval_table(interval_table, interval); +} + +static gboolean update_pos_tracking_interval(lbs_server_interval_manipulation_type type, const gchar *client, int method, guint interval, guint prev_interval, gpointer user_data) +{ + LOG_GPS(DBG_LOW, ">>> update_pos_tracking_interval"); + if (user_data == NULL) + return FALSE; if (client == NULL) { LOG_GPS(DBG_ERR, "client is NULL"); return FALSE; } - - if (method < LBS_SERVER_METHOD_GPS || method >= LBS_SERVER_METHOD_SIZE) { + if (type == LBS_SERVER_INTERVAL_REMOVE && method == LBS_SERVER_METHOD_FUSED) { + location_fused_client_remove_all_requests(client); + return FALSE; + } + if (method != LBS_SERVER_METHOD_GPS) { LOG_GPS(DBG_ERR, "unavailable method [%d]", method); return FALSE; } - gboolean ret_val = FALSE; - lbs_server_s *lbs_server = (lbs_server_s *)userdata; + lbs_server_s *lbs_server = (lbs_server_s *)user_data; /* manipulate logic for dynamic-interval hash table */ switch (type) { case LBS_SERVER_INTERVAL_ADD: { - LOG_GPS(DBG_LOW, "ADD, client[%s], method[%d], interval[%u]", client, method, interval); gchar *client_cpy = NULL; client_cpy = g_strdup(client); - guint *interval_array = (guint *) g_hash_table_lookup(lbs_server->dynamic_interval_table, client_cpy); - if (!interval_array) { - /* LOG_GPS(DBG_LOW, "first add key[%s] to interval-table", client); */ - interval_array = (guint *)g_malloc0(LBS_SERVER_METHOD_SIZE * sizeof(guint)); - if (!interval_array) { - /* LOG_GPS(DBG_ERR, "interval_array is NULL"); */ - g_free(client_cpy); - return FALSE; - } - g_hash_table_insert(lbs_server->dynamic_interval_table, (gpointer)client_cpy, (gpointer)interval_array); + GHashTable *interval_table = (GHashTable *) g_hash_table_lookup(lbs_server->dynamic_interval_table, client_cpy); + LOG_GPS(DBG_LOW, "ADD, interval[%u], client[%s], interval_table(%p)", interval, client, interval_table); + if (interval_table) { + __add_interval_table(interval_table, interval); + } else { + LOG_GPS(DBG_LOW, "create gps hash_table to add first key[%s]", client); + interval_table = g_hash_table_new(g_direct_hash, g_direct_equal); + g_hash_table_insert(interval_table, GUINT_TO_POINTER(interval), GUINT_TO_POINTER(1)); + g_hash_table_insert(lbs_server->dynamic_interval_table, (gpointer) client_cpy, (gpointer) interval_table); } - interval_array[method] = interval; - lbs_server->temp_minimum_interval = interval; - /* LOG_GPS(DBG_LOW, "ADD done"); */ + + LOG_GPS(DBG_LOW, "ADD, done"); break; } case LBS_SERVER_INTERVAL_REMOVE: { - LOG_GPS(DBG_LOW, "REMOVE, client[%s], method[%d]", client, method); - lbs_server->temp_minimum_interval = 120; /* interval max value */ - guint *interval_array = (guint *) g_hash_table_lookup(lbs_server->dynamic_interval_table, client); - if (!interval_array) { + GHashTable *interval_table = (GHashTable *) g_hash_table_lookup(lbs_server->dynamic_interval_table, client); + LOG_GPS(DBG_LOW, "REMOVE, interval[%u, %u] , client[%s], interval_table(%p)", interval, prev_interval, client, interval_table); + if (interval_table) { + if ((interval == 0) && (prev_interval == 0)) { + LOG_GPS(DBG_INFO, "client[%s] removed by force.", client); + g_hash_table_remove_all(interval_table); + g_hash_table_destroy(interval_table); + if (g_hash_table_contains(lbs_server->dynamic_interval_table, client)) { + if (g_hash_table_remove(lbs_server->dynamic_interval_table, client) != TRUE) + LOG_GPS(DBG_LOW, "g_hash_table_remove is failed."); + } + break; + } else { + __remove_interval_table(interval_table, interval); + } + } else { LOG_GPS(DBG_INFO, "Client[%s] Method[%d] is already removed from interval-table", client, method); break; } - LOG_GPS(DBG_LOW, "Found interval_array[%d](%p):[%u] from interval-table", method, interval_array, interval_array[method]); - interval_array[method] = 0; - - int i = 0; - guint interval_each = 0; - gboolean is_need_remove_client_from_table = TRUE; - for (i = 0; i < LBS_SERVER_METHOD_SIZE; i++) { - interval_each = interval_array[i]; - if (interval_each != 0) { - LOG_GPS(DBG_LOW, "[%s] method[%d]'s interval is not zero - interval: %d. It will not be removed.", client, i, interval_each); - is_need_remove_client_from_table = FALSE; - break; + + if (g_hash_table_size(interval_table) == 0) { + LOG_GPS(DBG_LOW, "Remove client[%s] from dynamic_interval_table", client); + g_hash_table_destroy(interval_table); + if (g_hash_table_contains(lbs_server->dynamic_interval_table, client)) { + if (g_hash_table_remove(lbs_server->dynamic_interval_table, client) != TRUE) + LOG_GPS(DBG_LOW, "g_hash_table_remove is failed."); } } - if (is_need_remove_client_from_table) { - LOG_GPS(DBG_LOW, "is_need_remove_client_from_table is TRUE"); - if (!g_hash_table_remove(lbs_server->dynamic_interval_table, client)) - LOG_GPS(DBG_ERR, "g_hash_table_remove is failed."); + LOG_GPS(DBG_LOW, "REMOVE, done."); - LOG_GPS(DBG_LOW, "REMOVE done."); - } break; } case LBS_SERVER_INTERVAL_UPDATE: { - LOG_GPS(DBG_LOW, "UPDATE client[%s], method[%d], interval[%u]", client, method, interval); - guint *interval_array = (guint *) g_hash_table_lookup(lbs_server->dynamic_interval_table, client); - if (!interval_array) { - LOG_GPS(DBG_LOW, "Client[%s] is not exist in interval-table", client, method); + GHashTable *interval_table = (GHashTable *) g_hash_table_lookup(lbs_server->dynamic_interval_table, client); + LOG_GPS(DBG_LOW, "UPDATE, interval[%u -> %u], client[%s], interval_table(%p)", prev_interval, interval, client, interval_table); + if (interval_table) { + __update_interval_table(interval_table, interval, prev_interval); + } else { + LOG_GPS(DBG_LOW, "Client[%s] is not exist in interval-table", client); break; } - interval_array[method] = interval; - lbs_server->temp_minimum_interval = interval; - LOG_GPS(DBG_LOW, "UPDATE done."); + LOG_GPS(DBG_LOW, "UPDATE, done."); break; } @@ -767,24 +816,28 @@ static gboolean update_pos_tracking_interval(lbs_server_interval_manipulation_ty /* update logic for optimized-interval value */ if (g_hash_table_size(lbs_server->dynamic_interval_table) == 0) { LOG_GPS(DBG_LOW, "dynamic_interval_table size is zero. It will be updated all value as 0"); + lbs_server->is_needed_changing_interval = FALSE; int i = 0; for (i = 0; i < LBS_SERVER_METHOD_SIZE; i++) lbs_server->optimized_interval_array[i] = 0; ret_val = FALSE; } else { + LOG_GPS(DBG_LOW, "update dynamic_interval_table"); dynamic_interval_updator_user_data updator_user_data; updator_user_data.lbs_server = lbs_server; updator_user_data.method = method; - g_hash_table_foreach(lbs_server->dynamic_interval_table, (GHFunc)update_dynamic_interval_table_foreach_cb, (gpointer)&updator_user_data); + lbs_server->temp_minimum_interval = MAX_INTERVAL; + g_hash_table_foreach(lbs_server->dynamic_interval_table, (GHFunc) update_dynamic_interval_table_foreach_cb, (gpointer) &updator_user_data); if (lbs_server->optimized_interval_array[method] != lbs_server->temp_minimum_interval) { - LOG_GPS(DBG_INFO, "Changing optimized_interval value [%u -> %u] for method [%d]", - lbs_server->optimized_interval_array[method], lbs_server->temp_minimum_interval, method); + LOG_GPS(DBG_INFO, "Changing method[GPS]'s optimized_interval value %u -> [ %u ]", + lbs_server->optimized_interval_array[method], lbs_server->temp_minimum_interval); lbs_server->optimized_interval_array[method] = lbs_server->temp_minimum_interval; /* need to send message to provider device */ + LOG_GPS(DBG_LOW, "--> set needed_changing_interval"); lbs_server->is_needed_changing_interval = TRUE; } @@ -798,8 +851,12 @@ static gboolean update_pos_tracking_interval(lbs_server_interval_manipulation_ty static void request_change_pos_update_interval(int method, gpointer userdata) { - LOG_GPS(DBG_LOW, "ENTER >>>"); - if (!userdata) return; +// LOG_GPS(DBG_LOW, "ENTER >>>"); + LOG_GPS(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + if (!userdata) { + LOG_GPS(DBG_ERR, LEAVE_FUNCTION_PREFIX("NULL pointer")); + return; + } lbs_server_s *lbs_server = (lbs_server_s *)userdata; switch (method) { @@ -809,6 +866,13 @@ static void request_change_pos_update_interval(int method, gpointer userdata) default: break; } + location_fused_external_clients_update( + lbs_server->gps_client_count, + lbs_server->nps_client_count, + lbs_server->optimized_interval_array[LBS_SERVER_METHOD_GPS], + lbs_server->optimized_interval_array[LBS_SERVER_METHOD_NPS], + lbs_server); + LOG_GPS(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); } static void get_nmea(int *timestamp, gchar **nmea_data, gpointer userdata) @@ -825,16 +889,182 @@ static void get_nmea(int *timestamp, gchar **nmea_data, gpointer userdata) static gboolean update_batch_tracking_interval(lbs_server_interval_manipulation_type type, const gchar *client, guint interval, guint period, gpointer userdata); #endif -static void set_options(GVariant *options, const gchar *client, gpointer userdata) +static void set_options_cmd_start( + gpointer lbs_server_p, + const gchar *client, + lbs_server_method_e method, + guint interval, + const char *app_id) { - LOG_GPS(DBG_LOW, "ENTER >>>"); - lbs_server_s *lbs_server = (lbs_server_s *)userdata; + LOG_GPS(LOG_DEBUG, ENTER_FUNCTION_PREFIX("METHOD [%d], INTERVAL [%u], APP_ID [%s]"), method, interval, app_id); + lbs_server_s *lbs_server = (lbs_server_s *)lbs_server_p; + if (client) { + LOG_GPS(DBG_LOW, "update_pos_tracking_interval -> START"); + update_pos_tracking_interval(LBS_SERVER_INTERVAL_ADD, client, method, interval, 0, lbs_server); + } + if (app_id && LBS_SERVER_METHOD_GPS == method) { + LOG_GPS(DBG_LOW, "gps_dump_log(\"START GPS\")"); + gps_dump_log("START GPS", app_id); + } + start_tracking(lbs_server, method); + if (lbs_server->is_needed_changing_interval) { + lbs_server->is_needed_changing_interval = FALSE; + request_change_pos_update_interval(method, (gpointer)lbs_server); + } + LOG_GPS(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +static void set_options_cmd_start_fused( + const gchar *client, + lbs_server_method_e method, + guint interval) +{ + LOG_GPS(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + LocationFusedMode fused_mode; + switch (method) { + case LBS_SERVER_METHOD_GPS: + fused_mode = LOCATION_FUSED_HIGH; + break; + case LBS_SERVER_METHOD_NPS: + fused_mode = LOCATION_FUSED_BALANCED; + break; + default: + LOG_FUSED(DBG_ERR, LEAVE_FUNCTION_PREFIX("Unhandled fused method")); + return; + } + location_fused_client_add_request( + client, + fused_mode, + interval); + + LOG_GPS(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +static void set_options_cmd_stop( + gpointer lbs_server_p, + const gchar *client, + lbs_server_method_e method, + guint interval, + const char *app_id) +{ + LOG_GPS(LOG_DEBUG, ENTER_FUNCTION_PREFIX("METHOD [%d], INTERVAL [%u], APP_ID [%s]"), method, interval, app_id); + lbs_server_s *lbs_server = (lbs_server_s *)lbs_server_p; + if (client) { + LOG_GPS(DBG_LOW, "update_pos_tracking_interval -> STOP"); + update_pos_tracking_interval(LBS_SERVER_INTERVAL_REMOVE, client, method, interval, 0, lbs_server); + } + if (app_id && LBS_SERVER_METHOD_GPS == method) { + LOG_GPS(DBG_LOW, "gps_dump_log(\"STOP GPS\")"); + gps_dump_log("STOP GPS", app_id); + } + update_pos_tracking_interval(LBS_SERVER_INTERVAL_REMOVE, client, method, interval, 0, lbs_server); + stop_tracking(lbs_server, method); + if (lbs_server->is_needed_changing_interval) { + lbs_server->is_needed_changing_interval = FALSE; + request_change_pos_update_interval(method, (gpointer)lbs_server); + } + LOG_GPS(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +static void set_options_cmd_stop_fused( + const gchar *client, + lbs_server_method_e method, + guint interval) +{ + LOG_GPS(LOG_DEBUG, ENTER_FUNCTION_PREFIX("CLIENT [%s]"), client); + + LocationFusedMode fused_mode; + switch (method) { + case LBS_SERVER_METHOD_GPS: + fused_mode = LOCATION_FUSED_HIGH; + break; + case LBS_SERVER_METHOD_NPS: + fused_mode = LOCATION_FUSED_BALANCED; + break; + default: + LOG_FUSED(DBG_ERR, LEAVE_FUNCTION_PREFIX("Unhandled fused method")); + return; + } + location_fused_client_remove_request( + client, + fused_mode, + interval); + + LOG_GPS(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +static void set_options_cmd_set_opt( + gpointer lbs_server_p, + const gchar *client, + lbs_server_method_e method, + guint prev_interval, + guint interval, + gboolean is_update_interval_method) +{ + lbs_server_s *lbs_server = (lbs_server_s *)lbs_server_p; + gboolean is_update_interval = FALSE; + + LOG_GPS(LOG_DEBUG, ENTER_FUNCTION_PREFIX("METHOD [%d], CLIENT [%s], PREV_INTERVAL [%u] --> INTERVAL_UPDATE [%u], min [%u]"), + method, + client, + prev_interval, + interval, + lbs_server->temp_minimum_interval); + + if ((interval != prev_interval) && (prev_interval > 0)) + is_update_interval = TRUE; + + if (is_update_interval && is_update_interval_method && client) { + LOG_GPS(DBG_LOW, "update_pos_tracking_interval -> SET:OPT"); + update_pos_tracking_interval(LBS_SERVER_INTERVAL_UPDATE, client, method, interval, prev_interval, lbs_server); + if (lbs_server->is_needed_changing_interval) { + lbs_server->is_needed_changing_interval = FALSE; + request_change_pos_update_interval(method, (gpointer)lbs_server); + } + } + + LOG_GPS(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +static void set_options_cmd_set_opt_fused( + const gchar *client, + lbs_server_method_e method, + guint prev_interval, + guint interval) +{ + LOG_GPS(LOG_DEBUG, ENTER_FUNCTION_PREFIX("CLIENT [%s], PREV_INTERVAL [%u] --> INTERVAL_UPDATE [%u]"), + client, + prev_interval, + interval); + + LocationFusedMode fused_mode; + switch (method) { + case LBS_SERVER_METHOD_GPS: + fused_mode = LOCATION_FUSED_HIGH; + break; + case LBS_SERVER_METHOD_NPS: + fused_mode = LOCATION_FUSED_BALANCED; + break; + default: + LOG_FUSED(DBG_ERR, LEAVE_FUNCTION_PREFIX("Unhandled fused method")); + return; + } + location_fused_client_update(client, fused_mode, prev_interval, interval); + + LOG_GPS(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +static void set_options(GVariant *options, const gchar *client, gpointer user_data) +{ +// LOG_GPS(DBG_LOW, "ENTER >>>"); + LOG_GPS(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + lbs_server_s *lbs_server = (lbs_server_s *)user_data; GVariantIter iter; gchar *key = NULL; GVariant *value = NULL; gboolean ret = FALSE; -#ifdef _TIZEN_PUBLIC_ +#ifndef _TIZEN_PUBLIC_ char *msg_header = NULL; char *msg_body = NULL; int size = 0; @@ -843,113 +1073,75 @@ static void set_options(GVariant *options, const gchar *client, gpointer userdat char *option = NULL; char *app_id = NULL; guint interval = 0; + guint prev_interval = 0; + gint fused_status = 0; lbs_server_method_e method = 0; g_variant_iter_init(&iter, options); ret = g_variant_iter_next(&iter, "{&sv}", &key, &value); if (ret == FALSE) { - LOG_GPS(DBG_ERR, "Invalid GVariant"); + LOG_GPS(DBG_ERR, LEAVE_FUNCTION_PREFIX("Invalid GVariant")); return; } if (!g_strcmp0(key, "CMD")) { LOG_GPS(DBG_LOW, "set_options [%s]", g_variant_get_string(value, &length)); if (!g_strcmp0(g_variant_get_string(value, &length), "START")) { - while (g_variant_iter_next(&iter, "{&sv}", &key, &value)) { - if (!g_strcmp0(key, "METHOD")) { + if (!g_strcmp0(key, "METHOD")) method = g_variant_get_int32(value); - LOG_GPS(DBG_LOW, "METHOD [%d]", method); - } - - if (!g_strcmp0(key, "INTERVAL")) { + else if (!g_strcmp0(key, "INTERVAL")) interval = g_variant_get_uint32(value); - LOG_GPS(DBG_LOW, "INTERVAL [%u]", interval); - } - - if (!g_strcmp0(key, "APP_ID")) { + else if (!g_strcmp0(key, "APP_ID")) app_id = g_variant_dup_string(value, &length); - LOG_GPS(DBG_LOW, "APP_ID [%s]", app_id); - } - } - - if (client) - update_pos_tracking_interval(LBS_SERVER_INTERVAL_ADD, client, method, interval, lbs_server); - - if (app_id) { - if (LBS_SERVER_METHOD_GPS == method) - gps_dump_log("START GPS", app_id); - - free(app_id); - } - - start_tracking(lbs_server, method); - - if (lbs_server->is_needed_changing_interval) { - lbs_server->is_needed_changing_interval = FALSE; - request_change_pos_update_interval(method, (gpointer)lbs_server); + else if (!g_strcmp0(key, "FUSED_STATUS")) + fused_status = g_variant_get_int32(value); } - } else if (!g_strcmp0(g_variant_get_string(value, &length), "STOP")) { - + if (fused_status != LBS_FUSED_STATUS_NONE) + set_options_cmd_start_fused(client, method, interval); + else + set_options_cmd_start(lbs_server, client, method, interval, app_id); + } + else if (!g_strcmp0(g_variant_get_string(value, &length), "STOP")) { while (g_variant_iter_next(&iter, "{&sv}", &key, &value)) { - if (!g_strcmp0(key, "METHOD")) { + if (!g_strcmp0(key, "METHOD")) method = g_variant_get_int32(value); - LOG_GPS(DBG_LOW, "METHOD [%d]", method); - } - - if (!g_strcmp0(key, "APP_ID")) { + else if (!g_strcmp0(key, "INTERVAL")) + interval = g_variant_get_uint32(value); + else if (!g_strcmp0(key, "APP_ID")) app_id = g_variant_dup_string(value, &length); - LOG_GPS(DBG_LOW, "APP_ID [%s]", app_id); - } - } - - if (client) - update_pos_tracking_interval(LBS_SERVER_INTERVAL_REMOVE, client, method, interval, lbs_server); - - if (app_id) { - if (LBS_SERVER_METHOD_GPS == method) - gps_dump_log("STOP GPS", app_id); - - free(app_id); - } - - stop_tracking(lbs_server, method); - - if (lbs_server->is_needed_changing_interval) { - lbs_server->is_needed_changing_interval = FALSE; - request_change_pos_update_interval(method, (gpointer)lbs_server); + else if (!g_strcmp0(key, "FUSED_STATUS")) + fused_status = g_variant_get_int32(value); } + if (fused_status != LBS_FUSED_STATUS_NONE) + set_options_cmd_stop_fused(client, method, interval); + else + set_options_cmd_stop(lbs_server, client, method, interval, app_id); } #ifndef TIZEN_DEVICE else if (!g_strcmp0(g_variant_get_string(value, &length), "START_BATCH")) { - gint b_interval = 0, b_period = 0; - while (g_variant_iter_next(&iter, "{&sv}", &key, &value)) { + while (g_variant_iter_next(&iter, "{&sv}", &key, &value)) { if (!g_strcmp0(key, "BATCH_INTERVAL")) b_interval = g_variant_get_int32(value); else if (!g_strcmp0(key, "BATCH_PERIOD")) b_period = g_variant_get_int32(value); } LOG_GPS(DBG_LOW, "BATCH_INTERVAL [%d], BATCH_PERIOD [%d]", b_interval, b_period); - if (client) update_batch_tracking_interval(LBS_SERVER_INTERVAL_ADD, client, b_interval, b_period, lbs_server); - start_batch_tracking(lbs_server, lbs_server->optimized_batch_array[LBS_BATCH_INTERVAL], lbs_server->optimized_batch_array[LBS_BATCH_PERIOD]); - - } else if (!g_strcmp0(g_variant_get_string(value, &length), "STOP_BATCH")) { - + } + else if (!g_strcmp0(g_variant_get_string(value, &length), "STOP_BATCH")) { if (client) update_batch_tracking_interval(LBS_SERVER_INTERVAL_REMOVE, client, 0, 0, lbs_server); - stop_batch_tracking(lbs_server, lbs_server->optimized_batch_array[LBS_BATCH_INTERVAL], lbs_server->optimized_batch_array[LBS_BATCH_PERIOD]); - } #endif -#ifdef _TIZEN_PUBLIC_ +#ifndef _TIZEN_PUBLIC_ else if (!g_strcmp0(g_variant_get_string(value, &length), "SUPLNI")) { while (g_variant_iter_next(&iter, "{&sv}", &key, &value)) { if (!g_strcmp0(key, "HEADER")) { @@ -963,71 +1155,72 @@ static void set_options(GVariant *options, const gchar *client, gpointer userdat } } request_supl_ni_session(msg_header, msg_body, size); - if (msg_header) g_free(msg_header); - if (msg_body) g_free(msg_body); + if (msg_header) + g_free(msg_header); + if (msg_body) + g_free(msg_body); } #endif else if (!g_strcmp0(g_variant_get_string(value, &length), "SET:OPT")) { LOG_GPS(DBG_LOW, "SET:OPT is called"); - gboolean is_update_interval = FALSE, is_update_interval_method = FALSE; + gboolean is_update_interval_method = FALSE; while (g_variant_iter_next(&iter, "{&sv}", &key, &value)) { - if (!g_strcmp0(key, "OPTION")) { option = g_variant_dup_string(value, &length); LOG_GPS(DBG_ERR, "option [%s]", option); - if (!g_strcmp0(option, "DELGPS")) { - if (request_delete_gps_data() != TRUE) + if (request_delete_gps_data() != TRUE) { LOG_GPS(DBG_ERR, "Fail to request_delete_gps_data"); + } } else if (!g_strcmp0(option, "USE_SV")) { g_mutex_lock(&lbs_server->mutex); if (lbs_server->sv_used == FALSE) lbs_server->sv_used = TRUE; g_mutex_unlock(&lbs_server->mutex); + } else if (!g_strcmp0(option, "USE_NMEA")) { + if (lbs_server->nmea_used == FALSE) + lbs_server->nmea_used = TRUE; + } else if (!g_strcmp0(option, "UNUSE_NMEA")) { + if (lbs_server->nmea_used == TRUE) + lbs_server->nmea_used = FALSE; } g_free(option); - } - - if (!g_strcmp0(key, "METHOD")) { + } else if (!g_strcmp0(key, "METHOD")) { method = g_variant_get_int32(value); LOG_GPS(DBG_LOW, "METHOD [%d]", method); is_update_interval_method = TRUE; - } - - if (!g_strcmp0(key, "INTERVAL_UPDATE")) { + } else if (!g_strcmp0(key, "INTERVAL_UPDATE")) { interval = g_variant_get_uint32(value); - LOG_GPS(DBG_LOW, "INTERVAL_UPDATE [%u] <-- [%u] ", interval, lbs_server->temp_minimum_interval); - if (interval != lbs_server->temp_minimum_interval) - is_update_interval = TRUE; + } else if (!g_strcmp0(key, "PREV_INTERVAL")) { + prev_interval = g_variant_get_uint32(value); + } else if (!g_strcmp0(key, "FUSED_STATUS")) { + fused_status = g_variant_get_int32(value); } } - if (is_update_interval && is_update_interval_method && client) { - update_pos_tracking_interval(LBS_SERVER_INTERVAL_UPDATE, client, method, interval, lbs_server); - if (lbs_server->is_needed_changing_interval) { - lbs_server->is_needed_changing_interval = FALSE; - request_change_pos_update_interval(method, (gpointer)lbs_server); - } - } + if (fused_status != LBS_FUSED_STATUS_NONE) + set_options_cmd_set_opt_fused(client, method, prev_interval, interval); + else + set_options_cmd_set_opt(lbs_server, client, method, prev_interval, interval, is_update_interval_method); + } } + free(app_id); + LOG_GPS(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); } static gboolean gps_remove_all_clients(lbs_server_s *lbs_server) { - LOG_GPS(DBG_LOW, "remove_all_clients GPS"); + LOG_GPS(DBG_LOW, "remove_all_clients[%d] GPS", lbs_server->gps_client_count); if (lbs_server->is_mock_running == LBS_SERVER_METHOD_GPS) { mock_stop_tracking(lbs_server); setting_ignore_key_changed(VCONFKEY_LOCATION_MOCK_ENABLED, __setting_mock_cb); } - - if (lbs_server->gps_client_count <= 0) { - lbs_server->gps_client_count = 0; + if (lbs_server->gps_client_count == 0) { return FALSE; } - - lbs_server->gps_client_count = 0; + __client_count_update(lbs_server, LBS_SERVER_METHOD_GPS, _LBS_CLIENT_REMOVE_ALL); stop_tracking(lbs_server, LBS_SERVER_METHOD_GPS); return TRUE; @@ -1035,7 +1228,10 @@ static gboolean gps_remove_all_clients(lbs_server_s *lbs_server) static void shutdown(gpointer userdata, gboolean *shutdown_arr) { - LOG_GPS(DBG_LOW, "shutdown callback gps:%d nps:%d", shutdown_arr[LBS_SERVER_METHOD_GPS], shutdown_arr[LBS_SERVER_METHOD_NPS]); + LOG_GPS(DBG_LOW, "shutdown callback gps:%d, nps:%d, fused:%d", + shutdown_arr[LBS_SERVER_METHOD_GPS], + shutdown_arr[LBS_SERVER_METHOD_NPS], + shutdown_arr[LBS_SERVER_METHOD_FUSED]); lbs_server_s *lbs_server = (lbs_server_s *)userdata; if (shutdown_arr[LBS_SERVER_METHOD_GPS]) { @@ -1067,16 +1263,46 @@ static void shutdown(gpointer userdata, gboolean *shutdown_arr) #endif } +static void __fused_callback(fl_location *location, gpointer user_data) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + if (location == NULL || user_data == NULL) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("NULL argument")); + return; + } + lbs_server_s *lbs_server = (lbs_server_s *)(user_data); + LbsPositionExtFields fields = (LBS_POSITION_EXT_FIELDS_LATITUDE + | LBS_POSITION_EXT_FIELDS_LONGITUDE + | LBS_POSITION_EXT_FIELDS_ALTITUDE + | LBS_POSITION_EXT_FIELDS_SPEED + | LBS_POSITION_EXT_FIELDS_DIRECTION + | LBS_POSITION_EXT_FIELDS_CLIMB); + GVariant *accuracy = g_variant_new("(idd)", LBS_ACCURACY_LEVEL_DETAILED, + location->accuracy.horizontal_accuracy, // TODO: or from location->sigma.of_horizontal_pos + location->accuracy.vertical_accuracy); // TODO: or from location->sigma.of_altitude + lbs_server_emit_position_changed(lbs_server->lbs_dbus_server, + LBS_SERVER_METHOD_FUSED, + fields, + location->pos.timestamp, + location->pos.latitude, + location->pos.longitude, + location->pos.altitude, + location->vel.speed, + location->vel.direction, + location->vel.climb, + accuracy); + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + static void gps_update_position_cb(pos_data_t *pos, gps_error_t error, void *user_data) { - /* LOG_GPS(DBG_LOW, "ENTER >>>"); */ GVariant *accuracy = NULL; LbsPositionExtFields fields; lbs_server_s *lbs_server = (lbs_server_s *)(user_data); if (lbs_server->is_mock_running != MOCK_RUNNING_OFF) - return ; + return; memcpy(&lbs_server->position, pos, sizeof(pos_data_t)); @@ -1093,13 +1319,31 @@ static void gps_update_position_cb(pos_data_t *pos, gps_error_t error, void *use gps_set_position(pos); - lbs_server_emit_position_changed(lbs_server->lbs_dbus_server, LBS_SERVER_METHOD_GPS, fields, pos->timestamp, - pos->latitude, pos->longitude, pos->altitude, pos->speed, pos->bearing, 0.0, accuracy); + location_fused_on_gps_position(pos->timestamp, + pos->latitude, + pos->longitude, + pos->altitude, + pos->speed, + pos->bearing, + pos->hor_accuracy, + pos->ver_accuracy); + + lbs_server_emit_position_changed( + lbs_server->lbs_dbus_server, + LBS_SERVER_METHOD_GPS, + fields, + pos->timestamp, + pos->latitude, + pos->longitude, + pos->altitude, + pos->speed, + pos->bearing, + 0.0, + accuracy); } static void gps_update_batch_cb(batch_data_t *batch, void *user_data) { - /* LOG_GPS(DBG_LOW, "gps_update_batch_cb"); */ lbs_server_s *lbs_server = (lbs_server_s *)(user_data); memcpy(&lbs_server->batch, batch, sizeof(batch_data_t)); @@ -1144,8 +1388,8 @@ static void gps_update_satellite_cb(sv_data_t *sv, void *user_data) } satellite_info = g_variant_builder_end(satellite_info_builder); - lbs_server_emit_satellite_changed(lbs_server->lbs_dbus_server, timestamp, satellite_used, sv->num_of_sat, - used_prn, satellite_info); + lbs_server_emit_satellite_changed(lbs_server->lbs_dbus_server, timestamp, satellite_used, + sv->num_of_sat, used_prn, satellite_info); } static void gps_update_nmea_cb(nmea_data_t *nmea, void *user_data) @@ -1168,7 +1412,7 @@ static void gps_update_nmea_cb(nmea_data_t *nmea, void *user_data) if (lbs_server->nmea_used == FALSE) return; - LOG_GPS(DBG_LOW, "[%d] %s", lbs_server->nmea.timestamp, lbs_server->nmea.data); + /* LOG_GPS(DBG_LOW, "[%d] %s", lbs_server->nmea.timestamp, lbs_server->nmea.data); */ lbs_server_emit_nmea_changed(lbs_server->lbs_dbus_server, lbs_server->nmea.timestamp, lbs_server->nmea.data); } @@ -1227,7 +1471,7 @@ static void lbs_server_init(lbs_server_s *lbs_server) nps_init(lbs_server); /* create resource for dynamic-interval */ - lbs_server->dynamic_interval_table = g_hash_table_new_full(g_str_hash, g_str_equal, g_free, g_free); + lbs_server->dynamic_interval_table = g_hash_table_new_full(g_str_hash, g_str_equal, g_free, NULL); lbs_server->optimized_interval_array = (guint *)g_malloc0(LBS_SERVER_METHOD_SIZE * sizeof(guint)); lbs_server->is_needed_changing_interval = FALSE; @@ -1239,9 +1483,16 @@ static void lbs_server_init(lbs_server_s *lbs_server) #ifndef TIZEN_DEVICE lbs_server->batch_interval_table = g_hash_table_new_full(g_str_hash, g_str_equal, g_free, g_free); lbs_server->optimized_batch_array = (guint *)g_malloc0(LBS_BATCH_SIZE * sizeof(guint)); - lbs_server->optimized_batch_array[LBS_BATCH_INTERVAL] = 150; - lbs_server->optimized_batch_array[LBS_BATCH_PERIOD] = 60000; + lbs_server->optimized_batch_array[LBS_BATCH_INTERVAL] = MAX_BATCH_INTERVAL; + lbs_server->optimized_batch_array[LBS_BATCH_PERIOD] = MAX_BATCH_PERIOD; #endif + + location_fused_init( + __fused_callback, + set_options_cmd_stop, + set_options_cmd_start, + set_options_cmd_set_opt, + lbs_server); } static void nps_get_last_position(lbs_server_s *lbs_server_nps) @@ -1333,7 +1584,7 @@ static void nps_init(lbs_server_s *lbs_server_nps) if (!get_nps_plugin_module()->load()) { LOG_NPS(DBG_ERR, "lbs_server_nps plugin load() failed."); - return ; + return; } nps_get_last_position(lbs_server_nps); @@ -1383,12 +1634,12 @@ static void _glib_log(const gchar *log_domain, GLogLevelFlags log_level, int main(int argc, char **argv) { lbs_server_s *lbs_server = NULL; - struct gps_callbacks cb; + struct gps_callbacks g_update_cb; int ret = 0; - cb.pos_cb = gps_update_position_cb; - cb.batch_cb = gps_update_batch_cb; - cb.sv_cb = gps_update_satellite_cb; - cb.nmea_cb = gps_update_nmea_cb; + g_update_cb.pos_cb = gps_update_position_cb; + g_update_cb.batch_cb = gps_update_batch_cb; + g_update_cb.sv_cb = gps_update_satellite_cb; + g_update_cb.nmea_cb = gps_update_nmea_cb; #if !GLIB_CHECK_VERSION(2, 31, 0) if (!g_thread_supported()) @@ -1413,7 +1664,7 @@ int main(int argc, char **argv) lbs_server_init(lbs_server); gps_init_log(); - register_update_callbacks(&cb, lbs_server); + register_update_callbacks(&g_update_cb, lbs_server); g_log_set_default_handler(_glib_log, lbs_server); @@ -1433,7 +1684,6 @@ int main(int argc, char **argv) lbs_dbus_callback->delete_hw_fence_cb = remove_fence; lbs_dbus_callback->pause_hw_fence_cb = pause_fence; lbs_dbus_callback->resume_hw_fence_cb = resume_fence; - lbs_dbus_callback->set_mock_location_cb = set_mock_location_cb; ret = lbs_server_create(SERVICE_NAME, SERVICE_PATH, "lbs-server", "lbs-server", @@ -1451,6 +1701,8 @@ int main(int argc, char **argv) LOG_GPS(DBG_LOW, "lbs_server deamon Stop...."); + location_fused_deinit(); + gps_deinit_log(); /* destroy resource for dynamic-interval */ @@ -1501,7 +1753,7 @@ static void set_mock_location_cb(gint method, gdouble latitude, gdouble longitud setting_ignore_key_changed(VCONFKEY_LOCATION_MOCK_ENABLED, __setting_mock_cb); g_mock_position.timestamp = 0; } - return ; + return; } time_t timestamp; @@ -1696,8 +1948,8 @@ static gboolean update_batch_tracking_interval(lbs_server_interval_manipulation_ } } - lbs_server->optimized_batch_array[LBS_BATCH_INTERVAL] = 151; - lbs_server->optimized_batch_array[LBS_BATCH_PERIOD] = 60001; + lbs_server->optimized_batch_array[LBS_BATCH_INTERVAL] = MAX_BATCH_INTERVAL + 1; + lbs_server->optimized_batch_array[LBS_BATCH_PERIOD] = MAX_BATCH_PERIOD + 1; if (g_hash_table_size(lbs_server->batch_interval_table) == 0) { return FALSE; @@ -1713,3 +1965,48 @@ static gboolean update_batch_tracking_interval(lbs_server_interval_manipulation_ return TRUE; } #endif + +static void __client_count_update(lbs_server_s *lbs_server, lbs_server_method_e method, lbs_client_count operation) +{ + LOG_GPS(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + if (lbs_server == NULL) { + LOG_GPS(DBG_ERR, LEAVE_FUNCTION_PREFIX("lbs_server == NULL")); + return; + } + + g_mutex_lock(&lbs_server->mutex); + switch (method) { + case LBS_SERVER_METHOD_GPS: { + if (operation == _LBS_CLIENT_ADD) { + lbs_server->gps_client_count++; + } else if (operation == _LBS_CLIENT_REMOVE && lbs_server->gps_client_count > 0) { + lbs_server->gps_client_count--; + } else if (operation == _LBS_CLIENT_REMOVE_ALL) { + lbs_server->gps_client_count = 0; + } + break; + } + case LBS_SERVER_METHOD_NPS: { + if (operation == _LBS_CLIENT_ADD) + lbs_server->nps_client_count++; + else if (operation == _LBS_CLIENT_REMOVE && lbs_server->nps_client_count > 0) + lbs_server->nps_client_count--; + else if (operation == _LBS_CLIENT_REMOVE_ALL) + lbs_server->nps_client_count = 0; + break; + } + default: { + g_mutex_unlock(&lbs_server->mutex); + LOG_GPS(DBG_ERR, LEAVE_FUNCTION_PREFIX("Unhandled method")); + return; + } + } + g_mutex_unlock(&lbs_server->mutex); + location_fused_external_clients_update( + lbs_server->gps_client_count, + lbs_server->nps_client_count, + lbs_server->optimized_interval_array[LBS_SERVER_METHOD_GPS], + lbs_server->optimized_interval_array[LBS_SERVER_METHOD_NPS], + lbs_server); + LOG_GPS(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} diff --git a/lbs-server/src/lbs_server.h b/lbs-server/src/lbs_server.h index 1b47303..fa32915 100755 --- a/lbs-server/src/lbs_server.h +++ b/lbs-server/src/lbs_server.h @@ -1,7 +1,7 @@ /* * lbs-server * - * Copyright (c) 2011-2013 Samsung Electronics Co., Ltd. All rights reserved. + * Copyright (c) 2011-2017 Samsung Electronics Co., Ltd. All rights reserved. * * Contact: Youngae Kang , Minjune Kim * Genie Kim @@ -24,6 +24,7 @@ #include #include +#include #define SERVICE_NAME "org.tizen.lbs.Providers.LbsServer" #define SERVICE_PATH "/org/tizen/lbs/Providers/LbsServer" @@ -33,15 +34,6 @@ #define UPDATE_INTERVAL 60 -typedef enum { - LBS_SERVER_METHOD_GPS = 0, - LBS_SERVER_METHOD_NPS, - LBS_SERVER_METHOD_AGPS, - LBS_SERVER_METHOD_GEOFENCE, - LBS_SERVER_METHOD_MOCK, - LBS_SERVER_METHOD_SIZE, -} lbs_server_method_e; - /** * This callback is called with position data. * diff --git a/lbs-server/src/location-fused.c b/lbs-server/src/location-fused.c new file mode 100644 index 0000000..065a9a3 --- /dev/null +++ b/lbs-server/src/location-fused.c @@ -0,0 +1,1240 @@ +/* + * lbs-server + * + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. All rights reserved. + * + * 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 __LOCATION_FUSED_C__ + +#define _USE_MATH_DEFINES +#include +#include +#include +#include "location-fused/types.h" +#include "location-fused/conversions.h" +#include "location-fused/engine.h" +#include "location-fused.h" +#include "location-setting.h" +#include "debug_util.h" +#include "lbs_server.h" + +#define PTR2ENUM(ptr, type) ((type)(int)((char*)(ptr) - ((char*)0))) +#define ENUM2PTR(value) (((char*)0) + (int)(value)) + +#define FUSED_NPS_CLIENT_NAME "fused_common_nps_client_name" +#define FUSED_GPS_CLIENT_NAME "fused_common_gps_client_name" +#define FUSED_APP_ID "fused_common_fake_app_id" + +/** Container of the sensor reference data */ +typedef struct { + sensor_h handle; + sensor_listener_h listener; +} _fl_sensor; + +/** Fused status flag-set */ +typedef enum { + FLIS_UNINITIALIZED = 0, + FLIS_INITIALIZED = (1 << 0), + FLIS_STARTED = (1 << 1), + FLIS_PAUSED = (1 << 2) +} _fl_status_flags; + +/** Combined status flags and bit-field */ +typedef union { + _fl_status_flags flags; + struct { + unsigned initialized: 1; + unsigned started: 1; + unsigned paused: 1; + }; +} _fl_status; + +/** Fused configuration */ +typedef struct { + guint interval; + LocationFusedMode fused_mode; +} FusedConfiguration; + +/** Fused location data */ +typedef struct { + GMutex mutex; + _fl_status status; + gboolean enabled; + fl_location last_location; + LocationFusedMode fused_mode; + _fl_sensor sensor[SUPPORTED_SENSORS_COUNT]; + fl_sensory_flags attached_sensors; + fl_sensory_flags started_sensors; + guint fused_high_mode_client_count; + guint fused_balanced_mode_client_count; + guint fused_interval; + guint gps_client_count; + guint nps_client_count; + guint gps_interval; + guint nps_interval; + gpointer lbs_server_p; + GHashTable *fused_clients; + lbs_server_fused_pos_callback_p lbs_server_fused_callback; + lbs_server_client_remove_p lbs_server_client_remove; + lbs_server_client_add_p lbs_server_client_add; + lbs_server_client_update_p lbs_server_client_update; +} LocationFusedData; + +static LocationFusedData __fused_data; + +static fl_sensory_flags __detect_sensors() +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + fl_sensory_flags use_sensors = NO_SENSOR_FLAG; + + if (sensor_get_default_sensor(SENSOR_ACCELEROMETER, &__fused_data.sensor[RAW_ACCELERATION_SENSOR].handle) == SENSOR_ERROR_NONE) + use_sensors |= RAW_ACCELERATION_SENSOR_FLAG; + + if (sensor_get_default_sensor(SENSOR_GYROSCOPE, &__fused_data.sensor[GYROSCOPE_SENSOR].handle) == SENSOR_ERROR_NONE) + use_sensors |= GYROSCOPE_SENSOR_FLAG; + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("0x%X"), use_sensors); + return use_sensors; +} /* __detect_sensors */ + +static void __attach_sensors() +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + fl_sensory_flags f; + unsigned s; + + __fused_data.attached_sensors = NO_SENSOR_FLAG; + for (s = 0, f = __detect_sensors(); f != NO_SENSOR_FLAG; ++s, f >>= 1) { + if (f & 1) { + sensor_error_e result; + + result = sensor_create_listener(__fused_data.sensor[s].handle, &__fused_data.sensor[s].listener); + if (result == SENSOR_ERROR_NONE) { + result = sensor_listener_set_event_cb(__fused_data.sensor[s].listener, FL_SENSOR_SAMPLING_INTERVAL, __on_sensor_event, ENUM2PTR(s)); + if (result == SENSOR_ERROR_NONE) { + sensor_listener_set_option(__fused_data.sensor[s].listener, SENSOR_OPTION_ALWAYS_ON); + __fused_data.attached_sensors |= (1 << s); + continue; + } + /* undo */ + sensor_destroy_listener(__fused_data.sensor[s].listener); + __fused_data.sensor[s].listener = NULL; + } + /* undo */ + while (s) { + --s; + if (__fused_data.sensor[s].listener) { + sensor_listener_unset_event_cb(__fused_data.sensor[s].listener); + sensor_destroy_listener(__fused_data.sensor[s].listener); + __fused_data.sensor[s].listener = NULL; + } + } + __fused_data.attached_sensors = NO_SENSOR_FLAG; + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("no sensors")); + return; + } + } + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("OK, attached_sensors=0x%X"), __fused_data.attached_sensors); +} /* __attach_sensors */ + +static void __detach_sensors() +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + fl_sensory_flags f; + unsigned s; + + LOG_FUSED(LOG_DEBUG, FUNCTION_PREFIX("attached_sensors=0x%X"), __fused_data.attached_sensors); + for (s = SUPPORTED_SENSORS_COUNT, f = __fused_data.attached_sensors; (f & SUPPORTED_SENSORS_MASK) != NO_SENSOR_FLAG && s;) { + --s; + f <<= 1; + if (f & (1 << SUPPORTED_SENSORS_COUNT)) { + sensor_listener_unset_event_cb(__fused_data.sensor[s].listener); + sensor_destroy_listener(__fused_data.sensor[s].listener); + __fused_data.sensor[s].handle = NULL; + __fused_data.sensor[s].listener = NULL; + } + } + __fused_data.attached_sensors = NO_SENSOR_FLAG; + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* __detach_sensors */ + +static LocationError __start_sensors(fl_sensory_flags sensors) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + if ((sensors & ~__fused_data.attached_sensors) == NO_SENSOR_FLAG) { + fl_sensory_flags f; + unsigned s; + sensor_error_e result; + + sensors &= ~__fused_data.started_sensors; + for (s = 0, f = sensors; f != NO_SENSOR_FLAG; ++s, f >>= 1) { + if (f & 1) { + result = sensor_listener_start(__fused_data.sensor[s].listener); + if (result == SENSOR_ERROR_NONE) + continue; + else { + /* undo */ + while (s) { + --s; + if (sensors & (1 << s)) // __fused_data.sensor[s].listener) + sensor_listener_stop(__fused_data.sensor[s].listener); + } + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("UNKNOWN, sensor_listener_start(): %d"), result); + return LOCATION_ERROR_CONFIGURATION; + } + } + } + __fused_data.started_sensors |= sensors; + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("(0x%X): OK"), sensors); + return LOCATION_ERROR_NONE; + } else { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("(0x%X): NOT_SUPPORTED"), sensors); + return LOCATION_ERROR_NOT_SUPPORTED; + } + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* __start_sensors */ + +static void __stop_sensors(fl_sensory_flags sensors) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + fl_sensory_flags f; + unsigned s; + + LOG_FUSED(LOG_DEBUG, FUNCTION_PREFIX("(0x%X)"), sensors); + sensors &= __fused_data.started_sensors; + for (s = SUPPORTED_SENSORS_COUNT, f = sensors; (f & SUPPORTED_SENSORS_MASK) != NO_SENSOR_FLAG && s;) { + --s; + f <<= 1; + if (f & (1 << SUPPORTED_SENSORS_COUNT)) + sensor_listener_stop(__fused_data.sensor[s].listener); + } + __fused_data.started_sensors &= ~sensors; + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* __stop_sensors */ + +static void __on_started(const char* vconfkey_location_enabled) // TODO: check if could use lbs server notification?! +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("%s"), vconfkey_location_enabled); + fused_engine_start(); + __fused_data.status.started = TRUE; + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* __on_started */ + +static LocationError __start(LocationFusedMode new_mode, guint new_interval) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + + if (__fused_data.status.started == TRUE) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("Already started")); + return LOCATION_ERROR_UNKNOWN; + } + if (new_mode == LOCATION_FUSED_HIGH && __fused_data.fused_high_mode_client_count == 0) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("There are no fused high mode clients")); + return LOCATION_ERROR_UNKNOWN; + } else if (new_mode == LOCATION_FUSED_BALANCED && __fused_data.fused_balanced_mode_client_count == 0) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("There are no fused balanced mode clients")); + return LOCATION_ERROR_UNKNOWN; + } + + __fused_data.fused_mode = new_mode; + __fused_data.fused_interval = new_interval; + + LocationError result = LOCATION_ERROR_UNKNOWN; + + switch (new_mode) { + case LOCATION_FUSED_HIGH: { + fl_sensory_flags sensors = __fused_data.status.paused ? RAW_ACCELERATION_SENSOR_FLAG : GYROSCOPE_SENSOR_FLAG | RAW_ACCELERATION_SENSOR_FLAG; + result = __start_sensors(sensors); + if (result == LOCATION_ERROR_NONE) { + if (__fused_data.status.paused == FALSE) { + if (__fused_data.lbs_server_client_add) { + __fused_data.lbs_server_client_add( // Start GPS + __fused_data.lbs_server_p, + FUSED_GPS_CLIENT_NAME, + LBS_SERVER_METHOD_GPS, + new_interval, + FUSED_APP_ID); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_add == NULL"); + } + } + __on_started(VCONFKEY_LOCATION_ENABLED); + } else { + /* undo */ + __stop_sensors(sensors); + } + break; + } + case LOCATION_FUSED_BALANCED: { +#if (FL_ACCEL_IN_BALANCED) + result = __start_sensors(RAW_ACCELERATION_SENSOR_FLAG); + if (result == LOCATION_ERROR_NONE) { +#endif + if (__fused_data.status.paused == FALSE) { + if (__fused_data.lbs_server_client_add) { + __fused_data.lbs_server_client_add( // Start WPS + __fused_data.lbs_server_p, + FUSED_NPS_CLIENT_NAME, + LBS_SERVER_METHOD_NPS, + new_interval, + FUSED_APP_ID); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_add == NULL"); + } + } + __on_started(VCONFKEY_LOCATION_NETWORK_ENABLED); +#if (FL_ACCEL_IN_BALANCED) + } else { + /* undo */ + __stop_sensors(RAW_ACCELERATION_SENSOR_FLAG); + } +#endif + break; + } + } + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); + return result; +} + +static void __stop() +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + + if (__fused_data.status.started == FALSE) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("Already stopped")); + return; + } + if (__fused_data.fused_mode == LOCATION_FUSED_HIGH && __fused_data.fused_high_mode_client_count > 0) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("Some fused high mode clients left")); + return; + } else if (__fused_data.fused_mode == LOCATION_FUSED_BALANCED && __fused_data.fused_balanced_mode_client_count > 0) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("Some fused balanced mode clients left")); + return; + } + + switch (__fused_data.fused_mode) { + case LOCATION_FUSED_HIGH: + if (__fused_data.lbs_server_client_remove) { + __fused_data.lbs_server_client_remove( // Stop GPS + __fused_data.lbs_server_p, + FUSED_GPS_CLIENT_NAME, + LBS_SERVER_METHOD_GPS, + __fused_data.fused_interval, + FUSED_APP_ID); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_remove == NULL"); + } + break; + case LOCATION_FUSED_BALANCED: + if (__fused_data.lbs_server_client_remove) { + __fused_data.lbs_server_client_remove( // Stop WPS + __fused_data.lbs_server_p, + FUSED_NPS_CLIENT_NAME, + LBS_SERVER_METHOD_NPS, + __fused_data.fused_interval, + FUSED_APP_ID); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_remove == NULL"); + } + break; + } + __stop_sensors(__fused_data.started_sensors); + __fused_data.status.started = FALSE; + __fused_data.fused_mode = LOCATION_FUSED_BALANCED; + __fused_data.fused_interval = 0; + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +static void __change_mode_and_interval(LocationFusedMode new_mode, guint new_interval) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + + if (__fused_data.status.started == FALSE) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("Not started")); + return; + } + if (new_mode == __fused_data.fused_mode) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("Mode argument is not new")); + return; + } + if (new_interval == __fused_data.fused_interval) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("Interval argument is not new")); + return; + } + + // Sign up for a new location source + if (__fused_data.status.paused == FALSE) { + switch (new_mode) { + case LOCATION_FUSED_HIGH: + if (__fused_data.lbs_server_client_add) { + __fused_data.lbs_server_client_add( // Start GPS + __fused_data.lbs_server_p, + FUSED_GPS_CLIENT_NAME, + LBS_SERVER_METHOD_GPS, + new_interval, + FUSED_APP_ID); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_add == NULL"); + } + break; + case LOCATION_FUSED_BALANCED: + if (__fused_data.lbs_server_client_add) { + __fused_data.lbs_server_client_add( // Start WPS + __fused_data.lbs_server_p, + FUSED_NPS_CLIENT_NAME, + LBS_SERVER_METHOD_NPS, + new_interval, + FUSED_APP_ID); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_add == NULL"); + } + break; + } + } + + // Resign from old source + switch (__fused_data.fused_mode) { + case LOCATION_FUSED_HIGH: + if (__fused_data.lbs_server_client_remove) { + __fused_data.lbs_server_client_remove( // Stop GPS + __fused_data.lbs_server_p, + FUSED_GPS_CLIENT_NAME, + LBS_SERVER_METHOD_GPS, + __fused_data.fused_interval, + FUSED_APP_ID); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_remove == NULL"); + } + break; + case LOCATION_FUSED_BALANCED: + if (__fused_data.lbs_server_client_remove) { + __fused_data.lbs_server_client_remove( // Stop WPS + __fused_data.lbs_server_p, + FUSED_NPS_CLIENT_NAME, + LBS_SERVER_METHOD_NPS, + __fused_data.fused_interval, + FUSED_APP_ID); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_remove == NULL"); + } + break; + } + + __fused_data.fused_mode = new_mode; + __fused_data.fused_interval = new_interval; + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +static void __change_mode(LocationFusedMode new_mode) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + + if (__fused_data.status.started == FALSE) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("Not started")); + return; + } + if (new_mode == __fused_data.fused_mode) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("Mode argument is not new")); + return; + } + + // Sign up for a new location source + if (__fused_data.status.paused == FALSE) { + switch (new_mode) { + case LOCATION_FUSED_HIGH: + if (__fused_data.lbs_server_client_add) { + __fused_data.lbs_server_client_add( // Start GPS + __fused_data.lbs_server_p, + FUSED_GPS_CLIENT_NAME, + LBS_SERVER_METHOD_GPS, + __fused_data.fused_interval, + FUSED_APP_ID); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_add == NULL"); + } + break; + case LOCATION_FUSED_BALANCED: + if (__fused_data.lbs_server_client_add) { + __fused_data.lbs_server_client_add( // Start WPS + __fused_data.lbs_server_p, + FUSED_NPS_CLIENT_NAME, + LBS_SERVER_METHOD_NPS, + __fused_data.fused_interval, + FUSED_APP_ID); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_add == NULL"); + } + break; + } + } + + // Resign from old source + switch (__fused_data.fused_mode) { + case LOCATION_FUSED_HIGH: + if (__fused_data.lbs_server_client_remove) { + __fused_data.lbs_server_client_remove( // Stop GPS + __fused_data.lbs_server_p, + FUSED_GPS_CLIENT_NAME, + LBS_SERVER_METHOD_GPS, + __fused_data.fused_interval, + FUSED_APP_ID); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_remove == NULL"); + } + break; + case LOCATION_FUSED_BALANCED: + if (__fused_data.lbs_server_client_remove) { + __fused_data.lbs_server_client_remove( // Stop WPS + __fused_data.lbs_server_p, + FUSED_NPS_CLIENT_NAME, + LBS_SERVER_METHOD_NPS, + __fused_data.fused_interval, + FUSED_APP_ID); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_remove == NULL"); + } + break; + } + + __fused_data.fused_mode = new_mode; + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +static void __change_interval(guint interval) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + + if (__fused_data.status.started == FALSE) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("Not started")); + return; + } + + switch (__fused_data.fused_mode) { + case LOCATION_FUSED_HIGH: + if (__fused_data.lbs_server_client_update) { + __fused_data.lbs_server_client_update( // Update GPS + __fused_data.lbs_server_p, + FUSED_GPS_CLIENT_NAME, + LBS_SERVER_METHOD_GPS, + __fused_data.fused_interval, + interval, + TRUE); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_update == NULL"); + } + break; + case LOCATION_FUSED_BALANCED: + if (__fused_data.lbs_server_client_update) { + __fused_data.lbs_server_client_update( // Update WPS + __fused_data.lbs_server_p, + FUSED_NPS_CLIENT_NAME, + LBS_SERVER_METHOD_NPS, + __fused_data.fused_interval, + interval, + TRUE); + } else { + LOG_FUSED(LOG_ERROR, "lbs_server_client_update == NULL"); + } + break; + } + + __fused_data.fused_interval = interval; + +} + +static void __find_best_config_foreach_request_cb(gpointer key, gpointer value, gpointer userdata) +{ + FusedConfiguration *request_config = (FusedConfiguration *)key; + FusedConfiguration *best_config = (FusedConfiguration *)userdata; + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("interval[%u], mode[%d]"), request_config->interval, request_config->fused_mode); + if (best_config->interval == 0 || request_config->interval < best_config->interval) + best_config->interval = request_config->interval; + if (request_config->fused_mode == LOCATION_FUSED_HIGH) + best_config->fused_mode = LOCATION_FUSED_HIGH; + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +static void __find_best_config_foreach_client_cb(gpointer key, gpointer value, gpointer userdata) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("client[%s]"), (gchar *)key); + GHashTable *request_table = (GHashTable *)value; + if (request_table) + g_hash_table_foreach(request_table, (GHFunc)__find_best_config_foreach_request_cb, userdata); + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +static void __on_configuration_change() +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("fused_interval[%u], fused_mode[%d]"), + __fused_data.fused_interval, + __fused_data.fused_mode); + + FusedConfiguration best_config; + best_config.fused_mode = LOCATION_FUSED_BALANCED; + best_config.interval = 0; + g_hash_table_foreach(__fused_data.fused_clients, (GHFunc)__find_best_config_foreach_client_cb, &best_config); + LOG_FUSED(LOG_DEBUG, FUNCTION_PREFIX("best_interval[%u], best_mode[%d]"), + best_config.interval, + best_config.fused_mode); + + if (__fused_data.status.started) { + if (__fused_data.fused_high_mode_client_count + __fused_data.fused_balanced_mode_client_count == 0) { + __stop(); + } else if (__fused_data.fused_mode != best_config.fused_mode + && __fused_data.fused_interval == best_config.interval) { + __change_mode(best_config.fused_mode); + } else if (__fused_data.fused_mode == best_config.fused_mode + && __fused_data.fused_interval != best_config.interval) { + __change_interval(best_config.interval); + } else if (__fused_data.fused_mode != best_config.fused_mode + && __fused_data.fused_interval != best_config.interval) { + __change_mode_and_interval(best_config.fused_mode, best_config.interval); + } else { + LOG_FUSED(LOG_DEBUG, "Nothing to do"); + } + } else { + if (__fused_data.fused_high_mode_client_count + __fused_data.fused_balanced_mode_client_count > 0 + && best_config.interval > 0) { + __start(best_config.fused_mode, best_config.interval); + } else { + LOG_FUSED(LOG_DEBUG, "There are no clients -> nothing to do"); + } + } + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +static void __remove_from_request_table(GHashTable *table, guint interval, LocationFusedMode fused_mode) { + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("table(%p), request(fused_mode = %s, interval = %d)"), + table, + (fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + interval); + + FusedConfiguration client_request; + client_request.interval = interval; + client_request.fused_mode = fused_mode; + + gpointer *value = (gpointer *)g_hash_table_lookup(table, &client_request); + if (value) { + guint count = GPOINTER_TO_UINT(value); + if (count == 1) { + if (g_hash_table_remove(table, &client_request) != TRUE) { + LOG_FUSED(LOG_DEBUG, "Remove request from g_hash_table failed"); + } else { + LOG_FUSED(LOG_DEBUG, "Remove request from g_hash_table success"); + } + } else { + g_hash_table_insert(table, &client_request, GUINT_TO_POINTER(count - 1)); + } + } else { + LOG_FUSED(LOG_DEBUG, "Remove request : lookup result is null"); + } + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +static guint __fused_configuration_g_hash_func(gconstpointer key) { + FusedConfiguration *request = (FusedConfiguration *)key; + guint ret = (request->interval << 2) + (guint)(request->fused_mode); + return ret; +} + +static gboolean __fused_configuration_g_equal_func(gconstpointer key1, gconstpointer key2) { + FusedConfiguration *r1 = (FusedConfiguration *)key1; + FusedConfiguration *r2 = (FusedConfiguration *)key2; + if (r1->interval == r2->interval && r1->fused_mode == r2->fused_mode) + return TRUE; + else + return FALSE; +} + +static void __add_to_request_table(GHashTable *table, guint interval, LocationFusedMode fused_mode) { + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("table(%p), request(fused_mode = %s, interval = %d)"), + table, + (fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + interval); + + FusedConfiguration *client_request = g_new(FusedConfiguration, 1); + client_request->interval = interval; + client_request->fused_mode = fused_mode; + + gpointer *value = (gpointer *)g_hash_table_lookup(table, (gpointer)client_request); + if (value) { + guint count = GPOINTER_TO_UINT(value); + g_hash_table_replace(table, (gpointer)client_request, GUINT_TO_POINTER(count + 1)); + } else { + g_hash_table_insert(table, (gpointer)client_request, GUINT_TO_POINTER(1)); + } + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +void location_fused_client_add_request( + const gchar *client, + LocationFusedMode fused_mode, + guint interval) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("client[%s], request(fused_mode = %s, interval = %d)"), + client, + (fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + interval); + + // Check if for this client exists request table + GHashTable *request_table = (GHashTable *)g_hash_table_lookup(__fused_data.fused_clients, client); + if (!request_table) { + LOG_FUSED(LOG_DEBUG, "Create new request table for client[%s]", client); + // Add new table + request_table = g_hash_table_new(__fused_configuration_g_hash_func, __fused_configuration_g_equal_func); + gchar *client_name = g_strdup(client); + g_hash_table_insert(__fused_data.fused_clients, (gpointer)client_name, (gpointer)request_table); + } + __add_to_request_table(request_table, interval, fused_mode); + + switch (fused_mode) { + case LOCATION_FUSED_HIGH: + __fused_data.fused_high_mode_client_count++; + break; + case LOCATION_FUSED_BALANCED: + __fused_data.fused_balanced_mode_client_count++; + break; + } + + if (__fused_data.status.started == FALSE // start engine + || (fused_mode == LOCATION_FUSED_HIGH && __fused_data.fused_mode == LOCATION_FUSED_BALANCED) // mode change to higher + || (interval < __fused_data.fused_interval)) { // interval change + __on_configuration_change(); + } else { + LOG_FUSED(LOG_DEBUG, "Config not change: client_mode[%s], fused_mode[%s], client_interval[%d], fused_interval[%d]", + (fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + (__fused_data.fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + interval, + __fused_data.fused_interval); + } + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* location_fused_client_add_request */ + +void location_fused_client_remove_request( + const gchar *client, + LocationFusedMode fused_mode, + guint interval) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("client[%s], request(fused_mode = %s, interval = %d)"), + client, + (fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + interval); + + + gboolean contains_client = g_hash_table_contains(__fused_data.fused_clients, client); + if (contains_client == FALSE) { + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("fused_clients hash table not contains client[%s]"), client); + return; + } + LOG_FUSED(LOG_DEBUG, "fused_clients hash table contains client[%s]", client); + + GHashTable *request_table = (GHashTable *)g_hash_table_lookup(__fused_data.fused_clients, client); + + + FusedConfiguration client_request; + client_request.interval = interval; + client_request.fused_mode = fused_mode; + + gboolean contains_request = g_hash_table_contains(request_table, &client_request); + if (contains_request == FALSE) { + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("request_table for client[%s] not contains request(%s, %ds)"), + client, + (fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + interval); + return; + } + LOG_FUSED(LOG_DEBUG, "request_table for client[%s] contains request(%s, %ds)", + client, + (fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + interval); + + __remove_from_request_table(request_table, interval, fused_mode); + + switch (fused_mode) { + case LOCATION_FUSED_HIGH: + __fused_data.fused_high_mode_client_count--; + break; + case LOCATION_FUSED_BALANCED: + __fused_data.fused_balanced_mode_client_count--; + break; + } + + if (g_hash_table_size(request_table) == 0) { // No requests left for client + LOG_FUSED(LOG_DEBUG, "Remove client[%s] from fused_clients", client); + g_hash_table_destroy(request_table); + if (g_hash_table_contains(__fused_data.fused_clients, client)) { + if (g_hash_table_remove(__fused_data.fused_clients, client)) { + LOG_FUSED(LOG_DEBUG, "Client[%s] removed from fused_clients", client); + } else { + LOG_FUSED(LOG_ERROR, "Client[%s] removal from fused_clients has FAILED", client); + } + } else { + LOG_FUSED(LOG_ERROR, "In the meantime client[%s] is missing from fused_clients", client); + } + } + + if ((__fused_data.fused_high_mode_client_count <= 0 && __fused_data.fused_mode == LOCATION_FUSED_HIGH) // mode change to lower + || (__fused_data.fused_high_mode_client_count <= 0 && __fused_data.fused_balanced_mode_client_count <= 0) // engine stop + || (interval <= __fused_data.fused_interval)) { // possible interval change + __on_configuration_change(); + } else { + LOG_FUSED(LOG_DEBUG, "Config not change: client_mode[%s], fused_mode[%s], client_interval[%d], fused_interval[%d]", + (fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + (__fused_data.fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + interval, + __fused_data.fused_interval); + } + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* location_fused_client_remove_request */ + +static void __request_table_remove_foreach_cb(gpointer key, gpointer value, gpointer userdata) +{ + FusedConfiguration *request = (FusedConfiguration *)key; + guint count = GPOINTER_TO_UINT(value); + gboolean *configuration_change = (gboolean *)userdata; + + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("request(%s, %u) count[%d], configuration_change = %d"), + (request->fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + request->interval, + count, + *configuration_change); + + switch (request->fused_mode) { + case LOCATION_FUSED_HIGH: + __fused_data.fused_high_mode_client_count -= count; + break; + case LOCATION_FUSED_BALANCED: + __fused_data.fused_balanced_mode_client_count -= count; + break; + } + + if (*configuration_change == FALSE) { + LOG_FUSED(LOG_DEBUG, "Check if something change with removal of client request"); + if ((__fused_data.fused_high_mode_client_count <= 0 && __fused_data.fused_mode == LOCATION_FUSED_HIGH) // mode change to lower + || (__fused_data.fused_high_mode_client_count <= 0 && __fused_data.fused_balanced_mode_client_count <= 0) // engine stop + || (request->interval <= __fused_data.fused_interval)) { // possible interval change + *configuration_change = TRUE; + } else { + LOG_FUSED(LOG_DEBUG, "Config not change: client_mode[%s], fused_mode[%s], client_interval[%d], fused_interval[%d]", + (request->fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + (__fused_data.fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + request->interval, + __fused_data.fused_interval); + } + } else { + LOG_FUSED(LOG_DEBUG, "Configuration already changed so there are no need to check one time for this client request"); + } + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} + +void location_fused_client_remove_all_requests(const gchar *client) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("client = \"%s\""), client); + + gboolean contains_client = g_hash_table_contains(__fused_data.fused_clients, client); + if (contains_client == FALSE) { + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("fused_clients hash table not contains client[%s]"), client); + return; + } + LOG_FUSED(LOG_DEBUG, "fused_clients hash table contains client[%s]", client); + + GHashTable *request_table = (GHashTable *)g_hash_table_lookup(__fused_data.fused_clients, client); + gboolean configuration_change = FALSE; + g_hash_table_foreach(request_table, (GHFunc)__request_table_remove_foreach_cb, &configuration_change); + g_hash_table_remove_all(request_table); + + g_hash_table_destroy(request_table); + if (g_hash_table_contains(__fused_data.fused_clients, client)) { + if (g_hash_table_remove(__fused_data.fused_clients, client) == TRUE) { + LOG_FUSED(LOG_DEBUG, "Client[%s] removed from fused_clients", client); + } else { + LOG_FUSED(LOG_ERROR, "Client[%s] removal from fused_clients has FAILED", client); + } + } else { + LOG_FUSED(LOG_ERROR, "In the meantime client[%s] is missing from fused_clients", client); + } + + if (configuration_change) { + __on_configuration_change(); + } else { + LOG_FUSED(LOG_DEBUG, "Config not change: fused_mode[%s], fused_interval[%d]", + (__fused_data.fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + __fused_data.fused_interval); + } + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* location_fused_client_remove_all_requests */ + +void location_fused_client_update( + const gchar *client, + LocationFusedMode fused_mode, + guint prev_interval, + guint interval) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("client = \"%s\", fused_mode = %s, prev_interval = %d, interval = %d"), + client, + (fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + prev_interval, + interval); + + + gboolean contains_client = g_hash_table_contains(__fused_data.fused_clients, client); + if (contains_client == FALSE) { + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("fused_clients hash table not contains client[%s]"), client); + return; + } + LOG_FUSED(LOG_DEBUG, "fused_clients hash table contains client[%s]", client); + + GHashTable *request_table = (GHashTable *)g_hash_table_lookup(__fused_data.fused_clients, client); + + + FusedConfiguration prev_client_request; + prev_client_request.interval = prev_interval; + prev_client_request.fused_mode = fused_mode; + + gboolean contains_request = g_hash_table_contains(request_table, &prev_client_request); + if (contains_request == FALSE) { + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("request_table for client[%s] not contains prev_request(%s, %ds)"), + client, + (fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + prev_interval); + } else { + LOG_FUSED(LOG_DEBUG, "request_table for client[%s] contains prev_request(%s, %ds)", + client, + (fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + prev_interval); + + __remove_from_request_table(request_table, prev_interval, fused_mode); + } + + __add_to_request_table(request_table, interval, fused_mode); + + + if ((interval < __fused_data.fused_interval) // interval change to lower + || (prev_interval == __fused_data.fused_interval && prev_interval > __fused_data.fused_interval)) { // possible interval change to higher + __on_configuration_change(); + } else { + LOG_FUSED(LOG_DEBUG, "Config not change: prev_interval[%d], interval[%d], fused_interval[%d]", + prev_interval, + interval, + __fused_data.fused_interval); + } + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* location_fused_client_update */ + +void location_fused_init( + lbs_server_fused_pos_callback_p fused_pos_cb, + lbs_server_client_remove_p client_remove, + lbs_server_client_add_p client_add, + lbs_server_client_update_p client_update, + gpointer lbs_server_p) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + if (fused_pos_cb == NULL) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("lbs_server_fused_pos_cb == NULL")); + return; + } + if (client_remove == NULL) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("lbs_server_client_remove == NULL")); + return; + } + if (client_add == NULL) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("lbs_server_client_add == NULL")); + return; + } + if (client_update == NULL) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("lbs_server_client_update == NULL")); + return; + } + if (lbs_server_p == NULL) { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("lbs_server_lbs_server_p == NULL")); + return; + } + + __fused_data.status.flags = FLIS_UNINITIALIZED; + __fused_data.fused_mode = LOCATION_FUSED_BALANCED; + __fused_data.attached_sensors = NO_SENSOR_FLAG; + __fused_data.started_sensors = NO_SENSOR_FLAG; + __fused_data.fused_high_mode_client_count = 0; + __fused_data.fused_balanced_mode_client_count = 0; + __fused_data.fused_interval = 0; + __fused_data.gps_client_count = 0; + __fused_data.nps_client_count = 0; + __fused_data.gps_interval = 0; + __fused_data.nps_interval = 0; + __fused_data.lbs_server_fused_callback = fused_pos_cb; + __fused_data.lbs_server_client_remove = client_remove; + __fused_data.lbs_server_client_add = client_add; + __fused_data.lbs_server_client_update = client_update; + __fused_data.lbs_server_p = lbs_server_p; + __fused_data.fused_clients = g_hash_table_new_full(g_str_hash, g_str_equal, g_free, NULL); + + g_mutex_init(&__fused_data.mutex); + __attach_sensors(); + fused_engine_init(__on_motion_state, __on_engine_position, lbs_server_p); + __fused_data.fused_mode = LOCATION_FUSED_BALANCED; + __fused_data.status.initialized = TRUE; + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* location_fused_init */ + +void location_fused_deinit() +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + + __fused_data.status.initialized = FALSE; + + // stop sensors + if (__fused_data.status.started) { + __fused_data.status.started = FALSE; + fused_engine_stop(); + __stop_sensors(__fused_data.started_sensors); + } + fused_engine_exit(); + __detach_sensors(); + + __fused_data.status.flags = FLIS_UNINITIALIZED; + __fused_data.fused_mode = LOCATION_FUSED_BALANCED; + __fused_data.fused_high_mode_client_count = 0; + __fused_data.fused_balanced_mode_client_count = 0; + __fused_data.gps_client_count = 0; + __fused_data.nps_client_count = 0; + __fused_data.gps_interval = 0; + __fused_data.nps_interval = 0; + __fused_data.lbs_server_fused_callback = NULL; + __fused_data.lbs_server_client_remove = NULL; + __fused_data.lbs_server_client_add = NULL; + __fused_data.lbs_server_client_update = NULL; + __fused_data.lbs_server_p = NULL; + g_hash_table_destroy(__fused_data.fused_clients); + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* location_fused_deinit */ + +void location_fused_on_gps_position( + time_t timestamp, + double latitude, + double longitude, + double altitude, + double speed, + double bearing, + double hor_accuracy, + double ver_accuracy) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("mode[%s], started[%d], lat[%.8f], lon[%.8f]"), + (__fused_data.fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + __fused_data.status.started, + latitude, + longitude); + + if (__fused_data.status.started && __fused_data.fused_mode == LOCATION_FUSED_HIGH) { + LocationPosition *pos = location_position_new(timestamp, latitude, longitude, altitude, LOCATION_STATUS_3D_FIX); + LocationVelocity *vel = location_velocity_new(timestamp, speed, bearing, 0.0); + LocationAccuracy *accuracy = location_accuracy_new(LOCATION_ACCURACY_LEVEL_DETAILED, hor_accuracy, ver_accuracy); + + static fl_uncertainty_union __u; + __u.accuracy = *accuracy; + __u.sigma.of_speed = __u.sigma.of_horizontal_pos * FL_DEFAULT_VELOCITY_SIGMA / FL_DEFAULT_POSITION_SIGMA; + __u.sigma.of_climb = __u.sigma.of_altitude * FL_DEFAULT_VELOCITY_SIGMA / FL_DEFAULT_POSITION_SIGMA; + const fl_sigma* sigma = &__u.sigma; + fused_engine_process_position_2x3d_event(pos, vel, sigma); + + location_position_free(pos); + location_velocity_free(vel); + location_accuracy_free(accuracy); + } + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* location_fused_on_gps_position */ + +void location_fused_on_wps_position( + time_t timestamp, + double latitude, + double longitude, + double hor_accuracy, + double ver_accuracy) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("mode[%s], started[%d]"), + (__fused_data.fused_mode == LOCATION_FUSED_HIGH) ? "LOCATION_FUSED_HIGH" : "LOCATION_FUSED_BALANCED", + __fused_data.status.started); + + if (__fused_data.status.started && __fused_data.fused_mode == LOCATION_FUSED_BALANCED) { + LocationPosition *pos = location_position_new(timestamp, latitude, longitude, 0.0, LOCATION_STATUS_2D_FIX); + LocationAccuracy *accuracy = location_accuracy_new(LOCATION_ACCURACY_LEVEL_DETAILED, hor_accuracy, ver_accuracy); + + static fl_uncertainty_union __u; + __u.accuracy = *accuracy; + __u.sigma.of_speed = -1; + __u.sigma.of_climb = -1; + const fl_sigma* sigma = &__u.sigma; + fused_engine_process_position_2d_event(pos, sigma); + + location_position_free(pos); + location_accuracy_free(accuracy); + } + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* location_fused_on_wps_position */ + +void location_fused_external_clients_update( + guint gps_client_count, + guint nps_client_count, + guint gps_interval, + guint nps_interval, + gpointer user_data) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("GPS (%dx %ds), NPS(%dx %ds)"), + gps_client_count, + gps_interval, + nps_client_count, + nps_interval); + if (__fused_data.gps_client_count != gps_client_count + || __fused_data.nps_client_count != nps_client_count + || __fused_data.gps_interval != gps_interval + || __fused_data.nps_interval != nps_interval + ) { + // Configuration change + __fused_data.gps_client_count = gps_client_count; + __fused_data.nps_client_count = nps_client_count; + __fused_data.gps_interval = gps_interval; + __fused_data.nps_interval = nps_interval; + LOG_FUSED(LOG_DEBUG, FUNCTION_PREFIX("External configuration change")); + } + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* location_fused_external_clients_update */ + +static void __on_engine_position(const fl_position_4d* position, gpointer user_data) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + fused_engine_get_location(&__fused_data.last_location.pos, &__fused_data.last_location.vel, &__fused_data.last_location.sigma); + if (__fused_data.lbs_server_fused_callback) { + __fused_data.lbs_server_fused_callback(&__fused_data.last_location, user_data); + } else { + LOG_FUSED(LOG_ERROR, LEAVE_FUNCTION_PREFIX("lbs_server_fused_callback == NULL")); + return; + } + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* __on_engine_position */ + +static void __on_sensor_event(sensor_h handle, sensor_event_s *event, gpointer sensor_id) +{ +// LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + if (event) { + fused_engine_process_sensory_event( + (fl_seconds)((double)(long long)event->timestamp / 1000000.0), + PTR2ENUM(sensor_id, fl_sensory_source), + event->values); + } else { + LOG_FUSED(LOG_ERROR, + LEAVE_FUNCTION_PREFIX("(handle=%p, event=%p, sensor_id=%d): UNSUPPORTED"), + handle, + event, + PTR2ENUM(sensor_id, fl_sensory_source)); + } +} /* __on_sensor_event */ + +#if (FL_MOTION_DETECTOR) +static void __pause() +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + if (__fused_data.status.paused == FALSE) { + __stop_sensors(__fused_data.started_sensors & ~RAW_ACCELERATION_SENSOR_FLAG); + } + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("")); +} /* __pause */ + +static LocationError __resume() +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("")); + LocationError result; + + if (__fused_data.status.paused) { + switch (__fused_data.fused_mode) { + case LOCATION_FUSED_HIGH: +// if (__fused_data.mod->ops.start_gps) { + result = __start_sensors(GYROSCOPE_SENSOR_FLAG); + if (result == LOCATION_ERROR_NONE) { +// result = __fused_data.mod->ops.start_gps(__fused_data.mod->handler, __fused_data.pos_interval, __on_fix_status, __on_gps_position, NULL, self); + if (result == LOCATION_ERROR_NONE) + goto Success; + /* undo */ + __stop_sensors(GYROSCOPE_SENSOR_FLAG); + } +// } +// else +// result = LOCATION_ERROR_NOT_AVAILABLE; + break; + + case LOCATION_FUSED_BALANCED: +// if (__fused_data.mod->ops.start_wps) { +// result = __fused_data.mod->ops.start_wps(__fused_data.mod->handler, __on_fix_status, __on_wps_position, NULL, self); + if (result == LOCATION_ERROR_NONE) + goto Success; +// } +// else +// result = LOCATION_ERROR_NOT_AVAILABLE; + break; + + Success: + __fused_data.status.paused = FALSE; + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("OK")); + return LOCATION_ERROR_NONE; + + default: + result = LOCATION_ERROR_UNKNOWN; + } + } else + result = LOCATION_ERROR_NOT_AVAILABLE; + + LOG_FUSED(LOG_DEBUG, LEAVE_FUNCTION_PREFIX("0x%X"), result); + return result; +} /* __resume */ + +static void __on_motion_state(fl_motion_state state, gpointer data) +{ + LOG_FUSED(LOG_DEBUG, ENTER_FUNCTION_PREFIX("(state=%u)"), state); + if (__fused_data.status.started) { + switch (state) { + case MOTI_MOVING: + if (__fused_data.status.paused) { + g_mutex_lock(&__fused_data.mutex); + __resume(&__fused_data); + g_mutex_unlock(&__fused_data.mutex); + } + break; + + case MOTI_SLEEP: + if (__fused_data.status.paused == FALSE) { + g_mutex_lock(&__fused_data.mutex); + __pause(&__fused_data); + g_mutex_unlock(&__fused_data.mutex); + } + break; + + default: + /* MOTI_UNDECIDED: */ + /* MOTI_IMMOBILE: */ + break; + } + } +} /* __on_motion_state */ +#endif diff --git a/lbs-server/src/location-fused.h b/lbs-server/src/location-fused.h new file mode 100644 index 0000000..cbf36c4 --- /dev/null +++ b/lbs-server/src/location-fused.h @@ -0,0 +1,245 @@ +/* + * lbs-server + * + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. All rights reserved. + * + * 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. + */ + +/** + * @file location-fused.h + * @brief This file contains the internal definitions and structures related to FUSED. + */ + +#ifndef __LOCATION_FUSED_H__ +#define __LOCATION_FUSED_H__ + +#include "gps_plugin_data_types.h" +#include "location-fused/types.h" +#include "lbs_dbus_server.h" +#include + +/***************************************************************************//** + * This callback is called with position data. + * + * @param[in] location Location data + * @param[in] user_data User defined data + */ +typedef void (*lbs_server_fused_pos_callback_p)(fl_location *location, gpointer user_data); + +/***************************************************************************//** + * TODO: TBD + */ +typedef void (*lbs_server_client_add_p)(gpointer lbs_server_p, const gchar *client, + lbs_server_method_e method, guint interval, const char *app_id); + +/***************************************************************************//** + * TODO: TBD + */ +typedef void (*lbs_server_client_remove_p)(gpointer lbs_server_p, const gchar *client, + lbs_server_method_e method, guint interval, const char *app_id); + +/***************************************************************************//** + * TODO: TBD + */ +typedef void (*lbs_server_client_update_p)(gpointer lbs_server_p, const gchar *client, + lbs_server_method_e method, guint prev_interval, guint interval, + gboolean is_update_interval_method); + +/***************************************************************************//** + * TODO: TBD + */ +void location_fused_init(lbs_server_fused_pos_callback_p fused_pos_cb, lbs_server_client_remove_p client_remove, + lbs_server_client_add_p client_add, lbs_server_client_update_p client_update, gpointer lbs_server_p); + +/***************************************************************************//** + * TODO: TBD + */ +void location_fused_deinit(); + +/***************************************************************************//** + * TODO: TBD + * + * @param[in] TODO: TBD + */ +void location_fused_client_add_request(const gchar *client, LocationFusedMode fused_mode, guint interval); + +/***************************************************************************//** + * TODO: TBD + * + * @param[in] TODO: TBD + */ +void location_fused_client_remove_request(const gchar *client, LocationFusedMode fused_mode, guint interval); + +/***************************************************************************//** + * TODO: TBD + * + * @param[in] TODO: TBD + */ +void location_fused_client_remove_all_requests(const gchar *client); + +/***************************************************************************//** + * TODO: TBD + * + * @param[in] TODO: TBD + */ +void location_fused_client_update(const gchar *client, LocationFusedMode fused_mode, guint prev_interval, guint interval); + +/***************************************************************************//** + * [callback] Pass the GPS location data to the engine for processing. + * + * @param[in] timestamp timestamp + * @param[in] latitude latitude + * @param[in] longitude longitude + * @param[in] altitude altitude + * @param[in] speed speed + * @param[in] bearing bearing + * @param[in] hor_accuracy hor_accuracy + * @param[in] ver_accuracy ver_accuracy + */ +void location_fused_on_gps_position(time_t timestamp, double latitude, double longitude, + double altitude, double speed, double bearing, double hor_accuracy, double ver_accuracy); + +/***************************************************************************//** + * [callback] Pass the WPS location data to the engine for processing. + * + * @param[in] timestamp timestamp + * @param[in] latitude latitude + * @param[in] longitude longitude + * @param[in] hor_accuracy hor_accuracy + * @param[in] ver_accuracy ver_accuracy + */ +void location_fused_on_wps_position(time_t timestamp, double latitude, double longitude, + double hor_accuracy, double ver_accuracy); + +/***************************************************************************//** + * [callback] Pass other localization methods client configuration to the engine for processing. + * Internal logic decide to start/stop GPS, NPS when no one such client exist or update interval is to long + * + * @param[in] gps_client_count gps_client_count + * @param[in] nps_client_count nps_client_count + * @param[in] gps_interval gps_interval [s] + * @param[in] nps_interval nps_interval [s] + * @param[in] user_data user_data + */ +void location_fused_external_clients_update(guint gps_client_count, guint nps_client_count, + guint gps_interval, guint nps_interval, gpointer user_data); + +#if defined(__LOCATION_FUSED_C__) + +/***************************************************************************//** + * [private] Detect sensors used for dead-reckoning and power-saving. + * + * @return + * Conjunction of the detected and used sensors as bit-set. + */ +static fl_sensory_flags __detect_sensors(); + +/***************************************************************************//** + * [private] Detect and attach sensors used for dead-reckoning and power-saving. + */ +static void __attach_sensors(); + +/***************************************************************************//** + * [private] Detach used sensors. + */ +static void __detach_sensors(); + +/***************************************************************************//** + * [private] Start selected subset of sensors. + * + * @param[in] sensors + * Bit-set indicator of the sensors to be started. + * + * @return + * - LOCATION_ERROR_NONE (zero) upon success; + * - LOCATION_ERROR_NOT_SUPPORTED if the sensors are not attached; + * - LOCATION_ERROR_CONFIGURATION if the sensors could not be started. + */ +static LocationError __start_sensors(fl_sensory_flags sensors); + +/***************************************************************************//** + * [private] Stop selected subset of sensors. + * + * @param[in] sensors + * Bit-set indicator of the sensors to be stopped. + */ +static void __stop_sensors(fl_sensory_flags sensors); + +/***************************************************************************//** + * [private, callback] This is common and final part of the __start() method: Enabling + * engine and subscribing to notifications. + * + * @param[in] vconfkey_location_enabled + * VConf key name to watch for changes + */ +static void __on_started(const char* vconfkey_location_enabled); + +/***************************************************************************//** + * [private, callback] Pass the sensory data to the engine. + * + * @param[in] handle + * Sensor handle + * @param[in] event + * VConf key name to watch for changes + * @param[in] sensor_id + * TODO: TBD + */ +static void __on_sensor_event(sensor_h handle, sensor_event_s *event, gpointer sensor_id); + +/***************************************************************************//** + * [private, callback] Invoked upon change of position reported by the engine. + * There is no distinction between predicted and fused one; it can be either of + * the two. Upon checking user-supplied conditions the notification is issued. + * + * @param[in] position + * New spatio-temporal (4D) position. + */ +static void __on_engine_position(const fl_position_4d *position, gpointer user_data); + +#if (FL_MOTION_DETECTOR) + +/***************************************************************************//** + * [private] Enters power-saving mode by unsubscribing from location source, and + * and unused sensor(s)). + */ +static void __pause(); + +/***************************************************************************//** + * [private] Resumes normal-power operation mode by subscribing to location + * source, and and used sensor(s). + * + * @return + * - LOCATION_ERROR_NONE (zero) upon success; + * - error code otherwise. + */ +static LocationError __resume(); + +/***************************************************************************//** + * [private, callback] Receive notifications about motion state changes, and + * pause/resume accordingly. + * + * @param[in] state + * New motion state + */ +static void __on_motion_state(fl_motion_state state, gpointer data); +#else /* (FL_MOTION_DETECTOR) */ +/***************************************************************************//** + * [private] Compilation stub. + */ + #define __on_motion_state NULL +#endif /* (FL_MOTION_DETECTOR) */ + +#endif /* defined(__LOCATION_FUSED_C__) */ + +#endif /* __LOCATION_FUSED_H__ */ diff --git a/lbs-server/src/location-fused/accelerometer-filter.c b/lbs-server/src/location-fused/accelerometer-filter.c new file mode 100644 index 0000000..91c0184 --- /dev/null +++ b/lbs-server/src/location-fused/accelerometer-filter.c @@ -0,0 +1,146 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +#if !defined(LOG_THRESHOLD) + /* custom logging threshold - keep undefined on the repo */ + /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ +#endif + +#define __FL_ACCELEROMETER_FILTER_C__ + +#define _USE_MATH_DEFINES +#include +#include "earth.h" +#include "lp-3d-filter.h" +#include "gravity-estimator.h" +#include "accelerometer-filter.h" +#include "debug_util.h" + +#define DEFAULT_SAMPLING_FREQUENCY ((fl_hertz)10.0) /* [Hz] accelerometer sampling frequency */ +#define DEFAULT_ACCELERATION_SIGMA2 SQUARE(FL_ACCEL_NOISE_LEVEL) /* [(m/s^2)^2] */ +#define FREQUENCY_ESTIMATOR_UPDATE_THRESHOLD (((fl_seconds)1.0) * ((fl_hertz)1.0)) + +void fl_accel_init(motion_state_changed_cb on_motion_state_changed, gpointer handler) +{ + static const fl_acceleration_vector av = {0, 0, EARTH_GRAVITY}; + + LOG_FUSED(DBG_LOW, ENTER_FUNCTION_PREFIX("()"), 0); + fl_lp3d_init(); + fl_gres_init(); + fl_moti_init(on_motion_state_changed, handler); + __frequency_estimator.samples = 0; + __frequency_estimator.last_update_time = FL_UNDEFINED_TIME; + fl_lp3d_reset_to(av); + fl_accel_set_sampling_frequency(DEFAULT_SAMPLING_FREQUENCY, FL_UNDEFINED_TIME); + LOG_FUSED(DBG_LOW, LEAVE_FUNCTION_PREFIX("()"), 0); +} /* fl_accel_init */ + +void fl_accel_exit(void) +{ + LOG_FUSED(DBG_LOW, ENTER_FUNCTION_PREFIX("()"), 0); + fl_moti_exit(); + fl_gres_exit(); + fl_lp3d_exit(); + LOG_FUSED(DBG_LOW, LEAVE_FUNCTION_PREFIX("()"), 0); +} /* fl_accel_exit */ + +void fl_accel_reset(const_fl_acceleration_vector_ref av) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("()"), 0); + __frequency_estimator.samples = 0; + __frequency_estimator.last_update_time = FL_UNDEFINED_TIME; + fl_moti_reset(); + fl_lp3d_reset_to(av); +} /* fl_accel_reset */ + +void fl_accel_set_sampling_frequency(fl_hertz f, fl_seconds t) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(f=%5.2f, t=%.3g)"), f, t); + if (f <= 0) { + LOG_FUSED(DBG_ERR, FUNCTION_PREFIX("f=%5.2f <= 0"), f); + return; + } + __frequency_estimator.samples = 0; + __frequency_estimator.last_update_time = t; + __frequency_estimator.last_update_frequency = f; + fl_lp3d_set_sampling_frequency(f); +} /* fl_accel_set_sampling_frequency */ + +void fl_accel_process( + fl_seconds t, + const_fl_acceleration_vector_ref am, + fl_acceleration_vector_ref al, + fl_vector_3d_ref gu, + fl_real *w2gu) +{ + fl_real g2; + fl_real af2; + const_fl_acceleration_vector_ref af; + + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(t=%.2f, a=(% 5.2f, % 5.2f, % 5.2f))"), t, am[GEO_SPATIAL_X], am[GEO_SPATIAL_Y], am[GEO_SPATIAL_Z]); + if (__frequency_estimator.last_update_time > FL_UNDEFINED_TIME) { + fl_seconds dt; + + ++(__frequency_estimator.samples); + dt = t - __frequency_estimator.last_update_time; + if (dt > 0) { + fl_hertz f, df; + + f = __frequency_estimator.samples / dt; + df = fabs(f - __frequency_estimator.last_update_frequency); + if (dt * df >= FREQUENCY_ESTIMATOR_UPDATE_THRESHOLD) + fl_accel_set_sampling_frequency(f, t); + } + } else + __frequency_estimator.last_update_time = t; + + af = fl_lp3d_process(am); + af2 = fl_vector_3d_length2(af); + + /* the z-value is passed through 2nd order Butterworth, then 1st order EMA */ + fl_real overlap = fl_vector_3d_dot_product(af, am); + g2 = fl_gres_process(overlap); + + LOG_FUSED(DBG_INFO, FUNCTION_PREFIX("(t=%.2f, a=(% 5.2f, % 5.2f, % 5.2f)), |af|=%5.2f, =%5.2f"), t, am[GEO_SPATIAL_X], am[GEO_SPATIAL_Y], am[GEO_SPATIAL_Z], sqrt(af2), sqrt(g2)); + if (af2 > SQUARE(FL_ACCEL_NOISE_LEVEL) && g2 > 1) { + fl_real norm_out = 1.0 / sqrt(af2); + + gu[GEO_SPATIAL_X] = af[GEO_SPATIAL_X] * norm_out; + gu[GEO_SPATIAL_Y] = af[GEO_SPATIAL_Y] * norm_out; + gu[GEO_SPATIAL_Z] = af[GEO_SPATIAL_Z] * norm_out; + *w2gu = __gres.base.decay_rate / (fl_square(gu[GEO_SPATIAL_X]) + fl_square(gu[GEO_SPATIAL_Y]) + fl_square(gu[GEO_SPATIAL_Z] - 1) + GEO_SPATIAL_DIMENSION * DEFAULT_ACCELERATION_SIGMA2); + } else { + gu[GEO_SPATIAL_X] = 0; + gu[GEO_SPATIAL_Y] = 0; + gu[GEO_SPATIAL_Z] = 1; /* default */ + *w2gu = __gres.base.decay_rate / (GEO_SPATIAL_DIMENSION * DEFAULT_ACCELERATION_SIGMA2); + } + + fl_acceleration g = sqrt(g2); + if (g > 1) { + fl_real calibration; + + calibration = EARTH_GRAVITY / g; + al[GEO_SPATIAL_X] = calibration * am[GEO_SPATIAL_X] - EARTH_GRAVITY * gu[GEO_SPATIAL_X]; + al[GEO_SPATIAL_Y] = calibration * am[GEO_SPATIAL_Y] - EARTH_GRAVITY * gu[GEO_SPATIAL_Y]; + al[GEO_SPATIAL_Z] = calibration * am[GEO_SPATIAL_Z] - EARTH_GRAVITY * gu[GEO_SPATIAL_Z]; + } else { + al[GEO_SPATIAL_X] = am[GEO_SPATIAL_X]; + al[GEO_SPATIAL_Y] = am[GEO_SPATIAL_Y]; + al[GEO_SPATIAL_Z] = am[GEO_SPATIAL_Z]; + } + fl_moti_process(t, al); +} /* fl_accel_process */ diff --git a/lbs-server/src/location-fused/accelerometer-filter.h b/lbs-server/src/location-fused/accelerometer-filter.h new file mode 100644 index 0000000..141cee1 --- /dev/null +++ b/lbs-server/src/location-fused/accelerometer-filter.h @@ -0,0 +1,125 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file accelerometer-filter.h + * @brief Accelerometer calibration (scale and sampling frequency), and + * low-pass 3D filtering. + */ + +#pragma once +#ifndef __FL_ACCELEROMETER_FILTER_H__ +#define __FL_ACCELEROMETER_FILTER_H__ + +#include "types.h" +#include "motion-detector.h" +#include "parameters.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************//** + * [public] Initializaton of the singleton unit (static constructor). + * + * @param[in] on_motion_state_changed + * Callback to be invoked when the detected state of motion changes. + * @param[in] handler + * Arbitrary user parameter passed to the callback. + */ +void fl_accel_init(motion_state_changed_cb on_motion_state_changed, gpointer handler); + +/***************************************************************************//** + * [public] Cleanup of the singleton unit (static destructor). + */ +void fl_accel_exit(void); + +/***************************************************************************//** + * [public] Reset of the internal state back to initial one. This function is + * used for repetitive testing and module soft restarts. + * + * @param[in] av + * Stationary acceleration state to be set (typically {0, 0, g}). + */ +void fl_accel_reset(const_fl_acceleration_vector_ref av); + +/***************************************************************************//** + * [public] Adapted internal state to the given sampling frequency. Call this + * method whenever the measured frequency changes significantly. + * + * @param[in] f + * Measured accelerometer sampling frequency in Hertz. + * @param[in] t + * Time of the event in seconds. + */ +void fl_accel_set_sampling_frequency(fl_hertz samplingFrequency, fl_seconds t); + +/***************************************************************************//** + * [public] Processing of a single sample from the accelerometer. This function + * performs: + * - estimation of the sampling frequency, + * - estimation of the gravity direction via low-pass 3D filter (cf. LP3D unit), + * - estimation of the gravity value (cf. GRES unit) and scale correction, + * - estimation of the linear acceleration i.e. with subtracted gravity component, + * - motion state detection (cf. MOTI unit) + * + * @param[in] t + * Time of the event in seconds. + * @param[in] am + * Measured 3D acceleration vector transformend to local frame. + * @param[out] al + * Linear acceleration in local frame. + * @param[out] gu + * Unit vector along gravity direction oriented upwards. + * @param[out] w2gu + * Estimated inverse covariance (squared weight) of @a gu. + */ +void fl_accel_process(fl_seconds t, const_fl_acceleration_vector_ref am, fl_acceleration_vector_ref al, fl_vector_3d_ref gu, fl_real *w2gu); + + +/** + * Accelerometer calibration (scale and sampling frequency), and low-pass 3D filtering. + */ + +#if defined(__FL_ACCELEROMETER_FILTER_C__) + #ifdef GTEST + #define PRIVATE + #else + #define PRIVATE \ + static + #endif +#else + #define PRIVATE \ + extern +#endif + +typedef struct { + unsigned samples; + fl_seconds last_update_time; + fl_hertz last_update_frequency; +} _fl_accel_frequency_estimator; + +PRIVATE _fl_accel_frequency_estimator __frequency_estimator; + +#if !defined(__FL_ACCELEROMETER_FILTER_C__) + #undef PRIVATE +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __FL_ACCELEROMETER_FILTER_H__ */ diff --git a/lbs-server/src/location-fused/aema-estimator.c b/lbs-server/src/location-fused/aema-estimator.c new file mode 100644 index 0000000..07b6faf --- /dev/null +++ b/lbs-server/src/location-fused/aema-estimator.c @@ -0,0 +1,170 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +#if !defined(LOG_THRESHOLD) + /* custom logging threshold - keep undefined on the repo */ + /* #define LOG_THRESHOLD LOG_LEVEL_DETAIL */ +#endif + +#define __FL_AEMA_ESTIMATOR_C__ + +#include +#include "aema-estimator.h" +#include "debug_util.h" + +void __aema_init(fl_aema_estimator* self, unsigned plateau_samples) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("[%p](bits=%u)"), self, plateau_samples); + self->plateau_samples = plateau_samples; + self->samples = 0; + self->update_rate = 1; + self->decay_rate = 0; +} /* __aema_init */ + +/***************************************************************************//** + * [protected] Cleanup of the base class (destructor). + */ +static inline void __aema_exit(fl_aema_estimator *self) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("[%p]()"), self); +} /* fl_aema_exit */ + +void fl_aema1d_init(fl_aema1d_estimator* self, unsigned plateau_samples) +{ + __aema_init(&self->base, plateau_samples); + self->value = 0; +} /* fl_aema1d_init */ + +void fl_aema2d_init(fl_aema2d_estimator* self, unsigned plateau_samples) +{ + __aema_init(&self->base, plateau_samples); + self->value[AXIAL_H] = 0; + self->value[AXIAL_V] = 0; +} /* fl_aema2d_init */ + +void fl_aema3d_init(fl_aema3d_estimator* self, unsigned plateau_samples) +{ + __aema_init(&self->base, plateau_samples); + self->value[GEO_SPATIAL_X] = 0; + self->value[GEO_SPATIAL_Y] = 0; + self->value[GEO_SPATIAL_Z] = 0; +} /* fl_aema3d_init */ + +void fl_aema1d_exit(fl_aema1d_estimator *self) +{ + __aema_exit(&self->base); +} /* fl_aema1d_exit */ + +void fl_aema2d_exit(fl_aema2d_estimator *self) +{ + __aema_exit(&self->base); +} /* fl_aema2d_exit */ + +void fl_aema3d_exit(fl_aema3d_estimator *self) +{ + __aema_exit(&self->base); +} /* fl_aema3d_exit */ + +void __aema_reset(fl_aema_estimator* self) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("[%p]()"), self); + self->samples = 0; + self->update_rate = 1; + self->decay_rate = 0; +} /* __aema_reset */ + +void fl_aema1d_reset(fl_aema1d_estimator* self) +{ + __aema_reset(&self->base); + self->value = 0; +} /* fl_aema1d_reset */ + +void fl_aema2d_reset(fl_aema2d_estimator* self) +{ + __aema_reset(&self->base); + self->value[PLANAR_X] = 0; + self->value[PLANAR_Y] = 0; +} /* fl_aema2d_reset */ + +void fl_aema3d_reset(fl_aema3d_estimator* self) +{ + __aema_reset(&self->base); + self->value[GEO_SPATIAL_X] = 0; + self->value[GEO_SPATIAL_Y] = 0; + self->value[GEO_SPATIAL_Z] = 0; +} /* fl_aema3d_reset */ + +fl_real fl_aema1d_process(fl_aema1d_estimator* self, fl_real value) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("[%p](%g)"), self, value); + __aema_process(&self->base); + /* exponential moving avarege */ + self->value = self->base.decay_rate * self->value + + self->base.update_rate * value; + + return self->value; +} /* fl_aema1d_process */ + +const_fl_vector_2d_ref fl_aema2d_process(fl_aema2d_estimator* self, fl_real value_H, fl_real value_V) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g)"), self, value_H, value_V); + + __aema_process(&self->base); +#define UPDATE_VALUE(i) self->value[AXIAL_##i] = self->base.decay_rate * self->value[AXIAL_##i] + self->base.update_rate * value_##i; + UPDATE_VALUE(H); + UPDATE_VALUE(V); +#undef UPDATE_VALUE + + return self->value; +} /* fl_aema2d_process */ + +const_fl_vector_3d_ref fl_aema3d_process(fl_aema3d_estimator* self, const_fl_vector_3d_ref value) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g, %g)"), self, value[0], value[1], value[2]); + + __aema_process(&self->base); +#define UPDATE_VALUE(i) self->value[i] = self->base.decay_rate * self->value[i] + self->base.update_rate * value[i]; + UPDATE_VALUE(GEO_SPATIAL_X); + UPDATE_VALUE(GEO_SPATIAL_Y); + UPDATE_VALUE(GEO_SPATIAL_Z); +#undef UPDATE_VALUE + + return self->value; +} /* fl_aema3d_process */ + +void fl_aema1d_set_estimate(fl_aema1d_estimator* self, fl_real value) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("[%p](%g)"), self, value); + __aema_set_estimate(&self->base); + self->value = value; +} /* fl_aema1d_set_estimate */ + +void fl_aema2d_set_estimate(fl_aema2d_estimator* self, fl_real value_H, fl_real value_V) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g)"), self, value_H, value_V); + __aema_set_estimate(&self->base); + self->value[AXIAL_H] = value_H; + self->value[AXIAL_V] = value_V; +} /* fl_aema2d_set_estimate */ + +void fl_aema3d_set_estimate(fl_aema3d_estimator* self, const_fl_vector_3d_ref value) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("[%p](%g, %g, %g)"), self, value[0], value[1], value[2]); + __aema_set_estimate(&self->base); + self->value[GEO_SPATIAL_X] = value[GEO_SPATIAL_X]; + self->value[GEO_SPATIAL_Y] = value[GEO_SPATIAL_Y]; + self->value[GEO_SPATIAL_Z] = value[GEO_SPATIAL_Z]; +} /* fl_aema3d_set_estimate */ diff --git a/lbs-server/src/location-fused/aema-estimator.h b/lbs-server/src/location-fused/aema-estimator.h new file mode 100644 index 0000000..36282f5 --- /dev/null +++ b/lbs-server/src/location-fused/aema-estimator.h @@ -0,0 +1,312 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file aema-estimator.h + * @brief Adaptive Exponential Moving Average (1st order) filter + */ + +#pragma once +#ifndef __FL_AEMA_ESTIMATOR_H__ +#define __FL_AEMA_ESTIMATOR_H__ + +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(__FL_AEMA_ESTIMATOR_C__) + #define INLINE \ + inline +#else + #define INLINE \ + static inline +#endif + +typedef struct { + unsigned plateau_samples; /* number of samples to collect before EMA kicks-in */ + unsigned samples; /* min(number of samples collected, plateau_samples) */ + fl_real update_rate; /* = 1 / samples */ + fl_real decay_rate; /* = 1 - update_rate */ +} fl_aema_estimator; + +typedef struct { + fl_aema_estimator base; /* base object */ + fl_real value; /* filter output (estimated value) */ +} fl_aema1d_estimator; + +typedef struct { + fl_aema_estimator base; /* base object */ + fl_vector_2d value; /* filter output (estimated value) */ +} fl_aema2d_estimator; + +typedef struct { + fl_aema_estimator base; /* base object */ + fl_vector_3d value; /* filter output (estimated value) */ +} fl_aema3d_estimator; + +/***************************************************************************//** + * [public] Initializaton of the 1D AEMA object (class constructor). + * + * @param[in] self + * Object pointer + * @param[in] plateau_samples + * Number of samples to collect before switching to standard EMA mode. + */ +void fl_aema1d_init(fl_aema1d_estimator *self, unsigned plateau_samples); + +/***************************************************************************//** + * [public] Initializaton of the 2D AEMA object (class constructor). + * + * @param[in] self + * Object pointer + * @param[in] plateau_samples + * Number of samples to collect before switching to standard EMA mode. + */ +void fl_aema2d_init(fl_aema2d_estimator *self, unsigned plateau_samples); + +/***************************************************************************//** + * [public] Initializaton of the 3D AEMA object (class constructor). + * + * @param[in] self + * Object pointer + * @param[in] plateau_samples + * Number of samples to collect before switching to standard EMA mode. + */ +void fl_aema3d_init(fl_aema3d_estimator *self, unsigned plateau_samples); + +/***************************************************************************//** + * [public] Cleanup of the 1D AEMA object (class destructor). + */ +void fl_aema1d_exit(fl_aema1d_estimator *self); + +/***************************************************************************//** + * [public] Cleanup of the 2D AEMA object (class destructor). + */ +void fl_aema2d_exit(fl_aema2d_estimator *self); + +/***************************************************************************//** + * [public] Cleanup of the 3D AEMA object (class destructor). + */ +void fl_aema3d_exit(fl_aema3d_estimator *self); + +/***************************************************************************//** + * [public] Reset the 1D AEMA object back to initial state. This function is + * used for repetitive testing and module soft restarts. + * + * @param[in] self + * Object pointer + */ +void fl_aema1d_reset(fl_aema1d_estimator *self); + +/***************************************************************************//** + * [public] Reset the 2D AEMA object back to initial state. This function is + * used for repetitive testing and module soft restarts. + * + * @param[in] self + * Object pointer + */ +void fl_aema2d_reset(fl_aema2d_estimator *self); + +/***************************************************************************//** + * [public] Reset the 3D AEMA object back to initial state. This function is + * used for repetitive testing and module soft restarts. + * + * @param[in] self + * Object pointer + */ +void fl_aema3d_reset(fl_aema3d_estimator *self); + +/***************************************************************************//** + * [public] Processing of a single data sample. + * + * @param[in] self + * Object pointer + * @param[in] value + * Input value to the filter. + * + * @return + * Output value of the filter after processing. This is the same as + * returned by fl_aema1d_get_estimate(self). + */ +fl_real fl_aema1d_process(fl_aema1d_estimator *self, fl_real value); + +/***************************************************************************//** + * [public] Processing of a double data sample. + * + * @param[in] self + * Object pointer + * @param[in] value_H + * First (horizontal) input value to the filter. + * @param[in] value_V + * Second (vertical) input value to the filter. + * + * @return + * Output value of the filter after processing. This is the same as + * returned by fl_aema2d_get_estimate(self). + */ +const_fl_vector_2d_ref fl_aema2d_process(fl_aema2d_estimator *self, fl_real value_X, fl_real value_Y); + +/***************************************************************************//** + * [public] Processing of a triple data sample. + * + * @param[in] self + * Object pointer + * @param[in] value + * Input 3D vector value to the filter. + * + * @return + * Output value of the filter after processing. This is the same as + * returned by fl_aema3d_get_estimate(self). + */ +const_fl_vector_3d_ref fl_aema3d_process(fl_aema3d_estimator *self, const_fl_vector_3d_ref value); + +/***************************************************************************//** + * [public] Bring the filter to a stationary state (EMA mode, input = output). + * This method is used for testing. + * + * @param[in] self + * Object pointer + * @param[in] value + * I/o value of the filter. + */ +void fl_aema1d_set_estimate(fl_aema1d_estimator *self, fl_real value); + +/***************************************************************************//** + * [public] Bring the filter to a stationary state (EMA mode, input = output). + * This method is used for testing. + * + * @param[in] self + * Object pointer + * @param[in] value_H + * First (horizontal) i/o value of the filter. + * @param[in] value_V + * Second (vertical) i/o value of the filter. + */ +void fl_aema2d_set_estimate(fl_aema2d_estimator *self, fl_real value_X, fl_real value_Y); + +/***************************************************************************//** + * [public] Bring the filter to a stationary state (EMA mode, input = output). + * This method is used for testing. + * + * @param[in] self + * Object pointer + * @param[in] value + * I/o 3D vector value of the filter. + */ +void fl_aema3d_set_estimate(fl_aema3d_estimator *self, const_fl_vector_3d_ref value); + +/***************************************************************************//** + * [public] Fetch the current filter output. + * + * @param[in] self + * Object pointer + * + * @return + * Output value after last processed sample. + */ +INLINE fl_real fl_aema1d_get_estimate(const fl_aema1d_estimator *self) +{ + return self->value; +} + +/***************************************************************************//** + * [public] Fetch the current filter output. + * + * @param[in] self + * Object pointer + * + * @return + * Output 2D vector after last processed sample. + */ +INLINE const_fl_vector_2d_ref fl_aema2d_get_estimate(const fl_aema2d_estimator *self) +{ + return self->value; +} + +/***************************************************************************//** + * [public] Fetch the current filter output. + * + * @param[in] self + * Object pointer + * + * @return + * Output 3D vector after last processed sample. + */ +INLINE const_fl_vector_3d_ref fl_aema3d_get_estimate(const fl_aema3d_estimator *self) +{ + return self->value; +} + +/***************************************************************************//** + * [protected] Initializaton of the base object (class constructor). + * + * @param[in] self + * Object pointer + * @param[in] plateau_samples + * Number of samples to collect before switching to standard EMA mode. + */ +void __aema_init(fl_aema_estimator *self, unsigned plateau_samples); + +/***************************************************************************//** + * [protected] Reset the AEMA object back to initial state. This function is + * used for repetitive testing and module soft restarts. + * + * @param[in] self + * Object pointer + */ +void __aema_reset(fl_aema_estimator *self); + +/***************************************************************************//** + * [protected] Processing of a data sample. This is the common part of the + * derived processing methods, which implements the AEMA update/decay rate. + * + * @param[in] self + * Object pointer + */ +INLINE void __aema_process(fl_aema_estimator *self) +{ + if (self->samples < self->plateau_samples) { + self->samples++; + self->update_rate = 1.0 / self->samples; + self->decay_rate = 1.0 - self->update_rate; + } +} + +/***************************************************************************//** + * [protected] Bring the base filter to a stationary state (EMA mode). This + * method is used for testing. + * + * @param[in] self + * Object pointer + */ +INLINE void __aema_set_estimate(fl_aema_estimator* self) +{ + self->samples = self->plateau_samples; + self->update_rate = 1.0 / self->plateau_samples; + self->decay_rate = 1.0 - self->update_rate; +} + +#if !defined(__FL_AEMA_ESTIMATOR_C__) + #undef INLINE +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __FL_AEMA_ESTIMATOR_H__ */ diff --git a/lbs-server/src/location-fused/conversions.c b/lbs-server/src/location-fused/conversions.c new file mode 100644 index 0000000..541548a --- /dev/null +++ b/lbs-server/src/location-fused/conversions.c @@ -0,0 +1,47 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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 __FL_CONVERSIONS_C__ + +#include "conversions.h" + +fl_degrees fl_radians_to_positive_degrees(fl_radians radians) +{ + fl_degrees d; + + d = (180 / M_PI) * radians; + /* When the value only ocassionally enters adjacent branches this is more + efficient a way than division modulo. */ + if (d < 0) + do { d += 360; } while (d < 0); + else if (d >= 360) + do { d -= 360; } while (d >= 360); + return d; +} /* fl_radians_to_positive_degrees */ + +fl_degrees fl_radians_to_balanced_degrees(fl_radians radians) +{ + fl_degrees d; + + d = (180 / M_PI) * radians; + /* When the value only ocassionally enters adjacent branches this is more + efficient a way than division modulo. */ + if (d < -180) + do { d += 360; } while (d < -180); + else if (d >= 180) + do { d -= 360; } while (d >= 180); + return d; +} /* fl_radians_to_balanced_degrees */ diff --git a/lbs-server/src/location-fused/conversions.h b/lbs-server/src/location-fused/conversions.h new file mode 100644 index 0000000..57f3f73 --- /dev/null +++ b/lbs-server/src/location-fused/conversions.h @@ -0,0 +1,204 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file conversions.h + * @brief Units conversion functions + */ + +#pragma once +#ifndef __FL_CONVERSIONS_H__ +#define __FL_CONVERSIONS_H__ + +#define _USE_MATH_DEFINES +#include "math-ext.h" +#include "earth.h" +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(__FL_CONVERSIONS_C__) + #define INLINE \ + inline /* thank checkpatchinit_tizen for this contraption */ +#else + #define INLINE \ + static inline +#endif + +#define FL_TIMESTAMP_UNITS 1 /**< [s] */ + +/***************************************************************************//** + * [public] Convert location time-stamp to seconds. Notice, on Tizen this is a + * formal methd since time-stamps are also resolved in seconds. + * + * @param[in] timestamp + * Time-stamp received from the location source (GPS, WPS). + * + * @return + * Corresponding value in seconds + */ +INLINE fl_seconds fl_timestamp_to_seconds(fl_timestamp timestamp) +{ + return (fl_seconds)timestamp / FL_TIMESTAMP_UNITS; +} + +/***************************************************************************//** + * [public] Convert time in seconds to location time-stamp. The necesary + * up-rounding is is half the time-stamp unit. + * + * @param[in] timestamp + * Time-stamp received from the location source (GPS, WPS). + * + * @return + * Corresponding value in seconds + */ +INLINE fl_timestamp fl_seconds_to_timestamp(fl_seconds t) +{ + return (fl_timestamp)(t * FL_TIMESTAMP_UNITS + 0.5); +} + +/***************************************************************************//** + * [public] Convert degrees to radians. + * + * @param[in] degrees + * Value in degrees. + * + * @return + * Corresponding value in radians. + */ +INLINE fl_radians fl_degrees_to_radians(fl_degrees degrees) +{ + return (M_PI / 180) * degrees; +} + +/***************************************************************************//** + * [public] Convert radians to degrees. + * + * @param[in] radians + * Value in radians. + * + * @return + * Corresponding value in degrees. + */ +INLINE fl_degrees fl_radians_to_degrees(fl_radians radians) +{ + return (180 / M_PI) * radians; +} + +/***************************************************************************//** + * [public] Convert radians to degrees in the [0,360) range + * + * @param[in] radians + * Arbitrary value in radians. + * + * @return + * Value in degrees projected onto the [0,360) branch. + */ +fl_degrees fl_radians_to_positive_degrees(fl_radians radians); + +/***************************************************************************//** + * [public] Convert radians to degrees in the [-180,180) range. + * + * @param[in] radians + * Arbitrary value in radians. + * + * @return + * Value in degrees projected onto the [-180,180) branch. + */ +fl_degrees fl_radians_to_balanced_degrees(fl_radians radians); + +/** [public] Convert radians to latitude degrees [-90,90] */ +#define fl_radians_to_latitude fl_radians_to_balanced_degrees + +/** [public] Convert radians to longitude degrees [0, 360) */ +#define fl_radians_to_longitude fl_radians_to_positive_degrees + +/** [public] Convert radians to longitude degrees [0, 360) */ +#define fl_radians_to_cog fl_radians_to_positive_degrees + +/***************************************************************************//** + * [public] Convert speed in [km/h] to velocity in [m/s]. + * + * @note The 'speed' is an i/o value of location interfaces, while 'velocity' + * expressed in SI units is used internally by the fused location engine. + * + * @param[in] km_h + * Speed in kilometers per hour. + * + * @return + * Corresponding value in meters per second. + */ +INLINE fl_velocity fl_km_h_to_m_s(fl_km_h km_h) +{ + return km_h * (1000.0 / (60.0 * 60.0)); +} + +/***************************************************************************//** + * [public] Convert velocity in [m/s] to speed in [km/h]. + * + * @note The 'speed' is an i/o value of location interfaces, while 'velocity' + * expressed in SI units is used internally by the fused location engine. + * + * @param[in] velocity + * Velocity in meters per second. + * + * @return + * Corresponding value in kilometers per hour. + */ +INLINE fl_km_h fl_m_s_to_km_h(fl_velocity velocity) +{ + return velocity * ((60.0 * 60.0) / 1000.0); +} + +/***************************************************************************//** + * [public] Convert spherical coordinate in radians to arc-length along + * the Earth's great circle in meters. + * + * @param[in] radians + * Angle spanned by the great arc. + * + * @return + * Corresponding value in meters along Earth's surface. + */ +INLINE fl_meters fl_radians_to_meters(fl_radians radians) +{ + return radians * EARTH_RADIUS; +} + +/***************************************************************************//** + * [public] Convert arc-length in meters along the Earth's great circle to arc + * angle in radians. + * + * @param[in] meters + * Arc length in meters. + * + * @return + * Corresponding angle in radians. + */ +INLINE fl_radians fl_meters_to_radians(fl_meters meters) +{ + return meters * (1.0 / EARTH_RADIUS); +} + +#undef INLINE + +#ifdef __cplusplus +} +#endif + +#endif /* __FL_CONVERSIONS_H_ */ diff --git a/lbs-server/src/location-fused/earth.h b/lbs-server/src/location-fused/earth.h new file mode 100644 index 0000000..4291251 --- /dev/null +++ b/lbs-server/src/location-fused/earth.h @@ -0,0 +1,45 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file earth.h + * @brief Constants and methods related to the Earth model. + */ + +#pragma once +#ifndef __FL_EARTH_H__ +#define __FL_EARTH_H__ + +#include "types.h" + +/***************************************************************************//** + * [public] Mean Earth gravity value in [m/s^2]. + * + * Notice that, strictly speaking, the mean Earth gravity acceleration at surface + * level is negative; the positive value we measure with an accelerometer is the + * ground reaction which counters the gravity and prevents us from falling into + * the interior. Also notice that given the inaccuracy (i.e. bias + * & miscalibration) of mobile device acceleromenters at ~1 [m/s^2] the number + * of significant digits in the reference value below is way too many. + */ +#define EARTH_GRAVITY ((fl_acceleration)9.80665) + +/***************************************************************************//** + * [public] Mean Earth radius in [m]. + */ +#define EARTH_RADIUS ((fl_meters)6371000.0) + +#endif /* __FL_EARTH_H__ */ diff --git a/lbs-server/src/location-fused/engine.h b/lbs-server/src/location-fused/engine.h new file mode 100644 index 0000000..1d58a11 --- /dev/null +++ b/lbs-server/src/location-fused/engine.h @@ -0,0 +1,38 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file engine.h + * @brief Fused Location engine interface + */ + +#pragma once +#ifndef __FL_ENGINE_H__ +#define __FL_ENGINE_H__ + +#include "kalman-filter.h" + +#define fused_engine_init(on_motion_state_changed, on_location_changed, data) \ + fl_kalf_init(on_motion_state_changed, on_location_changed, data) +#define fused_engine_exit() fl_kalf_exit() +#define fused_engine_start() fl_kalf_start() +#define fused_engine_stop() fl_kalf_stop() +#define fused_engine_process_position_2d_event(pos, sigma) fl_kalf_process_position_2d_event(pos, sigma) +#define fused_engine_process_position_2x3d_event(pos, vel, sigma) fl_kalf_process_position_2x3d_event(pos, vel, sigma) +#define fused_engine_process_sensory_event(t, sensor_id, data) fl_kalf_process_sensory_event(t, sensor_id, data) +#define fused_engine_get_location(pos, vel, sigma) fl_kalf_get_location(pos, vel, sigma) + +#endif /* __FL_ENGINE_H__ */ diff --git a/lbs-server/src/location-fused/gravity-estimator.c b/lbs-server/src/location-fused/gravity-estimator.c new file mode 100644 index 0000000..96e1a2b --- /dev/null +++ b/lbs-server/src/location-fused/gravity-estimator.c @@ -0,0 +1,43 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +#if !defined(LOG_THRESHOLD) + /* custom logging threshold - keep undefined on the repo */ + /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ +#endif + +#define __FL_GRAVITY_ESTIMATOR_C__ + +#include "gravity-estimator.h" +#include "debug_util.h" + +void fl_gres_init(void) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("()"), 0); + fl_aema1d_init(&__gres, 1 << FL_GRES_AEMA_PLATEAU_BITS); +} + +void fl_gres_exit(void) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("()"), 0); + fl_aema1d_exit(&__gres); +} + +void fl_gres_reset(void) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("()"), 0); + fl_aema1d_reset(&__gres); +} diff --git a/lbs-server/src/location-fused/gravity-estimator.h b/lbs-server/src/location-fused/gravity-estimator.h new file mode 100644 index 0000000..9d4a638 --- /dev/null +++ b/lbs-server/src/location-fused/gravity-estimator.h @@ -0,0 +1,113 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file gravity-estimator.h + * @brief Estimator of the gravity value used for accelerometer scale + * calibration. + */ + +#pragma once +#ifndef __FL_GRAVITY_ESTIMATOR_H__ +#define __FL_GRAVITY_ESTIMATOR_H__ + +#include "parameters.h" +#include "aema-estimator.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(__FL_GRAVITY_ESTIMATOR_C__) + #define INLINE \ + static inline /* thank checkpatchinit_tizen for this contraption */ + #define PRIVATE +#else + #define INLINE \ + static inline + #define PRIVATE \ + extern +#endif + +PRIVATE fl_aema1d_estimator __gres; + +/***************************************************************************//** + * [public] Initializaton of the singleton unit (static constructor). + */ +void fl_gres_init(void); + +/***************************************************************************//** + * [public] Cleanup of the singleton unit (static destructor). + */ +void fl_gres_exit(void); + +/***************************************************************************//** + * [public] Bring the internal state back to initial one. This function is used + * for repetitive testing and module soft restarts. + */ +void fl_gres_reset(void); + +/***************************************************************************//** + * [public] Processing of a single data sample. + * + * @param[in] g2 + * Modulus squared of the measured acceleration projected onto z-axis. + * + * @return + * Estimated value of the Earth gravity. + */ +INLINE fl_real fl_gres_process(fl_real g2) +{ + return fl_aema1d_process(&__gres, g2); +} + +/***************************************************************************//** + * [public] Bring the filter to a stationary state (EMA mode, input = output). + * This method is used for testing. + * + * @param[in] g2 + * Modulus squared value of Earth gravity measured by the accelerometer. + */ +INLINE void fl_gres_set_estimate(fl_real g2) +{ + fl_aema1d_set_estimate(&__gres, g2); +} + +/***************************************************************************//** + * [public] Fetch the current gravity estimate. + * + * @param[in] self + * Object pointer + * + * @return + * Output value after last processed sample. + */ +/** Retrieve current estimate */ +INLINE fl_real fl_gres_get_estimate(void) +{ + return fl_aema1d_get_estimate(&__gres); +} + +#if !defined(__FL_GRAVITY_ESTIMATOR_C__) + #undef PRIVATE + #undef INLINE +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __FL_GRAVITY_ESTIMATOR_H__ */ diff --git a/lbs-server/src/location-fused/gyroscope-filter.c b/lbs-server/src/location-fused/gyroscope-filter.c new file mode 100644 index 0000000..4a90fc4 --- /dev/null +++ b/lbs-server/src/location-fused/gyroscope-filter.c @@ -0,0 +1,105 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +#if !defined(LOG_THRESHOLD) + /* custom logging threshold - keep undefined on the repo */ + /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ +#endif + +#define __FL_GYROSCOPE_FILTER_C__ + +#define _USE_MATH_DEFINES +#include +#include "math-ext.h" +#include "gravity-estimator.h" +#include "gyroscope-filter.h" +#include "debug_util.h" + +#define GYRO_PLATEAU_EVIDENCE (1 << FL_GYRO_AEMA_PLATEAU_BITS) +#define GYRO_SCALE2_I (1.0 / (2 * SQUARE(FL_GYRO_SPIN_SIGMA_I))) +#define GYRO_SCALE2_F (1.0 / (2 * SQUARE(FL_GYRO_SPIN_SIGMA_F))) + +static fl_real __gyro_alpha; + +void fl_gyro_init(void) +{ + LOG_FUSED(DBG_LOW, ENTER_FUNCTION_PREFIX("()"), 0); + __gyro_alpha = (SQUARE(FL_GYRO_SPIN_SIGMA_I / FL_GYRO_SPIN_SIGMA_F) - 1) / SQUARE((fl_real)GYRO_PLATEAU_EVIDENCE); + fl_gyro_reset(); + LOG_FUSED(DBG_LOW, LEAVE_FUNCTION_PREFIX("()"), 0); +} /* fl_gyro_init */ + +void fl_gyro_exit(void) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("()"), 0); +} /* fl_gyro_exit */ + +void fl_gyro_reset(void) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("()"), 0); + __gyro_bias.scale2 = GYRO_SCALE2_I; + __gyro_bias.evidence = 0; + __gyro_bias.weight_norm = 1; + __gyro_bias.value[GEO_SPATIAL_X] = 0; + __gyro_bias.value[GEO_SPATIAL_Y] = 0; + __gyro_bias.value[GEO_SPATIAL_Z] = 0; +} /* fl_gyro_reset */ + +void fl_gyro_set(const_fl_spin_vector_ref w0) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(%g, %g, %g)"), w0[GEO_SPATIAL_X], w0[GEO_SPATIAL_Y], w0[GEO_SPATIAL_Z]); + __gyro_bias.scale2 = GYRO_SCALE2_F; + __gyro_bias.evidence = GYRO_PLATEAU_EVIDENCE; + __gyro_bias.weight_norm = 1.0 / GYRO_PLATEAU_EVIDENCE; + __gyro_bias.value[GEO_SPATIAL_X] = w0[GEO_SPATIAL_X]; + __gyro_bias.value[GEO_SPATIAL_Y] = w0[GEO_SPATIAL_Y]; + __gyro_bias.value[GEO_SPATIAL_Z] = w0[GEO_SPATIAL_Z]; +} /* fl_gyro_set */ + +const_fl_spin_vector_ref fl_gyro_process(fl_seconds t, const_fl_spin_vector_ref wm) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(t=%6.2fs)"), t); + static fl_spin_vector wf; + fl_real weight; + fl_real update_rate; + fl_real decay_rate; + fl_real fw2; + + fw2 = fl_vector_3d_length2(wf); + weight = exp(-fw2 * __gyro_bias.scale2); + if (__gyro_bias.evidence < GYRO_PLATEAU_EVIDENCE) { + __gyro_bias.evidence += weight; + if (__gyro_bias.evidence > GYRO_PLATEAU_EVIDENCE) + __gyro_bias.evidence = GYRO_PLATEAU_EVIDENCE; + __gyro_bias.weight_norm = 1.0 / __gyro_bias.evidence; + __gyro_bias.scale2 = GYRO_SCALE2_I * (1.0 + fl_square(__gyro_bias.evidence) * __gyro_alpha); + } + update_rate = weight * __gyro_bias.weight_norm; + decay_rate = 1.0 - update_rate; + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(t=%6.2fs), fw2=%g, e=%g, s2=%g, w=%g"), t, fw2, __gyro_bias.evidence, __gyro_bias.scale2, weight); + +# define UPDATE_VALUE(i) __gyro_bias.value[i] = decay_rate * __gyro_bias.value[i] + update_rate * wm[i]; + UPDATE_VALUE(GEO_SPATIAL_X); + UPDATE_VALUE(GEO_SPATIAL_Y); + UPDATE_VALUE(GEO_SPATIAL_Z); +# undef UPDATE_VALUE + + wf[GEO_SPATIAL_X] = wm[GEO_SPATIAL_X] - __gyro_bias.value[GEO_SPATIAL_X]; + wf[GEO_SPATIAL_Y] = wm[GEO_SPATIAL_Y] - __gyro_bias.value[GEO_SPATIAL_Y]; + wf[GEO_SPATIAL_Z] = wm[GEO_SPATIAL_Z] - __gyro_bias.value[GEO_SPATIAL_Z]; + + return wf; +} /* fl_gyro_process */ diff --git a/lbs-server/src/location-fused/gyroscope-filter.h b/lbs-server/src/location-fused/gyroscope-filter.h new file mode 100644 index 0000000..95783cc --- /dev/null +++ b/lbs-server/src/location-fused/gyroscope-filter.h @@ -0,0 +1,109 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file gyroscope-filter.h + * @brief Correction of the gyroscope bias. + */ + +#pragma once +#ifndef __FL_GYROSCOPE_FILTER_H__ +#define __FL_GYROSCOPE_FILTER_H__ + +#include "types.h" +#include "parameters.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************//** + * [public] Initializaton of the singleton unit (static constructor). + */ +void fl_gyro_init(void); + +/***************************************************************************//** + * [public] Cleanup of the singleton unit (static destructor). + */ +void fl_gyro_exit(void); + +/***************************************************************************//** + * [public] Reset of the internal state back to initial one. + */ +void fl_gyro_reset(void); + +/***************************************************************************//** + * [public] Bring the filter to a stationary state (EMA mode, input = output). + * This method is used for testing. + * + * @param[in] w0 + * 3D vector of angular velocity measured by the gyroscope in inertial + * coordinate frame. + */ +void fl_gyro_set(const_fl_spin_vector_ref w0); + +/***************************************************************************//** + * [public] Processing of a vector data sample. This method performs: + * - estimation of the gyroscope bias with the weighted AEMA filter; the + * individual samples are weighted by the distance from average using Gaussian + * curve with monotonically-decreasing sigma parameter; + * - correction of the measurement by the estimated bias. + * + * @param[in] t + * Event time in seconds. + * @param[in] wm + * 3D vector of measured angular velocity. + * + * @return + * 3D vector of bias-corrected angular velocity. + */ +const_fl_spin_vector_ref fl_gyro_process(fl_seconds t, const_fl_spin_vector_ref wm); + +#if defined(__FL_GYROSCOPE_FILTER_C__) + #ifdef GTEST + #define PRIVATE + #else + #define PRIVATE \ + static + #endif +#else + #define PRIVATE \ + extern +#endif + +/** Weighted AEMA filter. This is generalization of the 3D AEMA filter by + * using 'evidence' as the sum of weights rather than sample count. The + * sample weights are estimated using Gaussian curve with time-decreasing + * variance. + */ +typedef struct { + fl_real scale2; /* = 1 / (2 variance) */ + fl_real evidence; /* min(accumulated weight, plateau_evidence) */ + fl_real weight_norm; /* = 1 / evidence */ + fl_vector_3d value; /* filter output (estimated value) */ +} fl_waema3d_estimator; + +PRIVATE fl_waema3d_estimator __gyro_bias; + +#if !defined(__FL_GYROSCOPE_FILTER_C__) + #undef PRIVATE +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __FL_GYROSCOPE_FILTER_H__ */ diff --git a/lbs-server/src/location-fused/kalman-filter.c b/lbs-server/src/location-fused/kalman-filter.c new file mode 100644 index 0000000..9f10f0f --- /dev/null +++ b/lbs-server/src/location-fused/kalman-filter.c @@ -0,0 +1,1130 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +#if !defined(LOG_THRESHOLD) + /* custom logging threshold - keep undefined on the repo */ + /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ +#endif + +#define __FL_KALMAN_FILTER_C__ + +#include +#include +#include "math-ext.h" +#include "earth.h" +#include "conversions.h" +#include "accelerometer-filter.h" +#include "gyroscope-filter.h" +#include "aema-estimator.h" +#include "kalman-filter.h" +#include "debug_util.h" + +#define TIME_FORMAT "%.3fs" + +#define PRECISION 1e-4 +#define PRECISION2 SQUARE(PRECISION) + +/* the bigger the default standard deviantions, the swifter response to first measurements */ +#define INITIAL_POSITION_SIGMA2 SQUARE(1024 * M_2PI * EARTH_RADIUS) +#define INITIAL_VELOCITY_SIGMA2 (1024 * 1024 * SQUARE(FL_DEFAULT_VELOCITY_SIGMA)) +#define INITIAL_ACCELERATION_SIGMA2 DEFAULT_ACCELERATION_SIGMA2 + +#define __init_time_offset() fl_aema1d_init(&__timestamp_offset, 1 << FL_TIMO_AEMA_PLATEAU_BITS) +#define __exit_time_offset() fl_aema1d_exit(&__timestamp_offset) +#define __new_time_offset(dt) fl_aema1d_process(&__timestamp_offset, dt) +#define __get_time_offset() fl_aema1d_get_estimate(&__timestamp_offset) + +PRIVATE void __rectify_state(_fl_kalf_state *src) +{ + unsigned i; + + for (i = GEO_SPATIAL_DIMENSION; i--;) { + _fl_kalf_sigma_matrix_ref s2xi; + fl_real uuvv, uvvu; + + s2xi = src->s2x[i]; + /* diagonal elements */ + #define CHECK(u) \ + if (s2xi[STATE_##u][STATE_##u] < 0) { \ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(src=%p): src.s2x[%u][" #u "][" #u "] = %g < 0"), src, i, s2xi[STATE_##u][STATE_##u]); \ + s2xi[STATE_##u][STATE_##u] = 0; \ + } + CHECK(P); + CHECK(V); + CHECK(A); + #undef CHECK + + /* partial determinats */ + #define CHECK(u, v) \ + uuvv = s2xi[STATE_##u][STATE_##u] * s2xi[STATE_##v][STATE_##v]; \ + uvvu = s2xi[STATE_##u][STATE_##v] * s2xi[STATE_##v][STATE_##u]; \ + if (uuvv < uvvu) { \ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(src=%p): det(src.s2x[%u], " #u ", " #v ") = %g < 0"), src, i, uuvv - uvvu); \ + s2xi[STATE_##u][STATE_##v] = \ + s2xi[STATE_##v][STATE_##u] *= sqrt(uuvv / uvvu); \ + } + CHECK(P, V); + CHECK(P, A); + CHECK(A, V); + #undef CHECK + } +} /* __rectify_state */ + +void fl_kalf_init( + motion_state_changed_cb on_motion_state_changed, + location_changed_cb on_location_changed, + gpointer handler) +{ + LOG_FUSED(DBG_LOW, ENTER_FUNCTION_PREFIX("(motion_cb=%p, location_cb=%p, interval=%gs, handler=%p)"), + on_motion_state_changed, + on_location_changed, + handler); + + fl_accel_init(on_motion_state_changed, handler); + fl_gyro_init(); + __init_time_offset(); + __started = FALSE; + __on_location_changed = on_location_changed; + __handler = handler; + fl_kalf_reset(); + LOG_FUSED(DBG_LOW, LEAVE_FUNCTION_PREFIX("()")); +} /* fl_kalf_init */ + +void fl_kalf_exit(void) +{ + LOG_FUSED(DBG_LOW, ENTER_FUNCTION_PREFIX("()"), 0); + __exit_time_offset(); + fl_gyro_exit(); + fl_accel_exit(); + LOG_FUSED(DBG_LOW, LEAVE_FUNCTION_PREFIX("()"), 0); +} /* fl_kalf_exit */ + +void fl_kalf_start(void) +{ + if (__on_location_changed) { + if (__started == FALSE) { + __started = TRUE; + LOG_FUSED(DBG_LOW, LEAVE_FUNCTION_PREFIX("(): OK"), 0); + return; + } else { + LOG_FUSED(DBG_ERR, LEAVE_FUNCTION_PREFIX("ALREADY_STARTED")); + return; + } + } else { + LOG_FUSED(DBG_ERR, LEAVE_FUNCTION_PREFIX("UNINITIALIZED")); + return; + } +} /* fl_kalf_start */ + +void fl_kalf_stop(void) +{ + if (__started) { + __started = FALSE; + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(): OK"), 0); + } else { + LOG_FUSED(DBG_ERR, FUNCTION_PREFIX("(): E_NOT_STARTED"), 0); + } +} /* fl_kalf_stop */ + +void fl_kalf_reset(void) +{ + unsigned i; + + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("()"), 0); + __last_notification.time = FL_UNDEFINED_TIME; + __time_of.sensor_event.first = FL_UNDEFINED_TIME; + __time_of.sensor_event.last = FL_UNDEFINED_TIME; + __time_of.sensor_event.count = 0; + __time_of.last_measurement = FL_UNDEFINED_TIME; + __time_of.last_p_fixing = FL_UNDEFINED_TIME; + __time_of.last_v_fixing = FL_UNDEFINED_TIME; + __time_of.last_a_fixing = FL_UNDEFINED_TIME; + __time_of.last_r_fixing = FL_UNDEFINED_TIME; + __time_of.last_r_diffusion = FL_UNDEFINED_TIME; + __time_of.last_w_fixing = FL_UNDEFINED_TIME; +#if (FL_KALMAN_ANGULAR_VELOCITY) + __time_of.last_wz_fixing = FL_UNDEFINED_TIME; + __time_of.last_b_fixing = FL_UNDEFINED_TIME; +#endif + __time_of.last_reference = FL_UNDEFINED_TIME; + + memset(&__sp, 0, sizeof(__sp)); + memset(&__se, 0, sizeof(__se)); + memset(__wp, 0, sizeof(__wp)); + memset(__we, 0, sizeof(__we)); + memset(&__sre, 0, sizeof(__sre)); + memset(&__srp, 0, sizeof(__srp)); + __srp.w2[GEO_SPATIAL_X] = M_1_PI / 3; + __srp.w2[GEO_SPATIAL_Y] = M_1_PI / 3; + __srp.w2[GEO_SPATIAL_Z] = M_1_PI / 3; + __sre.w2[GEO_SPATIAL_X] = M_1_PI / 3; + __sre.w2[GEO_SPATIAL_Y] = M_1_PI / 3; + __sre.w2[GEO_SPATIAL_Z] = M_1_PI / 3; + __s2wp = DEFAULT_ROTATION_SIGMA2; + __s2we = DEFAULT_ROTATION_SIGMA2; + __wze = 0; +#if (FL_KALMAN_ANGULAR_VELOCITY) + __s2wze = DEFAULT_ROTATION_SIGMA2; + __be = 0; +#endif + + memset(&__te, 0, sizeof(__te)); + __sp.x[GEO_SPATIAL_Z][STATE_P] = EARTH_RADIUS; + __se.x[GEO_SPATIAL_Z][STATE_P] = EARTH_RADIUS; + for (i = GEO_SPATIAL_DIMENSION; i;) { + --i; + /* diagonal terms */ + __sp.s2x[i][STATE_P][STATE_P] = INITIAL_POSITION_SIGMA2; + __sp.s2x[i][STATE_V][STATE_V] = INITIAL_VELOCITY_SIGMA2; + __sp.s2x[i][STATE_A][STATE_A] = INITIAL_ACCELERATION_SIGMA2; + __se.s2x[i][STATE_P][STATE_P] = INITIAL_POSITION_SIGMA2; + __se.s2x[i][STATE_V][STATE_V] = INITIAL_VELOCITY_SIGMA2; + __se.s2x[i][STATE_A][STATE_A] = INITIAL_ACCELERATION_SIGMA2; + __sre.m[i][i] = 1; + __srp.m[i][i] = 1; + __te.fsr[i][i] = 1; + } + __te.crx = 1; + /* pass-throughs */ + __fix_status = LOCATION_STATUS_NO_FIX; +} /* fl_kalf_reset */ + +PRIVATE fl_seconds __timestamp_to_seconds(fl_timestamp timestamp) +{ + __time_of.last_measurement = fl_timestamp_to_seconds(timestamp); + if (__time_of.sensor_event.last != FL_UNDEFINED_TIME) { + fl_seconds dt; + + dt = __time_of.last_measurement - __get_time_offset() - __time_of.sensor_event.last; + if (dt < 0) { + __new_time_offset(__time_of.last_measurement - __time_of.sensor_event.last); + dt = 0; + } else if (__time_of.sensor_event.count > 1) { + fl_seconds mean_sensor_dt = (__time_of.sensor_event.last - __time_of.sensor_event.first) / (__time_of.sensor_event.count - 1); + + if (dt > mean_sensor_dt) { + __new_time_offset(__time_of.last_measurement - __time_of.sensor_event.last - mean_sensor_dt); + dt = mean_sensor_dt; + } + } + return __time_of.sensor_event.last + dt; + } + return __time_of.last_measurement; +} /* __timestamp_to_seconds */ + +PRIVATE void __matrix_3d_multiply_matrix_3d(const_fl_matrix_3d_ref m_1, const_fl_matrix_3d_ref m_2, fl_matrix_3d_ref out) +{ + unsigned i, j; + + i = GEO_SPATIAL_DIMENSION; + do { + --i; + j = GEO_SPATIAL_DIMENSION; + do { + --j; + out[i][j] = m_1[i][GEO_SPATIAL_X] * m_2[GEO_SPATIAL_X][j] + + m_1[i][GEO_SPATIAL_Y] * m_2[GEO_SPATIAL_Y][j] + + m_1[i][GEO_SPATIAL_Z] * m_2[GEO_SPATIAL_Z][j]; + } while (j); + } while (i); +} /* __matrix_3d_multiply_matrix_3d */ + +PRIVATE void __create_push_forward_map(_fl_kalf_state_matrix_ref x, LocationStatus status) +{ + fl_real crx, srx; + fl_real cry, sry; + fl_real yz2, r2; + fl_vector_3d p; + fl_vector_3d v; + + /* pull-back: from the old tangent frame to global coordinates */ + p[GEO_SPATIAL_X] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X]; + p[GEO_SPATIAL_Y] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y]; + p[GEO_SPATIAL_Z] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z]; + v[GEO_SPATIAL_X] = x[GEO_SPATIAL_X][STATE_V] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] + x[GEO_SPATIAL_Y][STATE_V] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] + x[GEO_SPATIAL_Z][STATE_V] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X]; + v[GEO_SPATIAL_Y] = x[GEO_SPATIAL_X][STATE_V] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Y][STATE_V] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Z][STATE_V] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y]; + v[GEO_SPATIAL_Z] = x[GEO_SPATIAL_X][STATE_V] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Y][STATE_V] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Z][STATE_V] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z]; + + yz2 = fl_square(p[GEO_SPATIAL_Y]) + fl_square(p[GEO_SPATIAL_Z]); + r2 = yz2 + fl_square(p[GEO_SPATIAL_X]); + if (r2 > PRECISION2) { + fl_real _r = 1.0 / sqrt(r2); + fl_real yz = sqrt(yz2); + + __te.rx = atan2(p[GEO_SPATIAL_X], yz); + __te.srx = srx = _r * p[GEO_SPATIAL_X]; + __te.crx = crx = _r * yz; + if (yz > PRECISION) { + fl_real _yz = 1.0 / yz; + + __te.ry = atan2(p[GEO_SPATIAL_Y], p[GEO_SPATIAL_Z]); + sry = _yz * p[GEO_SPATIAL_Y]; + cry = _yz * p[GEO_SPATIAL_Z]; + } else { +UndefinedLongitude: + __te.ry = 0; + sry = 0; + cry = 1; + } + } else { + __te.rx = 0; + __te.srx = srx = 0; + __te.crx = crx = 1; + goto UndefinedLongitude; + } + /* new push-forward map */ + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] = crx; + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] = 0; + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X] = srx; + + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] = -sry * srx; + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] = cry; + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y] = sry * crx; + + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] = -cry * srx; + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] = -sry; + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z] = cry * crx; + /* push-forward: from the global onto the new tangent frame */ + x[GEO_SPATIAL_X][STATE_P] = __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] * p[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] * p[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] * p[GEO_SPATIAL_Z]; + x[GEO_SPATIAL_Y][STATE_P] = __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] * p[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * p[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * p[GEO_SPATIAL_Z]; + x[GEO_SPATIAL_X][STATE_V] = __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] * v[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] * v[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] * v[GEO_SPATIAL_Z]; + x[GEO_SPATIAL_Y][STATE_V] = __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] * v[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * v[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * v[GEO_SPATIAL_Z]; + x[GEO_SPATIAL_Z][STATE_P] = __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X] * p[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * p[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * p[GEO_SPATIAL_Z]; + x[GEO_SPATIAL_Z][STATE_V] = __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X] * v[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * v[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * v[GEO_SPATIAL_Z]; +} /* __create_push_forward_map */ + +PRIVATE void __spherical_to_tangent( + const fl_spherical_position *pos, + const fl_tangent_velocity *vel, + const fl_sigma *sigma, + fl_position_vector_ref pm, + fl_velocity_vector_ref vm, + fl_vector_3d_ref s2pm, + fl_vector_3d_ref s2vm) +{ + fl_real rp, rx, ry, rb; + fl_real crx, srx; + fl_position_vector pg; + int spatial; + + spatial = (pos->status == LOCATION_STATUS_3D_FIX); + /* pass-through */ + __fix_status = pos->status; + /* global coordinates */ + rx = fl_degrees_to_radians(pos->latitude); + ry = fl_degrees_to_radians(pos->longitude); + rp = spatial ? EARTH_RADIUS + pos->altitude : __se.x[GEO_SPATIAL_Z][STATE_P]; + srx = rp * sin(rx); + crx = rp * cos(rx); + pg[GEO_SPATIAL_X] = srx; + pg[GEO_SPATIAL_Y] = crx * sin(ry); + pg[GEO_SPATIAL_Z] = crx * cos(ry); + /* push-forward map: new position onto the current tangent space */ + pm[GEO_SPATIAL_X] = __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] * pg[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] * pg[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] * pg[GEO_SPATIAL_Z]; + pm[GEO_SPATIAL_Y] = __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] * pg[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * pg[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * pg[GEO_SPATIAL_Z]; + pm[GEO_SPATIAL_Z] = __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X] * pg[GEO_SPATIAL_X] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * pg[GEO_SPATIAL_Y] + __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * pg[GEO_SPATIAL_Z]; + + fl_real cd, cd2, sd2; + cd = pm[GEO_SPATIAL_Z] / rp; + cd2 = SQUARE(cd); + sd2 = 1 - cd2; + if (sd2 < 0) { + cd2 = 1; + sd2 = 0; + } + /* Notice: in principle we should rotate the diagonal sigma matrix here (two + * matrix multiplications). This approximation assumes the old and new points + * are close. */ + if (sigma->of_altitude > 0) { + fl_real s2h, s2v; + + s2h = fl_square(sigma->of_horizontal_pos); + s2v = fl_square(sigma->of_altitude); + s2pm[GEO_SPATIAL_X] = + s2pm[GEO_SPATIAL_Y] = cd2 * s2h + sd2 * s2v; + s2pm[GEO_SPATIAL_Z] = sd2 * s2h + cd2 * s2v; + } else { + s2pm[GEO_SPATIAL_X] = + s2pm[GEO_SPATIAL_Y] = + s2pm[GEO_SPATIAL_Z] = fl_square(sigma->of_horizontal_pos); + } + + if (vel && vm) { + fl_velocity vl; /* velocity vector length */ + + /* The velocity is given in the tangent frame of p (which is different than + * the currecnt tangent one). In principle we should pull v back into the + * global frame using p as the transformation generator, and then push it + * forward onto our frame. Except first time when the two frames are + * randomly far apart this composition is almost always near-identity, thus + * can safely be approximated with tiny correction to the bearing. + */ + rb = fl_degrees_to_radians(vel->direction) + - atan2(pm[GEO_SPATIAL_Y] * __te.srx, + -pm[GEO_SPATIAL_X] * __te.srx + EARTH_RADIUS * __te.crx); + vl = fl_km_h_to_m_s(vel->speed); + vm[GEO_SPATIAL_X] = vl * cos(rb); + vm[GEO_SPATIAL_Y] = vl * sin(rb); + vm[GEO_SPATIAL_Z] = spatial ? fl_km_h_to_m_s(vel->climb) : __se.x[GEO_SPATIAL_Z][STATE_V]; + if (sigma->of_climb > 0) { + s2vm[GEO_SPATIAL_X] = + s2vm[GEO_SPATIAL_Y] = (cd2 * fl_square(sigma->of_speed) + sd2 * fl_square(sigma->of_climb)); + s2vm[GEO_SPATIAL_Z] = (sd2 * fl_square(sigma->of_speed) + cd2 * fl_square(sigma->of_climb)); + } else { + s2vm[GEO_SPATIAL_X] = + s2vm[GEO_SPATIAL_Y] = + s2vm[GEO_SPATIAL_Z] = fl_square(sigma->of_speed); + } + } +} /* __spherical_to_tangent */ + +PRIVATE void __tangent_to_spherical( + const _fl_kalf_state_matrix_ref x, + const _fl_kalf_sigma_cube_ref s2x, + fl_spherical_position* pos, + fl_tangent_velocity* vel, + fl_sigma* sigma) +{ + if (pos) { + fl_position_vector p; + + /* pull-back map: from the tangent frame to global coordinates */ + p[GEO_SPATIAL_X] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X]; + p[GEO_SPATIAL_Y] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y]; + p[GEO_SPATIAL_Z] = x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z]; + + fl_real yz2, yz; + fl_real xyz2; + fl_radians rx; + fl_radians ry; + + yz2 = fl_square(p[GEO_SPATIAL_Y]) + fl_square(p[GEO_SPATIAL_Z]); + yz = sqrt(yz2); + if (yz2 > PRECISION2) + ry = atan2(p[GEO_SPATIAL_Y], p[GEO_SPATIAL_Z]); + else + ry = 0; /* avoid #IND values */ + xyz2 = yz2 + fl_square(p[GEO_SPATIAL_X]); + if (xyz2 > PRECISION2) + rx = atan2(p[GEO_SPATIAL_X], yz); + else + rx = 0; /* avoid #IND values */ + pos->timestamp = fl_seconds_to_timestamp(__last_notification.time > FL_UNDEFINED_TIME ? __last_notification.time : 0); + pos->latitude = fl_radians_to_latitude(rx); + pos->longitude = fl_radians_to_longitude(ry); + pos->altitude = sqrt(xyz2) - EARTH_RADIUS; + pos->status = __fix_status; + + fl_real rb; + fl_real u[PLANAR_DIMENSION]; + + u[PLANAR_X] = EARTH_RADIUS * __te.crx - x[GEO_SPATIAL_X][STATE_P] * __te.srx; + u[PLANAR_Y] = - x[GEO_SPATIAL_Y][STATE_P] * __te.srx; + rb = -atan2(x[GEO_SPATIAL_X][STATE_V] * u[PLANAR_Y] - x[GEO_SPATIAL_Y][STATE_V] * u[PLANAR_X], + x[GEO_SPATIAL_X][STATE_V] * u[PLANAR_X] + x[GEO_SPATIAL_Y][STATE_V] * u[PLANAR_Y]); + + if (vel) { + vel->timestamp = pos->timestamp; + vel->speed = fl_m_s_to_km_h(sqrt(fl_square(x[GEO_SPATIAL_Y][STATE_V]) + fl_square(x[GEO_SPATIAL_X][STATE_V]))); + vel->direction = fl_radians_to_cog(rb); + vel->climb = fl_m_s_to_km_h(x[GEO_SPATIAL_Z][STATE_V]); + } + if (sigma) { + sigma->level = LOCATION_ACCURACY_LEVEL_NONE; + sigma->of_horizontal_pos = sqrt((s2x[GEO_SPATIAL_X][STATE_P][STATE_P] + s2x[GEO_SPATIAL_Y][STATE_P][STATE_P]) * 0.5); + sigma->of_altitude = s2x[GEO_SPATIAL_Z][STATE_P][STATE_P]; + sigma->of_speed = sqrt((s2x[GEO_SPATIAL_X][STATE_V][STATE_V] + s2x[GEO_SPATIAL_Y][STATE_V][STATE_V]) * 0.5); + sigma->of_climb = s2x[GEO_SPATIAL_Z][STATE_V][STATE_V]; + } + } +} /* __tangent_to_spherical */ + +PRIVATE void __kalman_2d1o(fl_seconds t, _fl_kalf_state_component s, const_fl_vector_3d_ref xm, const_fl_vector_3d_ref s2xm) +{ + unsigned i; + + /* @note WPS-specific: restrict the update to horizontal position */ + i = PLANAR_DIMENSION; + do { + --i; + const_fl_vector_3d_ref s2pis; + _fl_kalf_state_matrix_ref s2ei; + _fl_kalf_state_matrix_ref s2pi; + fl_real s2k, s2mi, s2piss; + + s2mi = s2xm[i]; + s2ei = __se.s2x[i]; + s2pi = __sp.s2x[i]; + s2pis = s2pi[s]; + s2piss = s2pis[s]; + s2k = s2mi + s2piss; + if (s2k > PRECISION2) { + fl_real inv_s2k; + fl_real dx; + + inv_s2k = 1.0 / s2k; + dx = xm[i] - __sp.x[i][s]; + + /* reduced Joseph form in special 3x1 case: subtract scaled projection */ + #define SET_FIELD(a, b) s2ei[a][b] = inv_s2k * ((s2pi[a][b] * s2piss - s2pis[a] * s2pis[b]) + s2pi[a][b] * s2mi) + #define CPY_FIELD(a, b) s2ei[a][b] = s2ei[b][a] + SET_FIELD(STATE_P, STATE_P); + SET_FIELD(STATE_P, STATE_V); + SET_FIELD(STATE_P, STATE_A); + CPY_FIELD(STATE_V, STATE_P); + SET_FIELD(STATE_V, STATE_V); + SET_FIELD(STATE_V, STATE_A); + CPY_FIELD(STATE_A, STATE_P); + CPY_FIELD(STATE_A, STATE_V); + SET_FIELD(STATE_A, STATE_A); + #undef CPY_FIELD + #undef SET_FIELD + + #define SET_FIELD(a) __se.x[i][a] = __sp.x[i][a] + dx * (inv_s2k * s2pis[a]) + SET_FIELD(STATE_P); + SET_FIELD(STATE_V); + SET_FIELD(STATE_A); + #undef SET_FIELD + } + } while (i); + /* the third coordinate is of `special care': we want to preserve it in the tangent frame, along with its sigma */ + if (s == STATE_P) + __se.x[GEO_SPATIAL_Z][STATE_P] = xm[GEO_SPATIAL_Z]; + + __copy_state(&__sp, &__se); + __time_of.last_reference = t; +} // __kalman_2d1o + +/* Update of the 2D position */ +PRIVATE void __kalman_p(fl_seconds t, const_fl_position_vector_ref pm, const_fl_vector_3d_ref s2pm) +{ + __predict_pv(t); + __kalman_2d1o(t, STATE_P, pm, s2pm); + __time_of.last_p_fixing = t; + __time_of.last_reference = t; +} /* __kalman_p */ + +/* Update of the 3D position & velocity */ +PRIVATE void __kalman_pv( + fl_seconds t, + const_fl_position_vector_ref pm, /* p-measurement */ + const_fl_velocity_vector_ref vm, /* v-measurement */ + const_fl_vector_3d_ref s2pm, /* p-measurement variance */ + const_fl_vector_3d_ref s2vm /* v-measurement variance */ +) { + unsigned i; + + __predict_pv(t); + for (i = GEO_SPATIAL_DIMENSION; i;) { + --i; + + _fl_kalf_matrix_2d s2k; + fl_real det; + /* shortcuts */ + _const_fl_kalf_sigma_matrix_ref s2xpi = __sp.s2x[i]; + const fl_real *xpi = __sp .x[i]; + + /* Sk = Sp + Sm */ + s2k[STATE_P][STATE_P] = s2xpi[STATE_P][STATE_P] + s2pm[i]; + s2k[STATE_P][STATE_V] = /* == s2pi[STATE_P][STATE_V] */ + s2k[STATE_V][STATE_P] = s2xpi[STATE_V][STATE_P]; + s2k[STATE_V][STATE_V] = s2xpi[STATE_V][STATE_V] + s2vm[i]; + det = s2k[STATE_P][STATE_P] * s2k[STATE_V][STATE_V] - s2k[STATE_V][STATE_P] * s2k[STATE_P][STATE_V]; + if (fl_square(det) > PRECISION2) { + fl_real denom; + _fl_kalf_matrix_2d s2i; + _fl_kalf_matrix_2d k0, k1; + + /* Si = Sk^{-1} */ + denom = 1.0 / det; + s2i[STATE_P][STATE_P] = denom * s2k[STATE_V][STATE_V]; + s2i[STATE_P][STATE_V] = + s2i[STATE_V][STATE_P] = -denom * s2k[STATE_V][STATE_P]; + s2i[STATE_V][STATE_V] = denom * s2k[STATE_P][STATE_P]; + + /* K = Sp.Si */ + #define SET_K0(j, k) k0[j][k] = s2xpi[j][STATE_P] * s2i[STATE_P][k] + s2xpi[j][STATE_V] * s2i[STATE_V][k] + SET_K0(STATE_P, STATE_P); + SET_K0(STATE_P, STATE_V); + SET_K0(STATE_V, STATE_P); + SET_K0(STATE_V, STATE_V); + #undef SET_K0 + /* 1 - K = Sm.Si, use positive form */ + k1[STATE_P][STATE_P] = s2pm[i] * s2i[STATE_P][STATE_P]; + k1[STATE_P][STATE_V] = s2pm[i] * s2i[STATE_P][STATE_V]; + k1[STATE_V][STATE_P] = s2vm[i] * s2i[STATE_V][STATE_P]; + k1[STATE_V][STATE_V] = s2vm[i] * s2i[STATE_V][STATE_V]; + /* state update */ + __se.x[i][STATE_P] = k0[STATE_P][STATE_P] * pm[i] + k0[STATE_P][STATE_V] * vm[i] + + k1[STATE_P][STATE_P] * xpi[STATE_P] + k1[STATE_P][STATE_V] * xpi[STATE_V]; + __se.x[i][STATE_V] = k0[STATE_V][STATE_P] * pm[i] + k0[STATE_V][STATE_V] * vm[i] + + k1[STATE_V][STATE_P] * xpi[STATE_P] + k1[STATE_V][STATE_V] * xpi[STATE_V]; + /* covariance update Se = K1,K1.Sp + K,K.Sm */ + _fl_kalf_sigma_matrix_ref s2ei = __se.s2x[i]; + + #define SET_S2E(j, k) s2ei[j][k] = \ + k1[j][STATE_P] * k1[k][STATE_P] * s2xpi[STATE_P][STATE_P] + k0[j][STATE_P] * k0[k][STATE_P] * s2pm[i] \ + + k1[j][STATE_P] * k1[k][STATE_V] * s2xpi[STATE_P][STATE_V] \ + + k1[j][STATE_V] * k1[k][STATE_P] * s2xpi[STATE_V][STATE_P] \ + + k1[j][STATE_V] * k1[k][STATE_V] * s2xpi[STATE_V][STATE_V] + k0[j][STATE_V] * k0[k][STATE_V] * s2vm[i] + + SET_S2E(STATE_P, STATE_P); + SET_S2E(STATE_P, STATE_V); + SET_S2E(STATE_V, STATE_P); + SET_S2E(STATE_V, STATE_V); + #undef SET_S2E + } + /* upon arrival of new position-velocity pair (a maximally uncertain) the pair (p, v) decorrelates from a: */ + __se.s2x[i][STATE_P][STATE_A] = 0; + __se.s2x[i][STATE_V][STATE_A] = 0; + __se.s2x[i][STATE_A][STATE_P] = 0; + __se.s2x[i][STATE_A][STATE_V] = 0; + __se.s2x[i][STATE_A][STATE_A] = __sp.s2x[i][STATE_A][STATE_A]; + } + __copy_state(&__sp, &__se); + __time_of.last_p_fixing = t; + __time_of.last_v_fixing = t; + __time_of.last_reference = t; +} /* __kalman_pv */ + +#if (FL_KALMAN_ANGULAR_VELOCITY) +void __kalman_wz(fl_seconds t, fl_real wzm, fl_real s2wzm) +{ + fl_real s2wzp; + + if (__time_of.last_wz_fixing > FL_UNDEFINED_TIME) { + fl_real dt = t - __time_of.last_wz_fixing; + fl_real s2wzk; + #define wzp __wze /* constant prediction */ + + /* propagate/diffuse sigma^2 */ + s2wzp = __s2wze + dt * SQUARE(FL_DEFAULT_SPIN_SIGMA); + s2wzk = s2wzp + s2wzm; + if (s2wzk > PRECISION2) { + fl_real inv_s2k; + + inv_s2k = 1.0 / s2wzk; + LOG_FUSED(DBG_INFO, FUNCTION_PREFIX("(t=" TIME_FORMAT ", w=%.4g, s2w=%.4g): __wze:%g->%g, __s2wze:%g->%g"), + t, wzm, s2wzm, __wze, inv_s2k * (s2wzp * wzm + s2wzm * wzp), + __s2wze, inv_s2k * s2wzp * s2wzm * 2); + __s2wze = inv_s2k * s2wzp * s2wzm * 2; + __wze = inv_s2k * (s2wzp * wzm + s2wzm * wzp); + } else { + __wze = wzm; + __s2wze = s2wzm; + } + #undef wzp + } else { + __wze = wzm; + __s2wze = s2wzm; + } + __time_of.last_wz_fixing = t; +} /* __kalman_wz */ +#endif + +/* Blend two unit 3D vectors usign inverse covariances as weights (to avoid singularities) */ +PRIVATE void __inv_kalman_u( + fl_seconds t, + const_fl_vector_3d_ref u1, fl_real w2u1, + const_fl_vector_3d_ref u2, fl_real w2u2, + fl_vector_3d_ref out, fl_real *w2out) +{ + unsigned i; + + *w2out = 0.5 * (w2u1 + w2u2); + if (*w2out > PRECISION2) { + fl_real e2 = 0; + fl_real _w2out = 1.0 / (*w2out); + + i = GEO_SPATIAL_DIMENSION; + do { + fl_real e; + --i; + out[i] = e = _w2out * (u1[i] * w2u1 + u2[i] * w2u2); + e2 += SQUARE(e); + } while (i); + + if (e2 > PRECISION2) { + fl_real _e; + + /* normalize to unity */ + _e = 1.0 / sqrt(e2); + i = GEO_SPATIAL_DIMENSION; + do + out[--i] *= _e; + while (i); + } + } else { + i = GEO_SPATIAL_DIMENSION; + do + out[--i] = 0; + while (i); + } +} /* __inv_kalman_u */ + +/* 3D cross-product with normalization */ +PRIVATE void __cross_product(const_fl_vector_3d_ref in1, const_fl_vector_3d_ref in2, fl_vector_3d_ref out) +{ + fl_real out2; + + /* The cross-product preserves system handedness: it's right-handed in the + * right-handed system, and left-handed in the left-handed one. + */ + VECTOR_CROSS_VECTOR_3D(in1, in2, out); + + out2 = fl_square(out[GEO_SPATIAL_X]) + + fl_square(out[GEO_SPATIAL_Y]) + + fl_square(out[GEO_SPATIAL_Z]); + if (fl_square(out2 - 1) > PRECISION2) { + fl_real _out = 1.0 / sqrt(out2); + /* normalize */ + out[GEO_SPATIAL_X] *= _out; + out[GEO_SPATIAL_Y] *= _out; + out[GEO_SPATIAL_Z] *= _out; + } +} /* __cross_product */ + +PRIVATE void __signal_updated_location(fl_seconds t) +{ + if (FL_MIN_NOTIFICATION_FILTRATION && __started && __time_of.last_p_fixing > FL_UNDEFINED_TIME) { + t += __get_time_offset(); + if ((t - __last_notification.time) * 1000 >= FL_MIN_NOTIFICATION_INTERVAL) { + __get_prediction_vector(t, &__last_notification); + __on_location_changed(&__last_notification, __handler); + } + } +} /* __signal_updated_location */ + +/* Filter input: Enter the raw position measurement */ +void fl_kalf_process_position_2d_event(const fl_spherical_position* pos, const fl_sigma* sigma) +{ + if (pos && sigma) { + if (pos->status >= LOCATION_STATUS_2D_FIX) { + fl_seconds t; + fl_position_vector pm; /* p-measurement */ + fl_vector_3d s2pm; /* p-measurement variance */ + + t = __timestamp_to_seconds(pos->timestamp); + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", pos=(%g�, %g�), sp=%gm)"), + t, + pos->latitude, + pos->longitude, + sigma->of_horizontal_pos); + __spherical_to_tangent(pos, NULL, sigma, pm, NULL, s2pm, NULL); + /* Kalman merging of 2D position */ + __kalman_p(t, pm, s2pm); + __create_push_forward_map(__se.x, pos->status); + memcpy(__sp.x, __se.x, sizeof(__sp.x)); + __signal_updated_location(t); + } else { + LOG_FUSED(DBG_ERR, FUNCTION_PREFIX("(location.pos.status=%d)"), pos->status); + } + } else { + LOG_FUSED(DBG_ERR, FUNCTION_PREFIX("(%p, %p): INVALID ARGUMENT"), pos, sigma); + } +} /* fl_kalf_process_position_2d_event */ + +/* Filter input: Enter the raw position measurement */ +void fl_kalf_process_position_2x3d_event(const fl_spherical_position *pos, const fl_tangent_velocity *vel, const fl_sigma *sigma) +{ + if (pos && vel && sigma) { + if (pos->status >= LOCATION_STATUS_2D_FIX) { + fl_seconds t; + fl_position_vector pm; /* p-measurement */ + fl_velocity_vector vm; /* v-measurement */ + fl_vector_3d s2pm; /* p-measurement variance */ + fl_vector_3d s2vm; /* v-measurement variance */ + + t = __timestamp_to_seconds(pos->timestamp); + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", pos=(%g�, %g�, %gm), cog=%g�, sog=%gkm/h, climb=%gkm/h, sp=%gm, sv=%gm/s)"), + t, + pos->latitude, + pos->longitude, + pos->altitude, + vel->direction, + vel->speed, + vel->climb, + sigma->of_horizontal_pos, + sigma->of_speed); + __spherical_to_tangent(pos, vel, sigma, pm, vm, s2pm, s2vm); + fl_real vh = fl_km_h_to_m_s(vel->speed); + #if (FL_KALMAN_ANGULAR_VELOCITY) + fl_real vhe = sqrt(fl_square(__se.x[GEO_SPATIAL_X][STATE_V]) + fl_square(__se.x[GEO_SPATIAL_Y][STATE_V])); + fl_real b = fl_degrees_to_radians(vel->direction); + if (vh > PRECISION && __time_of.last_b_fixing > FL_UNDEFINED_TIME) { + fl_real dt = t - __time_of.last_b_fixing; + if (vh * vhe * dt > 0) { + fl_radians db; + fl_real wzm, s2wzm; + + /* circular difference */ + db = b - __be; + if (db > M_PI) + db -= M_2PI; + else if (db < -M_PI) + db += M_2PI; + + wzm = db / dt; + s2wzm = 4 * fl_square(atan2(2 * sqrt(vh * vhe), FL_DEFAULT_VELOCITY_SIGMA)); + __kalman_wz(t, wzm, s2wzm); + } + } + __be = b; + __time_of.last_b_fixing = t; + #endif + __kalman_pv(t, pm, vm, s2pm, s2vm); + __create_push_forward_map(__se.x, pos->status); + memcpy(__sp.x, __se.x, sizeof(__sp.x)); + __signal_updated_location(t); + } else { + LOG_FUSED(DBG_ERR, FUNCTION_PREFIX("(location.pos.status=%d)"), pos->status); + } + } else { + LOG_FUSED(DBG_ERR, FUNCTION_PREFIX("(%p, %p): INVALID ARGUMENT"), pos, sigma); + } +} /* fl_kalf_process_position_2x3d_event */ + +void fl_kalf_get_location(fl_spherical_position* pos, fl_tangent_velocity* vel, fl_sigma* sigma) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(), __last_notification.time=%gs"), __last_notification.time); + __tangent_to_spherical(__sp.x, __sp.s2x, pos, vel, sigma); +} /* fl_kalf_get_location */ + +PRIVATE void __get_prediction_vector(fl_seconds t, fl_position_4d* prediction) +{ + prediction->time = t; + /* pull-back map: from the old tangent frame back to global coordinates */ + prediction->spatial[GEO_SPATIAL_X] = __sp.x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_X] + __sp.x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_X] + __sp.x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_X]; + prediction->spatial[GEO_SPATIAL_Y] = __sp.x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Y] + __sp.x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + __sp.x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Y]; + prediction->spatial[GEO_SPATIAL_Z] = __sp.x[GEO_SPATIAL_X][STATE_P] * __te.fsr[GEO_SPATIAL_X][GEO_SPATIAL_Z] + __sp.x[GEO_SPATIAL_Y][STATE_P] * __te.fsr[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + __sp.x[GEO_SPATIAL_Z][STATE_P] * __te.fsr[GEO_SPATIAL_Z][GEO_SPATIAL_Z]; +} /* __get_prediction_vector */ + +fl_real fl_kalf_get_distance(const_fl_position_vector_ref p, const_fl_position_vector_ref q) +{ + return sqrt(fl_square(p[GEO_SPATIAL_X] - q[GEO_SPATIAL_X]) + + fl_square(p[GEO_SPATIAL_Y] - q[GEO_SPATIAL_Y]) + + fl_square(p[GEO_SPATIAL_Z] - q[GEO_SPATIAL_Z])); +} /* fl_kalf_get_distance */ + +PRIVATE void __quaternion_to_rotation(_const_fl_kalf_quaternion_ref q, fl_matrix_3d_ref out) +{ + out[GEO_SPATIAL_X][GEO_SPATIAL_X] = SQUARE(q[QTR_0]) + SQUARE(q[QTR_X]) - SQUARE(q[QTR_Y]) - SQUARE(q[QTR_Z]); + out[GEO_SPATIAL_X][GEO_SPATIAL_Y] = 2 * (q[QTR_X] * q[QTR_Y] + q[QTR_Z] * q[QTR_0]); + out[GEO_SPATIAL_X][GEO_SPATIAL_Z] = 2 * (q[QTR_X] * q[QTR_Z] - q[QTR_Y] * q[QTR_0]); + + out[GEO_SPATIAL_Y][GEO_SPATIAL_X] = 2 * (q[QTR_Y] * q[QTR_X] - q[QTR_Z] * q[QTR_0]); + out[GEO_SPATIAL_Y][GEO_SPATIAL_Y] = SQUARE(q[QTR_0]) - SQUARE(q[QTR_X]) + SQUARE(q[QTR_Y]) - SQUARE(q[QTR_Z]); + out[GEO_SPATIAL_Y][GEO_SPATIAL_Z] = 2 * (q[QTR_Y] * q[QTR_Z] + q[QTR_X] * q[QTR_0]); + + out[GEO_SPATIAL_Z][GEO_SPATIAL_X] = 2 * (q[QTR_Z] * q[QTR_X] + q[QTR_Y] * q[QTR_0]); + out[GEO_SPATIAL_Z][GEO_SPATIAL_Y] = 2 * (q[QTR_Z] * q[QTR_Y] - q[QTR_X] * q[QTR_0]); + out[GEO_SPATIAL_Z][GEO_SPATIAL_Z] = SQUARE(q[QTR_0]) - SQUARE(q[QTR_X]) - SQUARE(q[QTR_Y]) + SQUARE(q[QTR_Z]); +} /* __quaternion_to_rotation */ + +//PRIVATE void __process_linear_acceleration(fl_seconds t, const_fl_acceleration_vector_ref al) +//{ +// unsigned i; +// +// for (i = GEO_SPATIAL_DIMENSION; i--;) { +// __se. x[i][STATE_A] = 0; +// __se.s2x[i][STATE_A][STATE_A] = DEFAULT_ACCELERATION_SIGMA2; +// } +// __predict_pv(t); +// __time_of.last_a_fixing = t; +// __signal_updated_location(t); +//} /* __process_linear_acceleration */ + +void fl_kalf_process_sensory_event(fl_seconds t, fl_sensory_source sensor_id, const_sensor_vector_3d_ref data) +{ + if (__time_of.sensor_event.count == 0) + __time_of.sensor_event.first = t; + __time_of.sensor_event.last = t; + __time_of.sensor_event.count++; + + if (__time_of.last_measurement > FL_UNDEFINED_TIME) { + fl_seconds dt = t - (__time_of.last_measurement - __get_time_offset()); + if (dt < 0) + __new_time_offset(__time_of.last_measurement - t); + } + if (data) { + switch (sensor_id) { + case RAW_ACCELERATION_SENSOR: { + fl_acceleration_vector am; + fl_acceleration_vector al; + fl_real w2gu; + fl_vector_3d gu; + unsigned i; + + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", RAC, a=(%g, %g, %g)[m/s^2])"), t, data[DEV_SPATIAL_X], data[DEV_SPATIAL_Y], data[DEV_SPATIAL_Z]); + /* extrapolate the rotation matrix */ + __predict_rw(t); + + /* float, math system -> double, geographic system, rotated into local (tangent) frame */ + am[GEO_SPATIAL_X] = __srp.m[GEO_SPATIAL_X][GEO_SPATIAL_X] * data[DEV_SPATIAL_X] + __srp.m[GEO_SPATIAL_X][GEO_SPATIAL_Y] * data[DEV_SPATIAL_Y] + __srp.m[GEO_SPATIAL_X][GEO_SPATIAL_Z] * data[DEV_SPATIAL_Z]; + am[GEO_SPATIAL_Y] = __srp.m[GEO_SPATIAL_Y][GEO_SPATIAL_X] * data[DEV_SPATIAL_X] + __srp.m[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * data[DEV_SPATIAL_Y] + __srp.m[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * data[DEV_SPATIAL_Z]; + am[GEO_SPATIAL_Z] = __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_X] * data[DEV_SPATIAL_X] + __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * data[DEV_SPATIAL_Y] + __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * data[DEV_SPATIAL_Z]; + /* pass through gravity filter to estimate |g| and downward direction */ + fl_accel_process(t, am, al, gu, &w2gu); + if (w2gu > PRECISION2) { + fl_vector_3d gn; + + /* rotate back to device frame */ + VECTOR_DOT_MATRIX_3D(gu, __srp.m, gn); + /* w2gn = w2gu because w2gu is diagonal and srp orthogonal */ + /* update rotation matrix */ + __inv_kalman_u(t, gn, w2gu, __srp.m[GEO_SPATIAL_Z], __srp.w2[GEO_SPATIAL_Z], __sre.m[GEO_SPATIAL_Z], &__sre.w2[GEO_SPATIAL_Z]); + __cross_product(__sre.m[GEO_SPATIAL_Z], __srp.m[GEO_SPATIAL_X], __sre.m[GEO_SPATIAL_Y]); + __cross_product(__sre.m[GEO_SPATIAL_Y], __sre.m[GEO_SPATIAL_Z], __sre.m[GEO_SPATIAL_X]); + + __copy_system_rotation(&__srp, &__sre); + __time_of.last_r_fixing = t; + } + /* process linear acceleration */ + for (i = GEO_SPATIAL_DIMENSION; i--;) { + __se. x[i][STATE_A] = 0; + __se.s2x[i][STATE_A][STATE_A] = DEFAULT_ACCELERATION_SIGMA2; + } + __predict_pv(t); + __time_of.last_a_fixing = t; + __signal_updated_location(t); + // __process_linear_acceleration(t, al); + return; + } + case GYROSCOPE_SENSOR: { + fl_spin_vector wm; + + /* float, degrees, math/device coordinates (right-handed) -> double, radians, geographic/tangent coordinates (left-handed) */ + wm[GEO_SPATIAL_X] = -fl_degrees_to_radians(data[DEV_SPATIAL_X]); + wm[GEO_SPATIAL_Y] = -fl_degrees_to_radians(data[DEV_SPATIAL_Y]); + wm[GEO_SPATIAL_Z] = -fl_degrees_to_radians(data[DEV_SPATIAL_Z]); + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT ", GYR, w=(%g, %g, %g)[rad/s])"), t, wm[GEO_SPATIAL_X], wm[GEO_SPATIAL_Y], wm[GEO_SPATIAL_Z]); + __integrate_rw(t, fl_gyro_process(t, wm), DEFAULT_ROTATION_SIGMA2); + return; + } + default: + /* prevent compiler warning */ + return; + } /* Tizen style: two blocks aligned at the same indent... */ + } + LOG_FUSED(DBG_ERR, FUNCTION_PREFIX("(t=" TIME_FORMAT ", sensor_id=%d, data=%p)"), t, sensor_id, data); +} /* fl_kalf_process_sensory_event */ + +PRIVATE void __integrate_rw(fl_seconds t, const_fl_spin_vector_ref wm, fl_real s2wm) +{ + unsigned i; + + if (__time_of.last_w_fixing > FL_UNDEFINED_TIME) { + fl_seconds dt; + + dt = t - __time_of.last_w_fixing; + if (dt > 0) { + fl_real w2; + + __wp[GEO_SPATIAL_X] = 0.5 * (__we[GEO_SPATIAL_X] + wm[GEO_SPATIAL_X]); + __wp[GEO_SPATIAL_Y] = 0.5 * (__we[GEO_SPATIAL_Y] + wm[GEO_SPATIAL_Y]); + __wp[GEO_SPATIAL_Z] = 0.5 * (__we[GEO_SPATIAL_Z] + wm[GEO_SPATIAL_Z]); + w2 = fl_vector_3d_length2(__wp); + fl_real wz = __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_X] * __wp[GEO_SPATIAL_X] + + __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * __wp[GEO_SPATIAL_Y] + + __srp.m[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * __wp[GEO_SPATIAL_Z]; + + #if (FL_KALMAN_ANGULAR_VELOCITY) + __kalman_wz(t, wz, SQUARE(FL_DEFAULT_SPIN_SIGMA)); + #else + __wze = wz; + #endif + __predict_pv(t); + __se.s2x[GEO_SPATIAL_X][STATE_A][STATE_A] = fl_square(__sp.x[GEO_SPATIAL_X][STATE_A] - __se.x[GEO_SPATIAL_X][STATE_A]); + __se.s2x[GEO_SPATIAL_Y][STATE_A][STATE_A] = fl_square(__sp.x[GEO_SPATIAL_Y][STATE_A] - __se.x[GEO_SPATIAL_Y][STATE_A]); + __copy_state(&__se, &__sp); + __time_of.last_reference = t; + + /* Integrate over dt: We use the fact that for any scalar t the quaternions + * [exp(w), exp(tw)] = 0 + * commute. + */ + fl_real dt2 = dt * dt / 24; + + if (w2 > PRECISION2) { + fl_real w; + fl_real wt, swtw; + _fl_kalf_quaternion qwt; + fl_matrix_3d dsr; + fl_real qwt2; + + w = sqrt(w2); + wt = w * dt * 0.5; + swtw = sin(wt) / w; /* system rotates opposite to the device */ + qwt[QTR_0] = cos(wt); + qwt[QTR_X] = swtw * __wp[GEO_SPATIAL_X] + dt2 * (__we[GEO_SPATIAL_Y] * wm[GEO_SPATIAL_Z] - __we[GEO_SPATIAL_Z] * wm[GEO_SPATIAL_Y]); + qwt[QTR_Y] = swtw * __wp[GEO_SPATIAL_Y] + dt2 * (__we[GEO_SPATIAL_Z] * wm[GEO_SPATIAL_X] - __we[GEO_SPATIAL_X] * wm[GEO_SPATIAL_Z]); + qwt[QTR_Z] = swtw * __wp[GEO_SPATIAL_Z] + dt2 * (__we[GEO_SPATIAL_X] * wm[GEO_SPATIAL_Y] - __we[GEO_SPATIAL_Y] * wm[GEO_SPATIAL_X]); + qwt2 = fl_square(qwt[QTR_0]) + fl_vector_3d_length2(&qwt[QTR_X]); + if (qwt2 > PRECISION2) { + fl_real norm_qwt = 1.0 / sqrt(qwt2); + + qwt[QTR_0] *= norm_qwt; + qwt[QTR_X] *= norm_qwt; + qwt[QTR_Y] *= norm_qwt; + qwt[QTR_Z] *= norm_qwt; + } + __quaternion_to_rotation(qwt, dsr); + __matrix_3d_multiply_matrix_3d(__srp.m, dsr, __sre.m); + __copy_system_rotation(&__srp, &__sre); + __time_of.last_r_fixing = t; + } + } + } + i = GEO_SPATIAL_DIMENSION; + do { + --i; + // Kalmaning the rotation rates would amount to an EMA low-pass filter - we don't want it + __wp[i] = __we[i] = wm[i]; + } while (i); + __s2we = s2wm; + __time_of.last_w_fixing = t; +} /* __integrate_rw */ + +PRIVATE void __predict_rw(fl_seconds t) +{ + unsigned i; + + if (__time_of.last_w_fixing > FL_UNDEFINED_TIME) { + fl_seconds dt; + + dt = t - __time_of.last_w_fixing; + if (dt > 0) { + i = GEO_SPATIAL_DIMENSION; + do { + --i; + __wp[i] = __we[i]; + } while (i); + + /* Integrate over dt: We use the fact that for any scalar t the quaternions + * [exp(w), exp(tw)] = 0 + * commute. + */ + fl_real w2; + + w2 = fl_vector_3d_length2(__wp); + if (w2 > PRECISION2) { + fl_real w; + fl_real wt, swt; + _fl_kalf_quaternion qwt; + fl_matrix_3d srp; /* local copy */ + fl_matrix_3d dsr; + + w = sqrt(w2); + wt = w * dt * 0.5; + swt = sin(wt) / w; + qwt[QTR_0] = cos(wt); + qwt[QTR_X] = swt * __wp[GEO_SPATIAL_X]; + qwt[QTR_Y] = swt * __wp[GEO_SPATIAL_Y]; + qwt[QTR_Z] = swt * __wp[GEO_SPATIAL_Z]; + __quaternion_to_rotation(qwt, dsr); + /* inlined memcpy() */ + i = sizeof(__srp.m) / sizeof(fl_real); + do { + --i; + srp[0][i] = __srp.m[0][i]; + } while (i); + __matrix_3d_multiply_matrix_3d(srp, dsr, __srp.m); + } + } + } + /* otherwise there's no q change */ +} /* __predict_rw */ + +PRIVATE void __predict_pv(fl_seconds t) +{ + fl_seconds dt; + unsigned i; + + if (__time_of.last_reference > FL_UNDEFINED_TIME) + dt = t - __time_of.last_reference; + else + /* no prior data */ + return; + + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(t=" TIME_FORMAT "), dt=" TIME_FORMAT), t, dt); + if (dt > PRECISION2) { + fl_real wt = __wze * dt; + if (fabs(__wze) > PRECISION) { + fl_real cwt, swt; + fl_real cwtw, swtw; + fl_real inv_wz; + + cwt = cos(wt); + swt = sin(wt); + inv_wz = 1.0 / __wze; + cwtw = (cwt - 1) * inv_wz; + swtw = swt * inv_wz; + + __sp.x[GEO_SPATIAL_X][STATE_P] = __se.x[GEO_SPATIAL_X][STATE_V] * swtw + __se.x[GEO_SPATIAL_Y][STATE_V] * cwtw + __se.x[GEO_SPATIAL_X][STATE_P]; + __sp.x[GEO_SPATIAL_Y][STATE_P] = __se.x[GEO_SPATIAL_Y][STATE_V] * swtw - __se.x[GEO_SPATIAL_X][STATE_V] * cwtw + __se.x[GEO_SPATIAL_Y][STATE_P]; + __sp.x[GEO_SPATIAL_X][STATE_V] = __se.x[GEO_SPATIAL_X][STATE_V] * cwt - __se.x[GEO_SPATIAL_Y][STATE_V] * swt; + __sp.x[GEO_SPATIAL_Y][STATE_V] = __se.x[GEO_SPATIAL_Y][STATE_V] * cwt + __se.x[GEO_SPATIAL_X][STATE_V] * swt; + } else { + __sp.x[GEO_SPATIAL_X][STATE_P] = __se.x[GEO_SPATIAL_X][STATE_P] - dt * (wt * __se.x[GEO_SPATIAL_Y][STATE_V] * 0.5 - __se.x[GEO_SPATIAL_X][STATE_V]); + __sp.x[GEO_SPATIAL_Y][STATE_P] = __se.x[GEO_SPATIAL_Y][STATE_P] + dt * (wt * __se.x[GEO_SPATIAL_X][STATE_V] * 0.5 + __se.x[GEO_SPATIAL_Y][STATE_V]); + __sp.x[GEO_SPATIAL_X][STATE_V] = __se.x[GEO_SPATIAL_X][STATE_V] - wt * (wt * __se.x[GEO_SPATIAL_X][STATE_V] * 0.5 + __se.x[GEO_SPATIAL_Y][STATE_V]); + __sp.x[GEO_SPATIAL_Y][STATE_V] = __se.x[GEO_SPATIAL_Y][STATE_V] - wt * (wt * __se.x[GEO_SPATIAL_Y][STATE_V] * 0.5 - __se.x[GEO_SPATIAL_X][STATE_V]); + } + __sp.x[GEO_SPATIAL_Z][STATE_P] = __se.x[GEO_SPATIAL_Z][STATE_P] + dt * __se.x[GEO_SPATIAL_Z][STATE_V]; + __sp.x[GEO_SPATIAL_Z][STATE_V] = __se.x[GEO_SPATIAL_Z][STATE_V]; + + __sp.x[GEO_SPATIAL_X][STATE_A] = -__sp.x[GEO_SPATIAL_Y][STATE_V] * __wze; + __sp.x[GEO_SPATIAL_Y][STATE_A] = __sp.x[GEO_SPATIAL_X][STATE_V] * __wze; + __sp.x[GEO_SPATIAL_Z][STATE_A] = 0; + + for (i = GEO_SPATIAL_DIMENSION; i;) { + --i; + + _const_fl_kalf_sigma_matrix_ref s2ei = __se.s2x[i]; + _fl_kalf_sigma_matrix_ref s2pi = __sp.s2x[i]; + + s2pi[STATE_A][STATE_A] = s2ei[STATE_A][STATE_A]; + s2pi[STATE_A][STATE_V] = s2ei[STATE_A][STATE_V] + + s2ei[STATE_A][STATE_A] * dt; + s2pi[STATE_A][STATE_P] = s2ei[STATE_A][STATE_P] + + (s2ei[STATE_A][STATE_V] + + s2ei[STATE_A][STATE_A] * dt / 2) * dt; + + s2pi[STATE_V][STATE_A] = s2pi[STATE_A][STATE_V]; /* by symmetry */ + s2pi[STATE_V][STATE_V] = s2ei[STATE_V][STATE_V] + + (s2ei[STATE_V][STATE_A] * 2 + + s2ei[STATE_A][STATE_A] * dt) * dt; + s2pi[STATE_V][STATE_P] = s2ei[STATE_V][STATE_P] + + (s2ei[STATE_V][STATE_V] + + s2ei[STATE_P][STATE_A] + + (s2ei[STATE_V][STATE_A] * 3 + + s2ei[STATE_A][STATE_A] * dt) / 2 * dt) * dt; + + s2pi[STATE_P][STATE_A] = s2pi[STATE_A][STATE_P]; /* by symmetry */ + s2pi[STATE_P][STATE_V] = s2pi[STATE_V][STATE_P]; /* by symmetry */ + s2pi[STATE_P][STATE_P] = s2ei[STATE_P][STATE_P] + + (s2ei[STATE_P][STATE_V] * 2 + + (s2ei[STATE_V][STATE_V] + + s2ei[STATE_A][STATE_P] + + (s2ei[STATE_A][STATE_V] + + s2ei[STATE_A][STATE_A] * dt / 4) * dt) * dt) * dt; + } + // process noise + __sp.s2x[GEO_SPATIAL_X][STATE_A][STATE_A] += fl_square(__sp.x[GEO_SPATIAL_X][STATE_A] - __se.x[GEO_SPATIAL_X][STATE_A]); + __sp.s2x[GEO_SPATIAL_Y][STATE_A][STATE_A] += fl_square(__sp.x[GEO_SPATIAL_Y][STATE_A] - __se.x[GEO_SPATIAL_Y][STATE_A]); + __rectify_state(&__sp); + } +} /* __predict_pv */ diff --git a/lbs-server/src/location-fused/kalman-filter.h b/lbs-server/src/location-fused/kalman-filter.h new file mode 100644 index 0000000..307b2cf --- /dev/null +++ b/lbs-server/src/location-fused/kalman-filter.h @@ -0,0 +1,619 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file kalman-filter.h + * @brief Implementation of a 4D (2D position + 2D velocity) Kalman filter + */ + +#pragma once +#ifndef __FL_KALMAN_FILTER_H__ +#define __FL_KALMAN_FILTER_H__ + +#include +#include "types.h" +#include "parameters.h" +#include "motion-detector.h" +#include "aema-estimator.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************//** + * [public] Initializaton of the singleton unit (static constructor). This + * corresponds to service load. + * + * @param[in] on_motion_state_changed + * Callback to be invoked when the detected state of motion changes. This + * argument is optional, and can be NULL. + * @param[in] on_location_changed + * Callback to be invoked when the predicted or updated location changes. + * This argument is required. + * @param[in] handler + * Arbitrary user parameter passed to the callback. + */ +void fl_kalf_init( + motion_state_changed_cb on_motion_state_changed, + location_changed_cb on_location_changed, + gpointer handler); + +/***************************************************************************//** + * [public] Cleanup of the singleton unit (static destructor). This corresponds + * to service unload. + */ +void fl_kalf_exit(void); + +/***************************************************************************//** + * [public] Start processing the location and sensory inputs, and sending back + * notifications. This corresponds to service start. + */ +void fl_kalf_start(void); + +/***************************************************************************//** + * [public] Stops processing the inputs, and sending notifications. This + * corresponds to service stop. + */ +void fl_kalf_stop(void); + +/***************************************************************************//** + * [public] Reset of the internal state back to the initial one. The handlers + * and start/stop state are not changed. This function is used for + * initialization, soft restarts, and repetitive testing. + */ +void fl_kalf_reset(void); + +/***************************************************************************//** + * [public] Process 2D (horizontal) position without change of the altitude or + * speed (WPS). Kalman merging is carried out in tangent frame, and the origin + * is moved after each measurement so that the predictions always start at + * (0, 0, R + h). + * - Clocks synchronization + * - Kalman data blending + * - Repositioning of the tangent frame + * + * @param[in] pos + * Spherical position in geographic coordinates + * @param[in] sigma + * Level and standard deviations. + */ +void fl_kalf_process_position_2d_event(const fl_spherical_position* pos, const fl_sigma* sigma); + +/***************************************************************************//** + * [public] Process 3D position along with 3D velocity (GPS). Kalman merging is + * carried out in tangent frame, and the origin is moved after each measurement + * so that the predictions always start at (0, 0, R + h). + * - Clocks synchronization + * - Kalman data blending + * - Repositioning of the tangent frame + * If FL_KALMAN_ANGULAR_VELOCITY is enabled the angular velocity is derived + * from consecutive measurements of linear velocities and merged with gyroscope + * readings. + * + * @param[in] pos + * Spherical position in geographic coordinates, and altitude + * @param[in] vel + * Horizontal and vertical velocity (speed and climb) + * @param[in] sigma + * Level and standard deviations. + */ +void fl_kalf_process_position_2x3d_event(const fl_spherical_position* pos, const fl_tangent_velocity* vel, const fl_sigma* sigma); + +/***************************************************************************//** + * [public] Process sensory events (acceleration & gyrocope). The time offest is + * corrected if necessary. + * - Clocks synchronization + * - Device-to-tangent frame orientation + * - Prediction + * + * @param[in] t + * Sensory time-stamp + * @param[in] sensor_id + * Sensor identifier + * @param[in] data + * Vector of sensor-specific data. + */ +void fl_kalf_process_sensory_event(fl_seconds t, fl_sensory_source sensor_id, const_sensor_vector_3d_ref data); + +/***************************************************************************//** + * [public] Retrieve the last processed location, typically during the + * on_location_changed notification. This two-step procedure of extracting the + * location allows to spare the non-trivial conversion between internal and + * interface formats whenever the user code decides not to forward it through + * the API. + * + * @param[out] pos + * Current position in API format + * @param[in] sensor_id + * Current velocity in API format + * @param[in] data + * Current uncertainty in API format + */ +void fl_kalf_get_location(fl_spherical_position* pos, fl_tangent_velocity* vel, fl_sigma* sigma); + +/***************************************************************************//** + * [public] Compute distance along the Earth great arc between two arbitrary + * points. Notice that the altitude is ignored, i.e. this is metric is of a + * projective space. + * + * @param[out] p + * Position 3D vector in global Cartesian coordinate frame. + * @param[out] q + * Position 3D vector in global Cartesian coordinate frame. + * + * @return + * Spherical distance between @a p and @a q. + */ +fl_real fl_kalf_get_distance(const_fl_position_vector_ref p, const_fl_position_vector_ref q); + +#if defined(__FL_KALMAN_FILTER_C__) + #define INLINE \ + inline +#else + #define INLINE \ + inline +#endif +#if defined(__FL_KALMAN_FILTER_C__) + #ifdef GTEST + #define PRIVATE + #else + #define PRIVATE \ + static + #endif +#else + #define PRIVATE +#endif + +#define DEFAULT_ACCELERATION_SIGMA2 SQUARE(FL_ACCEL_NOISE_LEVEL) /* [(m/s^2)^2] */ +#define DEFAULT_ROTATION_SIGMA2 SQUARE(1.0 / 256) /* [(rad/s)^2] */ + +typedef enum { + STATE_P, /* position [m] */ + STATE_V, /* velocity [m/s] */ + STATE_A, /* acceleration [m/s^2] */ + /* always the last */ + STATE_DIMENSION +} _fl_kalf_state_component; + +typedef fl_real _fl_kalf_state_matrix[GEO_SPATIAL_DIMENSION][STATE_DIMENSION]; +typedef fl_real(*_fl_kalf_state_matrix_ref)[STATE_DIMENSION]; +typedef fl_real(*const _const_fl_kalf_state_matrix_ref)[STATE_DIMENSION]; + +typedef fl_real _fl_kalf_state_vector[GEO_SPATIAL_DIMENSION]; +typedef fl_real(*_fl_kalf_state_vector_ref); +typedef const fl_real(*_const_fl_kalf_state_vector_ref); + +typedef enum { + QTR_X, + QTR_Y, + QTR_Z, + QTR_0, + /* always the last */ + QUATERNION_DIMENSION +} _fl_kalf_quaternion_component; + +typedef fl_real _fl_kalf_quaternion[QUATERNION_DIMENSION]; +typedef fl_real(*_fl_kalf_quaternion_ref); +typedef const fl_real(*_const_fl_kalf_quaternion_ref); + +//! position on the Earth surface in Cartesian coordinates +typedef fl_meters position_vector[GEO_SPATIAL_DIMENSION]; +typedef struct { + fl_seconds time; + position_vector position; +} fl_prediction; + +typedef fl_real _fl_kalf_matrix_2d[PLANAR_DIMENSION][PLANAR_DIMENSION]; +typedef fl_real(*_fl_kalf_matrix_2d_ref)[PLANAR_DIMENSION]; +typedef fl_real(*const _const_fl_kalf_matrix_2d_ref)[PLANAR_DIMENSION]; + +typedef fl_real _fl_kalf_sigma_matrix[STATE_DIMENSION][STATE_DIMENSION]; +typedef fl_real(*_fl_kalf_sigma_matrix_ref)[STATE_DIMENSION]; +typedef fl_real(*const _const_fl_kalf_sigma_matrix_ref)[STATE_DIMENSION]; + +typedef _fl_kalf_state_matrix _fl_kalf_sigma_cube[GEO_SPATIAL_DIMENSION]; +typedef _fl_kalf_state_matrix(*_fl_kalf_sigma_cube_ref); + + +typedef struct { + fl_matrix_3d m; /* rotation matrix */ + fl_vector_3d w2; /* square weight (inverse covariance) vector of matrix rows */ +} _fl_kalf_system_rotation; + +typedef struct { + _fl_kalf_state_matrix x; /* state vector */ + _fl_kalf_sigma_cube s2x; /* covariance matrix */ +} _fl_kalf_state; + +/** Tangent space T(p)E to the embedding manifold E=SxR at point p */ +typedef struct { + fl_radians rx; /* latitude */ + fl_radians ry; /* longitude */ + fl_real srx; /* sin(latitude) */ + fl_real crx; /* cos(latitude) */ + fl_matrix_3d fsr; /* push-forward (tangent to global) coordinate system rotation matrix */ +} _fl_kalf_tangent_frame; + +typedef struct { + fl_seconds first; /* arrival of the first sensor event */ + fl_seconds last; /* arrival of the last sensor event */ + unsigned count; /* at 100Hz rate of arrival this will suffice for ~32 years */ +} _fl_kalf_sensor_times; + +typedef struct { + fl_seconds last_measurement; /* arrival of the last position measurement event */ + _fl_kalf_sensor_times sensor_event; + fl_seconds last_p_fixing; /* update of reference p (pe) */ + fl_seconds last_v_fixing; /* update of reference v (ve) => p */ + fl_seconds last_a_fixing; /* update of reference a (ae) => v, p */ + fl_seconds last_r_fixing; /* update of reference sr (sre) */ + fl_seconds last_r_diffusion; /* damping of sr weights */ + fl_seconds last_w_fixing; /* update of reference w (we) => q, sr */ +#if (FL_KALMAN_ANGULAR_VELOCITY) + fl_seconds last_wz_fixing; /* update of reference wz */ + fl_seconds last_b_fixing; /* update of reference b */ +#endif + fl_seconds last_reference; /* update of __se */ +} _fl_kalf_time_of; + +PRIVATE gboolean __started; +PRIVATE _fl_kalf_time_of __time_of; +PRIVATE fl_aema1d_estimator __timestamp_offset; +PRIVATE fl_position_4d __last_notification; +PRIVATE location_changed_cb __on_location_changed; +PRIVATE gpointer __handler; /* location event handler */ +PRIVATE _fl_kalf_tangent_frame __te; /* tangent frame map */ +PRIVATE _fl_kalf_system_rotation __srp; /* predicted system rotation */ +PRIVATE _fl_kalf_system_rotation __sre; /* reference system rotation */ +PRIVATE _fl_kalf_state __sp; /* predicted 3D state */ +PRIVATE _fl_kalf_state __se; /* reference 3D state */ +PRIVATE fl_vector_3d __wp; /* predicted angular velocity */ +PRIVATE fl_vector_3d __we; /* reference angular velocity */ +PRIVATE fl_real __s2wp; +PRIVATE fl_real __s2we; +PRIVATE fl_real __wze; /* reference angular z-velocity (horizontal plane) */ +#if (FL_KALMAN_ANGULAR_VELOCITY) +PRIVATE fl_real __s2wze; +PRIVATE fl_radians __be; /* reference North bearing */ +#endif +PRIVATE LocationStatus __fix_status; /* pass-through */ + +/***************************************************************************//** + * [private] Check for and correct the numerical errors which cause the + * covariance matrix to cease being positive-definite. This operation has to be + * performed regularly, or otherwise magic ensues, and every strange result + * becomes possible. + * + * @param[in, out] src + * The state object to be rectified. This is either the predicted (__sp), + * or estimated (__se) one. + */ +PRIVATE void __rectify_state(_fl_kalf_state *src); + +/***************************************************************************//** + * [private] Get the latest estimate of the difference between location and + * sensory clocks. + * + * @return + * Current time-offset value. + */ +PRIVATE INLINE fl_seconds __get_time_offset(void) +{ + return __timestamp_offset.value; +} + +/***************************************************************************//** + * [private] Convert location timestamp to internal time in seconds. The + * time-offset is adjusted if necessary to ensure temporal ordering of events. + * + * @param[in] timestamp + * Location timestamp (wall clock since 1970) + * + * @return + * Internal time. + */ +PRIVATE fl_seconds __timestamp_to_seconds(fl_timestamp timestamp); + +/***************************************************************************//** + * [private] Efficient (faster than memcpy) copy of the system rotation object. + * + * @param[out] dst + * Destination system rotation object + * @param[out] src + * Source system rotation object + */ +PRIVATE INLINE void __copy_system_rotation(_fl_kalf_system_rotation *dst, const _fl_kalf_system_rotation *src) +{ + unsigned i = sizeof(_fl_kalf_system_rotation) / sizeof(unsigned); + do { + --i; + ((unsigned*)dst)[i] = ((const unsigned*)src)[i]; + } while (i); +} + +/***************************************************************************//** + * [private] Efficient (faster than memcpy) copy of the state (3x3D position & + * covariance) object. + * + * @param[out] dst + * Destination state object + * @param[out] src + * Source state object + */ +PRIVATE INLINE void __copy_state(_fl_kalf_state *dst, _fl_kalf_state *src) +{ + __rectify_state(src); + unsigned i = sizeof(_fl_kalf_state) / sizeof(unsigned); + do { + --i; + ((unsigned*)dst)[i] = ((const unsigned*)src)[i]; + } while (i); +} + +/***************************************************************************//** + * [private] Approximate squared weigt (inverse covariance) of a variable which + * is a product of two unit vertors x = u*v, |u|=1, |v|=1. The exact expression is anything but + * trivial. + * + * @param[in] w2u + * Inverse covariance of u: w2u = 1/( - ^2) + * @param[in] w2v + * Inverse covariance of v: w2v = 1/( - ^2) + * + * @return + * Estimated weight of the product. + */ +PRIVATE INLINE fl_real __product_weights2(fl_real w2u, fl_real w2v) +{ + return w2u * w2v / (w2u + w2v); +} + +/***************************************************************************//** + * [private] 3D cross-product of two unit vectors. The resultant vector is + * normalized to unity, in case @a u1 and @a u2 are not orthogonal. + * + * @param[in] u1 + * Input unit vector + * @param[in] u2 + * Input unit vector + * @param[out] v + * Output unit vector + */ +PRIVATE void __cross_product(const_fl_vector_3d_ref u1, const_fl_vector_3d_ref u2, fl_vector_3d_ref v); + +/***************************************************************************//** + * [private] Create coordinate map from spherical M = S^2 x R to the tangent + * space TM. Notice that only two angles are being used, what means the course + * on the tangent plane is not straight North (x-axis), but still described by + * the bearing. + * + * @param[in] x + * Position and velocity in old tangent frame + * @param[out] status + * Status of the supplied data + */ +PRIVATE void __create_push_forward_map(_fl_kalf_state_matrix_ref x, LocationStatus status); + +/***************************************************************************//** + * [private] Convert API location to internal representation, i.e. perform + * conversion of: + * - units to SI; + * - coordinate system M -> TM. + * + * @param[in] pos + * Position in API format + * @param[in] vel + * Velocity in API format + * @param[in] sigma + * Uncertainty in API format + * @param[out] pm + * 3D position on TM in SI units + * @param[out] vm + * 3D velocity on TM in SI units + * @param[out] s2pm + * 3D position variances on TM in SI units + * @param[out] s2vm + * 3D velocity variances on TM in SI units + */ +PRIVATE void __spherical_to_tangent( + const fl_spherical_position *pos, + const fl_tangent_velocity *vel, + const fl_sigma *sigma, + fl_position_vector_ref pm, + fl_velocity_vector_ref vm, + fl_vector_3d_ref s2pm, + fl_vector_3d_ref s2vm); + +/***************************************************************************//** + * [private] Convert tangent frame data to API location representation, i.e. + * perform conversion of: + * - units from SI to API (degrees, km/h, timestamps); + * - coordinate system TM -> M. + * + * @param[in] x + * 3x3 state matrix + * @param[in] s2x + * 3x3x3 covariance cube + * @param[out] pos + * Position in API format + * @param[out] vel + * Velocity in API format + * @param[out] sigma + * Uncertainty in API format + */ +PRIVATE void __tangent_to_spherical( + const _fl_kalf_state_matrix_ref x, + const _fl_kalf_sigma_cube_ref s2x, + fl_spherical_position *pos, + fl_tangent_velocity *vel, + fl_sigma *sigma); + +/***************************************************************************//** + * [private] Retrieve the current prediction in global frame M. + * + * @param[in] t + * Internal time + * @param[out] prediction + * Spatio-temporal (4D) position in global coordinates. + */ +PRIVATE void __get_prediction_vector(fl_seconds t, fl_position_4d *prediction); + +/***************************************************************************//** + * [private] Map a quaternion into system rotation matrix. + * + * @param[in] q + * Quaternioninc (4D) representation of a rotation. + * @param[out] sr + * Corresponding system rotation matrix (3x3D) + */ +PRIVATE void __quaternion_to_rotation(_const_fl_kalf_quaternion_ref q, fl_matrix_3d_ref sr); + +/***************************************************************************//** + * [private] Update system rotation matrix (__se) given a measurement of the + * device angular velocity. + * + * @param[in] t + * Internal time of the measurement + * @param[in] wm + * 3D vector of angular velocity [rad/s] in device coordinate frame + * @param[in] s2wm + * Estimated variance of the measuremnt. + */ +PRIVATE void __integrate_rw(fl_seconds t, const_fl_spin_vector_ref wm, fl_real s2wm); + +/***************************************************************************//** + * [private] Extrapolate the system rotation (__sp) to the given time assuming + * constant angular velocity. + * + * @param[in] t + * Internal time + */ +PRIVATE void __predict_rw(fl_seconds t); + +/***************************************************************************//** + * [private] Planar (2D) single-order Kalman blending (of either position, + * velocity, or acceleration). Assumes the specific prediction is already + * performed. + * + * @param[in] t + * Internal time + * @param[in] s + * State component identifier to which @a xm and @a s2xm refer. + * @param[in] xm + * Measured data + * @param[in] s2xm + * Data variances + */ +PRIVATE void __kalman_2d1o(fl_seconds t, _fl_kalf_state_component s, const_fl_vector_3d_ref xm, const_fl_vector_3d_ref s2xm); + +/***************************************************************************//** + * [private] Planar (2D) Kalman blending of position. + * + * @param[in] t + * Internal time + * @param[in] pm + * Measured position + * @param[in] s2pm + * Position variances + */ +PRIVATE void __kalman_p(fl_seconds t, const_fl_position_vector_ref pm, const_fl_vector_3d_ref s2pm); + +/***************************************************************************//** + * [private] Spatial (3D) Kalman blending of position and velocity. + * + * @param[in] t + * Internal time + * @param[in] pm + * Measured position + * @param[in] vm + * Measured velocity + * @param[in] s2pm + * Position variances + * @param[in] s2vm + * Velocity variances + */ +PRIVATE void __kalman_pv( + fl_seconds t, + const_fl_position_vector_ref pm, + const_fl_velocity_vector_ref vm, + const_fl_vector_3d_ref s2pm, + const_fl_vector_3d_ref s2vm); + +/***************************************************************************//** + * [private] Axial (1D) Kalman blending of angular velocity along z-axis (turn). + * + * @param[in] t + * Internal time + * @param[in] wzm + * Z-projection of the measured angular velocity + * @param[in] s2wzm + * Corresponding variance + */ +void __kalman_wz(fl_seconds t, fl_real wzm, fl_real s2wzm); + +/***************************************************************************//** + * [private] Kalman blending two unit 3D vectors usign inverse covariances as + * weights (to avoid singularities). + * + * @param[in] t + * Internal time + * @param[in] u1 + * 3D vector + * @param[in] u2 + * 3D vector + * @param[in] w2u1 + * Inverse variance of @a u1 (can be zero) + * @param[in] w2u2 + * Inverse variance of @a u2 (can be zero) + * @param[out] v + * Output 3D vector + * @param[out] w2v + * Output inverse variance of @a v + */ +PRIVATE void __inv_kalman_u( + fl_seconds t, + const_fl_vector_3d_ref u1, fl_real w2u1, + const_fl_vector_3d_ref u2, fl_real w2u2, + fl_vector_3d_ref v, fl_real *w2v); + +/***************************************************************************//** + * [private] Extrapolate position, velocity, and the covariance-matrices up to + * the given time. + * + * @param[in] t + * Internal time + */ +PRIVATE void __predict_pv(fl_seconds t); + +/***************************************************************************//** + * [private] Check the notification conditions; if satisfied get the current + * prediction in global coordinate frame and signal it to the user. + * + * @param[in] t + * Internal time + */ +PRIVATE void __signal_updated_location(fl_seconds t); + +#if !defined(__FL_KALMAN_FILTER_C__) + #undef PRIVATE + #undef INLINE +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __FL_KALMAN_FILTER_H__ */ diff --git a/lbs-server/src/location-fused/lp-3d-filter.c b/lbs-server/src/location-fused/lp-3d-filter.c new file mode 100644 index 0000000..d77f635 --- /dev/null +++ b/lbs-server/src/location-fused/lp-3d-filter.c @@ -0,0 +1,126 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +#if !defined(LOG_THRESHOLD) + /* custom logging threshold - keep undefined on the repo */ + /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ +#endif + +#define __FL_LP_3D_FILTER_C__ + +#include +#include "math-ext.h" +#include "lp-3d-filter.h" +#include "debug_util.h" + +#define LP3D_INPUT_FORMAT "% 6.2f" + +void fl_lp3d_init(void) +{ + LOG_FUSED(DBG_LOW, ENTER_FUNCTION_PREFIX("()"), 0); + __lp3d_G = 0; + __lp3d_B1 = -2; + __lp3d_B2 = 1; + fl_lp3d_reset(); + LOG_FUSED(DBG_LOW, LEAVE_FUNCTION_PREFIX("(): OK"), 0); +} /* fl_lp3d_init */ + +void fl_lp3d_exit(void) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("()"), 0); +} /* fl_lp3d_exit */ + +void fl_lp3d_set_sampling_frequency(fl_hertz f) +{ + if (f > 0) { + double omegaC = tan(M_PI * FL_LP3D_CUTOFF_FREQUENCY / f); + double omegaC2 = fl_square(omegaC); + double _B0; + + _B0 = 1.0 / (1 + 2 * omegaC * cos(M_PI_4) + omegaC2); + __lp3d_B1 = _B0 * 2*(omegaC2 - 1); + __lp3d_B2 = _B0 * (1 - 2 * omegaC * cos(M_PI_4) + omegaC2); + __lp3d_G = _B0 * omegaC2; + + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(%g Hz): OK"), f); + return; + } else { + LOG_FUSED(DBG_ERR, FUNCTION_PREFIX("(%g Hz): E_INVALID_ARGUMENT"), f); + return; + } +} /* fl_lp3d_set_sampling_frequency */ + +void fl_lp3d_reset(void) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("()"), 0); + memset(__lp3d_u, 0, sizeof(__lp3d_u)); + memset(__lp3d_v, 0, sizeof(__lp3d_v)); +} /* fl_lp3d_reset */ + +void fl_lp3d_reset_to(const_fl_acceleration_vector_ref stationary) +{ + unsigned i; + + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("({" LP3D_INPUT_FORMAT ", " LP3D_INPUT_FORMAT ", " LP3D_INPUT_FORMAT "})"), + stationary[0], + stationary[1], + stationary[2]); + + i = GEO_SPATIAL_DIMENSION; + do { + --i; + /* up-conversion */ + __lp3d_u[TIME_SHIFT_1][i] = stationary[i]; + __lp3d_u[TIME_SHIFT_2][i] = stationary[i]; + __lp3d_v[TIME_SHIFT_1][i] = stationary[i]; + __lp3d_v[TIME_SHIFT_2][i] = stationary[i]; + } while (i); +} /* fl_lp3d_reset_to */ + +const_fl_acceleration_vector_ref fl_lp3d_process(const_fl_acceleration_vector_ref u) +{ + unsigned i; + + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("({" LP3D_INPUT_FORMAT ", " LP3D_INPUT_FORMAT ", " LP3D_INPUT_FORMAT "})"), + u[0], + u[1], + u[2]); + + i = GEO_SPATIAL_DIMENSION; + do { + --i; + + fl_acceleration u0 = u[i]; + #define u1 __lp3d_u[TIME_SHIFT_1][i] + #define u2 __lp3d_u[TIME_SHIFT_2][i] + fl_real v0; + #define v1 __lp3d_v[TIME_SHIFT_1][i] + #define v2 __lp3d_v[TIME_SHIFT_2][i] + + v0 = __lp3d_G * (u0 + 2 * u1 + u2) - __lp3d_B1 * v1 - __lp3d_B2 * v2; + u2 = u1; + u1 = u0; + v2 = v1; + v1 = v0; + + #undef u1 + #undef u2 + #undef v1 + #undef v2 + } while (i); + + return __lp3d_v[TIME_SHIFT_1]; +} /* fl_lp3d_process */ diff --git a/lbs-server/src/location-fused/lp-3d-filter.h b/lbs-server/src/location-fused/lp-3d-filter.h new file mode 100644 index 0000000..9277cc8 --- /dev/null +++ b/lbs-server/src/location-fused/lp-3d-filter.h @@ -0,0 +1,126 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file lp-3d-filter.h + * @brief Gravity direction estimator via low-pass 3D 2nd order Butterworth + * filter. + */ + +#pragma once +#ifndef __FL_LP_3D_FILTER_H__ +#define __FL_LP_3D_FILTER_H__ + +#include "types.h" +#include "math-ext.h" +#include "parameters.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************//** + * [public] Initializaton of the singleton unit (static constructor). + */ +void fl_lp3d_init(void); + +/***************************************************************************//** + * [public] Cleanup of the singleton unit (static destructor). + */ +void fl_lp3d_exit(void); + +/***************************************************************************//** + * [public] Adjust internal parameters to the given sampling frequency and reset + * the the past state memory. + * + * @param[in] f + * The sampling frequency in [Hz] of the accelerometer (or processing + * callback if downsampled). + */ +void fl_lp3d_set_sampling_frequency(fl_hertz f); + +/***************************************************************************//** + * [public] Reset the past state memory. + */ +void fl_lp3d_reset(void); + +/***************************************************************************//** + * [public] Reset to the given stationary state (input = output). + * + * @param[in] stationary + * 3D vector of acceleration measured in tangent coordinate frame. + */ +void fl_lp3d_reset_to(const_fl_acceleration_vector_ref stationary); + +/***************************************************************************//** + * [public] Process single input sample through the 3D Butterworth filter. + * + * @param[in] u + * 3D acceleration input vector. + * + * @return + * 3D acceleration output vector. + */ +const_fl_acceleration_vector_ref fl_lp3d_process(const_fl_acceleration_vector_ref u); + +#if defined(__FL_LP_3D_FILTER_C__) + #define INLINE \ + inline +#else + #define INLINE +#endif +#if defined(__FL_LP_3D_FILTER_C__) + #ifdef GTEST + #define PRIVATE + #else + #define PRIVATE \ + static + #endif +#else + #define PRIVATE \ + extern +#endif + +typedef enum { + TIME_SHIFT_1, /* t - 1 */ + TIME_SHIFT_2, /* t - 2 */ + /* always the last */ + TIME_SHIFT_COUNT +} _fl_lp3d_time_shift; + +/* filter coefficients */ +PRIVATE fl_real __lp3d_G; /* omega^2/B_0 */ +PRIVATE fl_real __lp3d_B1; /* B_1/B_0 */ +PRIVATE fl_real __lp3d_B2; /* B_2/B_0 */ +/* memory of past states */ +PRIVATE fl_acceleration_vector __lp3d_u[TIME_SHIFT_COUNT]; +PRIVATE fl_acceleration_vector __lp3d_v[TIME_SHIFT_COUNT]; + +#if (LP3D_FEATURE_SIGMA2) +PRIVATE fl_real __lp3d_u2[TIME_SHIFT_COUNT]; +PRIVATE fl_real __lp3d_v2[TIME_SHIFT_COUNT]; +#endif + +#if !defined(__FL_LP_3D_FILTER_C__) + #undef PRIVATE + #undef INLINE +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __FL_LP_3D_FILTER_H__ */ diff --git a/lbs-server/src/location-fused/math-ext.c b/lbs-server/src/location-fused/math-ext.c new file mode 100644 index 0000000..47e9357 --- /dev/null +++ b/lbs-server/src/location-fused/math-ext.c @@ -0,0 +1,50 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file math-ext.c + * @brief Math extensions + */ + +#if !defined(LOG_THRESHOLD) + /* custom logging threshold - keep undefined on the repo */ + /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ +#endif + +#define __FL_MATH_EXT_C__ + +#include +#include "math-ext.h" +#include "debug_util.h" + +#define PRECISION 1e-5 + +double fl_asin(double x) +{ + if (x >= 1) { + if (x > 1 + PRECISION) { + LOG_FUSED(DBG_ERR, FUNCTION_PREFIX("(%g) OUT_OF_DOMAIN"), x); + } + return M_PI_2; + } else if (x <= -1) { + if (x < -(1 + PRECISION)) { + LOG_FUSED(DBG_ERR, FUNCTION_PREFIX("(%g) OUT_OF_DOMAIN"), x); + } + return -M_PI_2; + } else + return asin(x); +} /* fl_asin */ + diff --git a/lbs-server/src/location-fused/math-ext.h b/lbs-server/src/location-fused/math-ext.h new file mode 100644 index 0000000..61e4eef --- /dev/null +++ b/lbs-server/src/location-fused/math-ext.h @@ -0,0 +1,207 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file math-ext.h + * @brief Math extensions + */ + +#pragma once +#ifndef __FL_MATH_EXT_H__ +#define __FL_MATH_EXT_H__ + +#define _USE_MATH_DEFINES +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(__FL_MATH_EXT_C__) + #define INLINE \ + static inline +#else + #define INLINE \ + static inline +#endif + +#if !defined(M_PI) /* C99 */ + #define M_PI 3.14159265358979323846 /* pi */ + #define M_PI_2 1.57079632679489661923 /* pi/2 */ + #define M_PI_4 0.785398163397448309616 /* pi/4 */ + #define M_1_PI 0.318309886183790671538 /* 1/pi */ + #define M_2_PI 0.636619772367581343076 /* 2/pi */ + #define M_2_SQRTPI 1.12837916709551257390 /* 2/sqrt(pi) */ + #define M_SQRT2 1.41421356237309504880 /* sqrt(2) */ + #define M_1_SQRT2 0.707106781186547524401 /* 1/sqrt(2) */ +#endif + #define M_SQRTPI 1.772453850905516027298 /* sqrt(pi) */ + #define M_SQRT3 1.73205080756887729353 /* sqrt(3) */ + #define M_1_SQRT3 0.577350269189625764509 /* 1/sqrt(3) */ + #define M_2PI (2 * M_PI) /* 2*pi */ + +/*******************************************************************//** + * [public] Second power of a number. Use this macro for preprocessed + * values and static variables. + * + * @param[in] x + * Arbitrary number + * + * @return + * Second power of @a x. + */ +#define SQUARE(x) ((x) * (x)) + +/*******************************************************************//** + * [public] Second power of a number. Use for dynamic variables. + * + * @param[in] x + * Arbitrary number + * + * @return + * Second power of @a x. + */ +INLINE double fl_square(double x) +{ + return SQUARE(x); +} + +/*******************************************************************//** + * [public] Domain-safe arcus-sine. If the argument runs out of asin() + * domain it is cast-back onto the nearest edge. If it departs beyond + * numerical error margin, the error is reported, and breakpoint fired. + * No exception is thrown in release mode. + * + * @param[in] x + * Number in the [-1, 1] range. + * + * @return + * - arcus sine of @x if x < 1 && x > -1, + * - -pi if x <= -1, + * - pi if x >= 1. + */ +double fl_asin(double x); + +/*******************************************************************//** + * [public] Squared length of a 2D vector in Euclidean metric. + * + * @param[in] x + * 2D vector + * + * @return + * Squared length of @a x, i.e. |x|^2 = x.x + */ +INLINE double fl_vector_2d_length2(const double x[]) +{ + return SQUARE(x[0]) + SQUARE(x[1]); +} + +/*******************************************************************//** + * [public] Squared length of a 3D vector in Euclidean metric. + * + * @param[in] x + * 3D vector + * + * @return + * Squared length of @a x, i.e. |x|^2 = x.x + */ +INLINE double fl_vector_3d_length2(const double x[]) +{ + return SQUARE(x[0]) + SQUARE(x[1]) + SQUARE(x[2]); +} + +/*******************************************************************//** + * [public] Inlined left-action of a linear operator on a 3D + * covector. + * + * @param[in] m + * 3x3D matrix + * @param[in] v + * 3D vector + * @param[out] out + * Left-action of the linear operator @a m on the vector @a v, i.e. + * out = m.v + */ +#define MATRIX_DOT_VECTOR_3D(m, v, out)\ + do {\ + (out)[GEO_SPATIAL_X] = (m)[GEO_SPATIAL_X][GEO_SPATIAL_X] * (v)[GEO_SPATIAL_X] + (m)[GEO_SPATIAL_X][GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_Y] + (m)[GEO_SPATIAL_X][GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_Z];\ + (out)[GEO_SPATIAL_Y] = (m)[GEO_SPATIAL_Y][GEO_SPATIAL_X] * (v)[GEO_SPATIAL_X] + (m)[GEO_SPATIAL_Y][GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_Y] + (m)[GEO_SPATIAL_Y][GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_Z];\ + (out)[GEO_SPATIAL_Z] = (m)[GEO_SPATIAL_Z][GEO_SPATIAL_X] * (v)[GEO_SPATIAL_X] + (m)[GEO_SPATIAL_Z][GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_Y] + (m)[GEO_SPATIAL_Z][GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_Z];\ + } while (0) + +/*******************************************************************//** + * [public] Inlined right-action of a linear operator on a 3D + * covector. In Euclidean metric this is the same as left action of the + * transposed operator on the vector. + * + * @param[in] v + * 3D vector + * @param[in] m + * 3x3D matrix + * @param[out] out + * Right-action of the linear operator @a m on the covector @a v, i.e. + * out = v.m + */ +#define VECTOR_DOT_MATRIX_3D(v, m, out)\ + do {\ + (out)[GEO_SPATIAL_X] = (v)[GEO_SPATIAL_X] * (m)[GEO_SPATIAL_X][GEO_SPATIAL_X] + (v)[GEO_SPATIAL_Y] * (m)[GEO_SPATIAL_Y][GEO_SPATIAL_X] + (v)[GEO_SPATIAL_Z] * (m)[GEO_SPATIAL_Z][GEO_SPATIAL_X];\ + (out)[GEO_SPATIAL_Y] = (v)[GEO_SPATIAL_X] * (m)[GEO_SPATIAL_X][GEO_SPATIAL_Y] + (v)[GEO_SPATIAL_Y] * (m)[GEO_SPATIAL_Y][GEO_SPATIAL_Y] + (v)[GEO_SPATIAL_Z] * (m)[GEO_SPATIAL_Z][GEO_SPATIAL_Y];\ + (out)[GEO_SPATIAL_Z] = (v)[GEO_SPATIAL_X] * (m)[GEO_SPATIAL_X][GEO_SPATIAL_Z] + (v)[GEO_SPATIAL_Y] * (m)[GEO_SPATIAL_Y][GEO_SPATIAL_Z] + (v)[GEO_SPATIAL_Z] * (m)[GEO_SPATIAL_Z][GEO_SPATIAL_Z];\ + } while (0) + +/*******************************************************************//** + * [public] Inlined cross-product of two 3D vectors. + * + * @param[in] u + * 3D vector + * @param[in] v + * 3D vector + * @param[out] out + * Cross-product of @a u and @a v + */ +#define VECTOR_CROSS_VECTOR_3D(u, v, out)\ + do {\ + (out)[GEO_SPATIAL_X] = (u)[GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_Z] - (u)[GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_Y];\ + (out)[GEO_SPATIAL_Y] = (u)[GEO_SPATIAL_Z] * (v)[GEO_SPATIAL_X] - (u)[GEO_SPATIAL_X] * (v)[GEO_SPATIAL_Z];\ + (out)[GEO_SPATIAL_Z] = (u)[GEO_SPATIAL_X] * (v)[GEO_SPATIAL_Y] - (u)[GEO_SPATIAL_Y] * (v)[GEO_SPATIAL_X];\ + } while (0) + +/*******************************************************************//** + * [public] Scalar Euclidean product of two 3D vectors. + * + * @param[in] x + * 3D vector + * @param[in] y + * 3D vector + * + * @return + * Scalar product of @a x and @a y. + */ +INLINE double fl_vector_3d_dot_product(const double x[], const double y[]) +{ + return x[0] * y[0] + x[1] * y[1] + x[2] * y[2]; +} + +#if !defined(__FL_MATH_EXT_C__) + #undef INLINE +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __FL_MATH_EXT_H__ */ diff --git a/lbs-server/src/location-fused/motion-detector.c b/lbs-server/src/location-fused/motion-detector.c new file mode 100644 index 0000000..9ec5285 --- /dev/null +++ b/lbs-server/src/location-fused/motion-detector.c @@ -0,0 +1,109 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +#if !defined(LOG_THRESHOLD) + /* custom logging threshold - keep undefined on the repo */ + /* #define LOG_THRESHOLD LOG_LEVEL_TRACE */ +#endif + +#define __FL_MOTION_DETECTOR_C__ + +#define _USE_MATH_DEFINES +#include +#include "math-ext.h" +#include "motion-detector.h" +#include "debug_util.h" + +#if (FL_MOTION_DETECTOR) + +void fl_moti_init(motion_state_changed_cb on_motion_state_changed, gpointer handler) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(motion_cb=%p, handler=%p)"), on_motion_state_changed, handler); + fl_aema1d_init(&__moti_al2, 1 << FL_MOTI_AEMA_PLATEAU_BITS); + __moti_immobile_time = 0; + __moti_state = MOTI_UNDECIDED; + __moti_last_notification = MOTI_UNDECIDED; + __moti_on_motion_state_changed = on_motion_state_changed; + __moti_handler = handler; +} /* fl_moti_init */ + +void fl_moti_exit(void) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("()"), 0); + __moti_handler = NULL; + __moti_on_motion_state_changed = NULL; + __moti_last_notification = MOTI_UNDECIDED; + __moti_state = MOTI_UNDECIDED; + __moti_immobile_time = 0; + fl_aema1d_exit(&__moti_al2); +} /* fl_moti_exit */ + +void fl_moti_reset(void) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("()"), 0); + __moti_immobile_time = 0; + __moti_state = MOTI_UNDECIDED; + __moti_last_notification = MOTI_UNDECIDED; + fl_aema1d_reset(&__moti_al2); +} /* fl_moti_reset */ + +PRIVATE void __moti_set_state(fl_motion_state state) +{ + LOG_FUSED(DBG_LOW, FUNCTION_PREFIX("(%u->%u)"), __moti_state, state); + __moti_state = state; + if (__moti_on_motion_state_changed && __moti_last_notification != state) { + __moti_on_motion_state_changed(state, __moti_handler); + __moti_last_notification = state; + } +} /* __moti_set_state */ + +void fl_moti_process(fl_seconds t, fl_acceleration_vector_ref al) +{ + fl_real al2; + + al2 = fl_aema1d_process(&__moti_al2, fl_vector_3d_length2(al)); + if (al2 > SQUARE(FL_MOTI_MOTION_LEVEL)) { + switch (__moti_state) { + case MOTI_UNDECIDED: + case MOTI_IMMOBILE: + case MOTI_SLEEP: + __moti_set_state(MOTI_MOVING); + break; + + default: /* MOTI_MOVING */ + break; + } + } else if (al2 < SQUARE(FL_MOTI_NOISE_LEVEL)) { + switch (__moti_state) { + case MOTI_UNDECIDED: + case MOTI_MOVING: + __moti_set_state(MOTI_IMMOBILE); + __moti_immobile_time = t; + break; + + case MOTI_IMMOBILE: + if (t - __moti_immobile_time > FL_MOTI_IMMOBILITY_INTERVAL) + __moti_set_state(MOTI_SLEEP); + break; + + default: + /* MOTI_SLEEP: */ + break; + } + } +} /* fl_moti_process */ + +#endif diff --git a/lbs-server/src/location-fused/motion-detector.h b/lbs-server/src/location-fused/motion-detector.h new file mode 100644 index 0000000..63e7b6b --- /dev/null +++ b/lbs-server/src/location-fused/motion-detector.h @@ -0,0 +1,133 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file motion-detector.h + * @brief Accelerometer-based detector of significant motion. + */ + +#pragma once +#ifndef __FL_MOTION_DETECTOR_H__ +#define __FL_MOTION_DETECTOR_H__ + +#include +#include "types.h" +#include "parameters.h" +#include "aema-estimator.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { + MOTI_UNDECIDED, /* initial */ + MOTI_MOVING, /* acceleration above MOTION_LEVEL */ + MOTI_IMMOBILE, /* acceleration below NOISE_LEVEL */ + MOTI_SLEEP, /* immobile for IMMOBILITY_INTERVAL */ +} fl_motion_state; + +/***************************************************************************//** + * [public] Callback invoked on every change in motion state. + * + * @param[in] state + * New motion state. + * @param[in] handler + * Arbitrary pointer to user data specified during the init() call. + */ +typedef void (*motion_state_changed_cb)(fl_motion_state state, gpointer handler); + +#if (FL_MOTION_DETECTOR) + +/***************************************************************************//** + * [public] Initializaton of the singleton unit (static constructor). + * + * @param[in] on_motion_state_changed + * Callback to be invoked when the detected state of motion changes. + * @param[in] handler + * Arbitrary user parameter passed to the callback. + */ +void fl_moti_init(motion_state_changed_cb on_motion_state_changed, gpointer handler); + +/***************************************************************************//** + * [public] Cleanup of the singleton unit (static destructor). + */ +void fl_moti_exit(void); + +/***************************************************************************//** + * [public] Reset of the internal state back to initial one. This function is + * used for repetitive testing and module soft restarts. + */ +void fl_moti_reset(void); + +/***************************************************************************//** + * [public] Processing of a single sample from the accelerometer. This function + * implments a hysteresis in the 2D space of linear acceleration modulus |al|^2 + * vs motion state: + * - Crossing the FL_MOTI_MOTION_LEVEL from below changes state to MOVING; + * - Crossing the FL_MOTI_NOISE_LEVEL from above changes state to IMMOBILE; + * - Staying IMMOBILE for longer than FL_MOTI_IMMOBILITY_INTERVAL changes + * state to SLEEP. + * + * @param[in] t + * Time of the event in seconds. + * @param[in] al + * 3D vector of linear acceleration. + */ +void fl_moti_process(fl_seconds t, fl_acceleration_vector_ref al); + +#if defined(__FL_MOTION_DETECTOR_C__) + #ifdef GTEST + #define PRIVATE + #else + #define PRIVATE \ + static + #endif +#else + #define PRIVATE \ + extern +#endif + +PRIVATE fl_aema1d_estimator __moti_al2; /**< Linear acceleration low-pass filter */ +PRIVATE fl_seconds __moti_immobile_time; /**< Start time of immobility */ +PRIVATE fl_motion_state __moti_state; /**< Curent state */ +PRIVATE fl_motion_state __moti_last_notification; /**< Last notified state */ +PRIVATE motion_state_changed_cb __moti_on_motion_state_changed; /**< User callback */ +PRIVATE gpointer __moti_handler; /**< User data */ + +/***************************************************************************//** + * [private] Change the state indicator and notify the user. + * + * @param[in] state + * New state value (does not have to be different than present one). + */ +PRIVATE void __moti_set_state(fl_motion_state state); + +#if !defined(__FL_MOTION_DETECTOR_C__) + #undef PRIVATE +#endif + +#else + #define fl_moti_init(on_motion_state_changed, handler) + #define fl_moti_exit() + #define fl_moti_reset() + #define fl_moti_process(t, al) +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __FL_MOTION_DETECTOR_H__ */ diff --git a/lbs-server/src/location-fused/parameters.h b/lbs-server/src/location-fused/parameters.h new file mode 100644 index 0000000..2f2c053 --- /dev/null +++ b/lbs-server/src/location-fused/parameters.h @@ -0,0 +1,74 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file parameters.h + * @brief Global switches and parameters of the Fused Location engine + */ + +#pragma once +#ifndef __FL_PARAMETERS_H__ +#define __FL_PARAMETERS_H__ + +/* Features */ + +/** 0 = off, 1 = detach location source (WPS/GPS) when sleeping (immobile for at least 30s) */ +#define FL_MOTION_DETECTOR 0 +/** 0 = no WPS pausing in BALANCED mode, 1 = WPS pausing + regular predictions between measurements */ +#define FL_ACCEL_IN_BALANCED (0 & FL_MOTION_DETECTOR) +/** 0 = off, 1 = Kalman-merging of angular velocities from GPS and gyroscope. */ +#define FL_KALMAN_ANGULAR_VELOCITY 1 +/** 0 = off, 1 = Filtration of output location notifications */ +#define FL_MIN_NOTIFICATION_FILTRATION 1 +/** Minimal interval in miliseconds between consecutive output location notifications */ +#define FL_MIN_NOTIFICATION_INTERVAL 1000 +/** 0 = off, 1 = fused devel logs on */ +#define FL_ENABLE_DEVEL_LOG 0 + +/* Free parameters */ + +/** [ms] Sensor sampling interval */ +#define FL_SENSOR_SAMPLING_INTERVAL 100 +/** [m] Default standard deviation of measured position */ +#define FL_DEFAULT_POSITION_SIGMA 8.0 +/** [m/s] Default standard deviation of measured linear velocity */ +#define FL_DEFAULT_VELOCITY_SIGMA 1.0 +/** [rad/s] Default standard deviation of measured angular velocity */ +#define FL_DEFAULT_SPIN_SIGMA 0.2 +/** [m/s^2] Accelerometer noise level */ +#define FL_ACCEL_NOISE_LEVEL ((fl_acceleration)0.25) +/** [Hz] Low-pass acceleration frequency threshold for estimation of gravity direction */ +#define FL_LP3D_CUTOFF_FREQUENCY ((fl_hertz)1.0) +/** [m/s^2] Upper level of IMMOBILE state */ +#define FL_MOTI_NOISE_LEVEL FL_ACCEL_NOISE_LEVEL +/** [m/s^2] Lower level of MOVING state */ +#define FL_MOTI_MOTION_LEVEL (3*FL_ACCEL_NOISE_LEVEL) +/** [s] Continuous immobility interval before transitioning to SLEEP state */ +#define FL_MOTI_IMMOBILITY_INTERVAL 90 +/** Log-2 of the AEMA averaging constant used for motion detection. */ +#define FL_MOTI_AEMA_PLATEAU_BITS 4 +/** Log-2 of the AEMA averaging constant used for gyro drift suppression. */ +#define FL_GYRO_AEMA_PLATEAU_BITS 14 +/** [rad/s] Initial width of the Gaussian window for gyro bias determination */ +#define FL_GYRO_SPIN_SIGMA_I FL_DEFAULT_SPIN_SIGMA +/** [rad/s] Final width of the Gaussian window for gyro bias determination */ +#define FL_GYRO_SPIN_SIGMA_F 0.0004 +/** Log-2 of the AEMA averaging constant used for gravity value (accelerometer calibration). */ +#define FL_GRES_AEMA_PLATEAU_BITS 16 +/** Log-2 of the AEMA averaging constant used for clock synchronization. */ +#define FL_TIMO_AEMA_PLATEAU_BITS 10 + +#endif /* __FL_PARAMETERS_H__ */ diff --git a/lbs-server/src/location-fused/types.h b/lbs-server/src/location-fused/types.h new file mode 100644 index 0000000..0e648bf --- /dev/null +++ b/lbs-server/src/location-fused/types.h @@ -0,0 +1,219 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * 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. + */ + +/** + * @file types.h + * @brief Core types definitions + */ + +#pragma once +#ifndef __FL_TYPES_H__ +#define __FL_TYPES_H__ + +#include "location-types.h" +#include "location-position.h" +#include "location-velocity.h" +#include "location-accuracy.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef double fl_real; /**< Default precision for the floating-point data in fused location */ +typedef float sensor_real; /**< Default precision for the floating-point data in sensors */ +typedef fl_real fl_seconds; /**< Time variables in SI units */ +typedef unsigned int fl_timestamp; /**< Time-stamps in [s] */ +typedef fl_real fl_hertz; /**< Frequency variables in SI units */ +typedef fl_real fl_meters; /**< Length variables in SI units */ +typedef fl_real fl_velocity; /**< Linear velocity in SI units [m/s] */ +typedef fl_real fl_km_h; /**< Linear velocity in SI-derived units [km/h] */ +typedef fl_real fl_acceleration; /**< Linear acceleration in SI units [m/s^2] */ +typedef sensor_real sensor_acceleration; /**< Linear acceleration in SI units [m/s^2] supplied by */ +typedef fl_real fl_radians; /**< Angular position in SI units [rad] modulo 3.14159 */ +typedef sensor_real sensor_radians; /**< Angular position in SI units [rad] modulo 3.14159 supplied by */ +typedef fl_real fl_degrees; /**< Angular position modulo 360 */ +typedef fl_real fl_spin; /**< Angular velocity in SI units [rad/s] modulo 3.14159 */ +typedef sensor_real sensor_spin; /**< Angular velocity in SI units [rad/s] modulo 3.14159 supplied by */ +typedef fl_real fl_utesla; /**< Magnetic field strength (B) in [uT] */ +typedef sensor_real sensor_utesla; /**< Magnetic field strength (B) in [uT] supplied by */ + +#define FL_UNDEFINED_TIME ((fl_seconds)-1e+48) + +/** Labels for a planar 2D space*/ +typedef enum { + PLANAR_X, + PLANAR_Y, + /* always the last */ + PLANAR_DIMENSION +} fl_planar; + +/** Labels for an axial (or cylyndrical without phase) 2D space */ +typedef enum { + AXIAL_H, + AXIAL_V, + /* always the last */ + AXIAL_DIMENSION +} fl_axial; + +/** Labels for a geographic coordinate system (left-handed) */ +typedef enum { + GEO_SPATIAL_X, + GEO_SPATIAL_Y, + GEO_SPATIAL_Z, + /* always the last */ + GEO_SPATIAL_DIMENSION +} fl_geo_spatial; + +/** Labels for a device coordinate system (right-handed) */ +typedef enum { + DEV_SPATIAL_X = GEO_SPATIAL_Y, + DEV_SPATIAL_Y = GEO_SPATIAL_X, + DEV_SPATIAL_Z = GEO_SPATIAL_Z, + DEV_SPATIAL_DIMENSION = GEO_SPATIAL_DIMENSION +} fl_dev_spatial; + +/** Labels for location source */ +typedef enum { + SOURCE_GPS, + SOURCE_WPS, + /* always the last */ + SOURCE_COUNT +} fl_positioning_source; + +/** Labels for sensors */ +typedef enum { + RAW_ACCELERATION_SENSOR, + GYROSCOPE_SENSOR, + /* always the last */ + SUPPORTED_SENSORS_COUNT +} fl_sensory_source; + +/** Flag indicators for sensors' combination */ +typedef enum { + NO_SENSOR_FLAG = 0, + RAW_ACCELERATION_SENSOR_FLAG = (1 << RAW_ACCELERATION_SENSOR), + GYROSCOPE_SENSOR_FLAG = (1 << GYROSCOPE_SENSOR), + /* always the last */ + SUPPORTED_SENSORS_MASK = (1 << SUPPORTED_SENSORS_COUNT) - 1 +} fl_sensory_flags; + +/** Three Cartesian coordinates of a point on the Earth surface */ +typedef fl_meters fl_position_vector[GEO_SPATIAL_DIMENSION]; +typedef fl_meters *fl_position_vector_ref; +typedef const fl_meters *const_fl_position_vector_ref; + +typedef fl_velocity fl_velocity_vector[GEO_SPATIAL_DIMENSION]; +typedef fl_velocity *fl_velocity_vector_ref; +typedef const fl_velocity *const_fl_velocity_vector_ref; + +/** Three device-oriented axes of the acceleration */ +typedef sensor_acceleration sensor_acceleration_vector[GEO_SPATIAL_DIMENSION]; +typedef sensor_acceleration *sensor_acceleration_vector_ref; +typedef const sensor_acceleration *const_sensor_acceleration_vector_ref; +typedef fl_acceleration fl_acceleration_vector[GEO_SPATIAL_DIMENSION]; +typedef fl_acceleration *fl_acceleration_vector_ref; +typedef const fl_acceleration *const_fl_acceleration_vector_ref; + +/** Three imaginary axes of the rotation quaternion */ +typedef sensor_spin sensor_spin_vector[GEO_SPATIAL_DIMENSION]; +typedef sensor_spin *sensor_spin_vector_ref; +typedef const sensor_spin *const_sensor_spin_vector_ref; +typedef fl_spin fl_spin_vector[GEO_SPATIAL_DIMENSION]; +typedef fl_spin *fl_spin_vector_ref; +typedef const fl_spin *const_fl_spin_vector_ref; + +typedef sensor_utesla sensor_magnetic_vector[GEO_SPATIAL_DIMENSION]; +typedef sensor_utesla *sensor_magnetic_vector_ref; +typedef const sensor_utesla *const_sensor_magnetic_vector_ref; +typedef fl_utesla fl_magnetic_vector[GEO_SPATIAL_DIMENSION]; +typedef fl_utesla *fl_magnetic_vector_ref; +typedef const fl_utesla *const_fl_magnetic_vector_ref; + +typedef const sensor_real *const_sensor_vector_3d_ref; + +typedef fl_real fl_vector_2d[AXIAL_DIMENSION]; +typedef fl_real *fl_vector_2d_ref; +typedef const fl_real *const_fl_vector_2d_ref; + +typedef fl_real fl_vector_3d[GEO_SPATIAL_DIMENSION]; +typedef fl_real *fl_vector_3d_ref; +typedef const fl_real *const_fl_vector_3d_ref; + +typedef fl_real fl_matrix_3d[GEO_SPATIAL_DIMENSION][GEO_SPATIAL_DIMENSION]; +typedef fl_real(*fl_matrix_3d_ref)[GEO_SPATIAL_DIMENSION]; +typedef fl_real(*const const_fl_matrix_3d_ref)[GEO_SPATIAL_DIMENSION]; + +/* Spatio-temporal position in Cartesian coordinates */ +typedef struct { + fl_position_vector spatial; + fl_seconds time; +} fl_position_4d; + +/*! Position in spherical coordintes */ +typedef LocationPosition fl_spherical_position; +/*! Velocity in coordinates of tangent space (tangent plane x ray coordinate) */ +typedef LocationVelocity fl_tangent_velocity; +/*! Standard deviations (1-sigma, or CEP 67%) */ +typedef struct { + LocationAccuracyLevel level; /**< ovelapps with 'accuracy.level' */ + fl_meters of_horizontal_pos; /**< std. dev. of horizontal position in [m] */ + fl_meters of_altitude; /**< std. dev. of vertical position in [m] */ + fl_velocity of_speed; /**< std. dev. of horizontal velocity in [m/s] */ + fl_velocity of_climb; /**< std. dev. of vertical velocity in [m/s] */ +} fl_sigma; + +/** Huge sigma value representing "almost flat" distribution. Unfortunately, + * we're forced by the API to use uncertainties (standard deviations, CEPs, + * or variances if squared) instead of the certainties (weights, or accuracies) + * which are their inverses and do not suffer this initialization problem. + * Even more peculiar is the custom of twisting the brain and calling + * uncertainty 'accuracy'. By this token zero represents the maximum, while + * infinity - the minimum. Beware. + */ +#define FLAT_SIGMA 1e24 +#define FLAT_SIGMA2 (FLAT_SIGMA * FLAT_SIGMA) + +typedef union { + LocationAccuracy accuracy; + fl_sigma sigma; +} fl_uncertainty_union; + +/*! Complete location data */ +typedef struct { + fl_spherical_position pos; + fl_tangent_velocity vel; + union { + LocationAccuracy accuracy; + fl_sigma sigma; + }; +} fl_location; + +/***************************************************************************//** + * Callback invoked on every new predicted position. + * + * @param[in] position + * The structure holds current filter output: spatio-temporal vector of + * dimension (3+1) in Cartesian coordinate system. + * @param[in] handler + * Arbitrary pointer to user data specified during the init() call. + */ +typedef void (*location_changed_cb)(const fl_position_4d* position, gpointer handler); + +#ifdef __cplusplus +} +#endif + +#endif /* __FL_TYPES_H__ */ diff --git a/lbs-server/src/server.c b/lbs-server/src/server.c index 54f349c..ad06721 100644 --- a/lbs-server/src/server.c +++ b/lbs-server/src/server.c @@ -76,6 +76,7 @@ #define QCOM8974_PATH PLATFORM_PATH"/msm8974_gps" #define QCOM8226_PATH PLATFORM_PATH"/msm8226_gps" #define QCOM8916_PATH PLATFORM_PATH"/msm8916_gps" +#define USB_PLUGIN_PATH "/etc/lbs-server/replay/nmea_usb.log" #define MPS_TO_KMPH 3.6 /* 1 m/s = 3.6 km/h */ @@ -466,18 +467,20 @@ int get_nmea_from_server(int *timestamp, char **nmea_data) return TRUE; } -static void _gps_plugin_handler_init(gps_server_t *server, char *module_name) +static gboolean _gps_plugin_handler_init(gps_server_t *server, char *module_name) { if (module_name == NULL) { LOG_GPS(DBG_ERR, "Fail : module_name is NULL"); - return; + return FALSE; } server->gps_plugin.handle = NULL; server->gps_plugin.name = (char *)malloc(strlen(module_name) + 1); - g_return_if_fail(server->gps_plugin.name); + g_return_val_if_fail(server->gps_plugin.name, FALSE); g_strlcpy(server->gps_plugin.name, module_name, strlen(module_name) + 1); + + return TRUE; } static void _gps_plugin_handler_deinit(gps_server_t *server) @@ -1085,6 +1088,8 @@ void check_plugin_module(char *module_name) if (get_replay_enabled() == TRUE) { g_strlcpy(module_name, "replay", strlen("replay") + 1); LOG_GPS(DBG_LOW, "REPLAY_ENABELD is TRUE"); + } else if (access(USB_PLUGIN_PATH, F_OK) == 0) { + g_strlcpy(module_name, "usb", strlen("usb") + 1); } else if (access(BRCM4752_PATH, F_OK) == 0 || access(BRCM47520_PATH, F_OK) == 0 || access(BRCM47522_PATH, F_OK) == 0 || access(BRCM477_PATH, F_OK) == 0) { g_strlcpy(module_name, "brcm", strlen("brcm") + 1); @@ -1209,8 +1214,13 @@ int initialize_server(int argc, char **argv) server = _initialize_gps_data(); if (server == NULL) return -1; + check_plugin_module(module_name); - _gps_plugin_handler_init(server, module_name); + if (!_gps_plugin_handler_init(server, module_name)) { + LOG_GPS(DBG_ERR, "Fail to init plugin handle"); + return -1; + } + _gps_get_nmea_replay_mode(server); _gps_notify_params(server); diff --git a/module/CMakeLists.txt b/module/CMakeLists.txt index d293ccd..ec39f9a 100644 --- a/module/CMakeLists.txt +++ b/module/CMakeLists.txt @@ -16,14 +16,11 @@ SET_TARGET_PROPERTIES(${gps_module} PROPERTIES VERSION ${FULLVER} SOVERSION ${MA SET_TARGET_PROPERTIES(${gps_module} PROPERTIES COMPILE_FLAGS ${MODULE_EXTRA_CFLAGS}) INSTALL(TARGETS ${gps_module} DESTINATION ${LIB_DIR}/location/module) -IF (ENABLE_WPS) - SET(nps_module "wps") - ADD_LIBRARY(${nps_module} SHARED ${CLIENT_SRCS_DIR}/nps_module.c) - TARGET_LINK_LIBRARIES(${nps_module} ${module_pkgs_LDFLAGS}) - SET_TARGET_PROPERTIES(${nps_module} PROPERTIES VERSION ${FULLVER} SOVERSION ${MAJORVER} CLEAN_DIRECT_OUTPUT 1) - SET_TARGET_PROPERTIES(${nps_module} PROPERTIES COMPILE_FLAGS ${MODULE_EXTRA_CFLAGS}) - INSTALL(TARGETS ${nps_module} DESTINATION ${LIB_DIR}/location/module) -ENDIF (ENABLE_WPS) +ADD_LIBRARY(${nps_module} SHARED ${CLIENT_SRCS_DIR}/nps_module.c) +TARGET_LINK_LIBRARIES(${nps_module} ${module_pkgs_LDFLAGS}) +SET_TARGET_PROPERTIES(${nps_module} PROPERTIES VERSION ${FULLVER} SOVERSION ${MAJORVER} CLEAN_DIRECT_OUTPUT 1) +SET_TARGET_PROPERTIES(${nps_module} PROPERTIES COMPILE_FLAGS ${MODULE_EXTRA_CFLAGS}) +INSTALL(TARGETS ${nps_module} DESTINATION ${LIB_DIR}/location/module) ADD_LIBRARY(${passive_module} SHARED ${CLIENT_SRCS_DIR}/passive_module.c) TARGET_LINK_LIBRARIES(${passive_module} ${module_pkgs_LDFLAGS}) diff --git a/module/fused_module.c b/module/fused_module.c index bcfcd0d..b673a02 100644 --- a/module/fused_module.c +++ b/module/fused_module.c @@ -1,7 +1,7 @@ /* * lbs-server * - * Copyright (c) 2011-2013 Samsung Electronics Co., Ltd. All rights reserved. + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. All rights reserved. * * Contact: Youngae Kang , Minjune Kim * Genie Kim @@ -38,39 +38,17 @@ #include "log.h" -#define MAX_GPS_LOC_ITEM 7 +#define MAX_GPS_LOC_ITEM 7 +#define LBS_FUSED_STATUS_USED 1 typedef struct { lbs_client_dbus_h lbs_client; -// LocModStatusCB status_cb; LocModPositionExtCB pos_cb; gpointer userdata; gboolean is_started; + guint pos_interval; } ModFusedData; -#if 0 -//TODO GPS blcking area -static void status_callback(GVariant *param, void *user_data) -{ - ModFusedData *mod_fused = (ModFusedData *) user_data; - g_return_if_fail(param); - g_return_if_fail(mod_fused); - g_return_if_fail(mod_fused->status_cb); - - int status = 0, method = 0; - g_variant_get(param, "(ii)", &method, &status); - MOD_LOGD("method(%d) status(%d)", method, status); - - if (status == 3) { - MOD_LOGD("LBS_STATUS_AVAILABLE"); - mod_fused->status_cb(TRUE, LOCATION_STATUS_3D_FIX, mod_fused->userdata); - } else { - MOD_LOGD("LBS_STATUS_ACQUIRING/ERROR/UNAVAILABLE. Status[%d]", status); - mod_fused->status_cb(FALSE, LOCATION_STATUS_NO_FIX, mod_fused->userdata); - } -} -#endif - static void position_callback(GVariant *param, void *user_data) { ModFusedData *mod_fused = (ModFusedData *)user_data; @@ -83,7 +61,14 @@ static void position_callback(GVariant *param, void *user_data) g_variant_get(param, "(iiidddddd@(idd))", &method, &fields, ×tamp, &latitude, &longitude, &altitude, &speed, &direction, &climb, &accuracy); - MOD_LOGD("position_callback [method: %d] GPS=0, WPS=1/AGPS=2/GEOFENCE/MOCK", method); +// MOD_FUSED_LOGD("position_callback [method: %d] GPS=0, WPS=1/AGPS=2/GEOFENCE/MOCK/FUSED", method); + + if (method != LBS_CLIENT_METHOD_FUSED) { + if (method != LBS_CLIENT_METHOD_MOCK) { +// MOD_FUSED_LOGD("Method is not LBS_CLIENT_METHOD_FUSED: %d", method); + return; + } + } g_variant_get(accuracy, "(idd)", &level, &horizontal, &vertical); @@ -108,98 +93,91 @@ static void on_signal_callback(const gchar *sig, GVariant *param, gpointer user_ if (!g_strcmp0(sig, "PositionChanged")) { position_callback(param, user_data); } -// else if (!g_strcmp0(sig, "StatusChanged")) { -// status_callback(param, user_data); -// } } static int start_gps(gpointer handle, guint pos_update_interval, LocModStatusCB status_cb, LocModPositionExtCB pos_cb, LocModSatelliteCB sat_cb, gpointer userdata) { - MOD_LOGD("start_gps"); - ModFusedData *mod_fused = (ModFusedData *) handle; + MOD_FUSED_LOGD("fused method"); + ModFusedData *mod_fused = (ModFusedData *)handle; g_return_val_if_fail(mod_fused, LOCATION_ERROR_NOT_AVAILABLE); -// g_return_val_if_fail(status_cb, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(pos_cb, LOCATION_ERROR_NOT_AVAILABLE); - MOD_LOGD("---------------------------"); -// mod_fused->status_cb = status_cb; mod_fused->pos_cb = pos_cb; mod_fused->userdata = userdata; - MOD_LOGD("---------------------------"); + MOD_FUSED_LOGD("---------------------------"); int ret = LBS_CLIENT_ERROR_NONE; - ret = lbs_client_create(LBS_CLIENT_METHOD_GPS , &(mod_fused->lbs_client)); + ret = lbs_client_create(LBS_CLIENT_METHOD_GPS, &(mod_fused->lbs_client)); if (ret != LBS_CLIENT_ERROR_NONE || !mod_fused->lbs_client) { - MOD_LOGE("Fail to create lbs_client_h. Error[%d]", ret); + MOD_FUSED_LOGE("Fail to create lbs_client_h. Error[%d]", ret); return LOCATION_ERROR_NOT_AVAILABLE; } - MOD_LOGD("gps-manger(%p) pos_cb (%p) user_data(%p)", mod_fused, mod_fused->pos_cb, mod_fused->userdata); + MOD_FUSED_LOGD("gps-manger(%p) pos_cb (%p) user_data(%p)", mod_fused, mod_fused->pos_cb, mod_fused->userdata); - ret = lbs_client_start(mod_fused->lbs_client, pos_update_interval, LBS_CLIENT_LOCATION_CB | LBS_CLIENT_LOCATION_STATUS_CB, on_signal_callback, mod_fused); + ret = lbs_client_start(mod_fused->lbs_client, pos_update_interval, LBS_CLIENT_LOCATION_CB | LBS_CLIENT_LOCATION_STATUS_CB, on_signal_callback, LBS_FUSED_STATUS_USED, mod_fused); if (ret != LBS_CLIENT_ERROR_NONE) { if (ret == LBS_CLIENT_ERROR_ACCESS_DENIED) { - MOD_LOGE("Access denied[%d]", ret); + MOD_FUSED_LOGE("Access denied[%d]", ret); return LOCATION_ERROR_NOT_ALLOWED; } - MOD_LOGE("Fail to start_gps lbs_client_h. Error[%d]", ret); + MOD_FUSED_LOGE("Fail to start_gps lbs_client_h. Error[%d]", ret); lbs_client_destroy(mod_fused->lbs_client); mod_fused->lbs_client = NULL; return LOCATION_ERROR_NOT_AVAILABLE; } + + mod_fused->pos_interval = pos_update_interval; return LOCATION_ERROR_NONE; } -static int start_wps(gpointer handle, LocModStatusCB status_cb, LocModPositionExtCB pos_cb, LocModSatelliteCB sat_cb, gpointer userdata) +static int start_wps(gpointer handle, guint pos_update_interval, LocModStatusCB status_cb, LocModPositionExtCB pos_cb, LocModSatelliteCB sat_cb, gpointer userdata) { - MOD_LOGD("start_wps"); - ModFusedData *mod_fused = (ModFusedData *) handle; + MOD_FUSED_LOGD("fused method"); + ModFusedData *mod_fused = (ModFusedData *)handle; g_return_val_if_fail(mod_fused, LOCATION_ERROR_NOT_AVAILABLE); - g_return_val_if_fail(status_cb, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(pos_cb, LOCATION_ERROR_NOT_AVAILABLE); - MOD_LOGD("---------------------------"); - -// mod_fused->status_cb = status_cb; mod_fused->pos_cb = pos_cb; mod_fused->userdata = userdata; - MOD_LOGD("---------------------------"); + MOD_FUSED_LOGD("---------------------------"); int ret = LBS_CLIENT_ERROR_NONE; - ret = lbs_client_create(LBS_CLIENT_METHOD_NPS , &(mod_fused->lbs_client)); + ret = lbs_client_create(LBS_CLIENT_METHOD_NPS, &(mod_fused->lbs_client)); if (ret != LBS_CLIENT_ERROR_NONE || !mod_fused->lbs_client) { - MOD_LOGE("Fail to create lbs_client_h. Error[%d]", ret); + MOD_FUSED_LOGE("Fail to create lbs_client_h. Error[%d]", ret); return LOCATION_ERROR_NOT_AVAILABLE; } - MOD_LOGD("wps-manger(%p) pos_cb (%p) user_data(%p)", mod_fused, mod_fused->pos_cb, mod_fused->userdata); + MOD_FUSED_LOGD("wps-manger(%p) pos_cb (%p) user_data(%p)", mod_fused, mod_fused->pos_cb, mod_fused->userdata); - ret = lbs_client_start(mod_fused->lbs_client, 1, LBS_CLIENT_LOCATION_CB | LBS_CLIENT_LOCATION_STATUS_CB, on_signal_callback, mod_fused); + ret = lbs_client_start(mod_fused->lbs_client, pos_update_interval, LBS_CLIENT_LOCATION_CB | LBS_CLIENT_LOCATION_STATUS_CB, on_signal_callback, LBS_FUSED_STATUS_USED, mod_fused); if (ret != LBS_CLIENT_ERROR_NONE) { if (ret == LBS_CLIENT_ERROR_ACCESS_DENIED) { - MOD_LOGE("Access denied[%d]", ret); + MOD_FUSED_LOGE("Access denied[%d]", ret); return LOCATION_ERROR_NOT_ALLOWED; } - MOD_LOGE("Fail to start_wps lbs_client_h. Error[%d]", ret); + MOD_FUSED_LOGE("Fail to start_wps lbs_client_h. Error[%d]", ret); lbs_client_destroy(mod_fused->lbs_client); mod_fused->lbs_client = NULL; return LOCATION_ERROR_NOT_AVAILABLE; } + mod_fused->pos_interval = pos_update_interval; return LOCATION_ERROR_NONE; } static int stop(gpointer handle) { - MOD_LOGD("stop"); + MOD_FUSED_LOGD("stop"); ModFusedData *mod_fused = (ModFusedData *) handle; g_return_val_if_fail(mod_fused, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(mod_fused->lbs_client, LOCATION_ERROR_NOT_AVAILABLE); -// g_return_val_if_fail(mod_fused->status_cb, LOCATION_ERROR_NOT_AVAILABLE); int ret = LBS_CLIENT_ERROR_NONE; - ret = lbs_client_stop(mod_fused->lbs_client); + ret = lbs_client_stop(mod_fused->lbs_client, mod_fused->pos_interval, LBS_FUSED_STATUS_USED); + MOD_FUSED_LOGD("stop gps interval [%d]", mod_fused->pos_interval); if (ret != LBS_CLIENT_ERROR_NONE) { - MOD_LOGE("Fail to stop. Error[%d]", ret); + MOD_FUSED_LOGE("Fail to stop. Error[%d]", ret); lbs_client_destroy(mod_fused->lbs_client); mod_fused->lbs_client = NULL; return LOCATION_ERROR_NOT_AVAILABLE; @@ -207,16 +185,10 @@ static int stop(gpointer handle) ret = lbs_client_destroy(mod_fused->lbs_client); if (ret != LBS_CLIENT_ERROR_NONE) { - MOD_LOGE("Fail to destroy. Error[%d]", ret); + MOD_FUSED_LOGE("Fail to destroy. Error[%d]", ret); return LOCATION_ERROR_NOT_AVAILABLE; } mod_fused->lbs_client = NULL; - -// if (mod_fused->status_cb) { -// mod_fused->status_cb(FALSE, LOCATION_STATUS_NO_FIX, mod_fused->userdata); -// } - -// mod_fused->status_cb = NULL; mod_fused->pos_cb = NULL; return LOCATION_ERROR_NONE; @@ -224,29 +196,25 @@ static int stop(gpointer handle) static int get_last_position(gpointer handle, LocationPosition **position, LocationVelocity **velocity, LocationAccuracy **accuracy) { - MOD_LOGD("get_last_position"); - ModFusedData *mod_fused = (ModFusedData *) handle; + MOD_FUSED_LOGD("get_last_position"); + + ModFusedData *mod_fused = (ModFusedData *)handle; g_return_val_if_fail(mod_fused, LOCATION_ERROR_NOT_AVAILABLE); - g_return_val_if_fail(position, LOCATION_ERROR_PARAMETER); - g_return_val_if_fail(velocity, LOCATION_ERROR_PARAMETER); - g_return_val_if_fail(accuracy, LOCATION_ERROR_PARAMETER); - int timestamp = 0, index = 0; + int timestamp = 0; char location[128] = {0,}; char *last_location[MAX_GPS_LOC_ITEM] = {0,}; - char *last = NULL, *str = NULL; + char *last = NULL; + char *str = NULL; double longitude = 0.0, latitude = 0.0, altitude = 0.0; double speed = 0.0, direction = 0.0; double hor_accuracy = 0.0, ver_accuracy = 0.0; + int index = 0; LocationStatus status = LOCATION_STATUS_NO_FIX; LocationAccuracyLevel level = LOCATION_ACCURACY_LEVEL_NONE; - *position = NULL; - *velocity = NULL; - *accuracy = NULL; - if (vconf_get_int(VCONFKEY_LOCATION_LAST_GPS_TIMESTAMP, ×tamp)) { - MOD_LOGD("Error to get VCONFKEY_LOCATION_LAST_GPS_TIMESTAMP"); + MOD_FUSED_LOGD("Error to get VCONFKEY_LOCATION_LAST_GPS_TIMESTAMP"); return LOCATION_ERROR_NOT_AVAILABLE; } else { if (timestamp != 0) { @@ -261,12 +229,12 @@ static int get_last_position(gpointer handle, LocationPosition **position, Locat } } else { if (vconf_get_int(VCONFKEY_LOCATION_NV_LAST_GPS_TIMESTAMP, ×tamp)) { - MOD_LOGD("Error to get VCONFKEY_LOCATION_NV_LAST_GPS_TIMESTAMP"); + MOD_FUSED_LOGD("Error to get VCONFKEY_LOCATION_NV_LAST_GPS_TIMESTAMP"); return LOCATION_ERROR_NOT_AVAILABLE; } str = vconf_get_str(VCONFKEY_LOCATION_NV_LAST_GPS_LOCATION); if (str == NULL) { - MOD_LOGD("Error to get VCONFKEY_LOCATION_NV_LAST_GPS_LOCATION"); + MOD_FUSED_LOGD("Error to get VCONFKEY_LOCATION_NV_LAST_GPS_LOCATION"); return LOCATION_ERROR_NOT_AVAILABLE; } snprintf(location, sizeof(location), "%s", str); @@ -313,16 +281,19 @@ static int get_last_position(gpointer handle, LocationPosition **position, Locat } level = LOCATION_ACCURACY_LEVEL_DETAILED; - *position = location_position_new(timestamp, latitude, longitude, altitude, status); - *velocity = location_velocity_new((guint) timestamp, speed, direction, 0.0); - *accuracy = location_accuracy_new(level, hor_accuracy, ver_accuracy); + if (position) + *position = location_position_new(timestamp, latitude, longitude, altitude, status); + if (velocity) + *velocity = location_velocity_new((guint) timestamp, speed, direction, 0.0); + if (accuracy) + *accuracy = location_accuracy_new(level, hor_accuracy, ver_accuracy); return LOCATION_ERROR_NONE; } LOCATION_MODULE_API gpointer init(LocModFusedOps *ops) { - MOD_LOGD("init"); + MOD_FUSED_LOGD("init"); g_return_val_if_fail(ops, NULL); ops->start_gps = start_gps; @@ -333,28 +304,28 @@ LOCATION_MODULE_API gpointer init(LocModFusedOps *ops) ModFusedData *mod_fused = g_new0(ModFusedData, 1); g_return_val_if_fail(mod_fused, NULL); -// mod_fused->status_cb = NULL; mod_fused->pos_cb = NULL; mod_fused->userdata = NULL; mod_fused->is_started = FALSE; + mod_fused->pos_interval = 0; return (gpointer) mod_fused; } LOCATION_MODULE_API void shutdown(gpointer handle) { - MOD_LOGD("shutdown"); + MOD_FUSED_LOGD("shutdown"); g_return_if_fail(handle); ModFusedData *mod_fused = (ModFusedData *) handle; if (mod_fused->lbs_client) { - lbs_client_stop(mod_fused->lbs_client); + lbs_client_stop(mod_fused->lbs_client, mod_fused->pos_interval, LBS_FUSED_STATUS_USED); lbs_client_destroy(mod_fused->lbs_client); mod_fused->lbs_client = NULL; } -// mod_fused->status_cb = NULL; mod_fused->pos_cb = NULL; + mod_fused->pos_interval = 0; g_free(mod_fused); mod_fused = NULL; diff --git a/module/gps_module.c b/module/gps_module.c index 79b79a8..74df9c3 100644 --- a/module/gps_module.c +++ b/module/gps_module.c @@ -40,6 +40,7 @@ #define MAX_GPS_LOC_ITEM 7 #define MOCK_LOCATION_CLEAR_VALUE 999 +#define LBS_FUSED_STATUS_NONE 0 typedef struct { lbs_client_dbus_h lbs_client; @@ -49,14 +50,16 @@ typedef struct { LocModSatelliteCB sat_cb; gpointer userdata; gboolean is_started; + guint pos_interval; + gint fused_status; } GpsManagerData; static void status_callback(GVariant *param, void *user_data) { - GpsManagerData *mod_gps = (GpsManagerData *) user_data; + GpsManagerData *module = (GpsManagerData *) user_data; g_return_if_fail(param); - g_return_if_fail(mod_gps); - g_return_if_fail(mod_gps->status_cb); + g_return_if_fail(module); + g_return_if_fail(module->status_cb); int status = 0, method = 0; g_variant_get(param, "(ii)", &method, &status); @@ -67,18 +70,18 @@ static void status_callback(GVariant *param, void *user_data) if (status == 3) { MOD_LOGD("LBS_STATUS_AVAILABLE : gps_module"); - mod_gps->status_cb(TRUE, LOCATION_STATUS_3D_FIX, mod_gps->userdata); + module->status_cb(TRUE, LOCATION_STATUS_3D_FIX, module->userdata); } else { MOD_LOGD("LBS_STATUS_ACQUIRING/ERROR/UNAVAILABLE. Status[%d]", status); - mod_gps->status_cb(FALSE, LOCATION_STATUS_NO_FIX, mod_gps->userdata); + module->status_cb(FALSE, LOCATION_STATUS_NO_FIX, module->userdata); } } static void satellite_callback(GVariant *param, void *user_data) { - GpsManagerData *mod_gps = (GpsManagerData *)user_data; - g_return_if_fail(mod_gps); - g_return_if_fail(mod_gps->sat_cb); + GpsManagerData *module = (GpsManagerData *)user_data; + g_return_if_fail(module); + g_return_if_fail(module->sat_cb); guint idx; guint used_idx; @@ -101,6 +104,7 @@ static void satellite_callback(GVariant *param, void *user_data) int num_of_used_prn = g_variant_iter_n_children(used_prn_iter); if (num_of_used_prn > 0) { used_prn_array = (guint *)g_new0(guint, num_of_used_prn); + g_return_if_fail(used_prn_array); for (idx = 0; idx < num_of_used_prn; idx++) { ret = g_variant_iter_next(used_prn_iter, "i", &tmp_prn); if (ret == FALSE) @@ -132,7 +136,7 @@ static void satellite_callback(GVariant *param, void *user_data) g_variant_unref(tmp_var); } - mod_gps->sat_cb(TRUE, sat, mod_gps->userdata); + module->sat_cb(TRUE, sat, module->userdata); location_satellite_free(sat); g_variant_iter_free(used_prn_iter); g_variant_iter_free(sat_iter); @@ -147,9 +151,9 @@ static void satellite_callback(GVariant *param, void *user_data) static void position_callback(GVariant *param, void *user_data) { - GpsManagerData *mod_gps = (GpsManagerData *)user_data; - g_return_if_fail(mod_gps); - g_return_if_fail(mod_gps->pos_cb); + GpsManagerData *module = (GpsManagerData *)user_data; + g_return_if_fail(module); + g_return_if_fail(module->pos_cb); int method = 0, fields = 0 , timestamp = 0 , level = 0; double latitude = 0.0, longitude = 0.0, altitude = 0.0, speed = 0.0, direction = 0.0, climb = 0.0, horizontal = 0.0, vertical = 0.0; @@ -174,7 +178,7 @@ static void position_callback(GVariant *param, void *user_data) vel = location_velocity_new(timestamp, speed, direction, climb); acc = location_accuracy_new(LOCATION_ACCURACY_LEVEL_DETAILED, horizontal, vertical); - mod_gps->pos_cb(TRUE, pos, vel, acc, mod_gps->userdata); + module->pos_cb(TRUE, pos, vel, acc, module->userdata); location_position_free(pos); location_velocity_free(vel); @@ -185,14 +189,14 @@ static void position_callback(GVariant *param, void *user_data) static void batch_callback(GVariant *param, void *user_data) { MOD_LOGD("batch_callback"); - GpsManagerData *mod_gps = (GpsManagerData *)user_data; - g_return_if_fail(mod_gps); - g_return_if_fail(mod_gps->batch_cb); + GpsManagerData *module = (GpsManagerData *)user_data; + g_return_if_fail(module); + g_return_if_fail(module->batch_cb); int num_of_location = 0; g_variant_get(param, "(i)", &num_of_location); - mod_gps->batch_cb(TRUE, num_of_location, mod_gps->userdata); + module->batch_cb(TRUE, num_of_location, module->userdata); } static void on_signal_batch_callback(const gchar *sig, GVariant *param, gpointer user_data) @@ -220,71 +224,73 @@ static void on_signal_callback(const gchar *sig, GVariant *param, gpointer user_ static int start(gpointer handle, guint pos_update_interval, LocModStatusCB status_cb, LocModPositionExtCB pos_cb, LocModSatelliteCB sat_cb, gpointer userdata) { MOD_LOGD("start"); - GpsManagerData *mod_gps = (GpsManagerData *) handle; - g_return_val_if_fail(mod_gps, LOCATION_ERROR_NOT_AVAILABLE); + GpsManagerData *module = (GpsManagerData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(status_cb, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(pos_cb, LOCATION_ERROR_NOT_AVAILABLE); - mod_gps->status_cb = status_cb; - mod_gps->pos_cb = pos_cb; - mod_gps->sat_cb = sat_cb; - mod_gps->userdata = userdata; + module->status_cb = status_cb; + module->pos_cb = pos_cb; + module->sat_cb = sat_cb; + module->userdata = userdata; int ret = LBS_CLIENT_ERROR_NONE; - ret = lbs_client_create(LBS_CLIENT_METHOD_GPS , &(mod_gps->lbs_client)); - if (ret != LBS_CLIENT_ERROR_NONE || !mod_gps->lbs_client) { + ret = lbs_client_create(LBS_CLIENT_METHOD_GPS , &(module->lbs_client)); + if (ret != LBS_CLIENT_ERROR_NONE || !module->lbs_client) { MOD_LOGE("Fail to create lbs_client_h. Error[%d]", ret); return LOCATION_ERROR_NOT_AVAILABLE; } - MOD_LOGD("gps-manger(%p) pos_cb (%p) user_data(%p)", mod_gps, mod_gps->pos_cb, mod_gps->userdata); + MOD_LOGD("gps-manger(%p) pos_cb (%p) user_data(%p)", module, module->pos_cb, module->userdata); - ret = lbs_client_start(mod_gps->lbs_client, pos_update_interval, LBS_CLIENT_LOCATION_CB | LBS_CLIENT_LOCATION_STATUS_CB | LBS_CLIENT_SATELLITE_CB | LBS_CLIENT_NMEA_CB, on_signal_callback, mod_gps); + ret = lbs_client_start(module->lbs_client, pos_update_interval, LBS_CLIENT_LOCATION_CB | LBS_CLIENT_LOCATION_STATUS_CB | LBS_CLIENT_SATELLITE_CB | LBS_CLIENT_NMEA_CB, on_signal_callback, LBS_FUSED_STATUS_NONE, module); if (ret != LBS_CLIENT_ERROR_NONE) { if (ret == LBS_CLIENT_ERROR_ACCESS_DENIED) { MOD_LOGE("Access denied[%d]", ret); return LOCATION_ERROR_NOT_ALLOWED; } MOD_LOGE("Fail to start lbs_client_h. Error[%d]", ret); - lbs_client_destroy(mod_gps->lbs_client); - mod_gps->lbs_client = NULL; + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; return LOCATION_ERROR_NOT_AVAILABLE; } + module->pos_interval = pos_update_interval; return LOCATION_ERROR_NONE; } static int stop(gpointer handle) { MOD_LOGD("stop"); - GpsManagerData *mod_gps = (GpsManagerData *) handle; - g_return_val_if_fail(mod_gps, LOCATION_ERROR_NOT_AVAILABLE); - g_return_val_if_fail(mod_gps->lbs_client, LOCATION_ERROR_NOT_AVAILABLE); - g_return_val_if_fail(mod_gps->status_cb, LOCATION_ERROR_NOT_AVAILABLE); + GpsManagerData *module = (GpsManagerData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); + g_return_val_if_fail(module->lbs_client, LOCATION_ERROR_NOT_AVAILABLE); + g_return_val_if_fail(module->status_cb, LOCATION_ERROR_NOT_AVAILABLE); int ret = LBS_CLIENT_ERROR_NONE; - ret = lbs_client_stop(mod_gps->lbs_client); + ret = lbs_client_stop(module->lbs_client, module->pos_interval, LBS_FUSED_STATUS_NONE); + MOD_LOGD("stop gps interval [%d]", module->pos_interval); if (ret != LBS_CLIENT_ERROR_NONE) { MOD_LOGE("Fail to stop. Error[%d]", ret); - lbs_client_destroy(mod_gps->lbs_client); - mod_gps->lbs_client = NULL; + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; return LOCATION_ERROR_NOT_AVAILABLE; } - ret = lbs_client_destroy(mod_gps->lbs_client); + ret = lbs_client_destroy(module->lbs_client); if (ret != LBS_CLIENT_ERROR_NONE) { MOD_LOGE("Fail to destroy. Error[%d]", ret); return LOCATION_ERROR_NOT_AVAILABLE; } - mod_gps->lbs_client = NULL; + module->lbs_client = NULL; - if (mod_gps->status_cb) - mod_gps->status_cb(FALSE, LOCATION_STATUS_NO_FIX, mod_gps->userdata); + if (module->status_cb) + module->status_cb(FALSE, LOCATION_STATUS_NO_FIX, module->userdata); - mod_gps->status_cb = NULL; - mod_gps->pos_cb = NULL; - mod_gps->sat_cb = NULL; + module->status_cb = NULL; + module->pos_cb = NULL; + module->sat_cb = NULL; return LOCATION_ERROR_NONE; } @@ -292,31 +298,31 @@ static int stop(gpointer handle) static int start_batch(gpointer handle, LocModBatchExtCB batch_cb, guint batch_interval, guint batch_period, gpointer userdata) { MOD_LOGD("start_batch"); - GpsManagerData *mod_gps = (GpsManagerData *) handle; - g_return_val_if_fail(mod_gps, LOCATION_ERROR_NOT_AVAILABLE); + GpsManagerData *module = (GpsManagerData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(batch_cb, LOCATION_ERROR_NOT_AVAILABLE); - mod_gps->batch_cb = batch_cb; - mod_gps->userdata = userdata; + module->batch_cb = batch_cb; + module->userdata = userdata; int ret = LBS_CLIENT_ERROR_NONE; - ret = lbs_client_create(LBS_CLIENT_METHOD_GPS , &(mod_gps->lbs_client)); - if (ret != LBS_CLIENT_ERROR_NONE || !mod_gps->lbs_client) { + ret = lbs_client_create(LBS_CLIENT_METHOD_GPS , &(module->lbs_client)); + if (ret != LBS_CLIENT_ERROR_NONE || !module->lbs_client) { MOD_LOGE("Fail to create lbs_client_h. Error[%d]", ret); return LOCATION_ERROR_NOT_AVAILABLE; } - MOD_LOGD("gps-manger(%p) batch_cb(%p) user_data(%p)", mod_gps, mod_gps->batch_cb, mod_gps->userdata); + MOD_LOGD("gps-manger(%p) batch_cb(%p) user_data(%p)", module, module->batch_cb, module->userdata); - ret = lbs_client_batch_start(mod_gps->lbs_client, LBS_CLIENT_BATCH_CB, on_signal_batch_callback, batch_interval, batch_period, mod_gps); + ret = lbs_client_batch_start(module->lbs_client, LBS_CLIENT_BATCH_CB, on_signal_batch_callback, batch_interval, batch_period, module); if (ret != LBS_CLIENT_ERROR_NONE) { if (ret == LBS_CLIENT_ERROR_ACCESS_DENIED) { MOD_LOGE("Access denied[%d]", ret); return LOCATION_ERROR_NOT_ALLOWED; } MOD_LOGE("Fail to start lbs_client_h. Error[%d]", ret); - lbs_client_destroy(mod_gps->lbs_client); - mod_gps->lbs_client = NULL; + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; return LOCATION_ERROR_NOT_AVAILABLE; } @@ -327,26 +333,26 @@ static int start_batch(gpointer handle, LocModBatchExtCB batch_cb, guint batch_i static int stop_batch(gpointer handle) { MOD_LOGD("stop_batch"); - GpsManagerData *mod_gps = (GpsManagerData *) handle; - g_return_val_if_fail(mod_gps, LOCATION_ERROR_NOT_AVAILABLE); + GpsManagerData *module = (GpsManagerData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); int ret = LBS_CLIENT_ERROR_NONE; - ret = lbs_client_batch_stop(mod_gps->lbs_client); + ret = lbs_client_batch_stop(module->lbs_client); if (ret != LBS_CLIENT_ERROR_NONE) { MOD_LOGE("Fail to stop batch. Error[%d]", ret); - lbs_client_destroy(mod_gps->lbs_client); - mod_gps->lbs_client = NULL; + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; return LOCATION_ERROR_NOT_AVAILABLE; } - ret = lbs_client_destroy(mod_gps->lbs_client); + ret = lbs_client_destroy(module->lbs_client); if (ret != LBS_CLIENT_ERROR_NONE) { MOD_LOGE("Fail to destroy. Error[%d]", ret); return LOCATION_ERROR_NOT_AVAILABLE; } - mod_gps->lbs_client = NULL; - mod_gps->batch_cb = NULL; + module->lbs_client = NULL; + module->batch_cb = NULL; return LOCATION_ERROR_NONE; } @@ -354,14 +360,14 @@ static int stop_batch(gpointer handle) static int get_nmea(gpointer handle, gchar **nmea_data) { MOD_LOGD("get_nmea"); - GpsManagerData *mod_gps = (GpsManagerData *) handle; - g_return_val_if_fail(mod_gps, LOCATION_ERROR_NOT_AVAILABLE); + GpsManagerData *module = (GpsManagerData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(nmea_data, LOCATION_ERROR_PARAMETER); gboolean ret = FALSE; int timestamp = 0; - ret = lbs_client_get_nmea(mod_gps->lbs_client, ×tamp, nmea_data); + ret = lbs_client_get_nmea(module->lbs_client, ×tamp, nmea_data); if (ret) { MOD_LOGE("Error getting nmea: %d", ret); return LOCATION_ERROR_NOT_AVAILABLE; @@ -373,8 +379,8 @@ static int get_nmea(gpointer handle, gchar **nmea_data) static int get_last_position(gpointer handle, LocationPosition **position, LocationVelocity **velocity, LocationAccuracy **accuracy) { MOD_LOGD("get_last_position"); - GpsManagerData *mod_gps = (GpsManagerData *) handle; - g_return_val_if_fail(mod_gps, LOCATION_ERROR_NOT_AVAILABLE); + GpsManagerData *module = (GpsManagerData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(position, LOCATION_ERROR_PARAMETER); g_return_val_if_fail(velocity, LOCATION_ERROR_PARAMETER); g_return_val_if_fail(accuracy, LOCATION_ERROR_PARAMETER); @@ -471,8 +477,8 @@ static int get_last_position(gpointer handle, LocationPosition **position, Locat static int set_option(gpointer handle, const char *option) { - GpsManagerData *mod_gps = (GpsManagerData *) handle; - g_return_val_if_fail(mod_gps, LOCATION_ERROR_NOT_AVAILABLE); + GpsManagerData *module = (GpsManagerData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(option, LOCATION_ERROR_PARAMETER); MOD_LOGD("set_option : %s", option); @@ -488,13 +494,13 @@ static int set_option(gpointer handle, const char *option) static int set_position_update_interval(gpointer handle, guint interval) { - MOD_LOGD("set_position_update_interval [%d]", interval); - GpsManagerData *mod_gps = (GpsManagerData *) handle; - g_return_val_if_fail(mod_gps, LOCATION_ERROR_NOT_AVAILABLE); - g_return_val_if_fail(mod_gps->lbs_client, LOCATION_ERROR_NOT_AVAILABLE); + GpsManagerData *module = (GpsManagerData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); + g_return_val_if_fail(module->lbs_client, LOCATION_ERROR_NOT_AVAILABLE); int ret; - ret = lbs_client_set_position_update_interval(mod_gps->lbs_client, interval); + MOD_LOGD("set_position_update inteval [%d] -> [%d]", module->pos_interval, interval); + ret = lbs_client_set_position_update_interval(module->lbs_client, interval, module->pos_interval); if (ret != LBS_CLIENT_ERROR_NONE) { if (ret == LBS_CLIENT_ERROR_ACCESS_DENIED) { MOD_LOGE("Access denied[%d]", ret); @@ -504,6 +510,8 @@ static int set_position_update_interval(gpointer handle, guint interval) return LOCATION_ERROR_NOT_AVAILABLE; } + module->pos_interval = interval; + return LOCATION_ERROR_NONE; } @@ -511,22 +519,22 @@ static int set_mock_location(gpointer handle, LocationPosition *position, Locati LocationAccuracy *accuracy, LocModStatusCB mock_status_cb, gpointer userdata) { MOD_LOGD("ENTER >>> set_mock_location"); - GpsManagerData *mod_gps = (GpsManagerData *) handle; - g_return_val_if_fail(mod_gps, LOCATION_ERROR_NOT_AVAILABLE); + GpsManagerData *module = (GpsManagerData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); int ret = LBS_CLIENT_ERROR_NONE; - if (mod_gps->lbs_client == NULL) { - ret = lbs_client_create(LBS_CLIENT_METHOD_GPS , &(mod_gps->lbs_client)); - if (ret != LBS_CLIENT_ERROR_NONE || !mod_gps->lbs_client) { + if (module->lbs_client == NULL) { + ret = lbs_client_create(LBS_CLIENT_METHOD_GPS , &(module->lbs_client)); + if (ret != LBS_CLIENT_ERROR_NONE || !module->lbs_client) { MOD_LOGE("Fail to create lbs_client_h. Error[%d]", ret); return LOCATION_ERROR_NOT_AVAILABLE; } } - mod_gps->userdata = userdata; + module->userdata = userdata; - ret = lbs_client_set_mock_location_async(mod_gps->lbs_client, LBS_CLIENT_METHOD_GPS, + ret = lbs_client_set_mock_location_async(module->lbs_client, LBS_CLIENT_METHOD_GPS, position->latitude, position->longitude, position->altitude, - velocity->speed, velocity->direction, accuracy->horizontal_accuracy, NULL, mod_gps); + velocity->speed, velocity->direction, accuracy->horizontal_accuracy, NULL, module); if (ret != LBS_CLIENT_ERROR_NONE) { if (ret == LBS_CLIENT_ERROR_ACCESS_DENIED) { @@ -534,8 +542,8 @@ static int set_mock_location(gpointer handle, LocationPosition *position, Locati return LOCATION_ERROR_NOT_ALLOWED; } MOD_LOGE("Fail to start lbs_client_h. Error[%d]", ret); - lbs_client_destroy(mod_gps->lbs_client); - mod_gps->lbs_client = NULL; + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; return LOCATION_ERROR_NOT_AVAILABLE; } @@ -546,21 +554,21 @@ static int set_mock_location(gpointer handle, LocationPosition *position, Locati static int clear_mock_location(gpointer handle, LocModStatusCB mock_status_cb, gpointer userdata) { MOD_LOGD("ENTER >>> clear_mock_location"); - GpsManagerData *mod_gps = (GpsManagerData *) handle; - g_return_val_if_fail(mod_gps, LOCATION_ERROR_NOT_AVAILABLE); + GpsManagerData *module = (GpsManagerData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); int ret = LBS_CLIENT_ERROR_NONE; - if (mod_gps->lbs_client == NULL) { - ret = lbs_client_create(LBS_CLIENT_METHOD_GPS , &(mod_gps->lbs_client)); - if (ret != LBS_CLIENT_ERROR_NONE || !mod_gps->lbs_client) { + if (module->lbs_client == NULL) { + ret = lbs_client_create(LBS_CLIENT_METHOD_GPS , &(module->lbs_client)); + if (ret != LBS_CLIENT_ERROR_NONE || !module->lbs_client) { MOD_LOGE("Fail to create lbs_client_h. Error[%d]", ret); return LOCATION_ERROR_NOT_AVAILABLE; } } - mod_gps->userdata = userdata; + module->userdata = userdata; - ret = lbs_client_set_mock_location_async(mod_gps->lbs_client, LBS_CLIENT_METHOD_GPS, - MOCK_LOCATION_CLEAR_VALUE, 0, 0, 0, 0, 0, NULL, mod_gps); + ret = lbs_client_set_mock_location_async(module->lbs_client, LBS_CLIENT_METHOD_GPS, + MOCK_LOCATION_CLEAR_VALUE, 0, 0, 0, 0, 0, NULL, module); if (ret != LBS_CLIENT_ERROR_NONE) { if (ret == LBS_CLIENT_ERROR_ACCESS_DENIED) { @@ -568,8 +576,8 @@ static int clear_mock_location(gpointer handle, LocModStatusCB mock_status_cb, g return LOCATION_ERROR_NOT_ALLOWED; } MOD_LOGE("Fail to start lbs_client_h. Error[%d]", ret); - lbs_client_destroy(mod_gps->lbs_client); - mod_gps->lbs_client = NULL; + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; return LOCATION_ERROR_NOT_AVAILABLE; } @@ -601,35 +609,38 @@ LOCATION_MODULE_API gpointer init(LocModGpsOps *ops) else MOD_LOGE("Invalid module name"); - GpsManagerData *mod_gps = g_new0(GpsManagerData, 1); - g_return_val_if_fail(mod_gps, NULL); + GpsManagerData *module = g_new0(GpsManagerData, 1); + g_return_val_if_fail(module, NULL); - mod_gps->status_cb = NULL; - mod_gps->pos_cb = NULL; - mod_gps->batch_cb = NULL; - mod_gps->userdata = NULL; - mod_gps->is_started = FALSE; + module->status_cb = NULL; + module->pos_cb = NULL; + module->batch_cb = NULL; + module->userdata = NULL; + module->is_started = FALSE; + module->pos_interval = 0; + module->fused_status = 0; - return (gpointer) mod_gps; + return (gpointer) module; } LOCATION_MODULE_API void shutdown(gpointer handle) { MOD_LOGD("shutdown"); g_return_if_fail(handle); - GpsManagerData *mod_gps = (GpsManagerData *) handle; - if (mod_gps->lbs_client) { - lbs_client_stop(mod_gps->lbs_client); - lbs_client_destroy(mod_gps->lbs_client); - mod_gps->lbs_client = NULL; + GpsManagerData *module = (GpsManagerData *) handle; + if (module->lbs_client) { + lbs_client_stop(module->lbs_client, module->pos_interval, LBS_FUSED_STATUS_NONE); + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; } - mod_gps->status_cb = NULL; - mod_gps->pos_cb = NULL; - mod_gps->batch_cb = NULL; - mod_gps->sat_cb = NULL; - - g_free(mod_gps); - mod_gps = NULL; + module->status_cb = NULL; + module->pos_cb = NULL; + module->batch_cb = NULL; + module->sat_cb = NULL; + module->pos_interval = 0; + module->fused_status = 0; + g_free(module); + module = NULL; } diff --git a/module/log.h b/module/log.h index 14fac72..9a392e3 100644 --- a/module/log.h +++ b/module/log.h @@ -1,7 +1,7 @@ /* * gps-manager * - * Copyright (c) 2011-2013 Samsung Electronics Co., Ltd. All rights reserved. + * Copyright (c) 2011-2017 Samsung Electronics Co., Ltd. All rights reserved. * * Contact: Youngae Kang , Minjune Kim * Genie Kim @@ -31,6 +31,13 @@ #define MOD_LOGE(fmt, args...) LOG(LOG_ERROR, TAG_LOCATION, fmt, ##args) #define MOD_SECLOG(fmt, args...) SECURE_LOG(LOG_DEBUG, TAG_LOCATION, fmt, ##args) +#define TAG_MOD_FUSED "LOCATION_FUSED" +#define MOD_FUSED_LOGD(fmt, args...) LOG(LOG_DEBUG, TAG_MOD_FUSED, fmt, ##args) +#define MOD_FUSED_LOGW(fmt, args...) LOG(LOG_WARN, TAG_MOD_FUSED, fmt, ##args) +#define MOD_FUSED_LOGI(fmt, args...) LOG(LOG_INFO, TAG_MOD_FUSED, fmt, ##args) +#define MOD_FUSED_LOGE(fmt, args...) LOG(LOG_ERROR, TAG_MOD_FUSED, fmt, ##args) +#define MOD_FUSED_SECLOG(fmt, args...) SECURE_LOG(LOG_DEBUG, TAG_MOD_FUSED, fmt, ##args) + #define TAG_MOD_NPS "LOCATION_NPS" #define MOD_NPS_LOGD(fmt, args...) LOG(LOG_DEBUG, TAG_MOD_NPS, fmt, ##args) #define MOD_NPS_LOGW(fmt, args...) LOG(LOG_WARN, TAG_MOD_NPS, fmt, ##args) diff --git a/module/nps_module.c b/module/nps_module.c index 86c4dfb..9fe6c03 100644 --- a/module/nps_module.c +++ b/module/nps_module.c @@ -38,6 +38,8 @@ #define MAX_NPS_LOC_ITEM 6 #define MOCK_LOCATION_CLEAR_VALUE 999 +#define UNNECESSARY_INTERVAL 1 +#define LBS_FUSED_STATUS_NONE 0 typedef struct { lbs_client_dbus_h lbs_client; @@ -45,14 +47,15 @@ typedef struct { LocModStatusCB status_cb; LocModPositionExtCB pos_cb; gboolean is_started; + gint fused_status; } ModNpsData; static void status_callback(GVariant *param, void *user_data) { - ModNpsData *mod_nps = (ModNpsData *) user_data; - g_return_if_fail(mod_nps); + ModNpsData *module = (ModNpsData *) user_data; + g_return_if_fail(module); g_return_if_fail(param); - g_return_if_fail(mod_nps->status_cb); + g_return_if_fail(module->status_cb); int status = 0, method = 0; @@ -64,18 +67,18 @@ static void status_callback(GVariant *param, void *user_data) if (status == 3) { /*TODO: LBS_STATUS_AVAILABLE ? */ MOD_NPS_LOGD("LBS_STATUS_AVAILABLE"); - mod_nps->status_cb(TRUE, LOCATION_STATUS_2D_FIX, mod_nps->userdata); + module->status_cb(TRUE, LOCATION_STATUS_2D_FIX, module->userdata); } else { MOD_NPS_LOGD("LBS_STATUS_ACQUIRING/ERROR/UNAVAILABLE"); - mod_nps->status_cb(FALSE, LOCATION_STATUS_NO_FIX, mod_nps->userdata); + module->status_cb(FALSE, LOCATION_STATUS_NO_FIX, module->userdata); } } static void position_callback(GVariant *param, void *user_data) { - ModNpsData *mod_nps = (ModNpsData *)user_data; - g_return_if_fail(mod_nps); - g_return_if_fail(mod_nps->pos_cb); + ModNpsData *module = (ModNpsData *)user_data; + g_return_if_fail(module); + g_return_if_fail(module->pos_cb); int method = 0, fields = 0 , timestamp = 0 , level = 0; double latitude = 0.0, longitude = 0.0, altitude = 0.0, speed = 0.0, direction = 0.0, climb = 0.0, horizontal = 0.0, vertical = 0.0; @@ -104,7 +107,7 @@ static void position_callback(GVariant *param, void *user_data) acc = location_accuracy_new(LOCATION_ACCURACY_LEVEL_DETAILED, horizontal, vertical); MOD_NPS_LOGD("method(%d)", method); - mod_nps->pos_cb(TRUE, pos, vel, acc, mod_nps->userdata); + module->pos_cb(TRUE, pos, vel, acc, module->userdata); location_position_free(pos); location_velocity_free(vel); @@ -125,23 +128,23 @@ static void on_signal_callback(const gchar *sig, GVariant *param, gpointer user_ static int start(gpointer handle, LocModStatusCB status_cb, LocModPositionExtCB pos_cb, gpointer userdata) { MOD_NPS_LOGD("start"); - ModNpsData *mod_nps = (ModNpsData *) handle; - g_return_val_if_fail(mod_nps, LOCATION_ERROR_NOT_AVAILABLE); + ModNpsData *module = (ModNpsData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(status_cb, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(pos_cb, LOCATION_ERROR_NOT_AVAILABLE); - mod_nps->status_cb = status_cb; - mod_nps->pos_cb = pos_cb; - mod_nps->userdata = userdata; + module->status_cb = status_cb; + module->pos_cb = pos_cb; + module->userdata = userdata; int ret = LBS_CLIENT_ERROR_NONE; - ret = lbs_client_create(LBS_CLIENT_METHOD_NPS, &(mod_nps->lbs_client)); - if (ret != LBS_CLIENT_ERROR_NONE || !mod_nps->lbs_client) { + ret = lbs_client_create(LBS_CLIENT_METHOD_NPS, &(module->lbs_client)); + if (ret != LBS_CLIENT_ERROR_NONE || !module->lbs_client) { MOD_NPS_LOGE("Fail to create lbs_client_h. Error[%d]", ret); return LOCATION_ERROR_NOT_AVAILABLE; } - ret = lbs_client_start(mod_nps->lbs_client, 1, LBS_CLIENT_LOCATION_CB | LBS_CLIENT_LOCATION_STATUS_CB, on_signal_callback, mod_nps); + ret = lbs_client_start(module->lbs_client, UNNECESSARY_INTERVAL, LBS_CLIENT_LOCATION_CB | LBS_CLIENT_LOCATION_STATUS_CB, on_signal_callback, LBS_FUSED_STATUS_NONE, module); if (ret != LBS_CLIENT_ERROR_NONE) { if (ret == LBS_CLIENT_ERROR_ACCESS_DENIED) { MOD_NPS_LOGE("Access denied[%d]", ret); @@ -149,8 +152,8 @@ static int start(gpointer handle, LocModStatusCB status_cb, LocModPositionExtCB } MOD_NPS_LOGE("Fail to start lbs_client_h. Error[%d]", ret); - lbs_client_destroy(mod_nps->lbs_client); - mod_nps->lbs_client = NULL; + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; return LOCATION_ERROR_NOT_AVAILABLE; } @@ -160,29 +163,29 @@ static int start(gpointer handle, LocModStatusCB status_cb, LocModPositionExtCB static int stop(gpointer handle) { MOD_NPS_LOGD("stop"); - ModNpsData *mod_nps = (ModNpsData *) handle; - g_return_val_if_fail(mod_nps, LOCATION_ERROR_NOT_AVAILABLE); - g_return_val_if_fail(mod_nps->lbs_client, LOCATION_ERROR_NOT_AVAILABLE); - g_return_val_if_fail(mod_nps->status_cb, LOCATION_ERROR_NOT_AVAILABLE); + ModNpsData *module = (ModNpsData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); + g_return_val_if_fail(module->lbs_client, LOCATION_ERROR_NOT_AVAILABLE); + g_return_val_if_fail(module->status_cb, LOCATION_ERROR_NOT_AVAILABLE); int ret = LBS_CLIENT_ERROR_NONE; - ret = lbs_client_stop(mod_nps->lbs_client); + ret = lbs_client_stop(module->lbs_client, UNNECESSARY_INTERVAL, LBS_FUSED_STATUS_NONE); if (ret != LBS_CLIENT_ERROR_NONE) { MOD_NPS_LOGE("Fail to stop. Error[%d]", ret); - lbs_client_destroy(mod_nps->lbs_client); - mod_nps->lbs_client = NULL; + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; return LOCATION_ERROR_NOT_AVAILABLE; } - lbs_client_destroy(mod_nps->lbs_client); - mod_nps->lbs_client = NULL; + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; - if (mod_nps->status_cb) - mod_nps->status_cb(FALSE, LOCATION_STATUS_NO_FIX, mod_nps->userdata); + if (module->status_cb) + module->status_cb(FALSE, LOCATION_STATUS_NO_FIX, module->userdata); - mod_nps->status_cb = NULL; - mod_nps->pos_cb = NULL; + module->status_cb = NULL; + module->pos_cb = NULL; return LOCATION_ERROR_NONE; } @@ -190,8 +193,8 @@ static int stop(gpointer handle) static int get_last_position(gpointer handle, LocationPosition **position, LocationVelocity **velocity, LocationAccuracy **accuracy) { MOD_NPS_LOGD("get_last_position"); - ModNpsData *mod_nps = (ModNpsData *) handle; - g_return_val_if_fail(mod_nps, LOCATION_ERROR_NOT_AVAILABLE); + ModNpsData *module = (ModNpsData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(position, LOCATION_ERROR_PARAMETER); g_return_val_if_fail(velocity, LOCATION_ERROR_PARAMETER); g_return_val_if_fail(accuracy, LOCATION_ERROR_PARAMETER); @@ -239,34 +242,43 @@ static int get_last_position(gpointer handle, LocationPosition **position, Locat snprintf(location, sizeof(location), "%s", str); free(str); + index = 0; last_location[index] = (char *)strtok_r(location, ";", &last); - while (last_location[index++] != NULL) { - if (index == MAX_NPS_LOC_ITEM) + while (last_location[index] != NULL) { + switch (index) { + case 0: + latitude = strtod(last_location[index], NULL); + break; + case 1: + longitude = strtod(last_location[index], NULL); + break; + case 2: + altitude = strtod(last_location[index], NULL); + break; + case 3: + speed = strtod(last_location[index], NULL); break; + case 4: + direction = strtod(last_location[index], NULL); + break; + case 5: + hor_accuracy = strtod(last_location[index], NULL); + break; + default: + break; + } + if (++index == MAX_NPS_LOC_ITEM) break; last_location[index] = (char *)strtok_r(NULL, ";", &last); } - - if (index != MAX_NPS_LOC_ITEM) { - MOD_NPS_LOGD("Error item index"); - return LOCATION_ERROR_NOT_AVAILABLE; - } - index = 0; - latitude = strtod(last_location[index++], NULL); - longitude = strtod(last_location[index++], NULL); - altitude = strtod(last_location[index++], NULL); - speed = strtod(last_location[index++], NULL); - direction = strtod(last_location[index++], NULL); - hor_accuracy = strtod(last_location[index], NULL); } } - level = LOCATION_ACCURACY_LEVEL_STREET; - if (timestamp) status = LOCATION_STATUS_3D_FIX; else return LOCATION_ERROR_NOT_AVAILABLE; + level = LOCATION_ACCURACY_LEVEL_STREET; *position = location_position_new((guint) timestamp, latitude, longitude, altitude, status); *velocity = location_velocity_new((guint) timestamp, speed, direction, 0.0); *accuracy = location_accuracy_new(level, hor_accuracy, -1.0); @@ -278,22 +290,22 @@ static int set_mock_location(gpointer handle, LocationPosition *position, Locati LocationAccuracy *accuracy, LocModStatusCB mock_status_cb, gpointer userdata) { MOD_LOGD("ENTER >>> set_mock_location"); - ModNpsData *mod_nps = (ModNpsData *) handle; - g_return_val_if_fail(mod_nps, LOCATION_ERROR_NOT_AVAILABLE); + ModNpsData *module = (ModNpsData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); int ret = LBS_CLIENT_ERROR_NONE; - if (mod_nps->lbs_client == NULL) { - ret = lbs_client_create(LBS_CLIENT_METHOD_NPS , &(mod_nps->lbs_client)); - if (ret != LBS_CLIENT_ERROR_NONE || !mod_nps->lbs_client) { + if (module->lbs_client == NULL) { + ret = lbs_client_create(LBS_CLIENT_METHOD_NPS , &(module->lbs_client)); + if (ret != LBS_CLIENT_ERROR_NONE || !module->lbs_client) { MOD_LOGE("Fail to create lbs_client_h. Error[%d]", ret); return LOCATION_ERROR_NOT_AVAILABLE; } } - mod_nps->userdata = userdata; + module->userdata = userdata; - ret = lbs_client_set_mock_location_async(mod_nps->lbs_client, LBS_CLIENT_METHOD_NPS, + ret = lbs_client_set_mock_location_async(module->lbs_client, LBS_CLIENT_METHOD_NPS, position->latitude, position->longitude, position->altitude, - velocity->speed, velocity->direction, accuracy->horizontal_accuracy, NULL, mod_nps); + velocity->speed, velocity->direction, accuracy->horizontal_accuracy, NULL, module); if (ret != LBS_CLIENT_ERROR_NONE) { if (ret == LBS_CLIENT_ERROR_ACCESS_DENIED) { @@ -301,8 +313,8 @@ static int set_mock_location(gpointer handle, LocationPosition *position, Locati return LOCATION_ERROR_NOT_ALLOWED; } MOD_LOGE("Fail to start lbs_client_h. Error[%d]", ret); - lbs_client_destroy(mod_nps->lbs_client); - mod_nps->lbs_client = NULL; + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; return LOCATION_ERROR_NOT_AVAILABLE; } @@ -313,21 +325,21 @@ static int set_mock_location(gpointer handle, LocationPosition *position, Locati static int clear_mock_location(gpointer handle, LocModStatusCB mock_status_cb, gpointer userdata) { MOD_LOGD("ENTER >>> clear_mock_location"); - ModNpsData *mod_nps = (ModNpsData *) handle; - g_return_val_if_fail(mod_nps, LOCATION_ERROR_NOT_AVAILABLE); + ModNpsData *module = (ModNpsData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); int ret = LBS_CLIENT_ERROR_NONE; - if (mod_nps->lbs_client == NULL) { - ret = lbs_client_create(LBS_CLIENT_METHOD_NPS , &(mod_nps->lbs_client)); - if (ret != LBS_CLIENT_ERROR_NONE || !mod_nps->lbs_client) { + if (module->lbs_client == NULL) { + ret = lbs_client_create(LBS_CLIENT_METHOD_NPS , &(module->lbs_client)); + if (ret != LBS_CLIENT_ERROR_NONE || !module->lbs_client) { MOD_LOGE("Fail to create lbs_client_h. Error[%d]", ret); return LOCATION_ERROR_NOT_AVAILABLE; } } - mod_nps->userdata = userdata; + module->userdata = userdata; - ret = lbs_client_set_mock_location_async(mod_nps->lbs_client, LBS_CLIENT_METHOD_NPS, - MOCK_LOCATION_CLEAR_VALUE, 0, 0, 0, 0, 0, NULL, mod_nps); + ret = lbs_client_set_mock_location_async(module->lbs_client, LBS_CLIENT_METHOD_NPS, + MOCK_LOCATION_CLEAR_VALUE, 0, 0, 0, 0, 0, NULL, module); if (ret != LBS_CLIENT_ERROR_NONE) { if (ret == LBS_CLIENT_ERROR_ACCESS_DENIED) { @@ -335,8 +347,8 @@ static int clear_mock_location(gpointer handle, LocModStatusCB mock_status_cb, g return LOCATION_ERROR_NOT_ALLOWED; } MOD_LOGE("Fail to start lbs_client_h. Error[%d]", ret); - lbs_client_destroy(mod_nps->lbs_client); - mod_nps->lbs_client = NULL; + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; return LOCATION_ERROR_NOT_AVAILABLE; } @@ -354,34 +366,35 @@ LOCATION_MODULE_API gpointer init(LocModWpsOps *ops) ops->set_mock_location = set_mock_location; ops->clear_mock_location = clear_mock_location; - ModNpsData *mod_nps = g_new0(ModNpsData, 1); - g_return_val_if_fail(mod_nps, NULL); + ModNpsData *module = g_new0(ModNpsData, 1); + g_return_val_if_fail(module, NULL); - mod_nps->userdata = NULL; - mod_nps->status_cb = NULL; - mod_nps->pos_cb = NULL; - mod_nps->is_started = FALSE; + module->userdata = NULL; + module->status_cb = NULL; + module->pos_cb = NULL; + module->is_started = FALSE; + module->fused_status = 0; - return mod_nps; + return module; } LOCATION_MODULE_API void shutdown(gpointer handle) { MOD_NPS_LOGD("shutdown"); - ModNpsData *mod_nps = (ModNpsData *) handle; - g_return_if_fail(mod_nps); - - if (mod_nps->lbs_client) { - if (mod_nps->is_started) - lbs_client_stop(mod_nps->lbs_client); - - lbs_client_destroy(mod_nps->lbs_client); - mod_nps->lbs_client = NULL; + ModNpsData *module = (ModNpsData *) handle; + g_return_if_fail(module); + + if (module->lbs_client) { + if (module->is_started) + lbs_client_stop(module->lbs_client, UNNECESSARY_INTERVAL, LBS_FUSED_STATUS_NONE); + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; } - mod_nps->status_cb = NULL; - mod_nps->pos_cb = NULL; + module->status_cb = NULL; + module->pos_cb = NULL; + module->fused_status = 0; - g_free(mod_nps); - mod_nps = NULL; + g_free(module); + module = NULL; } diff --git a/module/passive_module.c b/module/passive_module.c index 7adfba6..e08693d 100644 --- a/module/passive_module.c +++ b/module/passive_module.c @@ -40,16 +40,198 @@ #define MAX_GPS_LOC_ITEM 7 #define MAX_NPS_LOC_ITEM 6 +#define UNNECESSARY_INTERVAL 1 +#define LBS_FUSED_STATUS_NONE 0 typedef struct { + lbs_client_dbus_h lbs_client; + LocModPositionExtCB pos_cb; + LocModSatelliteCB sat_cb; gpointer userdata; + gint fused_status; } ModPassiveData; +static void satellite_callback(GVariant *param, void *user_data) +{ + ModPassiveData *module = (ModPassiveData *)user_data; + g_return_if_fail(module); + g_return_if_fail(module->sat_cb); + + guint idx; + guint used_idx; + guint *used_prn_array = NULL; + gboolean ret = FALSE; + int timestamp = 0, satellite_used = 0, satellite_visible = 0; + + LocationSatellite *sat = NULL; + GVariant *used_prn = NULL; + GVariantIter *used_prn_iter = NULL; + GVariant *sat_info = NULL; + GVariantIter *sat_iter = NULL; + int prn = 0, elev = 0, azim = 0, snr = 0; + + g_variant_get(param, "(iii@ai@a(iiii))", ×tamp, &satellite_used, &satellite_visible, &used_prn, &sat_info); + g_variant_get(used_prn, "ai", &used_prn_iter); + g_variant_get(sat_info, "a(iiii)", &sat_iter); + MOD_LOGD("timestamp [%d], satellite_used [%d], satellite_visible[%d]", timestamp, satellite_used, satellite_visible); + int tmp_prn = 0; + int num_of_used_prn = g_variant_iter_n_children(used_prn_iter); + if (num_of_used_prn > 0) { + used_prn_array = (guint *)g_new0(guint, num_of_used_prn); + g_return_if_fail(used_prn_array); + for (idx = 0; idx < num_of_used_prn; idx++) { + ret = g_variant_iter_next(used_prn_iter, "i", &tmp_prn); + if (ret == FALSE) + break; + used_prn_array[idx] = tmp_prn; + } + } + sat = location_satellite_new(satellite_visible); + + sat->timestamp = timestamp; + sat->num_of_sat_inview = satellite_visible; + sat->num_of_sat_used = satellite_used; + + GVariant *tmp_var = NULL; + for (idx = 0; idx < satellite_visible; idx++) { + gboolean used = FALSE; + tmp_var = g_variant_iter_next_value(sat_iter); + g_variant_get(tmp_var, "(iiii)", &prn, &elev, &azim, &snr); + if (used_prn_array != NULL) { + for (used_idx = 0; used_idx < satellite_used; used_idx++) { + if (prn == used_prn_array[used_idx]) { + used = TRUE; + break; + } + } + } + location_satellite_set_satellite_details(sat, idx, prn, used, elev, azim, snr); + g_variant_unref(tmp_var); + } + + module->sat_cb(TRUE, sat, module->userdata); + location_satellite_free(sat); + g_variant_iter_free(used_prn_iter); + g_variant_iter_free(sat_iter); + g_variant_unref(used_prn); + g_variant_unref(sat_info); + + if (used_prn_array) { + g_free(used_prn_array); + used_prn_array = NULL; + } +} + +static void position_callback(GVariant *param, void *user_data) +{ + ModPassiveData *module = (ModPassiveData *)user_data; + g_return_if_fail(module); + g_return_if_fail(module->pos_cb); + + int method = 0, fields = 0 , timestamp = 0 , level = 0; + double latitude = 0.0, longitude = 0.0, altitude = 0.0, speed = 0.0, direction = 0.0, climb = 0.0, horizontal = 0.0, vertical = 0.0; + GVariant *accuracy = NULL; + + g_variant_get(param, "(iiidddddd@(idd))", &method, &fields, ×tamp, &latitude, &longitude, &altitude, &speed, &direction, &climb, &accuracy); + g_variant_get(accuracy, "(idd)", &level, &horizontal, &vertical); + + LocationPosition *pos = NULL; + LocationVelocity *vel = NULL; + LocationAccuracy *acc = NULL; + + pos = location_position_new(timestamp, latitude, longitude, altitude, LOCATION_STATUS_3D_FIX); + vel = location_velocity_new(timestamp, speed, direction, climb); + acc = location_accuracy_new(LOCATION_ACCURACY_LEVEL_DETAILED, horizontal, vertical); + + module->pos_cb(TRUE, pos, vel, acc, module->userdata); + + location_position_free(pos); + location_velocity_free(vel); + location_accuracy_free(acc); + g_variant_unref(accuracy); +} + +static void on_signal_callback(const gchar *sig, GVariant *param, gpointer user_data) +{ + if (!g_strcmp0(sig, "SatelliteChanged")) + satellite_callback(param, user_data); + else if (!g_strcmp0(sig, "PositionChanged")) + position_callback(param, user_data); + else + MOD_LOGD("Invaild signal[%s]", sig); +} + +static int start(gpointer handle, guint pos_update_interval, LocModStatusCB status_cb, LocModPositionExtCB pos_cb, LocModSatelliteCB sat_cb, gpointer userdata) +{ + MOD_LOGD("start"); + ModPassiveData *module = (ModPassiveData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); + g_return_val_if_fail(pos_cb, LOCATION_ERROR_NOT_AVAILABLE); + + module->pos_cb = pos_cb; + module->sat_cb = sat_cb; + module->userdata = userdata; + + int ret = LBS_CLIENT_ERROR_NONE; + ret = lbs_client_create(LBS_CLIENT_METHOD_PASSIVE , &(module->lbs_client)); + if (ret != LBS_CLIENT_ERROR_NONE || !module->lbs_client) { + MOD_LOGE("Fail to create lbs_client_h. Error[%d]", ret); + return LOCATION_ERROR_NOT_AVAILABLE; + } + MOD_LOGD("fused handle(%p) pos_cb(%p) user_data(%p)", module, module->pos_cb, module->userdata); + + ret = lbs_client_start(module->lbs_client, UNNECESSARY_INTERVAL, LBS_CLIENT_LOCATION_CB | LBS_CLIENT_SATELLITE_CB, on_signal_callback, LBS_FUSED_STATUS_NONE, module); + if (ret != LBS_CLIENT_ERROR_NONE) { + if (ret == LBS_CLIENT_ERROR_ACCESS_DENIED) { + MOD_LOGE("Access denied[%d]", ret); + return LOCATION_ERROR_NOT_ALLOWED; + } + MOD_LOGE("Fail to start lbs_client_h. Error[%d]", ret); + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; + + return LOCATION_ERROR_NOT_AVAILABLE; + } + + return LOCATION_ERROR_NONE; +} + +static int stop(gpointer handle) +{ + MOD_LOGD("stop"); + ModPassiveData *module = (ModPassiveData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); + g_return_val_if_fail(module->lbs_client, LOCATION_ERROR_NOT_AVAILABLE); + + int ret = LBS_CLIENT_ERROR_NONE; + + ret = lbs_client_stop(module->lbs_client, UNNECESSARY_INTERVAL, LBS_FUSED_STATUS_NONE); + MOD_LOGE("stop gps interval [%d]", UNNECESSARY_INTERVAL); + if (ret != LBS_CLIENT_ERROR_NONE) { + MOD_LOGE("Fail to stop. Error[%d]", ret); + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; + return LOCATION_ERROR_NOT_AVAILABLE; + } + + ret = lbs_client_destroy(module->lbs_client); + if (ret != LBS_CLIENT_ERROR_NONE) { + MOD_LOGE("Fail to destroy. Error[%d]", ret); + return LOCATION_ERROR_NOT_AVAILABLE; + } + module->lbs_client = NULL; + + module->pos_cb = NULL; + module->sat_cb = NULL; + + return LOCATION_ERROR_NONE; +} + static int get_last_position(gpointer handle, LocationPosition **position, LocationVelocity **velocity, LocationAccuracy **accuracy) { MOD_LOGD("get_last_position"); - ModPassiveData *mod_passive = (ModPassiveData *) handle; - g_return_val_if_fail(mod_passive, LOCATION_ERROR_NOT_AVAILABLE); + ModPassiveData *module = (ModPassiveData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(position, LOCATION_ERROR_PARAMETER); g_return_val_if_fail(velocity, LOCATION_ERROR_PARAMETER); g_return_val_if_fail(accuracy, LOCATION_ERROR_PARAMETER); @@ -145,8 +327,8 @@ static int get_last_position(gpointer handle, LocationPosition **position, Locat static int get_last_wps_position(gpointer handle, LocationPosition **position, LocationVelocity **velocity, LocationAccuracy **accuracy) { MOD_LOGD("get_last_wps_position"); - ModPassiveData *mod_passive = (ModPassiveData *) handle; - g_return_val_if_fail(mod_passive, LOCATION_ERROR_NOT_AVAILABLE); + ModPassiveData *module = (ModPassiveData *) handle; + g_return_val_if_fail(module, LOCATION_ERROR_NOT_AVAILABLE); g_return_val_if_fail(position, LOCATION_ERROR_PARAMETER); g_return_val_if_fail(velocity, LOCATION_ERROR_PARAMETER); g_return_val_if_fail(accuracy, LOCATION_ERROR_PARAMETER); @@ -243,24 +425,38 @@ LOCATION_MODULE_API gpointer init(LocModPassiveOps *ops) MOD_LOGD("init"); g_return_val_if_fail(ops, NULL); + ops->start = start; + ops->stop = stop; ops->get_last_position = get_last_position; ops->get_last_wps_position = get_last_wps_position; - ModPassiveData *mod_passive = g_new0(ModPassiveData, 1); - g_return_val_if_fail(mod_passive, NULL); + ModPassiveData *module = g_new0(ModPassiveData, 1); + g_return_val_if_fail(module, NULL); - mod_passive->userdata = NULL; + module->pos_cb = NULL; + module->sat_cb = NULL; + module->userdata = NULL; + module->fused_status = 0; - return (gpointer) mod_passive; + return (gpointer) module; } LOCATION_MODULE_API void shutdown(gpointer handle) { MOD_LOGD("shutdown"); g_return_if_fail(handle); - ModPassiveData *mod_passive = (ModPassiveData *) handle; + ModPassiveData *module = (ModPassiveData *) handle; + + if (module->lbs_client) { + lbs_client_stop(module->lbs_client, UNNECESSARY_INTERVAL, LBS_FUSED_STATUS_NONE); + lbs_client_destroy(module->lbs_client); + module->lbs_client = NULL; + } - g_free(mod_passive); - mod_passive = NULL; + module->pos_cb = NULL; + module->sat_cb = NULL; + g_free(module); + module = NULL; + module->fused_status = 0; } diff --git a/packaging/lbs-server.changes b/packaging/lbs-server.changes index 8742859..17b0ab6 100644 --- a/packaging/lbs-server.changes +++ b/packaging/lbs-server.changes @@ -1,3 +1,34 @@ +[Version] lbs-server_1.1.6 +[Date] 28 Mar 2017 +[Changes] Fix dynamic interval to support multi handle +[Developer] Kyoungjun Sung + +================================================================================ +[Version] lbs-server_1.0.12 +[Date] 1 Feb 2017 +[Changes] Support satellite for passive mode + Fix to support SUPLNI +[Developer] Kyoungjun Sung + +================================================================================ +[Version] lbs-server_1.0.11 +[Date] 20 Dec 2016 +[Changes] GPS plugin list updated to support serial mode +[Developer] Kyoungjun Sung + +================================================================================ +[Version] lbs-server_1.0.10 +[Date] 21 Nov 2016 +[Changes] Fix to robustness test +[Developer] Kyoungjun Sung + +================================================================================ +[Version] lbs-server_1.0.9 +[Date] 28 Oct 2016 +[Changes] Dynamic interval table for multi handle +[Developer] Kyoungjun Sung + +================================================================================ [Version] lbs-server_1.0.8 [Date] 6 Sep 2016 [Changes] Passive location. diff --git a/packaging/lbs-server.spec b/packaging/lbs-server.spec index 111512a..1ae8d32 100644 --- a/packaging/lbs-server.spec +++ b/packaging/lbs-server.spec @@ -1,6 +1,6 @@ Name: lbs-server Summary: LBS Server for Tizen -Version: 1.0.8 +Version: 1.1.7 Release: 1 Group: Location/Service License: Apache-2.0 @@ -71,9 +71,6 @@ export FFLAGS="$FFLAGS -DTIZEN_DEVICE" MAJORVER=`echo %{version} | awk 'BEGIN {FS="."}{print $1}'` cmake . -DCMAKE_INSTALL_PREFIX=%{_prefix} -DFULLVER=%{version} -DMAJORVER=${MAJORVER} \ -DLIB_DIR=%{_libdir} -DINCLUDE_DIR=%{_includedir} -DSYSCONF_DIR=%{_sysconfdir} \ -#%if 0%{?model_build_feature_location_position_wps} -# -DENABLE_WPS=YES \ -#%endif make %{?jobs:-j%jobs} @@ -125,14 +122,12 @@ rm -rf %{buildroot} %files -n location-lbs-server %manifest location-lbs-server.manifest +%license LICENSE %{_libdir}/location/module/libgps.so* +%{_libdir}/location/module/libwps.so* %{_libdir}/location/module/libpassive.so* %{_libdir}/location/module/libfused.so* -%if 0%{?model_build_feature_location_position_wps} -%{_libdir}/location/module/libwps.so* -%endif - %files -n lbs-server-plugin-devel %{_libdir}/pkgconfig/lbs-server-plugin.pc %{_includedir}/lbs-server-plugin/*.h