path: root/lbs-server/src/fused.c

/*
 * 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 <time.h>
#include <math.h>
#include <stdlib.h>
#include <sensor.h>
#include <vconf.h>
#include "fused/FusionEngine.h"
#include "fused.h"
#include "debug_util.h"
#include <pthread.h>

#define PTR2ENUM(ptr, type)   ((type)(int)((char*)(ptr) - ((char*)0)))
#define ENUM2PTR(value)       (((char*)0) + (int)(value))

#define PEDOMETER_URI "http://samsung.com/sensor/healthinfo/pedometer/samsung_pedometer"

/** Container of the standard sensor reference data */
typedef struct {
	sensor_type_e type;
	sensor_h handle;
	sensor_listener_h listener;
	gboolean is_available;
	gboolean is_started;
} _fl_sensor;

/** Fused location data */
typedef struct {
	bool                      is_started;
	bool                      is_initialized;
	WgsLocation               last_location;
	_fl_sensor                sensors[SENSOR_SOURCE_COUNT];
	gpointer                  lbs_server_handle;
	fused_updated_cb          fused_pos_cb;
	FusionEngine              fusion_engine;
} LocationFusedData;

static LocationFusedData fused;
static pthread_mutex_t fusion_engine_input_sync_mutex = PTHREAD_MUTEX_INITIALIZER;

static void sensors_create()
{
	LOG_FUSED_FUNC;
	sensor_error_e ret;
	unsigned int i;
	for (i = 0; i < SENSOR_SOURCE_COUNT; i++) {
		if (i == PEDOMETER)
			ret = sensor_get_default_sensor_by_uri(PEDOMETER_URI, &fused.sensors[i].handle);
		else
			ret = sensor_get_default_sensor(fused.sensors[i].type, &fused.sensors[i].handle);

		if (ret != SENSOR_ERROR_NONE) {
			LOG_FUSED(LOG_ERROR, "sensor_get_default_sensor() FAILED, [%d]", i);
			fused.sensors[i].is_available = FALSE;
		}

		ret = sensor_create_listener(fused.sensors[i].handle, &fused.sensors[i].listener);
		if (ret == SENSOR_ERROR_NONE) {
			ret = sensor_listener_set_event_cb(fused.sensors[i].listener, FL_SENSOR_SAMPLING_INTERVAL, on_sensor_event, ENUM2PTR(i));
			if (ret == SENSOR_ERROR_NONE) {
				sensor_listener_set_option(fused.sensors[i].listener, SENSOR_OPTION_ALWAYS_ON);
				fused.sensors[i].is_available = TRUE;
			}
		}
	}
}

static void sensors_clear()
{
	LOG_FUSED_FUNC;
	unsigned int i;
	for (i = 0; i < SENSOR_SOURCE_COUNT; i++) {
		fused.sensors[i].handle = NULL;
		fused.sensors[i].listener = NULL;
		fused.sensors[i].is_available = FALSE;
		fused.sensors[i].is_started = FALSE;
	}
}

static void sensors_reset_listeners()
{
	LOG_FUSED_FUNC;
	unsigned int i;
	for (i = 0; i < SENSOR_SOURCE_COUNT; i++) {
		if (fused.sensors[i].listener) {
			sensor_listener_unset_event_cb(fused.sensors[i].listener);
			sensor_destroy_listener(fused.sensors[i].listener);
			fused.sensors[i].listener = NULL;
		}
	}
}

static void sensors_destroy()
{
	LOG_FUSED_FUNC;
	sensors_reset_listeners();
	sensors_clear();
}

static void sensor_start(fl_sensor_source sensor)
{
	LOG_FUSED_FUNC;
	if (fused.sensors[sensor].handle
			&& fused.sensors[sensor].listener
			&& fused.sensors[sensor].is_available
			&& !fused.sensors[sensor].is_started) {
		sensor_error_e ret = sensor_listener_start(fused.sensors[sensor].listener);
		if (ret == SENSOR_ERROR_NONE)
			fused.sensors[sensor].is_started = TRUE;
	}
}

