From 8042b5f823082d890f267e10fb5b242f5e261503 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 24 Feb 2015 13:43:40 +0530 Subject: [PATCH] Adding implementation for gaming_rv class Adding file for implementation of gaming_rv class methods Change-Id: Ie6362ff80ab55d6c6283b8d9ecb06414bd7791a0 --- src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp | 337 +++++++++++++++++++++ 1 file changed, 337 insertions(+) create mode 100755 src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp diff --git a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp new file mode 100755 index 0000000..acfd003 --- /dev/null +++ b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp @@ -0,0 +1,337 @@ +/* + * sensord + * + * Copyright (c) 2015 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define SENSOR_NAME "GAMING_RV_SENSOR" +#define SENSOR_TYPE_GAMING_RV "GAMING_ROTATION_VECTOR" + +#define ACCELEROMETER_ENABLED 0x01 +#define GYROSCOPE_ENABLED 0x02 +#define GAMING_RV_ENABLED 3 + +#define INITIAL_VALUE -1 + +#define MS_TO_US 1000 + +#define ELEMENT_NAME "NAME" +#define ELEMENT_VENDOR "VENDOR" +#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT" +#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME" +#define ELEMENT_ACCEL_STATIC_BIAS "ACCEL_STATIC_BIAS" +#define ELEMENT_GYRO_STATIC_BIAS "GYRO_STATIC_BIAS" +#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION "GYRO_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_ACCEL_SCALE "ACCEL_SCALE" +#define ELEMENT_GYRO_SCALE "GYRO_SCALE" + +void pre_process_data(sensor_data &data_out, const float *data_in, float *bias, int *sign, float scale) +{ + data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale; + data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale; + data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale; +} + +gaming_rv_sensor::gaming_rv_sensor() +: m_accel_sensor(NULL) +, m_gyro_sensor(NULL) +, m_accuracy(-1) +, m_time(0) +{ + cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + + m_name = string(SENSOR_NAME); + register_supported_event(GAMING_RV_RAW_DATA_EVENT); + m_enable_gaming_rv = 0; + + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_VENDOR, m_vendor)) { + ERR("[VENDOR] is empty\n"); + throw ENXIO; + } + + INFO("m_vendor = %s", m_vendor.c_str()); + + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) { + ERR("[DEFAULT_SAMPLING_TIME] is empty\n"); + throw ENXIO; + } + + INFO("m_default_sampling_time = %d", m_default_sampling_time); + + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) { + ERR("[ACCEL_STATIC_BIAS] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]); + + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_GYRO_STATIC_BIAS, m_gyro_static_bias,3)) { + ERR("[GYRO_STATIC_BIAS] is empty\n"); + throw ENXIO; + } + + INFO("m_gyro_static_bias = (%f, %f, %f)", m_gyro_static_bias[0], m_gyro_static_bias[1], m_gyro_static_bias[2]); + + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) { + ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_rotation_direction_compensation = (%d, %d, %d)", m_accel_rotation_direction_compensation[0], m_accel_rotation_direction_compensation[1], m_accel_rotation_direction_compensation[2]); + + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION, m_gyro_rotation_direction_compensation, 3)) { + ERR("[GYRO_ROTATION_DIRECTION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_gyro_rotation_direction_compensation = (%d, %d, %d)", m_gyro_rotation_direction_compensation[0], m_gyro_rotation_direction_compensation[1], m_gyro_rotation_direction_compensation[2]); + + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_ACCEL_SCALE, &m_accel_scale)) { + ERR("[ACCEL_SCALE] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_scale = %f", m_accel_scale); + + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_GYRO_SCALE, &m_gyro_scale)) { + ERR("[GYRO_SCALE] is empty\n"); + throw ENXIO; + } + + INFO("m_gyro_scale = %f", m_gyro_scale); + + m_interval = m_default_sampling_time * MS_TO_US; +} + +gaming_rv_sensor::~gaming_rv_sensor() +{ + INFO("gaming_rv_sensor is destroyed!\n"); +} + +bool gaming_rv_sensor::init() +{ + m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); + + if (!m_accel_sensor || !m_gyro_sensor) { + ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x", + m_accel_sensor, m_gyro_sensor); + return false; + } + + INFO("%s is created!\n", sensor_base::get_name()); + + return true; +} + +sensor_type_t gaming_rv_sensor::get_type(void) +{ + return GAMING_RV_SENSOR; +} + +bool gaming_rv_sensor::on_start(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); + m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); + m_accel_sensor->start(); + m_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(); + + activate(); + return true; +} + +bool gaming_rv_sensor::on_stop(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); + m_accel_sensor->delete_interval((intptr_t)this, false); + m_accel_sensor->stop(); + m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT); + m_gyro_sensor->delete_interval((intptr_t)this, false); + m_gyro_sensor->stop(); + + deactivate(); + return true; +} + +bool gaming_rv_sensor::add_interval(int client_id, unsigned int interval) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->add_interval(client_id, interval, false); + m_gyro_sensor->add_interval(client_id, interval, false); + + return sensor_base::add_interval(client_id, interval, false); +} + +bool gaming_rv_sensor::delete_interval(int client_id) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_interval(client_id, false); + m_gyro_sensor->delete_interval(client_id, false); + + return sensor_base::delete_interval(client_id, false); +} + +void gaming_rv_sensor::synthesize(const sensor_event_t& event, vector &outs) +{ + const float MIN_DELIVERY_DIFF_FACTOR = 0.75f; + unsigned long long diff_time; + + sensor_event_t rv_event; + quaternion quaternion_gaming_rv; + + if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); + + m_accel.m_time_stamp = event.data.timestamp; + + m_enable_gaming_rv |= ACCELEROMETER_ENABLED; + } + else if (event.event_type == GYROSCOPE_RAW_DATA_EVENT) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale); + + m_gyro.m_time_stamp = event.data.timestamp; + + m_enable_gaming_rv |= GYROSCOPE_ENABLED; + } + + if (m_enable_gaming_rv == GAMING_RV_ENABLED) { + m_enable_gaming_rv = 0; + + { + AUTOLOCK(m_fusion_mutex); + quaternion_gaming_rv = m_orientation_filter.get_gaming_quaternion(m_accel, m_gyro); + } + + m_time = get_timestamp(); + + rv_event.sensor_id = get_id(); + rv_event.event_type = GAMING_RV_RAW_DATA_EVENT; + rv_event.data.accuracy = SENSOR_ACCURACY_GOOD; + rv_event.data.timestamp = m_time; + rv_event.data.value_count = 4; + rv_event.data.values[0] = quaternion_gaming_rv.m_quat.m_vec[1]; + rv_event.data.values[1] = quaternion_gaming_rv.m_quat.m_vec[2]; + rv_event.data.values[2] = quaternion_gaming_rv.m_quat.m_vec[3]; + rv_event.data.values[3] = quaternion_gaming_rv.m_quat.m_vec[0]; + + push(rv_event); + } + + return; +} + +int gaming_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) +{ + sensor_data accel; + sensor_data gyro; + + sensor_data_t accel_data; + sensor_data_t gyro_data; + + quaternion quaternion_gaming_rv; + + if (event_type != GAMING_RV_RAW_DATA_EVENT) + return -1; + + m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); + m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data); + + pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); + pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale); + accel.m_time_stamp = accel_data.timestamp; + gyro.m_time_stamp = gyro_data.timestamp; + + { + AUTOLOCK(m_fusion_mutex); + quaternion_gaming_rv = m_orientation_filter.get_gaming_quaternion(m_accel, m_gyro); + } + + data.accuracy = SENSOR_ACCURACY_GOOD; + data.timestamp = m_time; + data.value_count = 4; + data.values[0] = quaternion_gaming_rv.m_quat.m_vec[1]; + data.values[1] = quaternion_gaming_rv.m_quat.m_vec[2]; + data.values[2] = quaternion_gaming_rv.m_quat.m_vec[3]; + data.values[3] = quaternion_gaming_rv.m_quat.m_vec[0]; + + return 0; +} + +bool gaming_rv_sensor::get_properties(sensor_properties_s &properties) +{ + properties.vendor = m_vendor; + properties.name = SENSOR_NAME; + properties.min_range = -1; + properties.max_range = 1; + properties.resolution = 0.000001; + properties.fifo_count = 0; + properties.max_batch_count = 0; + properties.min_interval = 1; + + return true; +} + +extern "C" sensor_module* create(void) +{ + gaming_rv_sensor *sensor; + + try { + sensor = new(std::nothrow) gaming_rv_sensor; + } catch (int err) { + ERR("Failed to create module, err: %d, cause: %s", err, strerror(err)); + return NULL; + } + + sensor_module *module = new(std::nothrow) sensor_module; + retvm_if(!module || !sensor, NULL, "Failed to allocate memory"); + + module->sensors.push_back(sensor); + return module; +} -- 2.7.4