From: kibak.yoon Date: Wed, 11 May 2016 08:59:00 +0000 (+0900) Subject: sensord: enable rotation vector/orientation sensors X-Git-Tag: accepted/tizen/common/20160526.145737~1^2~13 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=refs%2Fchanges%2F58%2F69058%2F3;p=platform%2Fcore%2Fsystem%2Fsensord.git sensord: enable rotation vector/orientation sensors - applied simple fusion algorithm Change-Id: I31b0c09a29595804946f8a105198bee381d090ae Signed-off-by: kibak.yoon --- diff --git a/packaging/sensord.spec b/packaging/sensord.spec index c917263..c3f0488 100644 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -35,6 +35,10 @@ Requires: libsensord = %{version}-%{release} %define gyroscope_uncal_state OFF %define build_test_suite ON +%ifarch %{ix86} x86_64 +%define BUILD_ARCH EMULATOR +%endif + %description Sensor daemon @@ -81,7 +85,7 @@ cmake . -DCMAKE_INSTALL_PREFIX=%{_prefix} -DMAJORVER=${MAJORVER} -DFULLVER=%{ver -DLINEAR_ACCEL=%{linear_accel_state} -DRV=%{rv_state} \ -DGEOMAGNETIC_RV=%{geomagnetic_rv_state} -DGAMING_RV=%{gaming_rv_state} \ -DGYROSCOPE_UNCAL=%{gyroscope_uncal_state} -DAUTO_ROTATION=%{auto_rotation_state} \ - -DTILT=%{tilt_state} -DTEST_SUITE=%{build_test_suite} + -DTILT=%{tilt_state} -DTEST_SUITE=%{build_test_suite} -DARCH=%{BUILD_ARCH} %build make %{?jobs:-j%jobs} diff --git a/src/sensor/CMakeLists.txt b/src/sensor/CMakeLists.txt index 2dfab5b..7328a1d 100644 --- a/src/sensor/CMakeLists.txt +++ b/src/sensor/CMakeLists.txt @@ -7,10 +7,15 @@ SET(HRM_VIRT "OFF") SET(AUTO_ROTATION "ON") SET(GRAVITY "ON") SET(LINEAR_ACCEL "ON") +IF(${ARCH} STREQUAL "EMULATOR") +SET(RV "ON") +SET(ORIENTATION "ON") +ELSE() +SET(RV "OFF") SET(ORIENTATION "OFF") +ENDIF() SET(FUSION "OFF") SET(MOTION "OFF") -SET(RV "OFF") INCLUDE_DIRECTORIES( ${CMAKE_SOURCE_DIR}/src/shared @@ -49,10 +54,14 @@ IF("${LINEAR_ACCEL}" STREQUAL "ON") SET(SENSOR_DEFINITIONS ${SENSOR_DEFINITIONS} "-DENABLE_LINEAR_ACCEL") ENDIF() IF("${ORIENTATION}" STREQUAL "ON") -add_subdirectory(orientation) + FILE(GLOB_RECURSE SENSOR_SRCS ${SENSOR_SRCS} ${CMAKE_CURRENT_SOURCE_DIR}/orientation/*.cpp) + SET(SENSOR_HEADERS ${SENSOR_HEADERS} ${CMAKE_CURRENT_SOURCE_DIR}/orientation) + SET(SENSOR_DEFINITIONS ${SENSOR_DEFINITIONS} "-DENABLE_ORIENTATION") ENDIF() IF("${RV}" STREQUAL "ON") -add_subdirectory(rotation_vector) + FILE(GLOB_RECURSE SENSOR_SRCS ${SENSOR_SRCS} ${CMAKE_CURRENT_SOURCE_DIR}/rotation_vector/*.cpp) + SET(SENSOR_HEADERS ${SENSOR_HEADERS} ${CMAKE_CURRENT_SOURCE_DIR}/rotation_vector) + SET(SENSOR_DEFINITIONS ${SENSOR_DEFINITIONS} "-DENABLE_ROTATION_VECTOR") ENDIF() IF("${FUSION}" STREQUAL "ON") add_subdirectory(fusion) diff --git a/src/sensor/fusion_util.cpp b/src/sensor/fusion_util.cpp index c02c263..e14c223 100644 --- a/src/sensor/fusion_util.cpp +++ b/src/sensor/fusion_util.cpp @@ -16,15 +16,26 @@ * limitations under the License. * */ - -#include +#include #include #include +#include "fusion_util.h" + +#define RAD2DEGREE (180/M_PI) +#define QUAT (M_PI/4) +#define HALF (M_PI/2) +#define ARCTAN(x, y) ((x) == 0 ? 0 : (y) != 0 ? atan2((x), (y)) : (x) > 0 ? M_PI/2.0 : -M_PI/2.0) + +static float clamp(float v) +{ + return (v < 0) ? 0.0 : v; +} + int quat_to_matrix(const float *quat, float *R) { if(quat == NULL || R == NULL) - return -1; + return -EINVAL; float q0 = quat[3]; float q1 = quat[0]; @@ -53,3 +64,146 @@ int quat_to_matrix(const float *quat, float *R) return 0; } + +int matrix_to_quat(const float *R, float *quat) +{ + if (R == NULL || quat == NULL) + return -EINVAL; + + const float Hx = R[0]; + const float My = R[4]; + const float Az = R[8]; + quat[0] = sqrtf(clamp(Hx - My - Az + 1) * 0.25f); + quat[1] = sqrtf(clamp(-Hx + My - Az + 1) * 0.25f); + quat[2] = sqrtf(clamp(-Hx - My + Az + 1) * 0.25f); + quat[3] = sqrtf(clamp(Hx + My + Az + 1) * 0.25f); + quat[0] = copysignf(quat[0], R[7] - R[5]); + quat[1] = copysignf(quat[1], R[2] - R[6]); + quat[2] = copysignf(quat[2], R[3] - R[1]); + + return 0; +} + +int calculate_rotation_matrix(float *accel, float *geo, float *R, float *I) +{ + if (accel == NULL || geo == NULL || R == NULL || I == NULL) + return -EINVAL; + + float Ax = accel[0]; + float Ay = accel[1]; + float Az = accel[2]; + float Ex = geo[0]; + float Ey = geo[1]; + float Ez = geo[2]; + float Hx = Ey*Az - Ez*Ay; + float Hy = Ez*Ax - Ex*Az; + float Hz = Ex*Ay - Ey*Ax; + float normH = (float)sqrt(Hx*Hx + Hy*Hy + Hz*Hz); + if (normH < 0.1f) + return -EINVAL; + + float invH = 1.0f / normH; + Hx *= invH; + Hy *= invH; + Hz *= invH; + float invA = 1.0f / (float)sqrt(Ax*Ax + Ay*Ay + Az*Az); + Ax *= invA; + Ay *= invA; + Az *= invA; + float Mx = Ay*Hz - Az*Hy; + float My = Az*Hx - Ax*Hz; + float Mz = Ax*Hy - Ay*Hx; + + R[0] = Hx; R[1] = Hy; R[2] = Hz; + R[3] = Mx; R[4] = My; R[5] = Mz; + R[6] = Ax; R[7] = Ay; R[8] = Az; + + float invE = 1.0 / (float)sqrt(Ex*Ex + Ey*Ey + Ez*Ez); + float c = (Ex*Mx + Ey*My + Ez*Mz) * invE; + float s = (Ex*Ax + Ey*Ay + Ez*Az) * invE; + + I[0] = 1; I[1] = 0; I[2] = 0; + I[3] = 0; I[4] = c; I[5] = s; + I[6] = 0; I[7] = -s; I[8] = c; + + return 0; +} + +int quat_to_orientation(const float *quat, float &azimuth, float &pitch, float &roll) +{ + int error; + float g[3]; + float R[9]; + + error = quat_to_matrix(quat, R); + + if (error < 0) + return error; + + float xyz_z = ARCTAN(R[3], R[0]); + float yxz_x = asinf(R[7]); + float yxz_y = ARCTAN(-R[6], R[8]); + float yxz_z = ARCTAN(-R[1], R[4]); + + float a = fabs(yxz_x / HALF); + a = a * a; + + float p = (fabs(yxz_y) / HALF - 1.0); + + if (p < 0) + p = 0; + + float v = 1 + (1 - a) / a * p; + + if (v > 20) + v = 20; + + if (yxz_x * yxz_y > 0) { + if (yxz_z > 0 && xyz_z < 0) + xyz_z += M_PI * 2; + } else { + if (yxz_z < 0 && xyz_z > 0) + xyz_z -= M_PI * 2; + } + + g[0] = (1 - a * v) * yxz_z + (a * v) * xyz_z; + g[0] *= -1; + + float tmp = R[7]; + + if (tmp > 1.0f) + tmp = 1.0f; + else if (tmp < -1.0f) + tmp = -1.0f; + + g[1] = -asinf(tmp); + if (R[8] < 0) + g[1] = M_PI - g[1]; + + if (g[1] > M_PI) + g[1] -= M_PI * 2; + + if ((fabs(R[7]) > QUAT)) + g[2] = (float) atan2f(R[6], R[7]); + else + g[2] = (float) atan2f(R[6], R[8]); + + if (g[2] > HALF) + g[2] = M_PI - g[2]; + else if (g[2] < -HALF) + g[2] = -M_PI - g[2]; + + g[0] *= RAD2DEGREE; + g[1] *= RAD2DEGREE; + g[2] *= RAD2DEGREE; + + if (g[0] < 0) + g[0] += 360; + + azimuth = g[0]; + pitch = g[1]; + roll = g[2]; + + return 0; +} + diff --git a/src/sensor/fusion_util.h b/src/sensor/fusion_util.h index b4d2bc2..c3d6a52 100644 --- a/src/sensor/fusion_util.h +++ b/src/sensor/fusion_util.h @@ -26,6 +26,9 @@ extern "C" #endif int quat_to_matrix(const float *quat, float *R); +int matrix_to_quat(const float *R, float *quat); +int calculate_rotation_matrix(float *accel, float *geo, float *R, float *I); +int quat_to_orientation(const float *rotation, float &azimuth, float &pitch, float &roll); #ifdef __cplusplus } diff --git a/src/sensor/gravity/gravity_sensor.cpp b/src/sensor/gravity/gravity_sensor.cpp index 8b41fc5..e071226 100644 --- a/src/sensor/gravity/gravity_sensor.cpp +++ b/src/sensor/gravity/gravity_sensor.cpp @@ -161,7 +161,10 @@ void gravity_sensor::synthesize_rv(const sensor_event_t& event) float rotation[4] = {x, y, z, w}; - rotation_to_gravity(rotation, gravity); + if (!rotation_to_gravity(rotation, gravity)) { + _D("Invalid rotation_vector: [%10f] [%10f] [%10f] [%10f]", x, y, z, w); + return; + } gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); if (!gravity_event) { diff --git a/src/sensor/orientation/orientation_sensor.cpp b/src/sensor/orientation/orientation_sensor.cpp index 32e3ef7..940aed3 100644 --- a/src/sensor/orientation/orientation_sensor.cpp +++ b/src/sensor/orientation/orientation_sensor.cpp @@ -1,7 +1,7 @@ /* * sensord * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * Copyright (c) 2016 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. @@ -25,311 +25,187 @@ #include #include #include + #include +#include + +#include +#include #include #include -#include -#include - -using std::string; -using std::vector; - -#define SENSOR_NAME "ORIENTATION_SENSOR" -#define SENSOR_TYPE_ORIENTATION "ORIENTATION" +#include -#define INITIAL_VALUE -1 - -#define MS_TO_US 1000 -#define MIN_DELIVERY_DIFF_FACTOR 0.75f - -#define PI 3.141593 -#define AZIMUTH_OFFSET_DEGREES 360 -#define AZIMUTH_OFFSET_RADIANS (2 * PI) - -#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" -#define ELEMENT_AZIMUTH_ROTATION_COMPENSATION "AZIMUTH_ROTATION_COMPENSATION" +#define SENSOR_NAME "SENSOR_ORIENTATION" orientation_sensor::orientation_sensor() -: m_accel_sensor(NULL) -, m_gyro_sensor(NULL) -, m_magnetic_sensor(NULL) -, m_fusion_sensor(NULL) +: m_rotation_vector_sensor(NULL) +, m_azimuth(-1) +, m_pitch(-1) +, m_roll(-1) +, m_accuracy(-1) , m_time(0) { - virtual_sensor_config &config = virtual_sensor_config::get_instance(); - - sensor_hal *fusion_sensor_hal = sensor_loader::get_instance().get_sensor_hal(SENSOR_HAL_TYPE_FUSION); - if (!fusion_sensor_hal) - m_hardware_fusion = false; - else - m_hardware_fusion = true; - - m_name = string(SENSOR_NAME); - register_supported_event(ORIENTATION_RAW_DATA_EVENT); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_VENDOR, m_vendor)) { - _E("[VENDOR] is empty\n"); - throw ENXIO; - } - - _I("m_vendor = %s", m_vendor.c_str()); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) { - _E("[RAW_DATA_UNIT] is empty\n"); - throw ENXIO; - } - - _I("m_raw_data_unit = %s", m_raw_data_unit.c_str()); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) { - _E("[DEFAULT_SAMPLING_TIME] is empty\n"); - throw ENXIO; - } - - _I("m_default_sampling_time = %d", m_default_sampling_time); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) { - _E("[AZIMUTH_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) { - _E("[PITCH_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) { - _E("[ROLL_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_roll_rotation_compensation = %d", m_roll_rotation_compensation); - - m_interval = m_default_sampling_time * MS_TO_US; } orientation_sensor::~orientation_sensor() { - _I("orientation_sensor is destroyed!\n"); + _I("%s is destroyed!", SENSOR_NAME); } -bool orientation_sensor::init(void) +bool orientation_sensor::init() { - m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); - m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); - m_magnetic_sensor = sensor_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); + m_rotation_vector_sensor = sensor_loader::get_instance().get_sensor(ROTATION_VECTOR_SENSOR); - m_fusion_sensor = sensor_loader::get_instance().get_sensor(FUSION_SENSOR); - - if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) { - _E("Failed to load sensors, accel: %#x, gyro: %#x, mag: %#x, fusion: %#x", - m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor); + if (!m_rotation_vector_sensor) { + _E("cannot load sensor[%s]", SENSOR_NAME); return false; } - - _I("%s is created!\n", sensor_base::get_name()); - + _I("%s is created!", SENSOR_NAME); return true; } -void orientation_sensor::get_types(vector &types) +sensor_type_t orientation_sensor::get_type(void) { - types.push_back(ORIENTATION_SENSOR); + return ORIENTATION_SENSOR; } -bool orientation_sensor::on_start(void) +unsigned int orientation_sensor::get_event_type(void) { - AUTOLOCK(m_mutex); - - if (!m_hardware_fusion) { - 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_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_gyro_sensor->start(); - m_magnetic_sensor->add_client(GEOMAGNETIC_RAW_DATA_EVENT); - m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_magnetic_sensor->start(); - } - - m_fusion_sensor->register_supported_event(FUSION_EVENT); - m_fusion_sensor->register_supported_event(FUSION_ORIENTATION_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(); - - activate(); - return true; + return ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME; } -bool orientation_sensor::on_stop(void) +const char* orientation_sensor::get_name(void) { - AUTOLOCK(m_mutex); - - if (!m_hardware_fusion) { - m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->delete_interval((intptr_t)this, false); - m_accel_sensor->stop(); - m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->delete_interval((intptr_t)this, false); - m_gyro_sensor->stop(); - m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT); - m_magnetic_sensor->delete_interval((intptr_t)this, false); - m_magnetic_sensor->stop(); - } + return SENSOR_NAME; +} - 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_ORIENTATION_ENABLED); - m_fusion_sensor->stop(); +bool orientation_sensor::get_sensor_info(sensor_info &info) +{ + info.set_type(get_type()); + info.set_id(get_id()); + info.set_privilege(SENSOR_PRIVILEGE_PUBLIC); + info.set_name(get_name()); + info.set_vendor("Samsung Electronics"); + info.set_min_range(-180); + info.set_max_range(360); + info.set_resolution(0.01); + info.set_min_interval(1); + info.set_fifo_count(0); + info.set_max_batch_count(0); + info.set_supported_event(get_event_type()); + info.set_wakeup_supported(false); - deactivate(); return true; } -bool orientation_sensor::add_interval(int client_id, unsigned int interval) +void orientation_sensor::synthesize(const sensor_event_t& event) { - AUTOLOCK(m_mutex); + int error; + sensor_event_t *orientation_event; + float azimuth, pitch, roll; - if (!m_hardware_fusion) { - m_accel_sensor->add_interval(client_id, interval, false); - m_gyro_sensor->add_interval(client_id, interval, false); - m_magnetic_sensor->add_interval(client_id, interval, false); - } + if (CONVERT_ID_TYPE(event.sensor_id) != ROTATION_VECTOR_SENSOR) + return; - m_fusion_sensor->add_interval(client_id, interval, false); + error = quat_to_orientation(event.data->values, azimuth, pitch, roll); + ret_if(error); - return sensor_base::add_interval(client_id, interval, false); + orientation_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); + if (!orientation_event) { + _E("Failed to allocate memory"); + return; + } + orientation_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + if (!orientation_event->data) { + _E("Failed to allocate memory"); + free(orientation_event); + return; + } + + orientation_event->sensor_id = get_id(); + orientation_event->event_type = CONVERT_TYPE_EVENT(ORIENTATION_SENSOR); + orientation_event->data_length = sizeof(sensor_data_t); + orientation_event->data->accuracy = event.data->accuracy; + orientation_event->data->timestamp = event.data->timestamp; + orientation_event->data->value_count = 3; + orientation_event->data->values[0] = azimuth; + orientation_event->data->values[1] = pitch; + orientation_event->data->values[2] = roll; + push(orientation_event); + + m_azimuth = azimuth; + m_pitch = pitch; + m_roll = roll; + m_time = event.data->timestamp; + m_accuracy = event.data->accuracy; + + _D("[orientation] : [%10f] [%10f] [%10f]", m_azimuth, m_pitch, m_roll); } -bool orientation_sensor::delete_interval(int client_id) +int orientation_sensor::get_data(sensor_data_t **data, int *length) { - AUTOLOCK(m_mutex); + /* if It is batch sensor, remains can be 2+ */ + int remains = 1; - if (!m_hardware_fusion) { - m_accel_sensor->delete_interval(client_id, false); - m_gyro_sensor->delete_interval(client_id, false); - m_magnetic_sensor->delete_interval(client_id, false); - } + sensor_data_t *sensor_data; + sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + + sensor_data->accuracy = m_accuracy; + sensor_data->timestamp = m_time; + sensor_data->value_count = 3; + sensor_data->values[0] = m_azimuth; + sensor_data->values[1] = m_pitch; + sensor_data->values[2] = m_roll; - m_fusion_sensor->delete_interval(client_id, false); + *data = sensor_data; + *length = sizeof(sensor_data_t); - return sensor_base::delete_interval(client_id, false); + return --remains; } -void orientation_sensor::synthesize(const sensor_event_t &event, vector &outs) +bool orientation_sensor::set_interval(unsigned long interval) { - sensor_event_t orientation_event; - unsigned long long diff_time; - float azimuth_offset; - - if (event.event_type == FUSION_EVENT) { - 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); - azimuth_offset = AZIMUTH_OFFSET_DEGREES; - } else { - azimuth_offset = AZIMUTH_OFFSET_RADIANS; - } - - euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation; - euler.m_ang.m_vec[1] *= m_roll_rotation_compensation; - euler.m_ang.m_vec[2] *= m_azimuth_rotation_compensation; - - m_time = get_timestamp(); - orientation_event.sensor_id = get_id(); - orientation_event.event_type = ORIENTATION_RAW_DATA_EVENT; - orientation_event.data.accuracy = event.data.accuracy; - orientation_event.data.timestamp = m_time; - orientation_event.data.value_count = 3; - orientation_event.data.values[1] = euler.m_ang.m_vec[0]; - orientation_event.data.values[2] = euler.m_ang.m_vec[1]; - if (euler.m_ang.m_vec[2] >= 0) - orientation_event.data.values[0] = euler.m_ang.m_vec[2]; - else - orientation_event.data.values[0] = euler.m_ang.m_vec[2] + azimuth_offset; - - push(orientation_event); - } - - return; + m_interval = interval; + return true; } -int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) +bool orientation_sensor::set_batch_latency(unsigned long latency) { - sensor_data_t fusion_data; - float azimuth_offset; + return false; +} - if (event_type != ORIENTATION_RAW_DATA_EVENT) - return -1; +bool orientation_sensor::on_start(void) +{ + if (m_rotation_vector_sensor) + m_rotation_vector_sensor->start(); - m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); + m_time = 0; - quaternion quat(fusion_data.values[0], fusion_data.values[1], - fusion_data.values[2], fusion_data.values[3]); + return activate(); +} - euler_angles euler = quat2euler(quat); +bool orientation_sensor::on_stop(void) +{ + if (m_rotation_vector_sensor) + m_rotation_vector_sensor->stop(); - if(m_raw_data_unit == "DEGREES") { - euler = rad2deg(euler); - azimuth_offset = AZIMUTH_OFFSET_DEGREES; - } else { - azimuth_offset = AZIMUTH_OFFSET_RADIANS; - } + m_time = 0; - data.accuracy = fusion_data.accuracy; - data.timestamp = get_timestamp(); - data.value_count = 3; - data.values[1] = euler.m_ang.m_vec[0]; - data.values[2] = euler.m_ang.m_vec[1]; - if (euler.m_ang.m_vec[2] >= 0) - data.values[0] = euler.m_ang.m_vec[2]; - else - data.values[0] = euler.m_ang.m_vec[2] + azimuth_offset; - - data.values[1] *= m_pitch_rotation_compensation; - data.values[2] *= m_roll_rotation_compensation; - data.values[0] *= m_azimuth_rotation_compensation; - - return 0; + return deactivate(); } -bool orientation_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties) +bool orientation_sensor::add_interval(int client_id, unsigned int interval, bool is_processor) { - if(m_raw_data_unit == "DEGREES") { - properties.min_range = -180; - properties.max_range = 360; - } else { - properties.min_range = -PI; - properties.max_range = 2 * 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; + if (m_rotation_vector_sensor) + m_rotation_vector_sensor->add_interval(client_id, interval, true); - return true; + return sensor_base::add_interval(client_id, interval, is_processor); } +bool orientation_sensor::delete_interval(int client_id, bool is_processor) +{ + if (m_rotation_vector_sensor) + m_rotation_vector_sensor->delete_interval(client_id, true); + + return sensor_base::delete_interval(client_id, is_processor); +} diff --git a/src/sensor/orientation/orientation_sensor.h b/src/sensor/orientation/orientation_sensor.h index 070a21b..4e5f4c3 100644 --- a/src/sensor/orientation/orientation_sensor.h +++ b/src/sensor/orientation/orientation_sensor.h @@ -1,7 +1,7 @@ /* * sensord * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * Copyright (c) 2016 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. @@ -20,50 +20,50 @@ #ifndef _ORIENTATION_SENSOR_H_ #define _ORIENTATION_SENSOR_H_ -#include #include -#include +#include +#include class orientation_sensor : public virtual_sensor { public: orientation_sensor(); virtual ~orientation_sensor(); + /* initialize sensor */ bool init(void); - void synthesize(const sensor_event_t& event, std::vector &outs); + /* sensor info */ + virtual sensor_type_t get_type(void); + virtual unsigned int get_event_type(void); + virtual const char* get_name(void); - bool add_interval(int client_id, unsigned int interval); - bool delete_interval(int client_id); - virtual bool get_properties(sensor_type_t sensor_type, sensor_properties_s &properties); - virtual void get_types(std::vector &types); + virtual bool get_sensor_info(sensor_info &info); - int get_sensor_data(const unsigned int event_type, sensor_data_t &data); + /* synthesize event */ + virtual void synthesize(const sensor_event_t& event); -private: - sensor_base *m_accel_sensor; - sensor_base *m_gyro_sensor; - sensor_base *m_magnetic_sensor; - sensor_base *m_fusion_sensor; - - sensor_data m_accel; - sensor_data m_gyro; - sensor_data m_magnetic; + bool add_interval(int client_id, unsigned int interval, bool is_processor); + bool delete_interval(int client_id, bool is_processor); - cmutex m_value_mutex; + /* get data */ + virtual int get_data(sensor_data_t **data, int *length); +private: + sensor_base *m_rotation_vector_sensor; + float m_azimuth; + float m_pitch; + float m_roll; + int m_accuracy; unsigned long long m_time; - unsigned int m_interval; + unsigned long m_interval; + + virtual bool set_interval(unsigned long interval); + virtual bool set_batch_latency(unsigned long latency); - std::string m_vendor; - std::string m_raw_data_unit; - int m_default_sampling_time; - int m_azimuth_rotation_compensation; - int m_pitch_rotation_compensation; - int m_roll_rotation_compensation; + virtual bool on_start(void); + virtual bool on_stop(void); - bool on_start(void); - bool on_stop(void); + int rotation_to_orientation(const float *rotation, float &azimuth, float &pitch, float &roll); }; -#endif +#endif /* _ORIENTATION_SENSOR_H_ */ diff --git a/src/sensor/rotation_vector/rotation_vector_sensor.cpp b/src/sensor/rotation_vector/rotation_vector_sensor.cpp new file mode 100644 index 0000000..17c3655 --- /dev/null +++ b/src/sensor/rotation_vector/rotation_vector_sensor.cpp @@ -0,0 +1,261 @@ +/* + * sensord + * + * Copyright (c) 2016 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 +#include + +#define SENSOR_NAME "SENSOR_ROTATION_VECTOR" + +#define NORM(x, y, z) sqrt((x)*(x) + (y)*(y) + (z)*(z)) + +#define STATE_ACCEL 0x1 +#define STATE_MAGNETIC 0x2 + +rotation_vector_sensor::rotation_vector_sensor() +: m_accel_sensor(NULL) +, m_mag_sensor(NULL) +, m_x(-1) +, m_y(-1) +, m_z(-1) +, m_w(-1) +, m_time(0) +, m_state(0) +{ +} + +rotation_vector_sensor::~rotation_vector_sensor() +{ + _I("%s is destroyed!", SENSOR_NAME); +} + +bool rotation_vector_sensor::init() +{ + m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_mag_sensor = sensor_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); + + if (!m_accel_sensor || !m_mag_sensor) { + _E("cannot load sensors[%s]", SENSOR_NAME); + return false; + } + + _I("%s is created!", SENSOR_NAME); + return true; +} + +sensor_type_t rotation_vector_sensor::get_type(void) +{ + return ROTATION_VECTOR_SENSOR; +} + +unsigned int rotation_vector_sensor::get_event_type(void) +{ + return CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR); +} + +const char* rotation_vector_sensor::get_name(void) +{ + return SENSOR_NAME; +} + +bool rotation_vector_sensor::get_sensor_info(sensor_info &info) +{ + info.set_type(get_type()); + info.set_id(get_id()); + info.set_privilege(SENSOR_PRIVILEGE_PUBLIC); + info.set_name(get_name()); + info.set_vendor("Samsung Electronics"); + info.set_min_range(0); + info.set_max_range(1); + info.set_resolution(1); + info.set_min_interval(1); + info.set_fifo_count(0); + info.set_max_batch_count(0); + info.set_supported_event(get_event_type()); + info.set_wakeup_supported(false); + + return true; +} + +void rotation_vector_sensor::synthesize(const sensor_event_t& event) +{ + sensor_event_t *rotation_vector_event; + float R[9]; + float I[9]; + float quat[4]; + int error; + + if (event.event_type != GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME && + event.event_type != ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) + return; + + if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) { + m_mag[0] = event.data->values[0]; + m_mag[1] = event.data->values[1]; + m_mag[2] = event.data->values[2]; + m_accuracy = event.data->accuracy; + + m_state |= STATE_MAGNETIC; + } + + if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + m_acc[0] = event.data->values[0]; + m_acc[1] = event.data->values[1]; + m_acc[2] = event.data->values[2]; + + m_state |= STATE_ACCEL; + } + + if (m_state != (STATE_ACCEL | STATE_MAGNETIC)) + return; + + m_state = 0; + + unsigned long long timestamp = event.data->timestamp; + + error = calculate_rotation_matrix(m_acc, m_mag, R, I); + ret_if(error < 0); + + error = matrix_to_quat(R, quat); + ret_if(error < 0); + + rotation_vector_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); + if (!rotation_vector_event) { + _E("Failed to allocate memory"); + return; + } + rotation_vector_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + if (!rotation_vector_event->data) { + _E("Failed to allocate memory"); + free(rotation_vector_event); + return; + } + + rotation_vector_event->sensor_id = get_id(); + rotation_vector_event->event_type = CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR); + rotation_vector_event->data_length = sizeof(sensor_data_t); + rotation_vector_event->data->accuracy = m_accuracy; + rotation_vector_event->data->timestamp = timestamp; + rotation_vector_event->data->value_count = 4; + rotation_vector_event->data->values[0] = quat[0]; + rotation_vector_event->data->values[1] = quat[1]; + rotation_vector_event->data->values[2] = quat[2]; + rotation_vector_event->data->values[3] = quat[3]; + push(rotation_vector_event); + + m_time = timestamp; + m_x = quat[0]; + m_y = quat[1]; + m_z = quat[2]; + m_w = quat[3]; + m_accuracy = event.data->accuracy; + + _D("[rotation_vector] : [%10f] [%10f] [%10f] [%10f]", m_x, m_y, m_z, m_w); +} + +int rotation_vector_sensor::get_data(sensor_data_t **data, int *length) +{ + sensor_data_t *sensor_data; + sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + + sensor_data->accuracy = m_accuracy; + sensor_data->timestamp = m_time; + sensor_data->value_count = 3; + sensor_data->values[0] = m_x; + sensor_data->values[1] = m_y; + sensor_data->values[2] = m_z; + + *data = sensor_data; + *length = sizeof(sensor_data_t); + + return 0; +} + +bool rotation_vector_sensor::set_interval(unsigned long interval) +{ + m_interval = interval; + return true; +} + +bool rotation_vector_sensor::set_batch_latency(unsigned long latency) +{ + return false; +} + +bool rotation_vector_sensor::on_start(void) +{ + if (m_accel_sensor) + m_accel_sensor->start(); + + if (m_mag_sensor) + m_mag_sensor->start(); + + m_time = 0; + return activate(); +} + +bool rotation_vector_sensor::on_stop(void) +{ + if (m_accel_sensor) + m_accel_sensor->stop(); + + if (m_mag_sensor) + m_mag_sensor->stop(); + + m_time = 0; + m_state = 0; + + return deactivate(); +} + +bool rotation_vector_sensor::add_interval(int client_id, unsigned int interval, bool is_processor) +{ + if (m_accel_sensor) + m_accel_sensor->add_interval(client_id, interval, true); + + if (m_mag_sensor) + m_mag_sensor->add_interval(client_id, interval, true); + + return sensor_base::add_interval(client_id, interval, is_processor); +} + +bool rotation_vector_sensor::delete_interval(int client_id, bool is_processor) +{ + if (m_accel_sensor) + m_accel_sensor->delete_interval(client_id, true); + + if (m_mag_sensor) + m_mag_sensor->delete_interval(client_id, true); + + return sensor_base::delete_interval(client_id, is_processor); +} diff --git a/src/sensor/rotation_vector/rotation_vector_sensor.h b/src/sensor/rotation_vector/rotation_vector_sensor.h new file mode 100644 index 0000000..fcd4a4b --- /dev/null +++ b/src/sensor/rotation_vector/rotation_vector_sensor.h @@ -0,0 +1,72 @@ +/* + * sensord + * + * Copyright (c) 2016 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 _ROTATION_VECTOR_SENSOR_H_ +#define _ROTATION_VECTOR_SENSOR_H_ + +#include +#include + +class rotation_vector_sensor : public virtual_sensor { +public: + rotation_vector_sensor(); + virtual ~rotation_vector_sensor(); + + /* initialize sensor */ + bool init(void); + + /* sensor info */ + virtual sensor_type_t get_type(void); + virtual unsigned int get_event_type(void); + virtual const char* get_name(void); + + virtual bool get_sensor_info(sensor_info &info); + + /* synthesize event */ + virtual void synthesize(const sensor_event_t& event); + + bool add_interval(int client_id, unsigned int interval, bool is_processor); + bool delete_interval(int client_id, bool is_processor); + + /* get data */ + virtual int get_data(sensor_data_t **data, int *length); +private: + sensor_base *m_accel_sensor; + sensor_base *m_mag_sensor; + + float m_x; + float m_y; + float m_z; + float m_w; + int m_accuracy; + unsigned long long m_time; + unsigned long m_interval; + + float m_acc[3]; + float m_mag[3]; + int m_state; + + virtual bool set_interval(unsigned long interval); + virtual bool set_batch_latency(unsigned long latency); + + virtual bool on_start(void); + virtual bool on_stop(void); +}; + +#endif /* _ROTATION_VECTOR_SENSOR_H_ */ diff --git a/src/server/sensor_loader.cpp b/src/server/sensor_loader.cpp index d525d98..d5961c8 100644 --- a/src/server/sensor_loader.cpp +++ b/src/server/sensor_loader.cpp @@ -40,6 +40,12 @@ #ifdef ENABLE_LINEAR_ACCEL #include #endif +#ifdef ENABLE_ORIENTATION +#include +#endif +#ifdef ENABLE_ROTATION_VECTOR +#include +#endif using std::vector; using std::string; @@ -155,12 +161,18 @@ void sensor_loader::create_sensors(void) #ifdef ENABLE_AUTO_ROTATION create_virtual_sensors("Auto Rotation"); #endif +#ifdef ENABLE_ROTATION_VECTOR + create_virtual_sensors("Rotation Vector"); +#endif #ifdef ENABLE_GRAVITY create_virtual_sensors("Gravity"); #endif #ifdef ENABLE_LINEAR_ACCEL create_virtual_sensors("Linear Accel"); #endif +#ifdef ENABLE_ORIENTATION + create_virtual_sensors("Orientation"); +#endif } template diff --git a/src/shared/sensor_common.h b/src/shared/sensor_common.h index 5bf16fe..d505918 100644 --- a/src/shared/sensor_common.h +++ b/src/shared/sensor_common.h @@ -32,8 +32,12 @@ #define SENSOR_ID_INVALID -1 #define SENSOR_TYPE_SHIFT 32 +#define SENSOR_EVENT_SHIFT 16 #define SENSOR_INDEX_MASK 0xFFFFFFFF +#define CONVERT_ID_TYPE(id) ((id) >> SENSOR_TYPE_SHIFT) +#define CONVERT_TYPE_EVENT(type) ((type) << SENSOR_EVENT_SHIFT | 0x1) + #ifndef NAME_MAX #define NAME_MAX 256 #endif