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)
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}\"")
${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
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")
pdns_lookup_addr = ptr;
ptr = (char *)strtok_r(NULL, ":", &last);
+
if (ptr)
*port = atoi(ptr);
}
/*
* 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 <youngae.kang@samsung.com>, Minjune Kim <sena06.kim@samsung.com>
* Genie Kim <daejins.kim@samsung.com>
#include <glib.h>
#include <libgen.h>
+#include "location-fused/parameters.h"
#ifdef __cplusplus
extern "C" {
#endif
#include <dlog.h>
-#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_ */
*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;
}
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_ */
/*
* 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 <youngae.kang@samsung.com>, Minjune Kim <sena06.kim@samsung.com>
* Genie Kim <daejins.kim@samsung.com>
#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 */
gboolean nmea_used;
gboolean is_gps_running;
- gint gps_client_count;
+ guint gps_client_count;
/* nps variables */
NpsManagerHandle handle;
Plugin_LocationInfo location;
gboolean is_nps_running;
- gint nps_client_count;
+ guint nps_client_count;
unsigned char *token;
/* shared variables */
} 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)
{
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);
}
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");
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");
}
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;
}
#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;
}
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");
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;
/* 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)
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");
}
}
+/*
+* 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;
}
/* 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;
}
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) {
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)
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;
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")) {
}
}
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;
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]) {
#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));
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));
}
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)
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);
}
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;
#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)
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);
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())
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);
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",
LOG_GPS(DBG_LOW, "lbs_server deamon Stop....");
+ location_fused_deinit();
+
gps_deinit_log();
/* destroy resource for dynamic-interval */
setting_ignore_key_changed(VCONFKEY_LOCATION_MOCK_ENABLED, __setting_mock_cb);
g_mock_position.timestamp = 0;
}
- return ;
+ return;
}
time_t timestamp;
}
}
- 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;
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(""));
+}
/*
* 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 <youngae.kang@samsung.com>, Minjune Kim <sena06.kim@samsung.com>
* Genie Kim <daejins.kim@samsung.com>
#include <gps_plugin_data_types.h>
#include <gps_plugin_extra_data_types.h>
+#include <lbs_dbus_server.h>
#define SERVICE_NAME "org.tizen.lbs.Providers.LbsServer"
#define SERVICE_PATH "/org/tizen/lbs/Providers/LbsServer"
#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.
*
--- /dev/null
+/*
+ * 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 <math.h>
+#include <stdlib.h>
+#include <sensor.h>
+#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
--- /dev/null
+/*
+ * 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 <glib.h>
+
+/***************************************************************************//**
+ * 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__ */
--- /dev/null
+/*
+ * 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 <math.h>
+#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, <g>=%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 */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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 <math.h>
+#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 */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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 */
--- /dev/null
+/*
+ * 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_ */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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);
+}
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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 <math.h>
+#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 */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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 <float.h>
+#include <memory.h>
+#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 */
--- /dev/null
+/*
+ * 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 <glib.h>
+#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/(<u^2> - <u>^2)
+ * @param[in] w2v
+ * Inverse covariance of v: w2v = 1/(<v^2> - <v>^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__ */
--- /dev/null
+/*
+ * 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 <memory.h>
+#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 */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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 <stdio.h>
+#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 */
+
--- /dev/null
+/*
+ * 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 <math.h>
+#include <stdlib.h>
+
+#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__ */
--- /dev/null
+/*
+ * 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 <math.h>
+#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
--- /dev/null
+/*
+ * 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 <glib.h>
+#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__ */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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 <sensor.h> */
+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 <sensor.h> */
+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 <sensor.h> */
+typedef fl_real fl_utesla; /**< Magnetic field strength (B) in [uT] */
+typedef sensor_real sensor_utesla; /**< Magnetic field strength (B) in [uT] supplied by <sensor.h> */
+
+#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__ */
#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 */
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)
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);
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);
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})
/*
* 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 <youngae.kang@samsung.com>, Minjune Kim <sena06.kim@samsung.com>
* Genie Kim <daejins.kim@samsung.com>
#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;
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);
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;
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;
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) {
}
} 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);
}
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;
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;
#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;
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);
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;
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)
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);
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;
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);
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)
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;
}
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;
}
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;
}
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;
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);
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);
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);
return LOCATION_ERROR_NOT_AVAILABLE;
}
+ module->pos_interval = interval;
+
return LOCATION_ERROR_NONE;
}
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) {
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;
}
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) {
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;
}
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;
}
/*
* 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 <youngae.kang@samsung.com>, Minjune Kim <sena06.kim@samsung.com>
* Genie Kim <daejins.kim@samsung.com>
#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)
#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;
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;
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;
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);
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);
}
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;
}
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;
}
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);
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);
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) {
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;
}
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) {
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;
}
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;
}
#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);
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);
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;
}
+[Version] lbs-server_1.1.6
+[Date] 28 Mar 2017
+[Changes] Fix dynamic interval to support multi handle
+[Developer] Kyoungjun Sung <kj7.sung@samsung.com>
+
+================================================================================
+[Version] lbs-server_1.0.12
+[Date] 1 Feb 2017
+[Changes] Support satellite for passive mode
+ Fix to support SUPLNI
+[Developer] Kyoungjun Sung <kj7.sung@samsung.com>
+
+================================================================================
+[Version] lbs-server_1.0.11
+[Date] 20 Dec 2016
+[Changes] GPS plugin list updated to support serial mode
+[Developer] Kyoungjun Sung <kj7.sung@samsung.com>
+
+================================================================================
+[Version] lbs-server_1.0.10
+[Date] 21 Nov 2016
+[Changes] Fix to robustness test
+[Developer] Kyoungjun Sung <kj7.sung@samsung.com>
+
+================================================================================
+[Version] lbs-server_1.0.9
+[Date] 28 Oct 2016
+[Changes] Dynamic interval table for multi handle
+[Developer] Kyoungjun Sung <kj7.sung@samsung.com>
+
+================================================================================
[Version] lbs-server_1.0.8
[Date] 6 Sep 2016
[Changes] Passive location.
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
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}
%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
projects / platform / core / location / lbs-server.git / commitdiff