%define gyroscope_uncal_state OFF
%define build_test_suite ON
+%ifarch %{ix86} x86_64
+%define BUILD_ARCH EMULATOR
+%endif
+
%description
Sensor daemon
-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}
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
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)
* limitations under the License.
*
*/
-
-#include <fusion_util.h>
+#include <errno.h>
#include <math.h>
#include <stdlib.h>
+#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];
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;
+}
+
#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
}
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) {
/*
* 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.
#include <time.h>
#include <sys/types.h>
#include <dlfcn.h>
+
#include <sensor_log.h>
+#include <sensor_types.h>
+
+#include <sensor_common.h>
+#include <virtual_sensor.h>
#include <orientation_sensor.h>
#include <sensor_loader.h>
-#include <orientation_filter.h>
-#include <virtual_sensor_config.h>
-
-using std::string;
-using std::vector;
-
-#define SENSOR_NAME "ORIENTATION_SENSOR"
-#define SENSOR_TYPE_ORIENTATION "ORIENTATION"
+#include <fusion_util.h>
-#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<sensor_type_t> &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<sensor_event_t> &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<float> quat(event.data.values[0], event.data.values[1],
- event.data.values[2], event.data.values[3]);
-
- euler_angles<float> 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<float> quat(fusion_data.values[0], fusion_data.values[1],
- fusion_data.values[2], fusion_data.values[3]);
+ return activate();
+}
- euler_angles<float> 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);
+}
/*
* 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.
#ifndef _ORIENTATION_SENSOR_H_
#define _ORIENTATION_SENSOR_H_
-#include <sensor_internal.h>
#include <virtual_sensor.h>
-#include <orientation_filter.h>
+#include <sensor_types.h>
+#include <sensor_fusion.h>
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<sensor_event_t> &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<sensor_type_t> &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<float> m_accel;
- sensor_data<float> m_gyro;
- sensor_data<float> 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_ */
--- /dev/null
+/*
+ * 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 <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 <sensor_log.h>
+#include <sensor_types.h>
+
+#include <sensor_common.h>
+#include <virtual_sensor.h>
+#include <rotation_vector_sensor.h>
+#include <sensor_loader.h>
+#include <fusion_util.h>
+
+#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);
+}
--- /dev/null
+/*
+ * 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 <virtual_sensor.h>
+#include <sensor_types.h>
+
+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_ */
#ifdef ENABLE_LINEAR_ACCEL
#include <linear_accel_sensor.h>
#endif
+#ifdef ENABLE_ORIENTATION
+#include <orientation_sensor.h>
+#endif
+#ifdef ENABLE_ROTATION_VECTOR
+#include <rotation_vector_sensor.h>
+#endif
using std::vector;
using std::string;
#ifdef ENABLE_AUTO_ROTATION
create_virtual_sensors<auto_rotation_sensor>("Auto Rotation");
#endif
+#ifdef ENABLE_ROTATION_VECTOR
+ create_virtual_sensors<rotation_vector_sensor>("Rotation Vector");
+#endif
#ifdef ENABLE_GRAVITY
create_virtual_sensors<gravity_sensor>("Gravity");
#endif
#ifdef ENABLE_LINEAR_ACCEL
create_virtual_sensors<linear_accel_sensor>("Linear Accel");
#endif
+#ifdef ENABLE_ORIENTATION
+ create_virtual_sensors<orientation_sensor>("Orientation");
+#endif
}
template<typename _sensor>
#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
projects / platform / core / system / sensord.git / commitdiff