From: kibak.yoon Date: Tue, 15 Mar 2016 02:52:42 +0000 (+0900) Subject: sensord: enable hrm/gravity/linear accel sensors X-Git-Tag: submit/tizen/20160315.065646^2~1 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=df57020c8580d24ed361b67be6a613153a09bb44;p=platform%2Fcore%2Fsystem%2Fsensord.git sensord: enable hrm/gravity/linear accel sensors Change-Id: I9595bbcb4a0f194b06b1d1f458fb410f179afcd6 Signed-off-by: kibak.yoon --- diff --git a/src/sensor/CMakeLists.txt b/src/sensor/CMakeLists.txt index b37f7bf..0e83c6d 100644 --- a/src/sensor/CMakeLists.txt +++ b/src/sensor/CMakeLists.txt @@ -2,11 +2,11 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) PROJECT(sensors CXX) INCLUDE(GNUInstallDirs) -SET(HRM "OFF") +SET(HRM "ON") SET(HRM_VIRT "OFF") SET(AUTO_ROTATION "ON") -SET(GRAVITY "OFF") -SET(LINEAR_ACCEL "OFF") +SET(GRAVITY "ON") +SET(LINEAR_ACCEL "ON") SET(ORIENTATION "OFF") SET(FUSION "OFF") SET(MOTION "OFF") diff --git a/src/sensor/fusion_util.cpp b/src/sensor/fusion_util.cpp new file mode 100644 index 0000000..1f2692d --- /dev/null +++ b/src/sensor/fusion_util.cpp @@ -0,0 +1,56 @@ +/* + * sensord + * + * Copyright (c) 2014 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 + +int quat_to_matrix(const float *quat, float *R) +{ + if(quat == NULL || R == NULL) + return -1; + + float q0 = quat[3]; + float q1 = quat[0]; + float q2 = quat[1]; + float q3 = quat[2]; + + float sq_q1 = 2 * q1 * q1; + float sq_q2 = 2 * q2 * q2; + float sq_q3 = 2 * q3 * q3; + float q1_q2 = 2 * q1 * q2; + float q3_q0 = 2 * q3 * q0; + float q1_q3 = 2 * q1 * q3; + float q2_q0 = 2 * q2 * q0; + float q2_q3 = 2 * q2 * q3; + float q1_q0 = 2 * q1 * q0; + + R[0] = 1 - sq_q2 - sq_q3; + R[1] = q1_q2 - q3_q0; + R[2] = q1_q3 + q2_q0; + R[3] = q1_q2 + q3_q0; + R[4] = 1 - sq_q1 - sq_q3; + R[5] = q2_q3 - q1_q0; + R[6] = q1_q3 - q2_q0; + R[7] = q2_q3 + q1_q0; + R[8] = 1 - sq_q1 - sq_q2; + + return 0; +} + diff --git a/src/sensor/fusion_util.h b/src/sensor/fusion_util.h new file mode 100644 index 0000000..b4d2bc2 --- /dev/null +++ b/src/sensor/fusion_util.h @@ -0,0 +1,34 @@ +/* + * sensord + * + * Copyright (c) 2014 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 _FUSION_UTIL_H_ +#define _FUSION_UTIL_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +int quat_to_matrix(const float *quat, float *R); + +#ifdef __cplusplus +} +#endif /* __cplusplus */ + +#endif /* _FUSION_UTIL_H_ */ diff --git a/src/sensor/gravity/gravity_sensor.cpp b/src/sensor/gravity/gravity_sensor.cpp index d81242a..49147a6 100644 --- a/src/sensor/gravity/gravity_sensor.cpp +++ b/src/sensor/gravity/gravity_sensor.cpp @@ -25,367 +25,498 @@ #include #include #include + #include +#include + +#include +#include #include #include -#include +#include -using std::string; -using std::vector; +#define SENSOR_NAME "GRAVITY_SENSOR" -#define INITIAL_VALUE -1 #define GRAVITY 9.80665 -#define DEVIATION 0.1 - -#define PI 3.141593 -#define AZIMUTH_OFFSET_DEGREES 360 -#define AZIMUTH_OFFSET_RADIANS (2 * PI) +#define PHASE_ACCEL_READY 0x01 +#define PHASE_GYRO_READY 0x02 +#define PHASE_FUSION_READY 0x03 +#define US_PER_SEC 1000000 +#define MS_PER_SEC 1000 +#define INV_ANGLE -1000 +#define TAU_LOW 0.4 +#define TAU_MID 0.75 +#define TAU_HIGH 0.99 -#define SENSOR_NAME "GRAVITY_SENSOR" -#define SENSOR_TYPE_GRAVITY "GRAVITY" -#define SENSOR_TYPE_ORIENTATION "ORIENTATION" - -#define MS_TO_US 1000 -#define MIN_DELIVERY_DIFF_FACTOR 0.75f - -#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_GRAVITY_SIGN_COMPENSATION "GRAVITY_SIGN_COMPENSATION" -#define ELEMENT_ORIENTATION_DATA_UNIT "RAW_DATA_UNIT" -#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 DEG2RAD(x) ((x) * M_PI / 180.0) +#define NORM(x, y, z) sqrt((x)*(x) + (y)*(y) + (z)*(z)) +#define ARCTAN(x, y) ((x) == 0 ? 0 : (y) != 0 ? atan2((x),(y)) : (x) > 0 ? M_PI/2.0 : -M_PI/2.0) gravity_sensor::gravity_sensor() -: m_accel_sensor(NULL) +: m_fusion(NULL) +, m_accel_sensor(NULL) , m_gyro_sensor(NULL) -, m_magnetic_sensor(NULL) -, m_fusion_sensor(NULL) +, m_fusion_phase(0) +, m_x(-1) +, m_y(-1) +, m_z(-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; +gravity_sensor::~gravity_sensor() +{ + _I("gravity_sensor is destroyed!\n"); +} - m_name = std::string(SENSOR_NAME); - register_supported_event(GRAVITY_RAW_DATA_EVENT); +bool gravity_sensor::init() +{ + /* Acc (+ Gyro) fusion */ + m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); - if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_VENDOR, m_vendor)) { - _E("[VENDOR] is empty\n"); - throw ENXIO; + if (!m_accel_sensor) { + _E("cannot load accelerometer sensor_hal[%s]", get_name()); + return false; } - _I("m_vendor = %s", m_vendor.c_str()); + m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); + + _I("%s (%s) is created!\n", get_name(), m_gyro_sensor ? "Acc+Gyro Fusion" : "LowPass Acc"); + return true; +} - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ORIENTATION_DATA_UNIT, m_orientation_data_unit)) { - _E("[ORIENTATION_DATA_UNIT] is empty\n"); - throw ENXIO; - } +sensor_type_t gravity_sensor::get_type(void) +{ + return GRAVITY_SENSOR; +} - _I("m_orientation_data_unit = %s", m_orientation_data_unit.c_str()); +unsigned int gravity_sensor::get_event_type(void) +{ + return GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME; +} - if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) { - _E("[RAW_DATA_UNIT] is empty\n"); - throw ENXIO; - } +const char* gravity_sensor::get_name(void) +{ + return SENSOR_NAME; +} - _I("m_raw_data_unit = %s", m_raw_data_unit.c_str()); +bool gravity_sensor::get_sensor_info(sensor_info &info) +{ + info.set_type(get_type()); + info.set_id(get_id()); + info.set_privilege(SENSOR_PRIVILEGE_PUBLIC); // FIXME + info.set_name("Gravity Sensor"); + info.set_vendor("Samsung Electronics"); + info.set_min_range(-19.6); + info.set_max_range(19.6); + 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); - if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) { - _E("[DEFAULT_SAMPLING_TIME] is empty\n"); - throw ENXIO; - } + return true; +} - _I("m_default_sampling_time = %d", m_default_sampling_time); +void gravity_sensor::synthesize(const sensor_event_t& event) +{ + /* If the rotation vector sensor is available */ + if (m_fusion) { + synthesize_rv(event); + return; + } - if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_GRAVITY_SIGN_COMPENSATION, m_gravity_sign_compensation, 3)) { - _E("[GRAVITY_SIGN_COMPENSATION] is empty\n"); - throw ENXIO; + /* If both Acc & Gyro are available */ + if (m_gyro_sensor) { + synthesize_fusion(event); + return; } - _I("m_gravity_sign_compensation = (%d, %d, %d)", m_gravity_sign_compensation[0], m_gravity_sign_compensation[1], m_gravity_sign_compensation[2]); + /* If only Acc is available */ + synthesize_lowpass(event); +} + +void gravity_sensor::synthesize_rv(const sensor_event_t& event) +{ + if (!m_fusion->is_data_ready()) + return; + + sensor_event_t *gravity_event; + float gravity[3]; + float x, y, z, w, heading_accuracy; + int accuracy; - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) { - _E("[AZIMUTH_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; + if (!m_fusion->get_rotation_vector(x, y, z, w, heading_accuracy, accuracy)) { + _E("Failed to get rotation vector"); + return; } - _I("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation); + unsigned long long timestamp = m_fusion->get_data_timestamp(); + + if (!check_sampling_time(timestamp)) + return; + + float rotation[4] = {x, y, z, w}; - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) { - _E("[PITCH_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; + rotation_to_gravity(rotation, gravity); + + gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); + if (!gravity_event) { + _E("Failed to allocate memory"); + return; + } + gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + if (!gravity_event->data) { + _E("Failed to allocate memory"); + return; } - _I("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation); + gravity_event->sensor_id = get_id(); + gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME; + gravity_event->data_length = sizeof(sensor_data_t); + gravity_event->data->accuracy = accuracy; + gravity_event->data->timestamp = m_fusion->get_data_timestamp(); + gravity_event->data->value_count = 3; + gravity_event->data->values[0] = gravity[0]; + gravity_event->data->values[1] = gravity[1]; + gravity_event->data->values[2] = gravity[2]; + push(gravity_event); + + m_time = event.data->timestamp; + m_x = gravity[0]; + m_y = gravity[1]; + m_z = gravity[2]; + m_accuracy = accuracy; +} - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) { - _E("[ROLL_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; +void gravity_sensor::synthesize_lowpass(const sensor_event_t& event) +{ + sensor_event_t *gravity_event; + float x, y, z, norm, alpha, tau, err; + + norm = NORM(event.data->values[0], event.data->values[1], event.data->values[2]); + x = event.data->values[0] / norm * GRAVITY; + y = event.data->values[1] / norm * GRAVITY; + z = event.data->values[2] / norm * GRAVITY; + + if (m_time > 0) { + err = fabs(norm - GRAVITY) / GRAVITY; + tau = (err < 0.1 ? TAU_LOW : err > 0.9 ? TAU_HIGH : TAU_MID); + alpha = tau / (tau + (float)(event.data->timestamp - m_time) / US_PER_SEC); + x = alpha * m_x + (1 - alpha) * x; + y = alpha * m_y + (1 - alpha) * y; + z = alpha * m_z + (1 - alpha) * z; + norm = NORM(x, y, z); + x = x / norm * GRAVITY; + y = y / norm * GRAVITY; + z = z / norm * GRAVITY; } - _I("m_roll_rotation_compensation = %d", m_roll_rotation_compensation); + gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); + if (!gravity_event) { + _E("Failed to allocate memory"); + return; + } + gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + if (!gravity_event->data) { + _E("Failed to allocate memory"); + return; + } - m_interval = m_default_sampling_time * MS_TO_US; + gravity_event->sensor_id = get_id(); + gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME; + gravity_event->data_length = sizeof(sensor_data_t); + gravity_event->data->accuracy = event.data->accuracy; + gravity_event->data->timestamp = event.data->timestamp; + gravity_event->data->value_count = 3; + gravity_event->data->values[0] = x; + gravity_event->data->values[1] = y; + gravity_event->data->values[2] = z; + push(gravity_event); + + m_time = event.data->timestamp; + m_x = x; + m_y = y; + m_z = z; + m_accuracy = event.data->accuracy; } -gravity_sensor::~gravity_sensor() +void gravity_sensor::synthesize_fusion(const sensor_event_t& event) { - _I("gravity_sensor is destroyed!\n"); + sensor_event_t *gravity_event; + + if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + fusion_set_accel(event); + m_fusion_phase |= PHASE_ACCEL_READY; + } else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) { + fusion_set_gyro(event); + m_fusion_phase |= PHASE_GYRO_READY; + } + + if (m_fusion_phase != PHASE_FUSION_READY) + return; + + m_fusion_phase = 0; + + fusion_update_angle(); + fusion_get_gravity(); + + gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); + if (!gravity_event) { + _E("Failed to allocate memory"); + return; + } + gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + if (!gravity_event->data) { + _E("Failed to allocate memory"); + return; + } + + gravity_event->sensor_id = get_id(); + gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME; + gravity_event->data_length = sizeof(sensor_data_t); + gravity_event->data->accuracy = m_accuracy; + gravity_event->data->timestamp = m_time; + gravity_event->data->value_count = 3; + gravity_event->data->values[0] = m_x; + gravity_event->data->values[1] = m_y; + gravity_event->data->values[2] = m_z; + push(gravity_event); } -bool gravity_sensor::init() +void gravity_sensor::fusion_set_accel(const sensor_event_t& event) { - 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); + double x = event.data->values[0]; + double y = event.data->values[1]; + double z = event.data->values[2]; - m_fusion_sensor = sensor_loader::get_instance().get_sensor(FUSION_SENSOR); + m_accel_mag = NORM(x, y, z); - if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) { - _E("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x, fusion: 0x%x", - m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor); - return false; - } + m_angle_n[0] = ARCTAN(z, y); + m_angle_n[1] = ARCTAN(x, z); + m_angle_n[2] = ARCTAN(y, x); - _I("%s is created!", sensor_base::get_name()); - return true; + m_time_new = event.data->timestamp; + + _D("AccIn: (%f, %f, %f)", x/m_accel_mag, y/m_accel_mag, z/m_accel_mag); } -void gravity_sensor::get_types(vector &types) +void gravity_sensor::fusion_set_gyro(const sensor_event_t& event) { - types.push_back(GRAVITY_SENSOR); + m_velocity[0] = -DEG2RAD(event.data->values[0]); + m_velocity[1] = -DEG2RAD(event.data->values[1]); + m_velocity[2] = -DEG2RAD(event.data->values[2]); + + m_time_new = event.data->timestamp; } -bool gravity_sensor::on_start(void) +void gravity_sensor::fusion_update_angle() { - AUTOLOCK(m_mutex); + _D("AngleIn: (%f, %f, %f)", m_angle_n[0], m_angle_n[1], m_angle_n[2]); + _D("AngAccl: (%f, %f, %f)", m_velocity[0], m_velocity[1], m_velocity[2]); + _D("Angle : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]); + + if (m_angle[0] == INV_ANGLE) { + /* 1st iteration */ + m_angle[0] = m_angle_n[0]; + m_angle[1] = m_angle_n[1]; + m_angle[2] = m_angle_n[2]; + } else { + complementary(m_time_new - m_time); + } - 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(); + _D("Angle' : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]); +} + +void gravity_sensor::fusion_get_gravity() +{ + double x = 0, y = 0, z = 0; + double norm; + double vec[3][3]; + + /* Rotating along y-axis then z-axis */ + vec[0][2] = cos(m_angle[1]); + vec[0][0] = sin(m_angle[1]); + vec[0][1] = vec[0][0] * tan(m_angle[2]); + + /* Rotating along z-axis then x-axis */ + vec[1][0] = cos(m_angle[2]); + vec[1][1] = sin(m_angle[2]); + vec[1][2] = vec[1][1] * tan(m_angle[0]); + + /* Rotating along x-axis then y-axis */ + vec[2][1] = cos(m_angle[0]); + vec[2][2] = sin(m_angle[0]); + vec[2][0] = vec[2][2] * tan(m_angle[1]); + + /* Normalize */ + for (int i = 0; i < 3; ++i) { + norm = NORM(vec[i][0], vec[i][1], vec[i][2]); + vec[i][0] /= norm; + vec[i][1] /= norm; + vec[i][2] /= norm; + x += vec[i][0]; + y += vec[i][1]; + z += vec[i][2]; } - 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(); + norm = NORM(x, y, z); - activate(); - return true; + m_x = x / norm * GRAVITY; + m_y = y / norm * GRAVITY; + m_z = z / norm * GRAVITY; + m_time = m_time_new; } -bool gravity_sensor::on_stop(void) +void gravity_sensor::complementary(unsigned long long time_diff) { - AUTOLOCK(m_mutex); + double err = fabs(m_accel_mag - GRAVITY) / GRAVITY; + double tau = (err < 0.1 ? TAU_LOW : err > 0.9 ? TAU_HIGH : TAU_MID); + double delta_t = (double)time_diff/ US_PER_SEC; + double alpha = tau / (tau + delta_t); - 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(); - } + _D("mag, err, tau, dt, alpha = %f, %f, %f, %f, %f", m_accel_mag, err, tau, delta_t, alpha); - 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(); + m_angle[0] = complementary(m_angle[0], m_angle_n[0], m_velocity[0], delta_t, alpha); + m_angle[1] = complementary(m_angle[1], m_angle_n[1], m_velocity[1], delta_t, alpha); + m_angle[2] = complementary(m_angle[2], m_angle_n[2], m_velocity[2], delta_t, alpha); +} - deactivate(); - return true; +double gravity_sensor::complementary(double angle, double angle_in, double vel, double delta_t, double alpha) +{ + double s, c; + angle = angle + vel * delta_t; + s = alpha * sin(angle) + (1 - alpha) * sin(angle_in); + c = alpha * cos(angle) + (1 - alpha) * cos(angle_in); + return ARCTAN(s, c); } -bool gravity_sensor::add_interval(int client_id, unsigned int interval) +int gravity_sensor::get_data(sensor_data_t **data, int *length) { - AUTOLOCK(m_mutex); - 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 It is batch sensor, remains can be 2+ */ + int remains = 1; - m_fusion_sensor->add_interval(client_id, interval, false); + sensor_data_t *sensor_data; + sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); - return sensor_base::add_interval(client_id, interval, false); + 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 --remains; } -bool gravity_sensor::delete_interval(int client_id) +bool gravity_sensor::set_batch_latency(unsigned long latency) { - AUTOLOCK(m_mutex); - 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); - } + return false; +} - m_fusion_sensor->delete_interval(client_id, false); +bool gravity_sensor::set_wakeup(int wakeup) +{ + return false; +} + +bool gravity_sensor::on_start(void) +{ + if (m_fusion) + m_fusion->start(); + + if (m_accel_sensor) + m_accel_sensor->start(); - return sensor_base::delete_interval(client_id, false); + if (m_gyro_sensor) + m_gyro_sensor->start(); + + m_time = 0; + m_fusion_phase = 0; + m_angle[0] = INV_ANGLE; + + return activate(); } -void gravity_sensor::synthesize(const sensor_event_t &event, vector &outs) +bool gravity_sensor::on_stop(void) { - sensor_event_t gravity_event; - float pitch, roll, azimuth; - 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_orientation_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; - - pitch = euler.m_ang.m_vec[0]; - roll = euler.m_ang.m_vec[1]; - if (euler.m_ang.m_vec[2] >= 0) - azimuth = euler.m_ang.m_vec[2]; - else - azimuth = euler.m_ang.m_vec[2] + azimuth_offset; - - if(m_orientation_data_unit == "DEGREES") { - azimuth *= DEG2RAD; - pitch *= DEG2RAD; - roll *= DEG2RAD; - } - - m_time = get_timestamp(); - gravity_event.sensor_id = get_id(); - gravity_event.event_type = GRAVITY_RAW_DATA_EVENT; - - if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) || - (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) { - gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth); - gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth); - gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll); - } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) || - (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) { - gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth); - gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth); - gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch); - } else { - gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll); - gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch); - gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch); - } - gravity_event.data.value_count = 3; - gravity_event.data.timestamp = m_time; - gravity_event.data.accuracy = SENSOR_ACCURACY_GOOD; - - push(gravity_event); - } + if (m_fusion) + m_fusion->stop(); + + if (m_accel_sensor) + m_accel_sensor->stop(); - return; + if (m_gyro_sensor) + m_gyro_sensor->stop(); + + m_time = 0; + + return deactivate(); } -int gravity_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) +bool gravity_sensor::add_interval(int client_id, unsigned int interval, bool is_processor) { - sensor_data_t fusion_data; - float azimuth_offset; - float pitch, roll, azimuth; + if (m_fusion) + m_fusion->set_interval(FUSION_EVENT_AGM, client_id, interval); - if (event_type != GRAVITY_RAW_DATA_EVENT) - return -1; + if (m_accel_sensor) + m_accel_sensor->add_interval(client_id, interval, true); - m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); + if (m_gyro_sensor) + m_gyro_sensor->add_interval(client_id, interval, true); - quaternion quat(fusion_data.values[0], fusion_data.values[1], - fusion_data.values[2], fusion_data.values[3]); + return sensor_base::add_interval(client_id, interval, is_processor); +} - euler_angles euler = quat2euler(quat); +bool gravity_sensor::delete_interval(int client_id, bool is_processor) +{ + if (m_fusion) + m_fusion->unset_interval(FUSION_EVENT_AGM, client_id); - if(m_orientation_data_unit == "DEGREES") { - euler = rad2deg(euler); - azimuth_offset = AZIMUTH_OFFSET_DEGREES; - } - else { - azimuth_offset = AZIMUTH_OFFSET_RADIANS; - } + if (m_accel_sensor) + m_accel_sensor->delete_interval(client_id, true); - pitch = euler.m_ang.m_vec[0]; - roll = euler.m_ang.m_vec[1]; + if (m_gyro_sensor) + m_gyro_sensor->delete_interval(client_id, true); - if (euler.m_ang.m_vec[2] >= 0) - azimuth = euler.m_ang.m_vec[2]; - else - azimuth = euler.m_ang.m_vec[2] + azimuth_offset; + return sensor_base::delete_interval(client_id, is_processor); +} - if(m_orientation_data_unit == "DEGREES") { - azimuth *= DEG2RAD; - pitch *= DEG2RAD; - roll *= DEG2RAD; - } +bool gravity_sensor::set_interval(unsigned long interval) +{ + m_interval = interval; + return true; +} - data.accuracy = SENSOR_ACCURACY_GOOD; - data.timestamp = get_timestamp(); - if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) || - (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) { - data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth); - data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth); - data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll); - } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) || - (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) { - data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth); - data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth); - data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch); - } else { - data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll); - data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch); - data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch); - } - data.value_count = 3; +bool gravity_sensor::rotation_to_gravity(const float *rotation, float *gravity) +{ + float R[9]; - return 0; + if (quat_to_matrix(rotation, R) < 0) + return false; + + gravity[0] = R[6] * GRAVITY; + gravity[1] = R[7] * GRAVITY; + gravity[2] = R[8] * GRAVITY; + + return true; } -bool gravity_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties) +bool gravity_sensor::check_sampling_time(unsigned long long timestamp) { - properties.min_range = -GRAVITY; - properties.max_range = GRAVITY; - properties.resolution = 0.000001; - properties.vendor = m_vendor; - properties.name = SENSOR_NAME; - properties.fifo_count = 0; - properties.max_batch_count = 0; - properties.min_interval = 1; + const float MIN_DELIVERY_DIFF_FACTOR = 0.75f; + const int MS_TO_US = 1000; + long long diff_time; + + diff_time = timestamp - m_time; + + if (m_time && (diff_time < m_interval * MS_TO_US * MIN_DELIVERY_DIFF_FACTOR)) + return false; return true; } diff --git a/src/sensor/gravity/gravity_sensor.h b/src/sensor/gravity/gravity_sensor.h index b5114ee..c9ed7d7 100644 --- a/src/sensor/gravity/gravity_sensor.h +++ b/src/sensor/gravity/gravity_sensor.h @@ -20,52 +20,71 @@ #ifndef _GRAVITY_SENSOR_H_ #define _GRAVITY_SENSOR_H_ -#include #include -#include +#include +#include class gravity_sensor : public virtual_sensor { public: gravity_sensor(); virtual ~gravity_sensor(); - bool init(); - virtual void get_types(std::vector &types); + /* 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_sensor_info(sensor_info &info); - int get_sensor_data(const unsigned int event_type, sensor_data_t &data); - virtual bool get_properties(sensor_type_t sensor_type, sensor_properties_s &properties); + /* 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_fusion *m_fusion; 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; - - cmutex m_value_mutex; + int m_fusion_phase; + float m_x; + float m_y; + float m_z; int m_accuracy; unsigned long long m_time; - unsigned int m_interval; - - std::string m_vendor; - std::string m_raw_data_unit; - std::string m_orientation_data_unit; - int m_default_sampling_time; - int m_gravity_sign_compensation[3]; - int m_azimuth_rotation_compensation; - int m_pitch_rotation_compensation; - int m_roll_rotation_compensation; - - bool on_start(void); - bool on_stop(void); + unsigned long m_interval; + + double m_angle[3]; + double m_angle_n[3]; + double m_accel_mag; + double m_velocity[3]; + unsigned long long m_time_new; + + virtual bool set_interval(unsigned long interval); + virtual bool set_batch_latency(unsigned long latency); + virtual bool set_wakeup(int wakeup); + + virtual bool on_start(void); + virtual bool on_stop(void); + + bool rotation_to_gravity(const float *rotation, float *gravity); + bool check_sampling_time(unsigned long long timestamp); + + void synthesize_rv(const sensor_event_t& event); + void synthesize_lowpass(const sensor_event_t& event); + void synthesize_fusion(const sensor_event_t& event); + + void fusion_set_accel(const sensor_event_t& event); + void fusion_set_gyro(const sensor_event_t& event); + void fusion_update_angle(); + void fusion_get_gravity(); + double complementary(double angle, double angle_in, double vel, double delta_t, double alpha); + void complementary(unsigned long long time_diff); }; #endif /* _GRAVITY_SENSOR_H_ */ diff --git a/src/sensor/hrm/hrm_sensor.cpp b/src/sensor/hrm/hrm_sensor.cpp new file mode 100644 index 0000000..908f1b5 --- /dev/null +++ b/src/sensor/hrm/hrm_sensor.cpp @@ -0,0 +1,34 @@ +/* + * 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 "hrm_sensor.h" + +hrm_sensor::hrm_sensor() +{ + set_permission(SENSOR_PERMISSION_BIO); + + _I("hrm_sensor is created : 0x%x", this); +} + +hrm_sensor::~hrm_sensor() +{ + +} diff --git a/src/sensor/hrm/hrm_sensor.h b/src/sensor/hrm/hrm_sensor.h new file mode 100644 index 0000000..1c64bc5 --- /dev/null +++ b/src/sensor/hrm/hrm_sensor.h @@ -0,0 +1,32 @@ +/* + * 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 _HRM_SENSOR_H_ +#define _HRM_SENSOR_H_ + +#include + +class hrm_sensor : public physical_sensor { +public: + hrm_sensor(); + virtual ~hrm_sensor(); +}; + +#endif /* _HRM_SENSOR_H_ */ + diff --git a/src/sensor/linear_accel/linear_accel_sensor.cpp b/src/sensor/linear_accel/linear_accel_sensor.cpp index 67095f8..33d1548 100644 --- a/src/sensor/linear_accel/linear_accel_sensor.cpp +++ b/src/sensor/linear_accel/linear_accel_sensor.cpp @@ -25,145 +25,32 @@ #include #include #include + #include +#include + +#include +#include #include #include -#include - -using std::string; -using std::vector; +#include #define SENSOR_NAME "LINEAR_ACCEL_SENSOR" -#define SENSOR_TYPE_LINEAR_ACCEL "LINEAR_ACCEL" -#define SENSOR_TYPE_GRAVITY "GRAVITY" -#define SENSOR_TYPE_ORIENTATION "ORIENTATION" - -#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_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" -#define ELEMENT_ACCEL_SCALE "ACCEL_SCALE" -#define ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION "LINEAR_ACCEL_SIGN_COMPENSATION" -#define ELEMENT_ORIENTATION_DATA_UNIT "RAW_DATA_UNIT" -#define ELEMENT_GRAVITY_SIGN_COMPENSATION "GRAVITY_SIGN_COMPENSATION" -#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 INITIAL_VALUE -1 -#define GRAVITY 9.80665 -#define DEVIATION 0.1 - -#define PI 3.141593 -#define AZIMUTH_OFFSET_DEGREES 360 -#define AZIMUTH_OFFSET_RADIANS (2 * PI) - -#define MS_TO_US 1000 -#define MIN_DELIVERY_DIFF_FACTOR 0.75f -#define ACCELEROMETER_ENABLED 0x01 -#define GRAVITY_ENABLED 0x02 -#define LINEAR_ACCEL_ENABLED 3 +#define GRAVITY 9.80665 linear_accel_sensor::linear_accel_sensor() : m_accel_sensor(NULL) -, m_gyro_sensor(NULL) -, m_magnetic_sensor(NULL) -, m_fusion_sensor(NULL) +, m_gravity_sensor(NULL) +, m_x(0) +, m_y(0) +, m_z(0) +, m_gx(0) +, m_gy(0) +, m_gz(0) +, m_accuracy(0) , m_time(0) { - virtual_sensor_config &config = virtual_sensor_config::get_instance(); - - m_name = string(SENSOR_NAME); - m_enable_linear_accel = 0; - register_supported_event(LINEAR_ACCEL_RAW_DATA_EVENT); - - 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; - - - if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_VENDOR, m_vendor)) { - _E("[VENDOR] is empty\n"); - throw ENXIO; - } - - _I("m_vendor = %s", m_vendor.c_str()); - - if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, 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_ORIENTATION_DATA_UNIT, m_orientation_data_unit)) { - _E("[ORIENTATION_DATA_UNIT] is empty\n"); - throw ENXIO; - } - - _I("m_orientation_data_unit = %s", m_orientation_data_unit.c_str()); - - if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, 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_LINEAR_ACCEL, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) { - _E("[ACCEL_STATIC_BIAS] is empty\n"); - throw ENXIO; - } - - if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) { - _E("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("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_GRAVITY, ELEMENT_GRAVITY_SIGN_COMPENSATION, m_gravity_sign_compensation, 3)) { - _E("[GRAVITY_SIGN_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_gravity_sign_compensation = (%d, %d, %d)", m_gravity_sign_compensation[0], m_gravity_sign_compensation[1], m_gravity_sign_compensation[2]); - - if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION, m_linear_accel_sign_compensation, 3)) { - _E("[LINEAR_ACCEL_SIGN_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_linear_accel_sign_compensation = (%d, %d, %d)", m_linear_accel_sign_compensation[0], m_linear_accel_sign_compensation[1], m_linear_accel_sign_compensation[2]); - - 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; - } linear_accel_sensor::~linear_accel_sensor() @@ -174,249 +61,177 @@ linear_accel_sensor::~linear_accel_sensor() bool linear_accel_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_fusion_sensor = sensor_loader::get_instance().get_sensor(FUSION_SENSOR); + if (!m_accel_sensor) { + _E("cannot load accelerometer sensor_hal[%s]", get_name()); + return false; + } + + m_gravity_sensor = sensor_loader::get_instance().get_sensor(GRAVITY_SENSOR); - if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) { - _E("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x, fusion: 0x%x", - m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor); + if (!m_gravity_sensor) { + _E("cannot load gravity sensor_hal[%s]", get_name()); return false; } - _I("%s is created!", sensor_base::get_name()); + _I("%s is created!\n", get_name()); + return true; } -void linear_accel_sensor::get_types(vector &types) +sensor_type_t linear_accel_sensor::get_type(void) { - types.push_back(LINEAR_ACCEL_SENSOR); + return LINEAR_ACCEL_SENSOR; } -bool linear_accel_sensor::on_start(void) +unsigned int linear_accel_sensor::get_event_type(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(); - - if (!m_hardware_fusion) { - 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 LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME; } -bool linear_accel_sensor::on_stop(void) +const char* linear_accel_sensor::get_name(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(); - - if (!m_hardware_fusion) { - 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 linear_accel_sensor::get_sensor_info(sensor_info &info) +{ + info.set_type(get_type()); + info.set_id(get_id()); + info.set_privilege(SENSOR_PRIVILEGE_PUBLIC); // FIXME + info.set_name("Linear Accelerometer Sensor"); + info.set_vendor("Samsung Electronics"); + info.set_min_range(-19.6); + info.set_max_range(19.6); + 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 linear_accel_sensor::add_interval(int client_id, unsigned int interval) +void linear_accel_sensor::synthesize(const sensor_event_t& event) { - AUTOLOCK(m_mutex); - m_accel_sensor->add_interval(client_id, interval, false); - - if (!m_hardware_fusion) { - m_gyro_sensor->add_interval(client_id, interval, false); - m_magnetic_sensor->add_interval(client_id, interval, false); - } + if (event.event_type == GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME) { + m_gx = event.data->values[0]; + m_gy = event.data->values[1]; + m_gz = event.data->values[2]; + return; + } + + if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + m_time = event.data->timestamp; + m_x = event.data->values[0] - m_gx; + m_y = event.data->values[1] - m_gy; + m_z = event.data->values[2] - m_gz; + + sensor_event_t *linear_accel_event; + sensor_data_t *linear_accel_data; + int data_length; + int remains; + + linear_accel_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); + if (!linear_accel_event) { + _E("Failed to allocate memory"); + return; + } - m_fusion_sensor->add_interval(client_id, interval, false); + remains = get_data(&linear_accel_data, &data_length); - return sensor_base::add_interval(client_id, interval, false); -} + if (remains < 0) + return; -bool linear_accel_sensor::delete_interval(int client_id) -{ - AUTOLOCK(m_mutex); - m_accel_sensor->delete_interval(client_id, false); + linear_accel_event->sensor_id = get_id(); + linear_accel_event->event_type = LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME; + linear_accel_event->data_length = data_length; + linear_accel_event->data = linear_accel_data; - if (!m_hardware_fusion) { - m_gyro_sensor->delete_interval(client_id, false); - m_magnetic_sensor->delete_interval(client_id, false); + push(linear_accel_event); } - - m_fusion_sensor->delete_interval(client_id, false); - - return sensor_base::delete_interval(client_id, false); } -sensor_data_t linear_accel_sensor::calculate_gravity(sensor_data_t data) +int linear_accel_sensor::get_data(sensor_data_t **data, int *length) { - sensor_data_t gravity_data; - float pitch, roll, azimuth; - float azimuth_offset; + /* if It is batch sensor, remains can be 2+ */ + int remains = 1; - quaternion quat(data.values[0], data.values[1], - data.values[2], data.values[3]); + sensor_data_t *sensor_data; + sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); - euler_angles euler = quat2euler(quat); - - if(m_orientation_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; - - pitch = euler.m_ang.m_vec[0]; - roll = euler.m_ang.m_vec[1]; - if (euler.m_ang.m_vec[2] >= 0) - azimuth = euler.m_ang.m_vec[2]; - else - azimuth = euler.m_ang.m_vec[2] + azimuth_offset; - - if(m_orientation_data_unit == "DEGREES") { - azimuth *= DEG2RAD; - pitch *= DEG2RAD; - roll *= DEG2RAD; - } + sensor_data->accuracy = SENSOR_ACCURACY_GOOD; + 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); - if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) || - (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) { - gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth); - gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth); - gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll); - } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) || - (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) { - gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth); - gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth); - gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch); - } else { - gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll); - gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch); - gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch); - } - gravity_data.value_count = 3; - gravity_data.timestamp = m_time; - gravity_data.accuracy = SENSOR_ACCURACY_GOOD; - - return gravity_data; + return --remains; } -void linear_accel_sensor::synthesize(const sensor_event_t &event, vector &outs) +bool linear_accel_sensor::add_interval(int client_id, unsigned int interval, bool is_processor) { - sensor_event_t lin_accel_event; - sensor_data_t gravity_data; + if (m_accel_sensor) + m_accel_sensor->add_interval(client_id, interval, true); - unsigned long long diff_time; + if (m_gravity_sensor) + m_gravity_sensor->add_interval(client_id, interval, true); - if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) { - diff_time = event.data.timestamp - m_time; + return sensor_base::add_interval(client_id, interval, is_processor); +} - if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) - return; +bool linear_accel_sensor::delete_interval(int client_id, bool is_processor) +{ + if (m_accel_sensor) + m_accel_sensor->delete_interval(client_id, true); - m_accel.m_data.m_vec[0] = m_accel_rotation_direction_compensation[0] * (event.data.values[0] - m_accel_static_bias[0]) / ACCEL_SCALE; - m_accel.m_data.m_vec[1] = m_accel_rotation_direction_compensation[1] * (event.data.values[1] - m_accel_static_bias[1]) / ACCEL_SCALE; - m_accel.m_data.m_vec[2] = m_accel_rotation_direction_compensation[2] * (event.data.values[2] - m_accel_static_bias[2]) / ACCEL_SCALE; + if (m_gravity_sensor) + m_gravity_sensor->delete_interval(client_id, true); - m_accel.m_time_stamp = event.data.timestamp; + return sensor_base::delete_interval(client_id, is_processor); +} - m_enable_linear_accel |= ACCELEROMETER_ENABLED; - } - else if (event.event_type == FUSION_EVENT) { - diff_time = event.data.timestamp - m_time; +bool linear_accel_sensor::set_interval(unsigned long interval) +{ + m_interval = interval; + return true; +} - if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) - return; +bool linear_accel_sensor::set_batch_latency(unsigned long latency) +{ + return false; +} - gravity_data = calculate_gravity(event.data); +bool linear_accel_sensor::set_wakeup(int wakeup) +{ + return false; +} - m_enable_linear_accel |= GRAVITY_ENABLED; - } +bool linear_accel_sensor::on_start(void) +{ + if (m_accel_sensor) + m_accel_sensor->start(); - if (m_enable_linear_accel == LINEAR_ACCEL_ENABLED) { - m_enable_linear_accel = 0; - - m_time = get_timestamp(); - lin_accel_event.sensor_id = get_id(); - lin_accel_event.event_type = LINEAR_ACCEL_RAW_DATA_EVENT; - lin_accel_event.data.value_count = 3; - lin_accel_event.data.timestamp = m_time; - lin_accel_event.data.accuracy = SENSOR_ACCURACY_GOOD; - lin_accel_event.data.values[0] = m_linear_accel_sign_compensation[0] * (m_accel.m_data.m_vec[0] - gravity_data.values[0]); - lin_accel_event.data.values[1] = m_linear_accel_sign_compensation[1] * (m_accel.m_data.m_vec[1] - gravity_data.values[1]); - lin_accel_event.data.values[2] = m_linear_accel_sign_compensation[2] * (m_accel.m_data.m_vec[2] - gravity_data.values[2]); - push(lin_accel_event); - } + if (m_gravity_sensor) + m_gravity_sensor->start(); - return; + m_time = 0; + return activate(); } -int linear_accel_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) +bool linear_accel_sensor::on_stop(void) { - sensor_data_t gravity_data, accel_data, fusion_data; - m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); - m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); - - gravity_data = calculate_gravity(fusion_data); - - accel_data.values[0] = m_accel_rotation_direction_compensation[0] * (accel_data.values[0] - m_accel_static_bias[0]) / ACCEL_SCALE; - accel_data.values[1] = m_accel_rotation_direction_compensation[1] * (accel_data.values[1] - m_accel_static_bias[1]) / ACCEL_SCALE; - accel_data.values[2] = m_accel_rotation_direction_compensation[2] * (accel_data.values[2] - m_accel_static_bias[2]) / ACCEL_SCALE; - - if (event_type != LINEAR_ACCEL_RAW_DATA_EVENT) - return -1; - - data.accuracy = SENSOR_ACCURACY_GOOD; - data.timestamp = get_timestamp(); - data.values[0] = m_linear_accel_sign_compensation[0] * (accel_data.values[0] - gravity_data.values[0]); - data.values[1] = m_linear_accel_sign_compensation[1] * (accel_data.values[1] - gravity_data.values[1]); - data.values[2] = m_linear_accel_sign_compensation[2] * (accel_data.values[2] - gravity_data.values[2]); - data.value_count = 3; - return 0; -} + if (m_accel_sensor) + m_accel_sensor->stop(); -bool linear_accel_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties) -{ - m_accel_sensor->get_properties(ACCELEROMETER_SENSOR, properties); - properties.name = "Linear Acceleration Sensor"; - properties.vendor = m_vendor; - properties.resolution = 0.000001; + if (m_gravity_sensor) + m_gravity_sensor->stop(); - return true; + m_time = 0; + return deactivate(); } - diff --git a/src/sensor/linear_accel/linear_accel_sensor.h b/src/sensor/linear_accel/linear_accel_sensor.h index a9219cd..1c2be40 100644 --- a/src/sensor/linear_accel/linear_accel_sensor.h +++ b/src/sensor/linear_accel/linear_accel_sensor.h @@ -20,57 +20,51 @@ #ifndef _LINEAR_ACCEL_SENSOR_H_ #define _LINEAR_ACCEL_SENSOR_H_ -#include #include -#include +#include class linear_accel_sensor : public virtual_sensor { public: linear_accel_sensor(); virtual ~linear_accel_sensor(); - bool init(); - virtual void get_types(std::vector &types); + /* 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_sensor_info(sensor_info &info); - int get_sensor_data(const unsigned int event_type, sensor_data_t &data); - virtual bool get_properties(sensor_type_t sensor_type, sensor_properties_s &properties); + /* 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_gyro_sensor; - sensor_base *m_magnetic_sensor; - sensor_base *m_fusion_sensor; - - sensor_data m_accel; - sensor_data m_gyro; - sensor_data m_magnetic; - - cmutex m_value_mutex; + sensor_base *m_gravity_sensor; + float m_x; + float m_y; + float m_z; + float m_gx; + float m_gy; + float m_gz; + int m_accuracy; unsigned long long m_time; - unsigned int m_interval; - - unsigned int m_enable_linear_accel; + unsigned long m_interval; - std::string m_vendor; - std::string m_raw_data_unit; - std::string m_orientation_data_unit; - int m_default_sampling_time; - float m_accel_static_bias[3]; - int m_accel_rotation_direction_compensation[3]; - int m_linear_accel_sign_compensation[3]; - int m_gravity_sign_compensation[3]; - int m_azimuth_rotation_compensation; - int m_pitch_rotation_compensation; - int m_roll_rotation_compensation; + virtual bool set_interval(unsigned long interval); + virtual bool set_batch_latency(unsigned long latency); + virtual bool set_wakeup(int wakeup); - bool on_start(void); - bool on_stop(void); - sensor_data_t calculate_gravity(sensor_data_t data); + virtual bool on_start(void); + virtual bool on_stop(void); }; #endif diff --git a/src/server/sensor_fusion.cpp b/src/server/sensor_fusion.cpp new file mode 100644 index 0000000..b6379f1 --- /dev/null +++ b/src/server/sensor_fusion.cpp @@ -0,0 +1,106 @@ +/* + * sensord + * + * Copyright (c) 2014 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 + +sensor_fusion::sensor_fusion() +{ + +} + +sensor_fusion::~sensor_fusion() +{ + +} + +bool sensor_fusion::is_fusion(void) +{ + return true; +} + +bool sensor_fusion::is_virtual(void) +{ + return false; +} + +bool sensor_fusion::is_data_ready(void) +{ + return true; +} + +void sensor_fusion::clear_data(void) +{ + return; +} + +bool sensor_fusion::get_rotation_matrix(arr33_t &rot, int &accuracy) +{ + return false; +} + +bool sensor_fusion::get_attitude(float &x, float &y, float &z, float &w) +{ + return false; +} + +bool sensor_fusion::get_gyro_bias(float &x, float &y, float &z) +{ + return false; +} + +bool sensor_fusion::get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy, int &accuracy) +{ + return false; +} + +bool sensor_fusion::get_linear_acceleration(float &x, float &y, float &z, int &accuracy) +{ + return false; +} + +bool sensor_fusion::get_gravity(float &x, float &y, float &z, int &accuracy) +{ + return false; +} + +bool sensor_fusion::get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy, int &accuracy) +{ + return false; +} + +bool sensor_fusion::get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w, int &accuracy) +{ + return false; +} + +bool sensor_fusion::get_orientation(float &azimuth, float &pitch, float &roll, int &accuracy) +{ + return false; +} + +bool sensor_fusion::set_interval(unsigned int event_type, int client_id, unsigned int interval) +{ + return sensor_base::add_interval(client_id, interval, true); +} + +bool sensor_fusion::unset_interval(unsigned int event_type, int client_id) +{ + return sensor_base::delete_interval(client_id, true); +} + diff --git a/src/server/sensor_fusion.h b/src/server/sensor_fusion.h new file mode 100644 index 0000000..ec9701d --- /dev/null +++ b/src/server/sensor_fusion.h @@ -0,0 +1,59 @@ +/* + * sensord + * + * Copyright (c) 2014 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_FUSION_H_ +#define _SENSOR_FUSION_H_ + +#include +#include + +enum fusion_event_type { + FUSION_EVENT_AG = (FUSION_SENSOR << 16) | 0x0001, + FUSION_EVENT_AM = (FUSION_SENSOR << 16) | 0x0002, + FUSION_EVENT_AGM = (FUSION_SENSOR << 16) | 0x0004, +}; + +typedef std::array ,3> arr33_t; + +class sensor_fusion : public sensor_base { +public: + sensor_fusion(); + virtual ~sensor_fusion(); + + virtual void fuse(const sensor_event_t& event) = 0; + bool is_fusion(void); + bool is_virtual(void); + virtual bool is_data_ready(void); + virtual void clear_data(void); + virtual bool set_interval(unsigned int event_type, int client_id, unsigned int interval); + virtual bool unset_interval(unsigned int event_type, int client_id); + virtual unsigned long long get_data_timestamp(void) = 0; + + virtual bool get_rotation_matrix(arr33_t &rot, int &accuracy); + virtual bool get_attitude(float &x, float &y, float &z, float &w); + virtual bool get_gyro_bias(float &x, float &y, float &z); + virtual bool get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy, int &accuracy); + virtual bool get_linear_acceleration(float &x, float &y, float &z, int &accuracy); + virtual bool get_gravity(float &x, float &y, float &z, int &accuracy); + virtual bool get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy, int &accuracy); + virtual bool get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w, int &accuracy); + virtual bool get_orientation(float &azimuth, float &pitch, float &roll, int &accuracy); +}; + +#endif diff --git a/src/server/sensor_loader.cpp b/src/server/sensor_loader.cpp index a050df7..dfc8f26 100644 --- a/src/server/sensor_loader.cpp +++ b/src/server/sensor_loader.cpp @@ -29,10 +29,17 @@ #include #include -#include +#include + #ifdef ENABLE_AUTO_ROTATION #include #endif +#ifdef ENABLE_GRAVITY +#include +#endif +#ifdef ENABLE_LINEAR_ACCEL +#include +#endif using std::vector; using std::string; @@ -127,10 +134,24 @@ bool sensor_loader::load_sensor_devices(const string &path, void* &handle) void sensor_loader::create_sensors(void) { - create_physical_sensors(ACCELEROMETER_SENSOR); + /* HRM sensors need SENSOR_PERMISSION_BIO */ + create_physical_sensors(HRM_RAW_SENSOR); + create_physical_sensors(HRM_SENSOR); + create_physical_sensors(HRM_LED_GREEN_SENSOR); + create_physical_sensors(HRM_LED_IR_SENSOR); + create_physical_sensors(HRM_LED_RED_SENSOR); + create_physical_sensors(UNKNOWN_SENSOR); +#ifdef ENABLE_AUTO_ROTATION create_virtual_sensors("Auto Rotation"); +#endif +#ifdef ENABLE_GRAVITY + create_virtual_sensors("Gravity"); +#endif +#ifdef ENABLE_LINEAR_ACCEL + create_virtual_sensors("Linear Accel"); +#endif } template