static void sensor_stop(fl_sensor_source sensor)
{
	LOG_FUSED_FUNC;
	if (fused.sensors[sensor].handle
			&& fused.sensors[sensor].listener
			&& fused.sensors[sensor].is_available
			&& fused.sensors[sensor].is_started) {
		sensor_error_e ret = sensor_listener_stop(fused.sensors[sensor].listener);
		if (ret == SENSOR_ERROR_NONE)
			fused.sensors[sensor].is_started = FALSE;
	}
}

unsigned long long get_current_boot_time()
{
	struct timespec time;
#ifdef CLOCK_BOOTTIME
	clock_gettime(CLOCK_BOOTTIME, &time);
#else
	clock_gettime(CLOCK_MONOTONIC_RAW, &time);
#endif
	return (unsigned long long) time.tv_sec * 1000000000 + time.tv_nsec;
}

void location_fused_start()
{
	LOG_FUSED_FUNC;

	if (!fused.is_initialized || fused.is_started) {
		LOG_FUSED(LOG_DEBUG, "[FUSED] Fused Location Start failed!!");
		return;
	}

	char *calibration = vconf_get_str(VCONFKEY_LOCATION_GYRO_DRIFT);
	LOG_FUSED(LOG_DEBUG, "calibration: vconf_get_str(VCONFKEY_LOCATION_GYRO_DRIFT) %s = \"%s\"",
			calibration != NULL ? "SUCCESS" : "FAILED",
			calibration != NULL ? calibration : "EMPTY");
	fusion_engine_set_calibration(&fused.fusion_engine, calibration);

	fused.is_started = TRUE;
	fusion_engine_start(&fused.fusion_engine);
	unsigned int i;
	for (i = 0; i < SENSOR_SOURCE_COUNT; i++) {
		sensor_start(i);
	}
	free(calibration);
	calibration = NULL;
}

void location_fused_stop()
{
	LOG_FUSED_FUNC;

	if (!fused.is_initialized || !fused.is_started)
		return;

	unsigned int i;
	for (i = 0; i < SENSOR_SOURCE_COUNT; i++) {
		sensor_stop(i);
	}

	char *calibration = fusion_engine_get_calibration(&fused.fusion_engine);
	if (calibration) {
		int ret = vconf_set_str(VCONFKEY_LOCATION_GYRO_DRIFT, calibration);
		LOG_FUSED(LOG_DEBUG, "calibration: vconf_set_str(VCONFKEY_LOCATION_GYRO_DRIFT) %s = \"%s\"",
				ret == VCONF_OK ? "SUCCESS" : "FAILED", calibration);
		free(calibration);
	} else {
		LOG_FUSED(LOG_DEBUG, "fusion_engine_get_calibration() FAILED");
	}

	fusion_engine_stop(&fused.fusion_engine);
	fused.is_started = FALSE;
}

bool location_fused_init(fused_updated_cb _fused_pos_cb, gpointer lbs_server)
{
	LOG_FUSED_FUNC;

	fused.is_initialized = FALSE;
	if (!_fused_pos_cb || !lbs_server) {
		LOG_FUSED(LOG_ERROR, "location_fused_init FAILED (NULL arguments)");
		return fused.is_initialized;
	}
	fused.fused_pos_cb = _fused_pos_cb;
	fused.lbs_server_handle = lbs_server;

	time_t now = time(NULL);
	unsigned long long boot_time_ns = get_current_boot_time();
	double utc_offset_sec = (double)now - (double)boot_time_ns / 1000000000.0;
	LOG_FUSED(LOG_DEBUG, "now = %lu, boot_time_ns = %llu, utc_offset_sec = %.10f", now, boot_time_ns, utc_offset_sec);
	fused.is_initialized = fusion_engine_init(&fused.fusion_engine, utc_offset_sec);
	if (!fused.is_initialized) {
		LOG_FUSED(LOG_ERROR, "location_fused_init FAILED (fusion_engine_init FAILED)");
		return fused.is_initialized;
	}

	fused.sensors[ACCELEROMETER].type = SENSOR_ACCELEROMETER;
	fused.sensors[GYROSCOPE].type = SENSOR_GYROSCOPE;
	fused.sensors[PEDOMETER].type = SENSOR_HUMAN_PEDOMETER;
	sensors_clear();
	sensors_create();

	fused.is_started = FALSE;

	return fused.is_initialized;
}

