From d104d0ac5b4772c105642ef6304809ce92808b59 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 15 May 2015 19:00:26 +0900 Subject: [PATCH] Adding Tilt Sensor and related files - Adding tilt sensor based on accelerometer only - uses intermediate quaternion output of sensor fusion - Used in Tizen Z1 for computing limited device orientation - Adding all api, build files related to tilt Change-Id: I7682da9238ca61e9d8fc1b392fb14afc33406d2d --- packaging/sensord.spec | 5 +- sensor_plugins.xml.in | 1 + src/CMakeLists.txt | 11 +- src/fusion/fusion_sensor.cpp | 4 +- src/libsensord/CMakeLists.txt | 1 + src/libsensord/client_common.cpp | 2 + src/libsensord/sensor_fusion.h | 13 +- src/libsensord/sensor_internal.h | 1 + src/libsensord/sensor_internal_deprecated.h | 1 + src/shared/sensor_common.h | 1 + src/tilt/CMakeLists.txt | 24 +++ src/tilt/tilt_sensor.cpp | 311 ++++++++++++++++++++++++++++ src/tilt/tilt_sensor.h | 69 ++++++ virtual_sensors.xml | 27 +++ 14 files changed, 459 insertions(+), 12 deletions(-) create mode 100755 src/tilt/CMakeLists.txt create mode 100755 src/tilt/tilt_sensor.cpp create mode 100755 src/tilt/tilt_sensor.h diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 48c8cf1..499c99e 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -30,7 +30,7 @@ BuildRequires: pkgconfig(capi-system-info) %define rv_state ON %define geomagnetic_rv_state ON %define gaming_rv_state ON -%define fusion_state ON +%define tilt_state ON %define build_test_suite OFF %description @@ -82,7 +82,7 @@ cmake . -DCMAKE_INSTALL_PREFIX=%{_prefix} -DACCEL=%{accel_state} \ -DORIENTATION=%{orientation_state} -DGRAVITY=%{gravity_state} \ -DLINEAR_ACCEL=%{linear_accel_state} -DRV=%{rv_state} \ -DGEOMAGNETIC_RV=%{geomagnetic_rv_state} -DGAMING_RV=%{gaming_rv_state} \ - -DFUSION=%{fusion_state} -DTEST_SUITE=%{build_test_suite} \ + -DTILT=%{tilt_state} -DTEST_SUITE=%{build_test_suite} \ -DLIBDIR=%{_libdir} -DINCLUDEDIR=%{_includedir} %build @@ -134,6 +134,7 @@ systemctl daemon-reload %{_libdir}/pkgconfig/sensor.pc %{_libdir}/pkgconfig/sf_common.pc %{_libdir}/pkgconfig/sensord-server.pc +%license LICENSE.APLv2 %if %{build_test_suite} == "ON" %files -n sensor-test diff --git a/sensor_plugins.xml.in b/sensor_plugins.xml.in index 9fde9d6..c1822ce 100755 --- a/sensor_plugins.xml.in +++ b/sensor_plugins.xml.in @@ -24,5 +24,6 @@ + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 556256a..65f0ec7 100755 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -44,6 +44,10 @@ ENDIF() IF("${GAMING_RV}" STREQUAL "ON") set(SENSOR_FUSION_ENABLE "1") ENDIF() +IF("${TILT}" STREQUAL "ON") +set(SENSOR_FUSION_ENABLE "1") +set(TILT_ENABLE "1") +ENDIF() IF("${GRAVITY}" STREQUAL "ON") set(SENSOR_FUSION_ENABLE "1") set(ORIENTATION_ENABLE "1") @@ -57,8 +61,6 @@ set(LINEAR_ACCELERATION_ENABLE "1") ENDIF() IF("${SENSOR_FUSION_ENABLE}" STREQUAL "1") add_subdirectory(sensor_fusion) -ENDIF() -IF("${FUSION}" STREQUAL "ON") add_subdirectory(fusion) ENDIF() IF("${ORIENTATION_ENABLE}" STREQUAL "1") @@ -70,8 +72,11 @@ ENDIF() IF("${LINEAR_ACCELERATION_ENABLE}" STREQUAL "1") add_subdirectory(linear_accel) ENDIF() -add_subdirectory(rotation_vector) +IF("${TILT_ENABLE}" STREQUAL "1") +add_subdirectory(tilt) +ENDIF() +add_subdirectory(rotation_vector) add_subdirectory(server) add_subdirectory(libsensord) add_subdirectory(shared) diff --git a/src/fusion/fusion_sensor.cpp b/src/fusion/fusion_sensor.cpp index 0bac151..42a8e41 100755 --- a/src/fusion/fusion_sensor.cpp +++ b/src/fusion/fusion_sensor.cpp @@ -39,6 +39,7 @@ #define ACCELEROMETER_ENABLED 0x01 #define GYROSCOPE_ENABLED 0x02 #define GEOMAGNETIC_ENABLED 0x04 +#define TILT_ENABLED 1 #define GAMING_RV_ENABLED 3 #define GEOMAGNETIC_RV_ENABLED 5 #define ORIENTATION_ENABLED 7 @@ -317,7 +318,8 @@ void fusion_sensor::synthesize(const sensor_event_t &event, vector log_map; diff --git a/src/libsensord/sensor_fusion.h b/src/libsensord/sensor_fusion.h index 7539be1..8f80499 100755 --- a/src/libsensord/sensor_fusion.h +++ b/src/libsensord/sensor_fusion.h @@ -36,12 +36,13 @@ extern "C" * @{ */ enum fusion_event_type { - FUSION_ORIENTATION_ENABLED = (FUSION_SENSOR << 16) | 0x0001, - FUSION_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0002, - FUSION_GAMING_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0003, - FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0004, - FUSION_EVENT = (FUSION_SENSOR << 16) | 0x0005, - FUSION_CALIBRATION_NEEDED_EVENT = (FUSION_SENSOR << 16) | 0x0006, + FUSION_EVENT = (FUSION_SENSOR << 16) | 0x0001, + FUSION_CALIBRATION_NEEDED_EVENT = (FUSION_SENSOR << 16) | 0x0002, + FUSION_ORIENTATION_ENABLED = (FUSION_SENSOR << 16) | 0x0003, + FUSION_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0004, + FUSION_GAMING_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0005, + FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0006, + FUSION_TILT_ENABLED = (FUSION_SENSOR << 16) | 0x0007, }; /** diff --git a/src/libsensord/sensor_internal.h b/src/libsensord/sensor_internal.h index 844e7af..7842cda 100755 --- a/src/libsensord/sensor_internal.h +++ b/src/libsensord/sensor_internal.h @@ -55,6 +55,7 @@ extern "C" #include #include #include +#include typedef void (*sensor_cb_t)(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data); diff --git a/src/libsensord/sensor_internal_deprecated.h b/src/libsensord/sensor_internal_deprecated.h index 4420044..fdcbab1 100755 --- a/src/libsensord/sensor_internal_deprecated.h +++ b/src/libsensord/sensor_internal_deprecated.h @@ -54,6 +54,7 @@ extern "C" #include #include #include +#include #define MAX_KEY_LEN 30 diff --git a/src/shared/sensor_common.h b/src/shared/sensor_common.h index 48346ac..d1df649 100755 --- a/src/shared/sensor_common.h +++ b/src/shared/sensor_common.h @@ -66,6 +66,7 @@ typedef enum { GEOMAGNETIC_RV_SENSOR, GAMING_RV_SENSOR, ORIENTATION_SENSOR, + TILT_SENSOR, PIR_SENSOR, PIR_LONG_SENSOR, TEMPERATURE_SENSOR, diff --git a/src/tilt/CMakeLists.txt b/src/tilt/CMakeLists.txt new file mode 100755 index 0000000..44e6a1a --- /dev/null +++ b/src/tilt/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 2.6) +project(tilt CXX) + +SET(SENSOR_NAME tilt_sensor) + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) +include_directories(${CMAKE_SOURCE_DIR}/src/sensor_fusion) + +FOREACH(flag ${tilt_pkgs_LDFLAGS}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") +ENDFOREACH(flag) + +FOREACH(flag ${tilt_pkgs_CFLAGS}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") +ENDFOREACH(flag) + +add_library(${SENSOR_NAME} SHARED + tilt_sensor.cpp + ) + +target_link_libraries(${SENSOR_NAME} ${tilt_pkgs_LDFLAGS} "-lm") + +install(TARGETS ${SENSOR_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/sensord) diff --git a/src/tilt/tilt_sensor.cpp b/src/tilt/tilt_sensor.cpp new file mode 100755 index 0000000..db40acf --- /dev/null +++ b/src/tilt/tilt_sensor.cpp @@ -0,0 +1,311 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define SENSOR_NAME "TILT_SENSOR" +#define SENSOR_TYPE_TILT "TILT" + +#define MIN_DELIVERY_DIFF_FACTOR 0.75f + +#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_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION" +#define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION" + +void pre_process_data(sensor_data &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; +} + +tilt_sensor::tilt_sensor() +: m_accel_sensor(NULL) +, m_fusion_sensor(NULL) +, m_time(0) +{ + cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + + m_name = string(SENSOR_NAME); + register_supported_event(TILT_RAW_DATA_EVENT); + m_enable_tilt = 0; + + if (!config.get(SENSOR_TYPE_TILT, ELEMENT_VENDOR, m_vendor)) { + ERR("[VENDOR] is empty\n"); + throw ENXIO; + } + + INFO("m_vendor = %s", m_vendor.c_str()); + + if (!config.get(SENSOR_TYPE_TILT, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) { + ERR("[RAW_DATA_UNIT] is empty\n"); + throw ENXIO; + } + + INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str()); + + if (!config.get(SENSOR_TYPE_TILT, 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_TILT, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) { + ERR("[PITCH_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation); + + if (!config.get(SENSOR_TYPE_TILT, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) { + ERR("[ROLL_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_roll_rotation_compensation = %d", m_roll_rotation_compensation); + + m_interval = m_default_sampling_time * MS_TO_US; + + INFO("ramasamy 1: Constructor completed"); +} + +tilt_sensor::~tilt_sensor() +{ + INFO("tilt_sensor is destroyed!\n"); +} + +bool tilt_sensor::init(void) +{ + m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_SENSOR); + + if (!m_accel_sensor || !m_fusion_sensor) { + ERR("Failed to load sensors, accel: 0x%x, fusion: 0x%x", + m_accel_sensor, m_fusion_sensor); + return false; + } + + INFO("%s is created!\n", sensor_base::get_name()); + + INFO("ramasamy 2: Init completed"); + + return true; +} + +sensor_type_t tilt_sensor::get_type(void) +{ + return TILT_SENSOR; +} + +bool tilt_sensor::on_start(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); + m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); + m_accel_sensor->start(); + + m_fusion_sensor->register_supported_event(FUSION_EVENT); + m_fusion_sensor->register_supported_event(FUSION_TILT_ENABLED); + m_fusion_sensor->add_client(FUSION_EVENT); + m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); + m_fusion_sensor->start(); + + INFO("ramasamy 3: ON_START completed"); + + activate(); + return true; +} + +bool tilt_sensor::on_stop(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); + m_accel_sensor->delete_interval((intptr_t)this, false); + m_accel_sensor->stop(); + + m_fusion_sensor->delete_client(FUSION_EVENT); + m_fusion_sensor->delete_interval((intptr_t)this, false); + m_fusion_sensor->unregister_supported_event(FUSION_EVENT); + m_fusion_sensor->unregister_supported_event(FUSION_TILT_ENABLED); + m_fusion_sensor->stop(); + + INFO("ramasamy 4: ON_STOP completed"); + + deactivate(); + return true; +} + +bool tilt_sensor::add_interval(int client_id, unsigned int interval) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->add_interval(client_id, interval, false); + m_fusion_sensor->add_interval(client_id, interval, false); + + INFO("ramasamy 5: ADD_INTERVAL completed"); + + return sensor_base::add_interval(client_id, interval, false); +} + +bool tilt_sensor::delete_interval(int client_id) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_interval(client_id, false); + m_fusion_sensor->delete_interval(client_id, false); + + INFO("ramasamy 6: DELETE_INTERVAL completed"); + + return sensor_base::delete_interval(client_id, false); +} + +void tilt_sensor::synthesize(const sensor_event_t &event, vector &outs) +{ + sensor_event_t tilt_event; + unsigned long long diff_time; + + INFO("ramasamy 7: SYNTHESIZE STARTED"); + + if (event.event_type == FUSION_EVENT) { + + INFO("ramasamy 8: SYNTHESIZE FUSION EVENT RECEIVED"); + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + quaternion quat(event.data.values[0], event.data.values[1], + event.data.values[2], event.data.values[3]); + + euler_angles euler = quat2euler(quat); + + if(m_raw_data_unit == "DEGREES") { + euler = rad2deg(euler); + } + + euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation; + euler.m_ang.m_vec[1] *= m_roll_rotation_compensation; + + m_time = get_timestamp(); + tilt_event.sensor_id = get_id(); + tilt_event.event_type = TILT_RAW_DATA_EVENT; + tilt_event.data.accuracy = event.data.accuracy; + tilt_event.data.timestamp = m_time; + tilt_event.data.value_count = 2; + tilt_event.data.values[0] = euler.m_ang.m_vec[0]; + tilt_event.data.values[1] = euler.m_ang.m_vec[1]; + + INFO("ramasamy 8: SYNTHESIZE FUSION EVENT SENT"); + + push(tilt_event); + } + + return; +} + +int tilt_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) +{ + sensor_data_t fusion_data; + + if (event_type != TILT_RAW_DATA_EVENT) + return -1; + + m_fusion_sensor->get_sensor_data(FUSION_TILT_ENABLED, fusion_data); + + quaternion quat(fusion_data.values[0], fusion_data.values[1], + fusion_data.values[2], fusion_data.values[3]); + + euler_angles euler = quat2euler(quat); + + if(m_raw_data_unit == "DEGREES") { + euler = rad2deg(euler); + } + + data.accuracy = fusion_data.accuracy; + data.timestamp = get_timestamp(); + data.value_count = 2; + data.values[0] = euler.m_ang.m_vec[0]; + data.values[1] = euler.m_ang.m_vec[1]; + + data.values[0] *= m_pitch_rotation_compensation; + data.values[1] *= m_roll_rotation_compensation; + + return 0; +} + +bool tilt_sensor::get_properties(sensor_properties_s &properties) +{ + if(m_raw_data_unit == "DEGREES") { + properties.min_range = -180; + properties.max_range = 180; + } + else { + properties.min_range = -PI; + properties.max_range = PI; + } + properties.resolution = 0.000001; + properties.vendor = m_vendor; + properties.name = SENSOR_NAME; + properties.min_interval = 1; + properties.fifo_count = 0; + properties.max_batch_count = 0; + + return true; +} + +extern "C" sensor_module* create(void) +{ + tilt_sensor *sensor; + + try { + sensor = new(std::nothrow) tilt_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; +} diff --git a/src/tilt/tilt_sensor.h b/src/tilt/tilt_sensor.h new file mode 100755 index 0000000..758751f --- /dev/null +++ b/src/tilt/tilt_sensor.h @@ -0,0 +1,69 @@ +/* + * 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 _TILT_SENSOR_H_ +#define _TILT_SENSOR_H_ + +#include +#include +#include + +class tilt_sensor : public virtual_sensor { +public: + tilt_sensor(); + virtual ~tilt_sensor(); + + bool init(void); + + void synthesize(const sensor_event_t &event, vector &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 event_type, sensor_data_t &data); + +private: + sensor_base *m_accel_sensor; + sensor_base *m_fusion_sensor; + + sensor_data m_accel; + + cmutex m_value_mutex; + + orientation_filter m_orientation_filter; + orientation_filter m_orientation_filter_poll; + + unsigned int m_enable_tilt; + + unsigned long long m_time; + unsigned int m_interval; + + string m_vendor; + string m_raw_data_unit; + int m_default_sampling_time; + int m_pitch_rotation_compensation; + int m_roll_rotation_compensation; + + bool on_start(void); + bool on_stop(void); +}; + +#endif /*_TILT_SENSOR_H_*/ diff --git a/virtual_sensors.xml b/virtual_sensors.xml index 3210e4f..3005df6 100755 --- a/virtual_sensors.xml +++ b/virtual_sensors.xml @@ -64,6 +64,15 @@ + + + + + + + + + @@ -130,6 +139,15 @@ + + + + + + + + + @@ -196,5 +214,14 @@ + + + + + + + + + -- 2.7.4