This is second of the four patches being submitted to merge the changes from devel/tizen to tizen branch.
This contains the changes for adding geomagnetic rotation vector virtual sensor.
Build has been checked for armv7l and aarch64 for public repo.
Change-Id: Ie91229c349a452e8d24e7d831c258f1b9d26d72e
%define gravity_state ON
%define linear_accel_state ON
%define rv_state ON
+%define geomagnetic_rv_state ON
%define build_test_suite OFF
%description
-DGEO=%{geo_state} -DPRESSURE=%{pressure_state} -DTEMPERATURE=%{temperature_state} \
-DORIENTATION=%{orientation_state} -DGRAVITY=%{gravity_state} \
-DLINEAR_ACCEL=%{linear_accel_state} -DRV=%{rv_state} \
+ -DGEOMAGNETIC_RV=%{geomagnetic_rv_state} \
-DTEST_SUITE=%{build_test_suite} \
-DLIBDIR=%{_libdir} -DINCLUDEDIR=%{_includedir}
%{_bindir}/temperature
%{_bindir}/light
%{_bindir}/rotation_vector
+%{_bindir}/geomagnetic_rv
%license LICENSE.APLv2
%endif
-
set(SENSOR_FUSION_ENABLE "1")
set(ORIENTATION_ENABLE "1")
ENDIF()
-IF("${RV}" STREQUAL "ON")
+IF("${GEOMAGNETIC_RV}" STREQUAL "ON")
set(SENSOR_FUSION_ENABLE "1")
ENDIF()
IF("${GRAVITY}" STREQUAL "ON")
install(FILES sensor_linear_accel.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/)
install(FILES sensor_orientation.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/)
install(FILES sensor_rv.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/)
+install(FILES sensor_geomagnetic_rv.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/)
install(FILES sensor_temperature.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/)
install(FILES sensor_motion.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/)
install(FILES sensor_deprecated.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/)
FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, ORIENTATION_SENSOR, 0, 1),
FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, TEMPERATURE_SENSOR, 0, 1),
FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, ROTATION_VECTOR_SENSOR, 0, 1),
+ FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, GEOMAGNETIC_RV_SENSOR, 0, 1),
FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_EVENT_CALIBRATION_NEEDED, 0, 1),
FILL_LOG_ELEMENT(LOG_ID_EVENT, PROXIMITY_EVENT_CHANGE_STATE, 0,1),
FILL_LOG_ELEMENT(LOG_ID_EVENT, PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10),
FILL_LOG_ELEMENT(LOG_ID_EVENT, TEMPERATURE_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10),
FILL_LOG_ELEMENT(LOG_ID_EVENT, ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10),
+ FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10),
FILL_LOG_ELEMENT(LOG_ID_DATA, ACCELEROMETER_BASE_DATA_SET, 0, 25),
FILL_LOG_ELEMENT(LOG_ID_DATA, GYRO_BASE_DATA_SET, 0, 25),
GEOMAGNETIC_RAW_DATA_SET = (GEOMAGNETIC_SENSOR << 16) | 0x0001,
};
-enum geomag_evet_type {
+enum geomag_event_type {
GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME = (GEOMAGNETIC_SENSOR << 16) | 0x0001,
GEOMAGNETIC_EVENT_CALIBRATION_NEEDED = (GEOMAGNETIC_SENSOR << 16) | 0x0002,
GEOMAGNETIC_EVENT_UNPROCESSED_DATA_REPORT_ON_TIME = (GEOMAGNETIC_SENSOR << 16) | 0x0003,
--- /dev/null
+/*
+ * libsensord
+ *
+ * Copyright (c) 2015 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.
+ *
+ */
+
+#ifndef __SENSOR_GEOMAGNETIC_RV_H__
+#define __SENSOR_GEOMAGNETIC_RV_H__
+
+//! Pre-defined events for the geomagnetic rotation vector sensor
+//! Sensor Plugin developer can add more event to their own headers
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @defgroup SENSOR_GEOMAGNETIC_RV Rotation Vector Sensor
+ * @ingroup SENSOR_FRAMEWORK
+ *
+ * These APIs are used to control the Geomagnetic Rotation Vector sensor.
+ * @{
+ */
+
+enum geomagnetic_rv_event_type {
+ GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME = (GEOMAGNETIC_RV_SENSOR << 16) | 0x0001,
+};
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+//! End of a file
+
#include <sensor_linear_accel.h>
#include <sensor_orientation.h>
#include <sensor_rv.h>
+#include <sensor_geomagnetic_rv.h>
#include <sensor_temperature.h>
#include <sensor_orientation.h>
#include <sensor_temperature.h>
#include <sensor_rv.h>
+#include <sensor_geomagnetic_rv.h>
#include <sensor_motion.h>
#include <sensor_deprecated.h>
LIGHT_BASE_DATA_SET = (LIGHT_SENSOR << 16) | 0x0002,
};
-enum light_evet_type {
+enum light_event_type {
LIGHT_EVENT_LUX_DATA_REPORT_ON_TIME = (LIGHT_SENSOR << 16) | 0x0001,
LIGHT_EVENT_LEVEL_DATA_REPORT_ON_TIME = (LIGHT_SENSOR << 16) | 0x0002,
LIGHT_EVENT_CHANGE_LEVEL = (LIGHT_SENSOR << 16) | 0x0004,
PRESSURE_BASE_DATA_SET = (PRESSURE_SENSOR << 16) | 0x0001,
};
-enum pressure_evet_type {
+enum pressure_event_type {
PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME = (PRESSURE_SENSOR << 16) | 0x0001,
};
PROXIMITY_DISTANCE_DATA_SET = (PROXIMITY_SENSOR << 16) | 0x0002,
};
-enum proxi_evet_type {
+enum proxi_event_type {
PROXIMITY_EVENT_CHANGE_STATE = (PROXIMITY_SENSOR << 16) | 0x0001,
PROXIMITY_EVENT_STATE_REPORT_ON_TIME = (PROXIMITY_SENSOR << 16) | 0x0002,
PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME = (PROXIMITY_SENSOR << 16) | 0x0004,
TEMPERATURE_BASE_DATA_SET = (TEMPERATURE_SENSOR << 16) | 0x0001,
};
-enum temperature_evet_type {
+enum temperature_event_type {
TEMPERATURE_EVENT_RAW_DATA_REPORT_ON_TIME = (TEMPERATURE_SENSOR << 16) | 0x0001,
};
IF("${RV}" STREQUAL "ON")
add_subdirectory(rv)
ENDIF()
+IF("${GEOMAGNETIC_RV}" STREQUAL "ON")
+add_subdirectory(geomagnetic_rv)
+ENDIF()
\ No newline at end of file
--- /dev/null
+cmake_minimum_required(VERSION 2.6)
+project(geomagnetic_rv CXX)
+
+SET(SENSOR_NAME geomagnetic_rv_sensor)
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+include_directories(${CMAKE_SOURCE_DIR}/src/libsensord)
+include_directories(${CMAKE_SOURCE_DIR}/src/sensor_fusion)
+
+INCLUDE(FindPkgConfig)
+PKG_CHECK_MODULES(geomagnetic_rv_pkgs REQUIRED vconf)
+
+FOREACH(flag ${geomagnetic_rv_pkgs_LDFLAGS})
+ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}")
+ENDFOREACH(flag)
+
+FOREACH(flag ${geomagnetic_rv_pkgs_CFLAGS})
+ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}")
+ENDFOREACH(flag)
+
+add_library(${SENSOR_NAME} SHARED
+ geomagnetic_rv_sensor.cpp
+)
+
+target_link_libraries(${SENSOR_NAME} ${geomagnetic_rv_pkgs_LDFLAGS} "-lm")
+
+install(TARGETS ${SENSOR_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/sensord)
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2015 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.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <time.h>
+#include <sys/types.h>
+#include <dlfcn.h>
+#include <common.h>
+#include <sf_common.h>
+#include <geomagnetic_rv_sensor.h>
+#include <sensor_plugin_loader.h>
+#include <orientation_filter.h>
+#include <cvirtual_sensor_config.h>
+
+#define SENSOR_NAME "GEOMAGNETIC_RV_SENSOR"
+#define SENSOR_TYPE_GEOMAGNETIC_RV "GEOMAGNETIC_ROTATION_VECTOR"
+
+#define ACCELEROMETER_ENABLED 0x01
+#define GEOMAGNETIC_ENABLED 0x02
+#define GEOMAGNETIC_RV_ENABLED 3
+
+#define INITIAL_VALUE -1
+
+#define MS_TO_US 1000
+
+#define ELEMENT_NAME "NAME"
+#define ELEMENT_VENDOR "VENDOR"
+#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT"
+#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME"
+#define ELEMENT_ACCEL_STATIC_BIAS "ACCEL_STATIC_BIAS"
+#define ELEMENT_GEOMAGNETIC_STATIC_BIAS "GEOMAGNETIC_STATIC_BIAS"
+#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION"
+#define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION"
+#define ELEMENT_ACCEL_SCALE "ACCEL_SCALE"
+#define ELEMENT_GEOMAGNETIC_SCALE "GEOMAGNETIC_SCALE"
+#define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR "MAGNETIC_ALIGNMENT_FACTOR"
+
+void pre_process_data(sensor_data<float> &data_out, const float *data_in, float *bias, int *sign, float scale)
+{
+ data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale;
+ data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale;
+ data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale;
+}
+
+geomagnetic_rv_sensor::geomagnetic_rv_sensor()
+: m_accel_sensor(NULL)
+, m_magnetic_sensor(NULL)
+, m_accuracy(-1)
+, m_time(0)
+{
+ cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance();
+
+ m_name = string(SENSOR_NAME);
+ register_supported_event(GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME);
+ m_enable_geomagnetic_rv = 0;
+
+ if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_VENDOR, m_vendor)) {
+ ERR("[VENDOR] is empty\n");
+ throw ENXIO;
+ }
+
+ INFO("m_vendor = %s", m_vendor.c_str());
+
+ if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
+ ERR("[DEFAULT_SAMPLING_TIME] is empty\n");
+ throw ENXIO;
+ }
+
+ INFO("m_default_sampling_time = %d", m_default_sampling_time);
+
+ if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) {
+ ERR("[ACCEL_STATIC_BIAS] is empty\n");
+ throw ENXIO;
+ }
+
+ INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]);
+
+ if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_GEOMAGNETIC_STATIC_BIAS, m_geomagnetic_static_bias, 3)) {
+ ERR("[GEOMAGNETIC_STATIC_BIAS] is empty\n");
+ throw ENXIO;
+ }
+
+ INFO("m_geomagnetic_static_bias = (%f, %f, %f)", m_geomagnetic_static_bias[0], m_geomagnetic_static_bias[1], m_geomagnetic_static_bias[2]);
+
+ if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) {
+ ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n");
+ throw ENXIO;
+ }
+
+ INFO("m_accel_rotation_direction_compensation = (%d, %d, %d)", m_accel_rotation_direction_compensation[0], m_accel_rotation_direction_compensation[1], m_accel_rotation_direction_compensation[2]);
+
+ if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION, m_geomagnetic_rotation_direction_compensation, 3)) {
+ ERR("[GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION] is empty\n");
+ throw ENXIO;
+ }
+
+ INFO("m_geomagnetic_rotation_direction_compensation = (%d, %d, %d)", m_geomagnetic_rotation_direction_compensation[0], m_geomagnetic_rotation_direction_compensation[1], m_geomagnetic_rotation_direction_compensation[2]);
+
+ if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_ACCEL_SCALE, &m_accel_scale)) {
+ ERR("[ACCEL_SCALE] is empty\n");
+ throw ENXIO;
+ }
+
+ INFO("m_accel_scale = %f", m_accel_scale);
+
+ if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_GEOMAGNETIC_SCALE, &m_geomagnetic_scale)) {
+ ERR("[GEOMAGNETIC_SCALE] is empty\n");
+ throw ENXIO;
+ }
+
+ INFO("m_geomagnetic_scale = %f", m_geomagnetic_scale);
+
+ if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_MAGNETIC_ALIGNMENT_FACTOR, &m_magnetic_alignment_factor)) {
+ ERR("[MAGNETIC_ALIGNMENT_FACTOR] is empty\n");
+ throw ENXIO;
+ }
+
+ INFO("m_magnetic_alignment_factor = %d", m_magnetic_alignment_factor);
+
+ m_interval = m_default_sampling_time * MS_TO_US;
+
+}
+
+geomagnetic_rv_sensor::~geomagnetic_rv_sensor()
+{
+ INFO("geomagnetic_rv_sensor is destroyed!\n");
+}
+
+bool geomagnetic_rv_sensor::init()
+{
+ m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
+ m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
+
+ if (!m_accel_sensor || !m_magnetic_sensor) {
+ ERR("Failed to load sensors, accel: 0x%x, mag: 0x%x",
+ m_accel_sensor, m_magnetic_sensor);
+ return false;
+ }
+
+ INFO("%s is created!\n", sensor_base::get_name());
+
+ return true;
+}
+
+sensor_type_t geomagnetic_rv_sensor::get_type(void)
+{
+ return GEOMAGNETIC_RV_SENSOR;
+}
+
+bool geomagnetic_rv_sensor::on_start(void)
+{
+ AUTOLOCK(m_mutex);
+
+ m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
+ m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
+ m_accel_sensor->start();
+ m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
+ m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
+ m_magnetic_sensor->start();
+
+ activate();
+ return true;
+}
+
+bool geomagnetic_rv_sensor::on_stop(void)
+{
+ AUTOLOCK(m_mutex);
+
+ m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
+ m_accel_sensor->delete_interval((intptr_t)this, false);
+ m_accel_sensor->stop();
+ m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
+ m_magnetic_sensor->delete_interval((intptr_t)this, false);
+ m_magnetic_sensor->stop();
+
+ deactivate();
+ return true;
+}
+
+bool geomagnetic_rv_sensor::add_interval(int client_id, unsigned int interval)
+{
+ AUTOLOCK(m_mutex);
+
+ m_accel_sensor->add_interval(client_id, interval, false);
+ m_magnetic_sensor->add_interval(client_id, interval, false);
+
+ return sensor_base::add_interval(client_id, interval, false);
+}
+
+bool geomagnetic_rv_sensor::delete_interval(int client_id)
+{
+ AUTOLOCK(m_mutex);
+
+ m_accel_sensor->delete_interval(client_id, false);
+ m_magnetic_sensor->delete_interval(client_id, false);
+
+ return sensor_base::delete_interval(client_id, false);
+}
+
+void geomagnetic_rv_sensor::synthesize(const sensor_event_t& event, vector<sensor_event_t> &outs)
+{
+ const float MIN_DELIVERY_DIFF_FACTOR = 0.75f;
+ unsigned long long diff_time;
+
+ sensor_event_t rv_event;
+ quaternion<float> quaternion_geo_rv;
+
+ if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
+ diff_time = event.data.timestamp - m_time;
+
+ if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
+ return;
+
+ pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
+
+ m_accel.m_time_stamp = event.data.timestamp;
+
+ m_enable_geomagnetic_rv |= ACCELEROMETER_ENABLED;
+ }
+ else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) {
+ diff_time = event.data.timestamp - m_time;
+
+ if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
+ return;
+
+ pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
+
+ m_magnetic.m_time_stamp = event.data.timestamp;
+
+ m_enable_geomagnetic_rv |= GEOMAGNETIC_ENABLED;
+ }
+
+ if (m_enable_geomagnetic_rv == GEOMAGNETIC_RV_ENABLED) {
+ m_enable_geomagnetic_rv = 0;
+
+ m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
+
+ {
+ AUTOLOCK(m_fusion_mutex);
+ quaternion_geo_rv = m_orientation_filter.get_geomagnetic_quaternion(m_accel, m_magnetic);
+ }
+
+ m_time = get_timestamp();
+
+ rv_event.sensor_id = get_id();
+ rv_event.event_type = GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME;
+ rv_event.data.accuracy = SENSOR_ACCURACY_GOOD;
+ rv_event.data.timestamp = m_time;
+ rv_event.data.value_count = 4;
+ rv_event.data.values[0] = quaternion_geo_rv.m_quat.m_vec[1];
+ rv_event.data.values[1] = quaternion_geo_rv.m_quat.m_vec[2];
+ rv_event.data.values[2] = quaternion_geo_rv.m_quat.m_vec[3];
+ rv_event.data.values[3] = quaternion_geo_rv.m_quat.m_vec[0];
+
+ push(rv_event);
+ }
+
+ return;
+}
+
+int geomagnetic_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data)
+{
+ sensor_data<float> accel;
+ sensor_data<float> magnetic;
+
+ sensor_data_t accel_data;
+ sensor_data_t magnetic_data;
+
+ quaternion<float> quaternion_geo_rv;
+
+ if (event_type != GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME)
+ return -1;
+
+ m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data);
+ m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data);
+
+ pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
+ pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
+ accel.m_time_stamp = accel_data.timestamp;
+ magnetic.m_time_stamp = magnetic_data.timestamp;
+
+ m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
+
+ {
+ AUTOLOCK(m_fusion_mutex);
+ quaternion_geo_rv = m_orientation_filter.get_geomagnetic_quaternion(m_accel, m_magnetic);
+ }
+
+ data.accuracy = SENSOR_ACCURACY_GOOD;
+ data.timestamp = m_time;
+ data.value_count = 4;
+ data.values[0] = quaternion_geo_rv.m_quat.m_vec[1];
+ data.values[1] = quaternion_geo_rv.m_quat.m_vec[2];
+ data.values[2] = quaternion_geo_rv.m_quat.m_vec[3];
+ data.values[3] = quaternion_geo_rv.m_quat.m_vec[0];
+
+ return 0;
+}
+
+bool geomagnetic_rv_sensor::get_properties(sensor_properties_s &properties)
+{
+ properties.vendor = m_vendor;
+ properties.name = SENSOR_NAME;
+ properties.min_range = -1;
+ properties.max_range = 1;
+ properties.resolution = 0.000001;
+ properties.fifo_count = 0;
+ properties.max_batch_count = 0;
+ properties.min_interval = 1;
+
+ return true;
+}
+
+extern "C" sensor_module* create(void)
+{
+ geomagnetic_rv_sensor *sensor;
+
+ try {
+ sensor = new(std::nothrow) geomagnetic_rv_sensor;
+ } catch (int err) {
+ ERR("Failed to create module, err: %d, cause: %s", err, strerror(err));
+ return NULL;
+ }
+
+ sensor_module *module = new(std::nothrow) sensor_module;
+ retvm_if(!module || !sensor, NULL, "Failed to allocate memory");
+
+ module->sensors.push_back(sensor);
+ return module;
+}
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2015 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.
+ *
+ */
+
+#ifndef _GEOMAGNETIC_RV_SENSOR_H_
+#define _GEOMAGNETIC_RV_SENSOR_H_
+
+#include <sensor_internal.h>
+#include <virtual_sensor.h>
+#include <orientation_filter.h>
+
+class geomagnetic_rv_sensor : public virtual_sensor {
+public:
+ geomagnetic_rv_sensor();
+ virtual ~geomagnetic_rv_sensor();
+
+ bool init(void);
+
+ void synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs);
+
+ bool add_interval(int client_id, unsigned int interval);
+ bool delete_interval(int client_id);
+ bool get_properties(sensor_properties_s &properties);
+ sensor_type_t get_type(void);
+
+ int get_sensor_data(const unsigned int data_id, sensor_data_t &data);
+
+private:
+ sensor_base *m_accel_sensor;
+ sensor_base *m_magnetic_sensor;
+
+ sensor_data<float> m_accel;
+ sensor_data<float> m_magnetic;
+
+ cmutex m_value_mutex;
+
+ orientation_filter<float> m_orientation_filter;
+
+ unsigned int m_enable_geomagnetic_rv;
+
+ int m_accuracy;
+ unsigned long long m_time;
+ unsigned int m_interval;
+
+ string m_vendor;
+ string m_raw_data_unit;
+ int m_default_sampling_time;
+ float m_accel_static_bias[3];
+ float m_geomagnetic_static_bias[3];
+ int m_accel_rotation_direction_compensation[3];
+ int m_geomagnetic_rotation_direction_compensation[3];
+ float m_accel_scale;
+ float m_geomagnetic_scale;
+ int m_magnetic_alignment_factor;
+
+ bool on_start(void);
+ bool on_stop(void);
+};
+
+#endif /*_GEOMAGNETIC_RV_SENSOR_H_*/
<FIGURE>
<center>
-<img src="./equation/equation_13.png" width="35%" height="3%">
+<img src="./equation/equation_13_updated.png" width="35%" height="5%">
</center>
</FIGURE>
--- /dev/null
+% axis_rot2quat
+%
+% Copyright (c) 2015 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.
+
+% Axis rotation to quaternion function
+%
+% - convert sensor rotation axis and angle values to quaternion
+function q = axis_rot2quat(axis, angle)
+ q0 = cos(angle/2);
+ q1 = -axis(1)*sin(angle/2);
+ q2 = -axis(2)*sin(angle/2);
+ q3 = -axis(3)*sin(angle/2);
+ q = [q0 q1 q2 q3];
+end
-% estimate_gravity
+% estimate_geomagnetic_rv
%
-% Copyright (c) 2014 Samsung Electronics Co., Ltd.
+% Copyright (c) 2015 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.
% See the License for the specific language governing permissions and
% limitations under the License.
-% Gravitational Force Estimation function
+% Geomagnetic Rotation Vector Estimation function
%
+% - Input orientation using only Accel and Geomagnetic sensors
% - Orientation Estimation using estimate_orientation function
-% - Project the orientation on the device reference axes to obtain
-% gravitational force on specific reference axes
-
+% - Output aiding system quaternion as geomagnetic rotation vector
function [Quat_aid] = estimate_geomagnetic_rv(Accel_data, Mag_data)
-GRAVITY = 9.80665;
-RAD2DEG = 57.2957795;
-
BUFFER_SIZE = size(Accel_data,2);
-Gravity = zeros(3,BUFFER_SIZE);
-
-OR_driv = zeros(3,BUFFER_SIZE);
Gyro_data = zeros(4,BUFFER_SIZE);
Quat_driv = zeros(4,BUFFER_SIZE);
% estimate_orientation
%
-% Copyright (c) 2014 Samsung Electronics Co., Ltd.
+% Copyright (c) 2015 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.
--- /dev/null
+% euler2quat
+%
+% Copyright (c) 2015 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.
+
+% Euler Angles to quaternion conversion
+%
+% - Implementation to convert orientation in terms of euler angles to quaternion
+
+function q = euler2quat(euler)
+ theta = euler(1);
+ phi = euler(2);
+ psi = euler(3);
+ R(1,1,1) = cos(psi)*cos(theta);
+ R(1,2,1) = -sin(psi)*cos(phi) + cos(psi)*sin(theta)*sin(phi);
+ R(1,3,1) = sin(psi)*sin(phi) + cos(psi)*sin(theta)*cos(phi);
+
+ R(2,1,1) = sin(psi)*cos(theta);
+ R(2,2,1) = cos(psi)*cos(phi) + sin(psi)*sin(theta)*sin(phi);
+ R(2,3,1) = -cos(psi)*sin(phi) + sin(psi)*sin(theta)*cos(phi);
+
+ R(3,1,1) = -sin(theta);
+ R(3,2,1) = cos(theta)*sin(phi);
+ R(3,3,1) = cos(theta)*cos(phi);
+
+ q = rot_mat2quat(R);
+end
\ No newline at end of file
scrsz = get(0,'ScreenSize');
set(hfig,'position',scrsz);
% Geomagnetic Rotation Vector Plot Results
+subplot(3,1,1)
UA = Orientation_RV(1,:);
p1 = plot(1:length(UA),UA(1,1:length(UA)),'k');
-hold on;
-grid on;
+legend(p1,'x-axis');
+title(['Pitch']);
+subplot(3,1,2)
UA = Orientation_RV(2,:);
-p2 = plot(1:length(UA),UA(1,1:length(UA)),'b');
-hold on;
-grid on;
+p1 = plot(1:length(UA),UA(1,1:length(UA)),'b');
+legend(p1,'y-axis');
+title(['Roll']);
+subplot(3,1,3)
UA = Orientation_RV(3,:);
-p3 = plot(1:length(UA),UA(1,1:length(UA)),'r');
-title(['Geomagnetic Rotation Vector']);
-legend([p1 p2 p3],'x-axis', 'y-axis', 'z-axis');
+p1 = plot(1:length(UA),UA(1,1:length(UA)),'r');
+legend(p1,'z-axis');
+title(['Yaw']);
#include <string>
#include <utility>
#include <permission_checker.h>
+#include <set>
using std::string;
using std::make_pair;
+using std::set;
command_worker::cmd_handler_t command_worker::m_cmd_handlers[];
sensor_raw_data_map command_worker::m_sensor_raw_data_map;
void insert_priority_list(unsigned int event_type)
{
- if (event_type == ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME) {
+ if (event_type == ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME ||
+ event_type == LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME ||
+ event_type == GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME ||
+ event_type == ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME) {
priority_list.insert(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
priority_list.insert(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME);
priority_list.insert(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
- }
-
- if (event_type == LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME) {
- priority_list.insert(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
- priority_list.insert(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME);
- priority_list.insert(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
- }
+ }
- if (event_type == GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME) {
+ if (event_type == GEOMAGNETIC_RV_EVENT_RAW_DATA_REPORT_ON_TIME) {
priority_list.insert(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
- priority_list.insert(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME);
priority_list.insert(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
- }
+ }
}
#define SECURITY_LIB "/usr/lib/libsecurity-server-client.so.1"
permission_checker::permission_checker()
-: security_server_check_privilege_by_sockfd(NULL)
+: m_security_server_check_privilege_by_sockfd(NULL)
, m_security_handle(NULL)
, m_permission_set(0)
-
{
init();
}
return false;
}
- security_server_check_privilege_by_sockfd =
+ m_security_server_check_privilege_by_sockfd =
(security_server_check_privilege_by_sockfd_t) dlsym(m_security_handle, "security_server_check_privilege_by_sockfd");
- if (!security_server_check_privilege_by_sockfd) {
+ if (!m_security_server_check_privilege_by_sockfd) {
ERR("Failed to load symbol");
dlclose(m_security_handle);
m_security_handle = NULL;
for (unsigned int i = 0; i < m_permission_infos.size(); ++i) {
if (!m_permission_infos[i]->need_to_check) {
permission |= m_permission_infos[i]->permission;
- } else if ((m_permission_set & m_permission_infos[i]->permission) && security_server_check_privilege_by_sockfd) {
- if (security_server_check_privilege_by_sockfd(sock_fd, m_permission_infos[i]->name.c_str(), m_permission_infos[i]->access_right.c_str()) == 1) {
+ } else if ((m_permission_set & m_permission_infos[i]->permission) && m_security_server_check_privilege_by_sockfd) {
+ if (m_security_server_check_privilege_by_sockfd(sock_fd, m_permission_infos[i]->name.c_str(), m_permission_infos[i]->access_right.c_str()) == 1) {
permission |= m_permission_infos[i]->permission;
}
}
bool init_security_lib(void);
void init();
- security_server_check_privilege_by_sockfd_t security_server_check_privilege_by_sockfd;
+ security_server_check_privilege_by_sockfd_t m_security_server_check_privilege_by_sockfd;
void *m_security_handle;
permission_info_vector m_permission_infos;
GRAVITY_SENSOR,
LINEAR_ACCEL_SENSOR,
ROTATION_VECTOR_SENSOR,
+ GEOMAGNETIC_RV_SENSOR,
ORIENTATION_SENSOR,
PIR_SENSOR,
PIR_LONG_SENSOR,
add_executable(temperature src/temperature.c)
add_executable(light src/light.c)
add_executable(rotation_vector src/rotation_vector.c)
+add_executable(geomagnetic_rv src/geomagnetic_rv.c)
SET_TARGET_PROPERTIES(accelerometer PROPERTIES LINKER_LANGUAGE C)
SET_TARGET_PROPERTIES(geomagnetic PROPERTIES LINKER_LANGUAGE C)
SET_TARGET_PROPERTIES(temperature PROPERTIES LINKER_LANGUAGE C)
SET_TARGET_PROPERTIES(light PROPERTIES LINKER_LANGUAGE C)
SET_TARGET_PROPERTIES(rotation_vector PROPERTIES LINKER_LANGUAGE C)
+SET_TARGET_PROPERTIES(geomagnetic_rv PROPERTIES LINKER_LANGUAGE C)
target_link_libraries(accelerometer glib-2.0 dlog sensor)
target_link_libraries(geomagnetic glib-2.0 dlog sensor)
target_link_libraries(temperature glib-2.0 dlog sensor)
target_link_libraries(light glib-2.0 dlog sensor)
target_link_libraries(rotation_vector glib-2.0 dlog sensor)
+target_link_libraries(geomagnetic_rv glib-2.0 dlog sensor)
INSTALL(TARGETS accelerometer DESTINATION /usr/bin/)
INSTALL(TARGETS geomagnetic DESTINATION /usr/bin/)
INSTALL(TARGETS temperature DESTINATION /usr/bin/)
INSTALL(TARGETS light DESTINATION /usr/bin/)
INSTALL(TARGETS rotation_vector DESTINATION /usr/bin/)
+INSTALL(TARGETS geomagnetic_rv DESTINATION /usr/bin/)
<GEOMAGNETIC_SCALE value="1" />
<MAGNETIC_ALIGNMENT_FACTOR value="1" />
</ROTATION_VECTOR>
+
+ <GEOMAGNETIC_ROTATION_VECTOR>
+ <NAME value="GEOMAGNETIC_RV_SENSOR" />
+ <VENDOR value="SAMSUNG" />
+ <DEFAULT_SAMPLING_TIME value="100" />
+ <ACCEL_STATIC_BIAS value1="0" value2="0" value3="0" />
+ <GEOMAGNETIC_STATIC_BIAS value1="0" value2="0" value3="0" />
+ <ACCEL_ROTATION_DIRECTION_COMPENSATION value1="-1" value2="-1" value3="-1" />
+ <GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION value1="-1" value2="-1" value3="-1" />
+ <ACCEL_SCALE value="1" />
+ <GEOMAGNETIC_SCALE value="1" />
+ <MAGNETIC_ALIGNMENT_FACTOR value="1" />
+ </GEOMAGNETIC_ROTATION_VECTOR>
</DEVICE>
<DEVICE type="Mobile-RD-PQ">
<ORIENTATION>
<GEOMAGNETIC_SCALE value="1" />
<MAGNETIC_ALIGNMENT_FACTOR value="1" />
</ROTATION_VECTOR>
+
+ <GEOMAGNETIC_ROTATION_VECTOR>
+ <NAME value="GEOMAGNETIC_RV_SENSOR" />
+ <VENDOR value="SAMSUNG" />
+ <DEFAULT_SAMPLING_TIME value="100" />
+ <ACCEL_STATIC_BIAS value1="0.098586" value2="0.18385" value3="0.274" />
+ <GEOMAGNETIC_STATIC_BIAS value1="0" value2="-37.6" value3="37.6" />
+ <ACCEL_ROTATION_DIRECTION_COMPENSATION value1="1" value2="1" value3="1" />
+ <GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION value1="1" value2="-1" value3="1" />
+ <ACCEL_SCALE value="1" />
+ <GEOMAGNETIC_SCALE value="1" />
+ <MAGNETIC_ALIGNMENT_FACTOR value="1" />
+ </GEOMAGNETIC_ROTATION_VECTOR>
</DEVICE>
</VIRTUAL_SENSOR>
projects / platform / core / system / sensord.git / commitdiff