void location_fused_deinit()
{
	LOG_FUSED_FUNC;
	if (fused.is_initialized) {
		if (fused.is_started)
			location_fused_stop();

		sensors_destroy();
		fusion_engine_destroy(&fused.fusion_engine);
		fused.fused_pos_cb = NULL;
		fused.lbs_server_handle = NULL;
		fused.is_started = FALSE;
		fused.is_initialized = FALSE;
	}
}

void send_gps_position_to_fused_engine(time_t timestamp, double latitude, double longitude, double altitude,
		double speed, double bearing, double hor_accuracy, double ver_accuracy)
{
	if (!fused.is_initialized || !fused.is_started)
		return;

	pthread_mutex_lock(&fusion_engine_input_sync_mutex);

	bool is_new_position = fusion_engine_gps_event(&fused.fusion_engine, (double)timestamp,	latitude, longitude, altitude,
			KMPH_TO_MPS(speed), bearing, hor_accuracy, ver_accuracy);

	if (is_new_position)
		on_engine_new_position();

	pthread_mutex_unlock(&fusion_engine_input_sync_mutex);
}

void send_wps_position_to_fused_engine(time_t timestamp, double latitude, double longitude, double hor_accuracy)
{
	if (!fused.is_initialized || !fused.is_started)
		return;

	pthread_mutex_lock(&fusion_engine_input_sync_mutex);

	bool is_new_position = fusion_engine_wps_event(&fused.fusion_engine, (double)timestamp, latitude, longitude, hor_accuracy);

	if (is_new_position)
		on_engine_new_position();

	pthread_mutex_unlock(&fusion_engine_input_sync_mutex);
}

static void on_engine_new_position()
{
	fusion_engine_get_wgs_location(&fused.fusion_engine, &fused.last_location);

	if (fused.fused_pos_cb)
		fused.fused_pos_cb((gint)fused.last_location.utc_time,
				(gdouble)fused.last_location.latitude,
				(gdouble)fused.last_location.longitude,
				(gdouble)fused.last_location.altitude,
				(gdouble)MPS_TO_KMPH(fused.last_location.speed),
				(gdouble)fused.last_location.direction,
				(gdouble)fused.last_location.climb,
				(gdouble)fused.last_location.hor_dev,
				(gdouble)fused.last_location.ver_dev,
				fused.lbs_server_handle);
	else
		LOG_FUSED(LOG_ERROR, "fused_pos_cb == NULL");
}

static void on_sensor_event(sensor_h handle, sensor_event_s *event, gpointer sensor_id)
{
	fl_sensor_source sensor_type = PTR2ENUM(sensor_id, fl_sensor_source);
	if (event) {
		pthread_mutex_lock(&fusion_engine_input_sync_mutex);

		bool is_new_position = false;
		switch (sensor_type) {
		case ACCELEROMETER:
			is_new_position = fusion_engine_acc_event(&fused.fusion_engine,
					(double)(long long)event->timestamp / 1000000.0,
					event->values[X], event->values[Y], event->values[Z]);
			break;

		case GYROSCOPE:
			is_new_position = fusion_engine_gyro_event(&fused.fusion_engine,
					(double)(long long)event->timestamp / 1000000.0,
					to_radians(event->values[X]),
					to_radians(event->values[Y]),
					to_radians(event->values[Z]));
			break;

		case PEDOMETER:
			is_new_position = fusion_engine_pedometer_event(&fused.fusion_engine,
					(double)(long long)event->timestamp / 1000000.0, event->values[5], 1.0);
			break;
		default:
			LOG_FUSED(LOG_ERROR, "unsupported sensor type[%d]", sensor_type);
			break;
		}

		if (is_new_position)
			on_engine_new_position();

		pthread_mutex_unlock(&fusion_engine_input_sync_mutex);
	} else {
		LOG_FUSED(LOG_ERROR, "event == NULL");
	}
}