From 2145ecc1e8c6e879bdc960d926a74d73b504ed88 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 11 May 2016 17:56:32 +0900 Subject: [PATCH 01/16] sensord: remove unused rotation vector source files Change-Id: I3a362c3204dc300c0f0b2a9ecbf37a85ce75f6f4 Signed-off-by: kibak.yoon --- .../rotation_vector/gaming_rv/gaming_rv_sensor.cpp | 313 --------------------- .../rotation_vector/gaming_rv/gaming_rv_sensor.h | 73 ----- .../geomagnetic_rv/geomagnetic_rv_sensor.cpp | 244 ---------------- .../geomagnetic_rv/geomagnetic_rv_sensor.h | 64 ----- src/sensor/rotation_vector/rv/rv_sensor.cpp | 259 ----------------- src/sensor/rotation_vector/rv/rv_sensor.h | 71 ----- 6 files changed, 1024 deletions(-) delete mode 100644 src/sensor/rotation_vector/gaming_rv/gaming_rv_sensor.cpp delete mode 100644 src/sensor/rotation_vector/gaming_rv/gaming_rv_sensor.h delete mode 100644 src/sensor/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp delete mode 100644 src/sensor/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h delete mode 100644 src/sensor/rotation_vector/rv/rv_sensor.cpp delete mode 100644 src/sensor/rotation_vector/rv/rv_sensor.h diff --git a/src/sensor/rotation_vector/gaming_rv/gaming_rv_sensor.cpp b/src/sensor/rotation_vector/gaming_rv/gaming_rv_sensor.cpp deleted file mode 100644 index bfccfaf..0000000 --- a/src/sensor/rotation_vector/gaming_rv/gaming_rv_sensor.cpp +++ /dev/null @@ -1,313 +0,0 @@ -/* - * 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 - -using std::string; -using std::vector; - -#define SENSOR_NAME "GYROSCOPE_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" - -gaming_rv_sensor::gaming_rv_sensor() -: m_accel_sensor(NULL) -, m_gyro_sensor(NULL) -, 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; - - _I("m_hardware_fusion = %d", m_hardware_fusion); - - 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)) { - _E("[VENDOR] is empty\n"); - throw ENXIO; - } - - _I("m_vendor = %s", m_vendor.c_str()); - - if (!config.get(SENSOR_TYPE_GAMING_RV, 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_GAMING_RV, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) { - _E("[ACCEL_STATIC_BIAS] is empty\n"); - throw ENXIO; - } - - _I("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)) { - _E("[GYRO_STATIC_BIAS] is empty\n"); - throw ENXIO; - } - - _I("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)) { - _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_GAMING_RV, ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION, m_gyro_rotation_direction_compensation, 3)) { - _E("[GYRO_ROTATION_DIRECTION_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("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]); - - m_interval = m_default_sampling_time * MS_TO_US; -} - -gaming_rv_sensor::~gaming_rv_sensor() -{ - _I("gaming_rv_sensor is destroyed!\n"); -} - -bool gaming_rv_sensor::init() -{ - m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); - m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); - - if (!m_accel_sensor || !m_gyro_sensor) { - _E("Failed to load sensors, accel: %#x, gyro: %#x", - m_accel_sensor, m_gyro_sensor); - return false; - } - - _I("%s is created!\n", sensor_base::get_name()); - - return true; -} - -void gaming_rv_sensor::get_types(vector &types) -{ - types.push_back(GYROSCOPE_RV_SENSOR); -} - -bool gaming_rv_sensor::on_start(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(); - } - - activate(); - return true; -} - -bool gaming_rv_sensor::on_stop(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(); - } - - deactivate(); - return true; -} - -bool gaming_rv_sensor::add_interval(int client_id, unsigned int interval) -{ - 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); - } - - return sensor_base::add_interval(client_id, interval, false); -} - -bool gaming_rv_sensor::delete_interval(int client_id) -{ - AUTOLOCK(m_mutex); - - if (!m_hardware_fusion) { - 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, 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, 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; - - m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, NULL); - - quaternion_gaming_rv = m_orientation_filter.m_quat_gaming_rv; - - 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, ACCEL_SCALE); - pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, GYRO_SCALE); - accel.m_time_stamp = accel_data.timestamp; - gyro.m_time_stamp = gyro_data.timestamp; - - m_orientation_filter_poll.get_device_orientation(&m_accel, &m_gyro, NULL); - - quaternion_gaming_rv = m_orientation_filter_poll.m_quat_gaming_rv; - - data.accuracy = SENSOR_ACCURACY_GOOD; - data.timestamp = get_timestamp(); - 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_type_t sensor_type, 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; -} - diff --git a/src/sensor/rotation_vector/gaming_rv/gaming_rv_sensor.h b/src/sensor/rotation_vector/gaming_rv/gaming_rv_sensor.h deleted file mode 100644 index ec89b21..0000000 --- a/src/sensor/rotation_vector/gaming_rv/gaming_rv_sensor.h +++ /dev/null @@ -1,73 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2015 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifndef _GYROSCOPE_RV_SENSOR_H_ -#define _GYROSCOPE_RV_SENSOR_H_ - -#include -#include -#include - -class gaming_rv_sensor : public virtual_sensor { -public: - gaming_rv_sensor(); - virtual ~gaming_rv_sensor(); - - bool init(void); - - void synthesize(const sensor_event_t &event, std::vector &outs); - - bool add_interval(int client_id, unsigned int interval); - bool delete_interval(int client_id); - virtual bool get_properties(sensor_type_t sensor_type, sensor_properties_s &properties); - virtual void get_types(std::vector &types); - - int get_sensor_data(const unsigned int event_type, sensor_data_t &data); - -private: - sensor_base *m_accel_sensor; - sensor_base *m_gyro_sensor; - - sensor_data m_accel; - sensor_data m_gyro; - - cmutex m_value_mutex; - - orientation_filter m_orientation_filter; - orientation_filter m_orientation_filter_poll; - - unsigned int m_enable_gaming_rv; - - int m_accuracy; - unsigned long long m_time; - unsigned int m_interval; - - std::string m_vendor; - std::string m_raw_data_unit; - int m_default_sampling_time; - - float m_accel_static_bias[3]; - float m_gyro_static_bias[3]; - int m_accel_rotation_direction_compensation[3]; - int m_gyro_rotation_direction_compensation[3]; - bool on_start(void); - bool on_stop(void); -}; - -#endif /*_GYROSCOPE_RV_SENSOR_H_*/ diff --git a/src/sensor/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp b/src/sensor/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp deleted file mode 100644 index a97b949..0000000 --- a/src/sensor/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp +++ /dev/null @@ -1,244 +0,0 @@ -/* - * 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 - -using std::string; -using std::vector; - -#define SENSOR_NAME "GEOMAGNETIC_RV_SENSOR" -#define SENSOR_TYPE_GEOMAGNETIC_RV "GEOMAGNETIC_ROTATION_VECTOR" - -#define MIN_DELIVERY_DIFF_FACTOR 0.75f - -#define MS_TO_US 1000 - -#define ELEMENT_VENDOR "VENDOR" -#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME" - -geomagnetic_rv_sensor::geomagnetic_rv_sensor() -: m_accel_sensor(NULL) -, m_magnetic_sensor(NULL) -, m_fusion_sensor(NULL) -, 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(GEOMAGNETIC_RV_RAW_DATA_EVENT); - - if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, ELEMENT_VENDOR, m_vendor)) { - _E("[VENDOR] is empty\n"); - throw ENXIO; - } - - _I("m_vendor = %s", m_vendor.c_str()); - - if (!config.get(SENSOR_TYPE_GEOMAGNETIC_RV, 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); - - m_interval = m_default_sampling_time * MS_TO_US; -} - -geomagnetic_rv_sensor::~geomagnetic_rv_sensor() -{ - _I("geomagnetic_rv_sensor is destroyed!\n"); -} - -bool geomagnetic_rv_sensor::init() -{ - m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_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 || !m_magnetic_sensor || !m_fusion_sensor) { - _E("Failed to load sensors, accel: %#x, mag: %#x, fusion: %#x", - m_accel_sensor, m_magnetic_sensor, m_fusion_sensor); - return false; - } - - _I("%s is created!\n", sensor_base::get_name()); - - return true; -} - -void geomagnetic_rv_sensor::get_types(vector &types) -{ - types.push_back(GEOMAGNETIC_RV_SENSOR); -} - -bool geomagnetic_rv_sensor::on_start(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_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_GEOMAGNETIC_ROTATION_VECTOR_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; -} - -bool geomagnetic_rv_sensor::on_stop(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_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT); - m_magnetic_sensor->delete_interval((intptr_t)this, false); - m_magnetic_sensor->stop(); - } - - m_fusion_sensor->delete_client(FUSION_EVENT); - m_fusion_sensor->delete_interval((intptr_t)this, false); - m_fusion_sensor->unregister_supported_event(FUSION_EVENT); - m_fusion_sensor->unregister_supported_event(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED); - m_fusion_sensor->stop(); - - deactivate(); - return true; -} - -bool geomagnetic_rv_sensor::add_interval(int client_id, unsigned int interval) -{ - AUTOLOCK(m_mutex); - - if (!m_hardware_fusion) { - m_accel_sensor->add_interval(client_id, interval, false); - m_magnetic_sensor->add_interval(client_id, interval, false); - } - - m_fusion_sensor->add_interval(client_id, interval, false); - - return sensor_base::add_interval(client_id, interval, false); -} - -bool geomagnetic_rv_sensor::delete_interval(int client_id) -{ - AUTOLOCK(m_mutex); - - if (!m_hardware_fusion) { - m_accel_sensor->delete_interval(client_id, false); - m_magnetic_sensor->delete_interval(client_id, false); - } - - m_fusion_sensor->delete_interval(client_id, false); - - return sensor_base::delete_interval(client_id, false); -} - -void geomagnetic_rv_sensor::synthesize(const sensor_event_t &event, vector &outs) -{ - unsigned long long diff_time; - - sensor_event_t rv_event; - - 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; - - m_time = get_timestamp(); - rv_event.sensor_id = get_id(); - rv_event.event_type = GEOMAGNETIC_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] = event.data.values[1]; - rv_event.data.values[1] = event.data.values[2]; - rv_event.data.values[2] = event.data.values[3]; - rv_event.data.values[3] = event.data.values[0]; - - push(rv_event); - } - - return; -} - -int geomagnetic_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) -{ - sensor_data_t fusion_data; - - if (event_type != GEOMAGNETIC_RV_RAW_DATA_EVENT) - return -1; - - m_fusion_sensor->get_sensor_data(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED, fusion_data); - - data.accuracy = SENSOR_ACCURACY_GOOD; - data.timestamp = get_timestamp(); - data.value_count = 4; - data.values[0] = fusion_data.values[1]; - data.values[1] = fusion_data.values[2]; - data.values[2] = fusion_data.values[3]; - data.values[3] = fusion_data.values[0]; - - return 0; -} - -bool geomagnetic_rv_sensor::get_properties(sensor_type_t sensor_type, 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; -} diff --git a/src/sensor/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h b/src/sensor/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h deleted file mode 100644 index f5ad4b7..0000000 --- a/src/sensor/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h +++ /dev/null @@ -1,64 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2015 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifndef _GEOMAGNETIC_RV_SENSOR_H_ -#define _GEOMAGNETIC_RV_SENSOR_H_ - -#include -#include -#include - -class geomagnetic_rv_sensor : public virtual_sensor { -public: - geomagnetic_rv_sensor(); - virtual ~geomagnetic_rv_sensor(); - - bool init(void); - - void synthesize(const sensor_event_t &event, std::vector &outs); - - bool add_interval(int client_id, unsigned int interval); - bool delete_interval(int client_id); - virtual bool get_properties(sensor_type_t sensor_type, sensor_properties_s &properties); - virtual void get_types(std::vector &types); - - int get_sensor_data(const unsigned int event_type, sensor_data_t &data); - -private: - sensor_base *m_accel_sensor; - sensor_base *m_magnetic_sensor; - sensor_base *m_fusion_sensor; - - sensor_data m_accel; - sensor_data m_magnetic; - - cmutex m_value_mutex; - - unsigned long long m_time; - unsigned int m_interval; - - std::string m_vendor; - std::string m_raw_data_unit; - int m_default_sampling_time; - - bool on_start(void); - bool on_stop(void); -}; - -#endif /*_GEOMAGNETIC_RV_SENSOR_H_*/ diff --git a/src/sensor/rotation_vector/rv/rv_sensor.cpp b/src/sensor/rotation_vector/rv/rv_sensor.cpp deleted file mode 100644 index 1f784e7..0000000 --- a/src/sensor/rotation_vector/rv/rv_sensor.cpp +++ /dev/null @@ -1,259 +0,0 @@ -/* - * 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using std::string; -using std::vector; - -#define SENSOR_NAME "RV_SENSOR" -#define SENSOR_TYPE_RV "ROTATION_VECTOR" - -#define MIN_DELIVERY_DIFF_FACTOR 0.75f - -#define INITIAL_VALUE -1 - -#define MS_TO_US 1000 - -#define ELEMENT_NAME "NAME" -#define ELEMENT_VENDOR "VENDOR" -#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT" -#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME" - -rv_sensor::rv_sensor() -: m_accel_sensor(NULL) -, m_gyro_sensor(NULL) -, m_magnetic_sensor(NULL) -, m_fusion_sensor(NULL) -, m_time(0) -{ - virtual_sensor_config &config = virtual_sensor_config::get_instance(); - - // Will check if fusion_sensor is in the list of hal sensors. - 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(ROTATION_VECTOR_RAW_DATA_EVENT); - m_enable_orientation = 0; - - if (!config.get(SENSOR_TYPE_RV, ELEMENT_VENDOR, m_vendor)) { - _E("[VENDOR] is empty\n"); - throw ENXIO; - } - - _I("m_vendor = %s", m_vendor.c_str()); - - if (!config.get(SENSOR_TYPE_RV, 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); - - m_interval = m_default_sampling_time * MS_TO_US; -} - -rv_sensor::~rv_sensor() -{ - _I("rv_sensor is destroyed!\n"); -} - -bool rv_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 || !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); - return false; - } - - _I("%s is created!\n", sensor_base::get_name()); - - return true; -} - -void rv_sensor::get_types(vector &types) -{ - types.push_back(ROTATION_VECTOR_SENSOR); -} - -bool rv_sensor::on_start(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_ROTATION_VECTOR_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; -} - -bool rv_sensor::on_stop(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(); - } - - 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_ROTATION_VECTOR_ENABLED); - m_fusion_sensor->stop(); - - deactivate(); - return true; -} - -bool rv_sensor::add_interval(int client_id, unsigned int interval) -{ - 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); - } - - m_fusion_sensor->add_interval(client_id, interval, false); - - return sensor_base::add_interval(client_id, interval, false); -} - -bool rv_sensor::delete_interval(int client_id) -{ - 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); - } - m_fusion_sensor->delete_interval(client_id, false); - - return sensor_base::delete_interval(client_id, false); -} - -void rv_sensor::synthesize(const sensor_event_t &event, vector &outs) -{ - unsigned long long diff_time; - - sensor_event_t rv_event; - - 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; - - m_time = get_timestamp(); - rv_event.sensor_id = get_id(); - rv_event.event_type = ROTATION_VECTOR_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] = event.data.values[1]; - rv_event.data.values[1] = event.data.values[2]; - rv_event.data.values[2] = event.data.values[3]; - rv_event.data.values[3] = event.data.values[0]; - - push(rv_event); - } - - return; -} - -int rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) -{ - sensor_data_t fusion_data; - - if (event_type != ROTATION_VECTOR_RAW_DATA_EVENT) - return -1; - - m_fusion_sensor->get_sensor_data(FUSION_ROTATION_VECTOR_ENABLED, fusion_data); - - data.accuracy = SENSOR_ACCURACY_GOOD; - data.timestamp = get_timestamp(); - data.value_count = 4; - data.values[0] = fusion_data.values[1]; - data.values[1] = fusion_data.values[2]; - data.values[2] = fusion_data.values[3]; - data.values[3] = fusion_data.values[0]; - - return 0; -} - -bool rv_sensor::get_properties(sensor_type_t sensor_type, 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; -} diff --git a/src/sensor/rotation_vector/rv/rv_sensor.h b/src/sensor/rotation_vector/rv/rv_sensor.h deleted file mode 100644 index 00f9761..0000000 --- a/src/sensor/rotation_vector/rv/rv_sensor.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - * 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 _RV_SENSOR_H_ -#define _RV_SENSOR_H_ - -#include -#include -#include - -class rv_sensor : public virtual_sensor { -public: - rv_sensor(); - virtual ~rv_sensor(); - - bool init(void); - - void synthesize(const sensor_event_t &event, std::vector &outs); - - bool add_interval(int client_id, unsigned int interval); - bool delete_interval(int client_id); - virtual bool get_properties(sensor_type_t sensor_type, sensor_properties_s &properties); - virtual void get_types(std::vector &types); - - int get_sensor_data(const unsigned int event_type, sensor_data_t &data); - -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; - - orientation_filter m_orientation_filter; - orientation_filter m_orientation_filter_poll; - - unsigned int m_enable_orientation; - - unsigned long long m_time; - unsigned int m_interval; - - std::string m_vendor; - std::string m_raw_data_unit; - int m_default_sampling_time; - - bool on_start(void); - bool on_stop(void); -}; - -#endif /*_RV_SENSOR_H_*/ -- 2.7.4 From 6ca45fa7211cbd07b2b7b658a0495c55af9c79b2 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 11 May 2016 17:59:00 +0900 Subject: [PATCH 02/16] sensord: enable rotation vector/orientation sensors - applied simple fusion algorithm Change-Id: I31b0c09a29595804946f8a105198bee381d090ae Signed-off-by: kibak.yoon --- packaging/sensord.spec | 6 +- src/sensor/CMakeLists.txt | 15 +- src/sensor/fusion_util.cpp | 160 ++++++++- src/sensor/fusion_util.h | 3 + src/sensor/gravity/gravity_sensor.cpp | 5 +- src/sensor/orientation/orientation_sensor.cpp | 372 +++++++-------------- src/sensor/orientation/orientation_sensor.h | 58 ++-- .../rotation_vector/rotation_vector_sensor.cpp | 261 +++++++++++++++ .../rotation_vector/rotation_vector_sensor.h | 72 ++++ src/server/sensor_loader.cpp | 12 + src/shared/sensor_common.h | 4 + 11 files changed, 683 insertions(+), 285 deletions(-) create mode 100644 src/sensor/rotation_vector/rotation_vector_sensor.cpp create mode 100644 src/sensor/rotation_vector/rotation_vector_sensor.h diff --git a/packaging/sensord.spec b/packaging/sensord.spec index c917263..c3f0488 100644 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -35,6 +35,10 @@ Requires: libsensord = %{version}-%{release} %define gyroscope_uncal_state OFF %define build_test_suite ON +%ifarch %{ix86} x86_64 +%define BUILD_ARCH EMULATOR +%endif + %description Sensor daemon @@ -81,7 +85,7 @@ cmake . -DCMAKE_INSTALL_PREFIX=%{_prefix} -DMAJORVER=${MAJORVER} -DFULLVER=%{ver -DLINEAR_ACCEL=%{linear_accel_state} -DRV=%{rv_state} \ -DGEOMAGNETIC_RV=%{geomagnetic_rv_state} -DGAMING_RV=%{gaming_rv_state} \ -DGYROSCOPE_UNCAL=%{gyroscope_uncal_state} -DAUTO_ROTATION=%{auto_rotation_state} \ - -DTILT=%{tilt_state} -DTEST_SUITE=%{build_test_suite} + -DTILT=%{tilt_state} -DTEST_SUITE=%{build_test_suite} -DARCH=%{BUILD_ARCH} %build make %{?jobs:-j%jobs} diff --git a/src/sensor/CMakeLists.txt b/src/sensor/CMakeLists.txt index 2dfab5b..7328a1d 100644 --- a/src/sensor/CMakeLists.txt +++ b/src/sensor/CMakeLists.txt @@ -7,10 +7,15 @@ SET(HRM_VIRT "OFF") SET(AUTO_ROTATION "ON") SET(GRAVITY "ON") SET(LINEAR_ACCEL "ON") +IF(${ARCH} STREQUAL "EMULATOR") +SET(RV "ON") +SET(ORIENTATION "ON") +ELSE() +SET(RV "OFF") SET(ORIENTATION "OFF") +ENDIF() SET(FUSION "OFF") SET(MOTION "OFF") -SET(RV "OFF") INCLUDE_DIRECTORIES( ${CMAKE_SOURCE_DIR}/src/shared @@ -49,10 +54,14 @@ IF("${LINEAR_ACCEL}" STREQUAL "ON") SET(SENSOR_DEFINITIONS ${SENSOR_DEFINITIONS} "-DENABLE_LINEAR_ACCEL") ENDIF() IF("${ORIENTATION}" STREQUAL "ON") -add_subdirectory(orientation) + FILE(GLOB_RECURSE SENSOR_SRCS ${SENSOR_SRCS} ${CMAKE_CURRENT_SOURCE_DIR}/orientation/*.cpp) + SET(SENSOR_HEADERS ${SENSOR_HEADERS} ${CMAKE_CURRENT_SOURCE_DIR}/orientation) + SET(SENSOR_DEFINITIONS ${SENSOR_DEFINITIONS} "-DENABLE_ORIENTATION") ENDIF() IF("${RV}" STREQUAL "ON") -add_subdirectory(rotation_vector) + FILE(GLOB_RECURSE SENSOR_SRCS ${SENSOR_SRCS} ${CMAKE_CURRENT_SOURCE_DIR}/rotation_vector/*.cpp) + SET(SENSOR_HEADERS ${SENSOR_HEADERS} ${CMAKE_CURRENT_SOURCE_DIR}/rotation_vector) + SET(SENSOR_DEFINITIONS ${SENSOR_DEFINITIONS} "-DENABLE_ROTATION_VECTOR") ENDIF() IF("${FUSION}" STREQUAL "ON") add_subdirectory(fusion) diff --git a/src/sensor/fusion_util.cpp b/src/sensor/fusion_util.cpp index c02c263..e14c223 100644 --- a/src/sensor/fusion_util.cpp +++ b/src/sensor/fusion_util.cpp @@ -16,15 +16,26 @@ * limitations under the License. * */ - -#include +#include #include #include +#include "fusion_util.h" + +#define RAD2DEGREE (180/M_PI) +#define QUAT (M_PI/4) +#define HALF (M_PI/2) +#define ARCTAN(x, y) ((x) == 0 ? 0 : (y) != 0 ? atan2((x), (y)) : (x) > 0 ? M_PI/2.0 : -M_PI/2.0) + +static float clamp(float v) +{ + return (v < 0) ? 0.0 : v; +} + int quat_to_matrix(const float *quat, float *R) { if(quat == NULL || R == NULL) - return -1; + return -EINVAL; float q0 = quat[3]; float q1 = quat[0]; @@ -53,3 +64,146 @@ int quat_to_matrix(const float *quat, float *R) return 0; } + +int matrix_to_quat(const float *R, float *quat) +{ + if (R == NULL || quat == NULL) + return -EINVAL; + + const float Hx = R[0]; + const float My = R[4]; + const float Az = R[8]; + quat[0] = sqrtf(clamp(Hx - My - Az + 1) * 0.25f); + quat[1] = sqrtf(clamp(-Hx + My - Az + 1) * 0.25f); + quat[2] = sqrtf(clamp(-Hx - My + Az + 1) * 0.25f); + quat[3] = sqrtf(clamp(Hx + My + Az + 1) * 0.25f); + quat[0] = copysignf(quat[0], R[7] - R[5]); + quat[1] = copysignf(quat[1], R[2] - R[6]); + quat[2] = copysignf(quat[2], R[3] - R[1]); + + return 0; +} + +int calculate_rotation_matrix(float *accel, float *geo, float *R, float *I) +{ + if (accel == NULL || geo == NULL || R == NULL || I == NULL) + return -EINVAL; + + float Ax = accel[0]; + float Ay = accel[1]; + float Az = accel[2]; + float Ex = geo[0]; + float Ey = geo[1]; + float Ez = geo[2]; + float Hx = Ey*Az - Ez*Ay; + float Hy = Ez*Ax - Ex*Az; + float Hz = Ex*Ay - Ey*Ax; + float normH = (float)sqrt(Hx*Hx + Hy*Hy + Hz*Hz); + if (normH < 0.1f) + return -EINVAL; + + float invH = 1.0f / normH; + Hx *= invH; + Hy *= invH; + Hz *= invH; + float invA = 1.0f / (float)sqrt(Ax*Ax + Ay*Ay + Az*Az); + Ax *= invA; + Ay *= invA; + Az *= invA; + float Mx = Ay*Hz - Az*Hy; + float My = Az*Hx - Ax*Hz; + float Mz = Ax*Hy - Ay*Hx; + + R[0] = Hx; R[1] = Hy; R[2] = Hz; + R[3] = Mx; R[4] = My; R[5] = Mz; + R[6] = Ax; R[7] = Ay; R[8] = Az; + + float invE = 1.0 / (float)sqrt(Ex*Ex + Ey*Ey + Ez*Ez); + float c = (Ex*Mx + Ey*My + Ez*Mz) * invE; + float s = (Ex*Ax + Ey*Ay + Ez*Az) * invE; + + I[0] = 1; I[1] = 0; I[2] = 0; + I[3] = 0; I[4] = c; I[5] = s; + I[6] = 0; I[7] = -s; I[8] = c; + + return 0; +} + +int quat_to_orientation(const float *quat, float &azimuth, float &pitch, float &roll) +{ + int error; + float g[3]; + float R[9]; + + error = quat_to_matrix(quat, R); + + if (error < 0) + return error; + + float xyz_z = ARCTAN(R[3], R[0]); + float yxz_x = asinf(R[7]); + float yxz_y = ARCTAN(-R[6], R[8]); + float yxz_z = ARCTAN(-R[1], R[4]); + + float a = fabs(yxz_x / HALF); + a = a * a; + + float p = (fabs(yxz_y) / HALF - 1.0); + + if (p < 0) + p = 0; + + float v = 1 + (1 - a) / a * p; + + if (v > 20) + v = 20; + + if (yxz_x * yxz_y > 0) { + if (yxz_z > 0 && xyz_z < 0) + xyz_z += M_PI * 2; + } else { + if (yxz_z < 0 && xyz_z > 0) + xyz_z -= M_PI * 2; + } + + g[0] = (1 - a * v) * yxz_z + (a * v) * xyz_z; + g[0] *= -1; + + float tmp = R[7]; + + if (tmp > 1.0f) + tmp = 1.0f; + else if (tmp < -1.0f) + tmp = -1.0f; + + g[1] = -asinf(tmp); + if (R[8] < 0) + g[1] = M_PI - g[1]; + + if (g[1] > M_PI) + g[1] -= M_PI * 2; + + if ((fabs(R[7]) > QUAT)) + g[2] = (float) atan2f(R[6], R[7]); + else + g[2] = (float) atan2f(R[6], R[8]); + + if (g[2] > HALF) + g[2] = M_PI - g[2]; + else if (g[2] < -HALF) + g[2] = -M_PI - g[2]; + + g[0] *= RAD2DEGREE; + g[1] *= RAD2DEGREE; + g[2] *= RAD2DEGREE; + + if (g[0] < 0) + g[0] += 360; + + azimuth = g[0]; + pitch = g[1]; + roll = g[2]; + + return 0; +} + diff --git a/src/sensor/fusion_util.h b/src/sensor/fusion_util.h index b4d2bc2..c3d6a52 100644 --- a/src/sensor/fusion_util.h +++ b/src/sensor/fusion_util.h @@ -26,6 +26,9 @@ extern "C" #endif int quat_to_matrix(const float *quat, float *R); +int matrix_to_quat(const float *R, float *quat); +int calculate_rotation_matrix(float *accel, float *geo, float *R, float *I); +int quat_to_orientation(const float *rotation, float &azimuth, float &pitch, float &roll); #ifdef __cplusplus } diff --git a/src/sensor/gravity/gravity_sensor.cpp b/src/sensor/gravity/gravity_sensor.cpp index 8b41fc5..e071226 100644 --- a/src/sensor/gravity/gravity_sensor.cpp +++ b/src/sensor/gravity/gravity_sensor.cpp @@ -161,7 +161,10 @@ void gravity_sensor::synthesize_rv(const sensor_event_t& event) float rotation[4] = {x, y, z, w}; - rotation_to_gravity(rotation, gravity); + if (!rotation_to_gravity(rotation, gravity)) { + _D("Invalid rotation_vector: [%10f] [%10f] [%10f] [%10f]", x, y, z, w); + return; + } gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); if (!gravity_event) { diff --git a/src/sensor/orientation/orientation_sensor.cpp b/src/sensor/orientation/orientation_sensor.cpp index 32e3ef7..940aed3 100644 --- a/src/sensor/orientation/orientation_sensor.cpp +++ b/src/sensor/orientation/orientation_sensor.cpp @@ -1,7 +1,7 @@ /* * sensord * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * Copyright (c) 2016 Samsung Electronics Co., Ltd. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -25,311 +25,187 @@ #include #include #include + #include +#include + +#include +#include #include #include -#include -#include - -using std::string; -using std::vector; - -#define SENSOR_NAME "ORIENTATION_SENSOR" -#define SENSOR_TYPE_ORIENTATION "ORIENTATION" +#include -#define INITIAL_VALUE -1 - -#define MS_TO_US 1000 -#define MIN_DELIVERY_DIFF_FACTOR 0.75f - -#define PI 3.141593 -#define AZIMUTH_OFFSET_DEGREES 360 -#define AZIMUTH_OFFSET_RADIANS (2 * PI) - -#define ELEMENT_NAME "NAME" -#define ELEMENT_VENDOR "VENDOR" -#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT" -#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME" -#define ELEMENT_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION" -#define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION" -#define ELEMENT_AZIMUTH_ROTATION_COMPENSATION "AZIMUTH_ROTATION_COMPENSATION" +#define SENSOR_NAME "SENSOR_ORIENTATION" orientation_sensor::orientation_sensor() -: m_accel_sensor(NULL) -, m_gyro_sensor(NULL) -, m_magnetic_sensor(NULL) -, m_fusion_sensor(NULL) +: m_rotation_vector_sensor(NULL) +, m_azimuth(-1) +, m_pitch(-1) +, m_roll(-1) +, m_accuracy(-1) , m_time(0) { - virtual_sensor_config &config = virtual_sensor_config::get_instance(); - - sensor_hal *fusion_sensor_hal = sensor_loader::get_instance().get_sensor_hal(SENSOR_HAL_TYPE_FUSION); - if (!fusion_sensor_hal) - m_hardware_fusion = false; - else - m_hardware_fusion = true; - - m_name = string(SENSOR_NAME); - register_supported_event(ORIENTATION_RAW_DATA_EVENT); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_VENDOR, m_vendor)) { - _E("[VENDOR] is empty\n"); - throw ENXIO; - } - - _I("m_vendor = %s", m_vendor.c_str()); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) { - _E("[RAW_DATA_UNIT] is empty\n"); - throw ENXIO; - } - - _I("m_raw_data_unit = %s", m_raw_data_unit.c_str()); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) { - _E("[DEFAULT_SAMPLING_TIME] is empty\n"); - throw ENXIO; - } - - _I("m_default_sampling_time = %d", m_default_sampling_time); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) { - _E("[AZIMUTH_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) { - _E("[PITCH_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) { - _E("[ROLL_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_roll_rotation_compensation = %d", m_roll_rotation_compensation); - - m_interval = m_default_sampling_time * MS_TO_US; } orientation_sensor::~orientation_sensor() { - _I("orientation_sensor is destroyed!\n"); + _I("%s is destroyed!", SENSOR_NAME); } -bool orientation_sensor::init(void) +bool orientation_sensor::init() { - m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); - m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); - m_magnetic_sensor = sensor_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); + m_rotation_vector_sensor = sensor_loader::get_instance().get_sensor(ROTATION_VECTOR_SENSOR); - m_fusion_sensor = sensor_loader::get_instance().get_sensor(FUSION_SENSOR); - - if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) { - _E("Failed to load sensors, accel: %#x, gyro: %#x, mag: %#x, fusion: %#x", - m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor); + if (!m_rotation_vector_sensor) { + _E("cannot load sensor[%s]", SENSOR_NAME); return false; } - - _I("%s is created!\n", sensor_base::get_name()); - + _I("%s is created!", SENSOR_NAME); return true; } -void orientation_sensor::get_types(vector &types) +sensor_type_t orientation_sensor::get_type(void) { - types.push_back(ORIENTATION_SENSOR); + return ORIENTATION_SENSOR; } -bool orientation_sensor::on_start(void) +unsigned int orientation_sensor::get_event_type(void) { - AUTOLOCK(m_mutex); - - if (!m_hardware_fusion) { - m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_accel_sensor->start(); - m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_gyro_sensor->start(); - m_magnetic_sensor->add_client(GEOMAGNETIC_RAW_DATA_EVENT); - m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_magnetic_sensor->start(); - } - - m_fusion_sensor->register_supported_event(FUSION_EVENT); - m_fusion_sensor->register_supported_event(FUSION_ORIENTATION_ENABLED); - m_fusion_sensor->add_client(FUSION_EVENT); - m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_fusion_sensor->start(); - - activate(); - return true; + return ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME; } -bool orientation_sensor::on_stop(void) +const char* orientation_sensor::get_name(void) { - AUTOLOCK(m_mutex); - - if (!m_hardware_fusion) { - m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->delete_interval((intptr_t)this, false); - m_accel_sensor->stop(); - m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->delete_interval((intptr_t)this, false); - m_gyro_sensor->stop(); - m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT); - m_magnetic_sensor->delete_interval((intptr_t)this, false); - m_magnetic_sensor->stop(); - } + return SENSOR_NAME; +} - m_fusion_sensor->delete_client(FUSION_EVENT); - m_fusion_sensor->delete_interval((intptr_t)this, false); - m_fusion_sensor->unregister_supported_event(FUSION_EVENT); - m_fusion_sensor->unregister_supported_event(FUSION_ORIENTATION_ENABLED); - m_fusion_sensor->stop(); +bool orientation_sensor::get_sensor_info(sensor_info &info) +{ + info.set_type(get_type()); + info.set_id(get_id()); + info.set_privilege(SENSOR_PRIVILEGE_PUBLIC); + info.set_name(get_name()); + info.set_vendor("Samsung Electronics"); + info.set_min_range(-180); + info.set_max_range(360); + info.set_resolution(0.01); + info.set_min_interval(1); + info.set_fifo_count(0); + info.set_max_batch_count(0); + info.set_supported_event(get_event_type()); + info.set_wakeup_supported(false); - deactivate(); return true; } -bool orientation_sensor::add_interval(int client_id, unsigned int interval) +void orientation_sensor::synthesize(const sensor_event_t& event) { - AUTOLOCK(m_mutex); + int error; + sensor_event_t *orientation_event; + float azimuth, pitch, roll; - if (!m_hardware_fusion) { - m_accel_sensor->add_interval(client_id, interval, false); - m_gyro_sensor->add_interval(client_id, interval, false); - m_magnetic_sensor->add_interval(client_id, interval, false); - } + if (CONVERT_ID_TYPE(event.sensor_id) != ROTATION_VECTOR_SENSOR) + return; - m_fusion_sensor->add_interval(client_id, interval, false); + error = quat_to_orientation(event.data->values, azimuth, pitch, roll); + ret_if(error); - return sensor_base::add_interval(client_id, interval, false); + orientation_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); + if (!orientation_event) { + _E("Failed to allocate memory"); + return; + } + orientation_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + if (!orientation_event->data) { + _E("Failed to allocate memory"); + free(orientation_event); + return; + } + + orientation_event->sensor_id = get_id(); + orientation_event->event_type = CONVERT_TYPE_EVENT(ORIENTATION_SENSOR); + orientation_event->data_length = sizeof(sensor_data_t); + orientation_event->data->accuracy = event.data->accuracy; + orientation_event->data->timestamp = event.data->timestamp; + orientation_event->data->value_count = 3; + orientation_event->data->values[0] = azimuth; + orientation_event->data->values[1] = pitch; + orientation_event->data->values[2] = roll; + push(orientation_event); + + m_azimuth = azimuth; + m_pitch = pitch; + m_roll = roll; + m_time = event.data->timestamp; + m_accuracy = event.data->accuracy; + + _D("[orientation] : [%10f] [%10f] [%10f]", m_azimuth, m_pitch, m_roll); } -bool orientation_sensor::delete_interval(int client_id) +int orientation_sensor::get_data(sensor_data_t **data, int *length) { - AUTOLOCK(m_mutex); + /* if It is batch sensor, remains can be 2+ */ + int remains = 1; - if (!m_hardware_fusion) { - m_accel_sensor->delete_interval(client_id, false); - m_gyro_sensor->delete_interval(client_id, false); - m_magnetic_sensor->delete_interval(client_id, false); - } + sensor_data_t *sensor_data; + sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + + sensor_data->accuracy = m_accuracy; + sensor_data->timestamp = m_time; + sensor_data->value_count = 3; + sensor_data->values[0] = m_azimuth; + sensor_data->values[1] = m_pitch; + sensor_data->values[2] = m_roll; - m_fusion_sensor->delete_interval(client_id, false); + *data = sensor_data; + *length = sizeof(sensor_data_t); - return sensor_base::delete_interval(client_id, false); + return --remains; } -void orientation_sensor::synthesize(const sensor_event_t &event, vector &outs) +bool orientation_sensor::set_interval(unsigned long interval) { - sensor_event_t orientation_event; - unsigned long long diff_time; - float azimuth_offset; - - if (event.event_type == FUSION_EVENT) { - diff_time = event.data.timestamp - m_time; - - if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) - return; - - quaternion quat(event.data.values[0], event.data.values[1], - event.data.values[2], event.data.values[3]); - - euler_angles euler = quat2euler(quat); - - if(m_raw_data_unit == "DEGREES") { - euler = rad2deg(euler); - azimuth_offset = AZIMUTH_OFFSET_DEGREES; - } else { - azimuth_offset = AZIMUTH_OFFSET_RADIANS; - } - - euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation; - euler.m_ang.m_vec[1] *= m_roll_rotation_compensation; - euler.m_ang.m_vec[2] *= m_azimuth_rotation_compensation; - - m_time = get_timestamp(); - orientation_event.sensor_id = get_id(); - orientation_event.event_type = ORIENTATION_RAW_DATA_EVENT; - orientation_event.data.accuracy = event.data.accuracy; - orientation_event.data.timestamp = m_time; - orientation_event.data.value_count = 3; - orientation_event.data.values[1] = euler.m_ang.m_vec[0]; - orientation_event.data.values[2] = euler.m_ang.m_vec[1]; - if (euler.m_ang.m_vec[2] >= 0) - orientation_event.data.values[0] = euler.m_ang.m_vec[2]; - else - orientation_event.data.values[0] = euler.m_ang.m_vec[2] + azimuth_offset; - - push(orientation_event); - } - - return; + m_interval = interval; + return true; } -int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) +bool orientation_sensor::set_batch_latency(unsigned long latency) { - sensor_data_t fusion_data; - float azimuth_offset; + return false; +} - if (event_type != ORIENTATION_RAW_DATA_EVENT) - return -1; +bool orientation_sensor::on_start(void) +{ + if (m_rotation_vector_sensor) + m_rotation_vector_sensor->start(); - m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); + m_time = 0; - quaternion quat(fusion_data.values[0], fusion_data.values[1], - fusion_data.values[2], fusion_data.values[3]); + return activate(); +} - euler_angles euler = quat2euler(quat); +bool orientation_sensor::on_stop(void) +{ + if (m_rotation_vector_sensor) + m_rotation_vector_sensor->stop(); - if(m_raw_data_unit == "DEGREES") { - euler = rad2deg(euler); - azimuth_offset = AZIMUTH_OFFSET_DEGREES; - } else { - azimuth_offset = AZIMUTH_OFFSET_RADIANS; - } + m_time = 0; - data.accuracy = fusion_data.accuracy; - data.timestamp = get_timestamp(); - data.value_count = 3; - data.values[1] = euler.m_ang.m_vec[0]; - data.values[2] = euler.m_ang.m_vec[1]; - if (euler.m_ang.m_vec[2] >= 0) - data.values[0] = euler.m_ang.m_vec[2]; - else - data.values[0] = euler.m_ang.m_vec[2] + azimuth_offset; - - data.values[1] *= m_pitch_rotation_compensation; - data.values[2] *= m_roll_rotation_compensation; - data.values[0] *= m_azimuth_rotation_compensation; - - return 0; + return deactivate(); } -bool orientation_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties) +bool orientation_sensor::add_interval(int client_id, unsigned int interval, bool is_processor) { - if(m_raw_data_unit == "DEGREES") { - properties.min_range = -180; - properties.max_range = 360; - } else { - properties.min_range = -PI; - properties.max_range = 2 * PI; - } - properties.resolution = 0.000001; - properties.vendor = m_vendor; - properties.name = SENSOR_NAME; - properties.min_interval = 1; - properties.fifo_count = 0; - properties.max_batch_count = 0; + if (m_rotation_vector_sensor) + m_rotation_vector_sensor->add_interval(client_id, interval, true); - return true; + return sensor_base::add_interval(client_id, interval, is_processor); } +bool orientation_sensor::delete_interval(int client_id, bool is_processor) +{ + if (m_rotation_vector_sensor) + m_rotation_vector_sensor->delete_interval(client_id, true); + + return sensor_base::delete_interval(client_id, is_processor); +} diff --git a/src/sensor/orientation/orientation_sensor.h b/src/sensor/orientation/orientation_sensor.h index 070a21b..4e5f4c3 100644 --- a/src/sensor/orientation/orientation_sensor.h +++ b/src/sensor/orientation/orientation_sensor.h @@ -1,7 +1,7 @@ /* * sensord * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * Copyright (c) 2016 Samsung Electronics Co., Ltd. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -20,50 +20,50 @@ #ifndef _ORIENTATION_SENSOR_H_ #define _ORIENTATION_SENSOR_H_ -#include #include -#include +#include +#include class orientation_sensor : public virtual_sensor { public: orientation_sensor(); virtual ~orientation_sensor(); + /* initialize sensor */ bool init(void); - void synthesize(const sensor_event_t& event, std::vector &outs); + /* sensor info */ + virtual sensor_type_t get_type(void); + virtual unsigned int get_event_type(void); + virtual const char* get_name(void); - bool add_interval(int client_id, unsigned int interval); - bool delete_interval(int client_id); - virtual bool get_properties(sensor_type_t sensor_type, sensor_properties_s &properties); - virtual void get_types(std::vector &types); + virtual bool get_sensor_info(sensor_info &info); - int get_sensor_data(const unsigned int event_type, sensor_data_t &data); + /* synthesize event */ + virtual void synthesize(const sensor_event_t& event); -private: - sensor_base *m_accel_sensor; - sensor_base *m_gyro_sensor; - sensor_base *m_magnetic_sensor; - sensor_base *m_fusion_sensor; - - sensor_data m_accel; - sensor_data m_gyro; - sensor_data m_magnetic; + bool add_interval(int client_id, unsigned int interval, bool is_processor); + bool delete_interval(int client_id, bool is_processor); - cmutex m_value_mutex; + /* get data */ + virtual int get_data(sensor_data_t **data, int *length); +private: + sensor_base *m_rotation_vector_sensor; + float m_azimuth; + float m_pitch; + float m_roll; + int m_accuracy; unsigned long long m_time; - unsigned int m_interval; + unsigned long m_interval; + + virtual bool set_interval(unsigned long interval); + virtual bool set_batch_latency(unsigned long latency); - std::string m_vendor; - std::string m_raw_data_unit; - int m_default_sampling_time; - int m_azimuth_rotation_compensation; - int m_pitch_rotation_compensation; - int m_roll_rotation_compensation; + virtual bool on_start(void); + virtual bool on_stop(void); - bool on_start(void); - bool on_stop(void); + int rotation_to_orientation(const float *rotation, float &azimuth, float &pitch, float &roll); }; -#endif +#endif /* _ORIENTATION_SENSOR_H_ */ diff --git a/src/sensor/rotation_vector/rotation_vector_sensor.cpp b/src/sensor/rotation_vector/rotation_vector_sensor.cpp new file mode 100644 index 0000000..17c3655 --- /dev/null +++ b/src/sensor/rotation_vector/rotation_vector_sensor.cpp @@ -0,0 +1,261 @@ +/* + * sensord + * + * Copyright (c) 2016 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#define SENSOR_NAME "SENSOR_ROTATION_VECTOR" + +#define NORM(x, y, z) sqrt((x)*(x) + (y)*(y) + (z)*(z)) + +#define STATE_ACCEL 0x1 +#define STATE_MAGNETIC 0x2 + +rotation_vector_sensor::rotation_vector_sensor() +: m_accel_sensor(NULL) +, m_mag_sensor(NULL) +, m_x(-1) +, m_y(-1) +, m_z(-1) +, m_w(-1) +, m_time(0) +, m_state(0) +{ +} + +rotation_vector_sensor::~rotation_vector_sensor() +{ + _I("%s is destroyed!", SENSOR_NAME); +} + +bool rotation_vector_sensor::init() +{ + m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_mag_sensor = sensor_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); + + if (!m_accel_sensor || !m_mag_sensor) { + _E("cannot load sensors[%s]", SENSOR_NAME); + return false; + } + + _I("%s is created!", SENSOR_NAME); + return true; +} + +sensor_type_t rotation_vector_sensor::get_type(void) +{ + return ROTATION_VECTOR_SENSOR; +} + +unsigned int rotation_vector_sensor::get_event_type(void) +{ + return CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR); +} + +const char* rotation_vector_sensor::get_name(void) +{ + return SENSOR_NAME; +} + +bool rotation_vector_sensor::get_sensor_info(sensor_info &info) +{ + info.set_type(get_type()); + info.set_id(get_id()); + info.set_privilege(SENSOR_PRIVILEGE_PUBLIC); + info.set_name(get_name()); + info.set_vendor("Samsung Electronics"); + info.set_min_range(0); + info.set_max_range(1); + info.set_resolution(1); + info.set_min_interval(1); + info.set_fifo_count(0); + info.set_max_batch_count(0); + info.set_supported_event(get_event_type()); + info.set_wakeup_supported(false); + + return true; +} + +void rotation_vector_sensor::synthesize(const sensor_event_t& event) +{ + sensor_event_t *rotation_vector_event; + float R[9]; + float I[9]; + float quat[4]; + int error; + + if (event.event_type != GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME && + event.event_type != ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) + return; + + if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) { + m_mag[0] = event.data->values[0]; + m_mag[1] = event.data->values[1]; + m_mag[2] = event.data->values[2]; + m_accuracy = event.data->accuracy; + + m_state |= STATE_MAGNETIC; + } + + if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + m_acc[0] = event.data->values[0]; + m_acc[1] = event.data->values[1]; + m_acc[2] = event.data->values[2]; + + m_state |= STATE_ACCEL; + } + + if (m_state != (STATE_ACCEL | STATE_MAGNETIC)) + return; + + m_state = 0; + + unsigned long long timestamp = event.data->timestamp; + + error = calculate_rotation_matrix(m_acc, m_mag, R, I); + ret_if(error < 0); + + error = matrix_to_quat(R, quat); + ret_if(error < 0); + + rotation_vector_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); + if (!rotation_vector_event) { + _E("Failed to allocate memory"); + return; + } + rotation_vector_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + if (!rotation_vector_event->data) { + _E("Failed to allocate memory"); + free(rotation_vector_event); + return; + } + + rotation_vector_event->sensor_id = get_id(); + rotation_vector_event->event_type = CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR); + rotation_vector_event->data_length = sizeof(sensor_data_t); + rotation_vector_event->data->accuracy = m_accuracy; + rotation_vector_event->data->timestamp = timestamp; + rotation_vector_event->data->value_count = 4; + rotation_vector_event->data->values[0] = quat[0]; + rotation_vector_event->data->values[1] = quat[1]; + rotation_vector_event->data->values[2] = quat[2]; + rotation_vector_event->data->values[3] = quat[3]; + push(rotation_vector_event); + + m_time = timestamp; + m_x = quat[0]; + m_y = quat[1]; + m_z = quat[2]; + m_w = quat[3]; + m_accuracy = event.data->accuracy; + + _D("[rotation_vector] : [%10f] [%10f] [%10f] [%10f]", m_x, m_y, m_z, m_w); +} + +int rotation_vector_sensor::get_data(sensor_data_t **data, int *length) +{ + sensor_data_t *sensor_data; + sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + + sensor_data->accuracy = m_accuracy; + sensor_data->timestamp = m_time; + sensor_data->value_count = 3; + sensor_data->values[0] = m_x; + sensor_data->values[1] = m_y; + sensor_data->values[2] = m_z; + + *data = sensor_data; + *length = sizeof(sensor_data_t); + + return 0; +} + +bool rotation_vector_sensor::set_interval(unsigned long interval) +{ + m_interval = interval; + return true; +} + +bool rotation_vector_sensor::set_batch_latency(unsigned long latency) +{ + return false; +} + +bool rotation_vector_sensor::on_start(void) +{ + if (m_accel_sensor) + m_accel_sensor->start(); + + if (m_mag_sensor) + m_mag_sensor->start(); + + m_time = 0; + return activate(); +} + +bool rotation_vector_sensor::on_stop(void) +{ + if (m_accel_sensor) + m_accel_sensor->stop(); + + if (m_mag_sensor) + m_mag_sensor->stop(); + + m_time = 0; + m_state = 0; + + return deactivate(); +} + +bool rotation_vector_sensor::add_interval(int client_id, unsigned int interval, bool is_processor) +{ + if (m_accel_sensor) + m_accel_sensor->add_interval(client_id, interval, true); + + if (m_mag_sensor) + m_mag_sensor->add_interval(client_id, interval, true); + + return sensor_base::add_interval(client_id, interval, is_processor); +} + +bool rotation_vector_sensor::delete_interval(int client_id, bool is_processor) +{ + if (m_accel_sensor) + m_accel_sensor->delete_interval(client_id, true); + + if (m_mag_sensor) + m_mag_sensor->delete_interval(client_id, true); + + return sensor_base::delete_interval(client_id, is_processor); +} diff --git a/src/sensor/rotation_vector/rotation_vector_sensor.h b/src/sensor/rotation_vector/rotation_vector_sensor.h new file mode 100644 index 0000000..fcd4a4b --- /dev/null +++ b/src/sensor/rotation_vector/rotation_vector_sensor.h @@ -0,0 +1,72 @@ +/* + * sensord + * + * Copyright (c) 2016 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef _ROTATION_VECTOR_SENSOR_H_ +#define _ROTATION_VECTOR_SENSOR_H_ + +#include +#include + +class rotation_vector_sensor : public virtual_sensor { +public: + rotation_vector_sensor(); + virtual ~rotation_vector_sensor(); + + /* initialize sensor */ + bool init(void); + + /* sensor info */ + virtual sensor_type_t get_type(void); + virtual unsigned int get_event_type(void); + virtual const char* get_name(void); + + virtual bool get_sensor_info(sensor_info &info); + + /* synthesize event */ + virtual void synthesize(const sensor_event_t& event); + + bool add_interval(int client_id, unsigned int interval, bool is_processor); + bool delete_interval(int client_id, bool is_processor); + + /* get data */ + virtual int get_data(sensor_data_t **data, int *length); +private: + sensor_base *m_accel_sensor; + sensor_base *m_mag_sensor; + + float m_x; + float m_y; + float m_z; + float m_w; + int m_accuracy; + unsigned long long m_time; + unsigned long m_interval; + + float m_acc[3]; + float m_mag[3]; + int m_state; + + virtual bool set_interval(unsigned long interval); + virtual bool set_batch_latency(unsigned long latency); + + virtual bool on_start(void); + virtual bool on_stop(void); +}; + +#endif /* _ROTATION_VECTOR_SENSOR_H_ */ diff --git a/src/server/sensor_loader.cpp b/src/server/sensor_loader.cpp index d525d98..d5961c8 100644 --- a/src/server/sensor_loader.cpp +++ b/src/server/sensor_loader.cpp @@ -40,6 +40,12 @@ #ifdef ENABLE_LINEAR_ACCEL #include #endif +#ifdef ENABLE_ORIENTATION +#include +#endif +#ifdef ENABLE_ROTATION_VECTOR +#include +#endif using std::vector; using std::string; @@ -155,12 +161,18 @@ void sensor_loader::create_sensors(void) #ifdef ENABLE_AUTO_ROTATION create_virtual_sensors("Auto Rotation"); #endif +#ifdef ENABLE_ROTATION_VECTOR + create_virtual_sensors("Rotation Vector"); +#endif #ifdef ENABLE_GRAVITY create_virtual_sensors("Gravity"); #endif #ifdef ENABLE_LINEAR_ACCEL create_virtual_sensors("Linear Accel"); #endif +#ifdef ENABLE_ORIENTATION + create_virtual_sensors("Orientation"); +#endif } template diff --git a/src/shared/sensor_common.h b/src/shared/sensor_common.h index 5bf16fe..d505918 100644 --- a/src/shared/sensor_common.h +++ b/src/shared/sensor_common.h @@ -32,8 +32,12 @@ #define SENSOR_ID_INVALID -1 #define SENSOR_TYPE_SHIFT 32 +#define SENSOR_EVENT_SHIFT 16 #define SENSOR_INDEX_MASK 0xFFFFFFFF +#define CONVERT_ID_TYPE(id) ((id) >> SENSOR_TYPE_SHIFT) +#define CONVERT_TYPE_EVENT(type) ((type) << SENSOR_EVENT_SHIFT | 0x1) + #ifndef NAME_MAX #define NAME_MAX 256 #endif -- 2.7.4 From fe1d3573a32e73600473caf53ebb58a838a738b0 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 18 May 2016 16:35:19 +0900 Subject: [PATCH 03/16] sensord: put a space after "if" - fixed all code in sensord Change-Id: Ia56bcb22e7ada3f559c9a650a31bbd5ed067b317 Signed-off-by: kibak.yoon --- src/client/client.cpp | 6 +++--- src/client/command_channel.cpp | 4 ++-- src/client/sensor_event_listener.cpp | 2 +- src/sensor/fusion_util.cpp | 2 +- src/server/client_info_manager.cpp | 20 ++++++++++---------- src/server/sensor_event_dispatcher.cpp | 2 +- src/server/virtual_sensor_config.cpp | 4 ++-- src/server/worker_thread.cpp | 2 +- src/shared/csocket.cpp | 2 +- src/shared/sensor_log.h | 20 ++++++++++---------- 10 files changed, 32 insertions(+), 32 deletions(-) diff --git a/src/client/client.cpp b/src/client/client.cpp index 73f4180..ae79346 100644 --- a/src/client/client.cpp +++ b/src/client/client.cpp @@ -602,7 +602,7 @@ API int sensord_connect(sensor_t sensor) if (!sensor_client_info::get_instance().has_client_id()) { first_connection = true; - if(!cmd_channel->cmd_get_id(client_id)) { + if (!cmd_channel->cmd_get_id(client_id)) { _E("Sending cmd_get_id() failed for %s", get_sensor_name(sensor_id)); sensor_client_info::get_instance().close_command_channel(sensor_id); sensor_client_info::get_instance().delete_handle(handle); @@ -623,7 +623,7 @@ API int sensord_connect(sensor_t sensor) sensor_client_info::get_instance().set_sensor_params(handle, SENSOR_STATE_STOPPED, SENSOR_OPTION_DEFAULT); if (!sensor_registered) { - if(!cmd_channel->cmd_hello(sensor_id)) { + if (!cmd_channel->cmd_hello(sensor_id)) { _E("Sending cmd_hello(%s, %d) failed for %s", get_sensor_name(sensor_id), client_id, get_client_name()); sensor_client_info::get_instance().close_command_channel(sensor_id); sensor_client_info::get_instance().delete_handle(handle); @@ -677,7 +677,7 @@ API bool sensord_disconnect(int handle) sensor_client_info::get_instance().set_client_id(CLIENT_ID_INVALID); if (!sensor_client_info::get_instance().is_sensor_registered(sensor_id)) { - if(!cmd_channel->cmd_byebye()) { + if (!cmd_channel->cmd_byebye()) { _E("Sending cmd_byebye(%d, %s) failed for %s", client_id, get_sensor_name(sensor_id), get_client_name()); return false; } diff --git a/src/client/command_channel.cpp b/src/client/command_channel.cpp index e4486b2..274552f 100644 --- a/src/client/command_channel.cpp +++ b/src/client/command_channel.cpp @@ -416,7 +416,7 @@ bool command_channel::cmd_register_events(event_type_vector &event_vec) auto it_event = event_vec.begin(); while (it_event != event_vec.end()) { - if(!cmd_register_event(*it_event)) + if (!cmd_register_event(*it_event)) return false; ++it_event; @@ -469,7 +469,7 @@ bool command_channel::cmd_unregister_events(event_type_vector &event_vec) auto it_event = event_vec.begin(); while (it_event != event_vec.end()) { - if(!cmd_unregister_event(*it_event)) + if (!cmd_unregister_event(*it_event)) return false; ++it_event; diff --git a/src/client/sensor_event_listener.cpp b/src/client/sensor_event_listener.cpp index 56cbfef..cb629af 100644 --- a/src/client/sensor_event_listener.cpp +++ b/src/client/sensor_event_listener.cpp @@ -270,7 +270,7 @@ ssize_t sensor_event_listener::sensor_event_poll(void* buffer, int buffer_len, s len = m_event_socket.recv(buffer, buffer_len); if (!len) { - if(!m_poller->poll(event)) + if (!m_poller->poll(event)) return -1; len = m_event_socket.recv(buffer, buffer_len); diff --git a/src/sensor/fusion_util.cpp b/src/sensor/fusion_util.cpp index e14c223..7314fb9 100644 --- a/src/sensor/fusion_util.cpp +++ b/src/sensor/fusion_util.cpp @@ -34,7 +34,7 @@ static float clamp(float v) int quat_to_matrix(const float *quat, float *R) { - if(quat == NULL || R == NULL) + if (quat == NULL || R == NULL) return -EINVAL; float q0 = quat[3]; diff --git a/src/server/client_info_manager.cpp b/src/server/client_info_manager.cpp index b61bdf1..b42c9f5 100644 --- a/src/server/client_info_manager.cpp +++ b/src/server/client_info_manager.cpp @@ -51,7 +51,7 @@ bool client_info_manager::get_registered_events(int client_id, sensor_id_t senso return false; } - if(!it_record->second.get_registered_events(sensor_id, event_vec)) + if (!it_record->second.get_registered_events(sensor_id, event_vec)) return false; return true; @@ -68,7 +68,7 @@ bool client_info_manager::register_event(int client_id, sensor_id_t sensor_id, u return false; } - if(!it_record->second.register_event(sensor_id, event_type)) + if (!it_record->second.register_event(sensor_id, event_type)) return false; return true; @@ -85,7 +85,7 @@ bool client_info_manager::unregister_event(int client_id, sensor_id_t sensor_id, return false; } - if(!it_record->second.unregister_event(sensor_id, event_type)) + if (!it_record->second.unregister_event(sensor_id, event_type)) return false; return true; @@ -130,7 +130,7 @@ bool client_info_manager::set_option(int client_id, sensor_id_t sensor_id, int o return false; } - if(!it_record->second.set_option(sensor_id, option)) + if (!it_record->second.set_option(sensor_id, option)) return false; return true; @@ -147,7 +147,7 @@ bool client_info_manager::set_start(int client_id, sensor_id_t sensor_id, bool s return false; } - if(!it_record->second.set_start(sensor_id, start)) + if (!it_record->second.set_start(sensor_id, start)) return false; return true; @@ -304,10 +304,10 @@ bool client_info_manager::remove_sensor_record(int client_id, sensor_id_t sensor return false; } - if(!it_record->second.remove_sensor_usage(sensor_id)) + if (!it_record->second.remove_sensor_usage(sensor_id)) return false; - if(!it_record->second.has_sensor_usage()) + if (!it_record->second.has_sensor_usage()) remove_client_record(client_id); return true; @@ -324,7 +324,7 @@ bool client_info_manager::has_sensor_record(int client_id, sensor_id_t sensor_id return false; } - if(!it_record->second.has_sensor_usage(sensor_id)) + if (!it_record->second.has_sensor_usage(sensor_id)) return false; return true; @@ -341,7 +341,7 @@ bool client_info_manager::has_sensor_record(int client_id) return false; } - if(!it_record->second.has_sensor_usage()) + if (!it_record->second.has_sensor_usage()) return false; return true; @@ -354,7 +354,7 @@ bool client_info_manager::get_listener_ids(sensor_id_t sensor_id, unsigned int e auto it_record = m_clients.begin(); while (it_record != m_clients.end()) { - if(it_record->second.is_listening_event(sensor_id, event_type)) + if (it_record->second.is_listening_event(sensor_id, event_type)) id_vec.push_back(it_record->first); ++it_record; diff --git a/src/server/sensor_event_dispatcher.cpp b/src/server/sensor_event_dispatcher.cpp index a8247fc..200cc95 100644 --- a/src/server/sensor_event_dispatcher.cpp +++ b/src/server/sensor_event_dispatcher.cpp @@ -66,7 +66,7 @@ void sensor_event_dispatcher::accept_event_channel(csocket client_socket) client_socket.set_transfer_mode(); - if(!get_client_info_manager().set_event_socket(client_id, client_socket)) { + if (!get_client_info_manager().set_event_socket(client_id, client_socket)) { _E("Failed to store event socket[%d] for %s", client_socket.get_socket_fd(), client_info_manager.get_client_info(client_id)); return; diff --git a/src/server/virtual_sensor_config.cpp b/src/server/virtual_sensor_config.cpp index 46562a5..fc57bd8 100644 --- a/src/server/virtual_sensor_config.cpp +++ b/src/server/virtual_sensor_config.cpp @@ -74,13 +74,13 @@ bool virtual_sensor_config::load_config(const string& config_path) } cur = xmlDocGetRootElement(doc); - if(cur == NULL) { + if (cur == NULL) { _E("There is no root element in %s\n", config_path.c_str()); xmlFreeDoc(doc); return false; } - if(xmlStrcmp(cur->name, (const xmlChar *)ROOT_ELEMENT)) { + if (xmlStrcmp(cur->name, (const xmlChar *)ROOT_ELEMENT)) { _E("Wrong type document: there is no [%s] root element in %s\n", ROOT_ELEMENT, config_path.c_str()); xmlFreeDoc(doc); return false; diff --git a/src/server/worker_thread.cpp b/src/server/worker_thread.cpp index 23cd6ff..9f88a24 100644 --- a/src/server/worker_thread.cpp +++ b/src/server/worker_thread.cpp @@ -40,7 +40,7 @@ worker_thread::~worker_thread() bool worker_thread::transition_function(trans_func_index index) { if (m_trans_func[index] != NULL) { - if(!m_trans_func[index](m_context)) { + if (!m_trans_func[index](m_context)) { _E("Transition[%d] function returning false", index); return false; } diff --git a/src/shared/csocket.cpp b/src/shared/csocket.cpp index 0dc07ce..8481333 100644 --- a/src/shared/csocket.cpp +++ b/src/shared/csocket.cpp @@ -441,7 +441,7 @@ bool csocket::set_connection_mode(void) return false; } - if(setsockopt(m_sock_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) { + if (setsockopt(m_sock_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) { _ERRNO(errno, _E, "Set SO_RCVTIMEO failed for %s, m_sock_fd : %d", get_client_name(), m_sock_fd); close(); return false; diff --git a/src/shared/sensor_log.h b/src/shared/sensor_log.h index 236eb1d..d478e07 100644 --- a/src/shared/sensor_log.h +++ b/src/shared/sensor_log.h @@ -61,31 +61,31 @@ #if defined(_DEBUG) # define warn_if(expr, fmt, arg...) do { \ - if(expr) { \ + if (expr) { \ _D("(%s) -> " fmt, #expr, ##arg); \ } \ } while (0) # define ret_if(expr) do { \ - if(expr) { \ + if (expr) { \ _D("(%s) -> %s() return", #expr, __FUNCTION__); \ return; \ } \ } while (0) # define retv_if(expr, val) do { \ - if(expr) { \ + if (expr) { \ _D("(%s) -> %s() return", #expr, __FUNCTION__); \ return (val); \ } \ } while (0) # define retm_if(expr, fmt, arg...) do { \ - if(expr) { \ + if (expr) { \ _E(fmt, ##arg); \ _D("(%s) -> %s() return", #expr, __FUNCTION__); \ return; \ } \ } while (0) # define retvm_if(expr, val, fmt, arg...) do { \ - if(expr) { \ + if (expr) { \ _E(fmt, ##arg); \ _D("(%s) -> %s() return", #expr, __FUNCTION__); \ return (val); \ @@ -94,28 +94,28 @@ #else # define warn_if(expr, fmt, arg...) do { \ - if(expr) { \ + if (expr) { \ _E(fmt, ##arg); \ } \ } while (0) # define ret_if(expr) do { \ - if(expr) { \ + if (expr) { \ return; \ } \ } while (0) # define retv_if(expr, val) do { \ - if(expr) { \ + if (expr) { \ return (val); \ } \ } while (0) # define retm_if(expr, fmt, arg...) do { \ - if(expr) { \ + if (expr) { \ _E(fmt, ##arg); \ return; \ } \ } while (0) # define retvm_if(expr, val, fmt, arg...) do { \ - if(expr) { \ + if (expr) { \ _E(fmt, ##arg); \ return (val); \ } \ -- 2.7.4 From 4292dc9d72ae057d436ec1272836f0d2d5f04181 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 18 May 2016 20:40:38 +0900 Subject: [PATCH 04/16] sensord: check whether client list is empty or not before using std::find() - if client list is empty, almost of functions in client_info_manager are unnecessary. Change-Id: Ifce3bba890238bf8e2456362f41687111a00340b Signed-off-by: kibak.yoon --- src/server/client_info_manager.cpp | 42 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/src/server/client_info_manager.cpp b/src/server/client_info_manager.cpp index b42c9f5..d4bb7c9 100644 --- a/src/server/client_info_manager.cpp +++ b/src/server/client_info_manager.cpp @@ -31,6 +31,8 @@ client_info_manager::client_info_manager() client_info_manager::~client_info_manager() { + AUTOLOCK(m_mutex); + m_clients.clear(); } @@ -44,6 +46,8 @@ bool client_info_manager::get_registered_events(int client_id, sensor_id_t senso { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -61,6 +65,8 @@ bool client_info_manager::register_event(int client_id, sensor_id_t sensor_id, u { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -78,6 +84,8 @@ bool client_info_manager::unregister_event(int client_id, sensor_id_t sensor_id, { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -95,6 +103,8 @@ bool client_info_manager::set_batch(int client_id, sensor_id_t sensor_id, unsign { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -109,6 +119,8 @@ bool client_info_manager::get_batch(int client_id, sensor_id_t sensor_id, unsign { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -123,6 +135,8 @@ bool client_info_manager::set_option(int client_id, sensor_id_t sensor_id, int o { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -140,6 +154,8 @@ bool client_info_manager::set_start(int client_id, sensor_id_t sensor_id, bool s { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -157,6 +173,8 @@ bool client_info_manager::is_started(int client_id, sensor_id_t sensor_id) { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -194,6 +212,8 @@ bool client_info_manager::remove_client_record(int client_id) { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -212,6 +232,8 @@ bool client_info_manager::has_client_record(int client_id) { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); return (it_record != m_clients.end()); @@ -237,6 +259,8 @@ const char* client_info_manager::get_client_info(int client_id) { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), NULL, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -251,6 +275,8 @@ bool client_info_manager::set_permission(int client_id, int permission) { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -266,6 +292,8 @@ bool client_info_manager::get_permission(int client_id, int &permission) { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -281,6 +309,8 @@ bool client_info_manager::create_sensor_record(int client_id, sensor_id_t sensor { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -297,6 +327,8 @@ bool client_info_manager::remove_sensor_record(int client_id, sensor_id_t sensor { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -317,6 +349,8 @@ bool client_info_manager::has_sensor_record(int client_id, sensor_id_t sensor_id { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -334,6 +368,8 @@ bool client_info_manager::has_sensor_record(int client_id) { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -351,6 +387,8 @@ bool client_info_manager::get_listener_ids(sensor_id_t sensor_id, unsigned int e { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.begin(); while (it_record != m_clients.end()) { @@ -367,6 +405,8 @@ bool client_info_manager::get_event_socket(int client_id, csocket &socket) { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { @@ -383,6 +423,8 @@ bool client_info_manager::set_event_socket(int client_id, const csocket &socket) { AUTOLOCK(m_mutex); + retvm_if(m_clients.empty(), false, "client list is empty"); + auto it_record = m_clients.find(client_id); if (it_record == m_clients.end()) { -- 2.7.4 From 2f5ef32c7261d3bed0a61b0523128e8c707678be Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 18 May 2016 20:46:29 +0900 Subject: [PATCH 05/16] sensord: add a newline for code readability Change-Id: Ida83fdfcdedf399a8de927fcb7ee344541267db4 Signed-off-by: kibak.yoon --- src/server/command_worker.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/server/command_worker.cpp b/src/server/command_worker.cpp index 89b569e..b2a61aa 100644 --- a/src/server/command_worker.cpp +++ b/src/server/command_worker.cpp @@ -357,6 +357,7 @@ bool command_worker::cmd_get_id(void *payload) get_client_info_manager().set_client_info(client_id, cr.pid, cmd->name); m_permission = get_permission(); + get_client_info_manager().set_permission(client_id, m_permission); _I("New client id [%d] created", client_id); -- 2.7.4 From b35608fb9048ba96bc0806c8b1d07feb3203cca1 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 18 May 2016 20:48:42 +0900 Subject: [PATCH 06/16] sensord: add lock in permission_checker functions Change-Id: I1969e07d02482f44d6db810b3c33b17b4282d28d Signed-off-by: kibak.yoon --- src/server/permission_checker.cpp | 6 ++++++ src/server/permission_checker.h | 13 ++++++++----- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/server/permission_checker.cpp b/src/server/permission_checker.cpp index e2ed12e..e811217 100644 --- a/src/server/permission_checker.cpp +++ b/src/server/permission_checker.cpp @@ -77,6 +77,8 @@ permission_checker& permission_checker::get_instance() void permission_checker::init() { + AUTOLOCK(m_mutex); + m_permission_infos.push_back(std::make_shared (SENSOR_PERMISSION_STANDARD, false, "")); m_permission_infos.push_back(std::make_shared (SENSOR_PERMISSION_BIO, true, "http://tizen.org/privilege/healthinfo")); @@ -96,6 +98,8 @@ void permission_checker::init() void permission_checker::deinit() { + AUTOLOCK(m_mutex); + if (cynara_env) cynara_finish(cynara_env); @@ -104,6 +108,8 @@ void permission_checker::deinit() int permission_checker::get_permission(int sock_fd) { + AUTOLOCK(m_mutex); + int permission = SENSOR_PERMISSION_NONE; for (unsigned int i = 0; i < m_permission_infos.size(); ++i) { diff --git a/src/server/permission_checker.h b/src/server/permission_checker.h index 53fa3b6..cf9f444 100644 --- a/src/server/permission_checker.h +++ b/src/server/permission_checker.h @@ -20,11 +20,17 @@ #ifndef _PERMISSION_CHECKER_H_ #define _PERMISSION_CHECKER_H_ +#include #include #include #include class permission_checker { +public: + static permission_checker& get_instance(); + + int get_permission(int sock_fd); + private: class permission_info { public: @@ -49,13 +55,10 @@ private: void init(); void deinit(); +private: permission_info_vector m_permission_infos; int m_permission_set; - -public: - static permission_checker& get_instance(); - - int get_permission(int sock_fd); + cmutex m_mutex; }; #endif /* _PERMISSION_CHECKER_H_ */ -- 2.7.4 From 7d2a17a88bd73eb9afa5b0279156df1710b054c2 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 18 May 2016 20:52:46 +0900 Subject: [PATCH 07/16] sensord: add log message in destructor of sensors - add NULL check in get_name() in physical_sensor Change-Id: Idef67bd894d46e141a674302415893015131437e Signed-off-by: kibak.yoon --- src/sensor/hrm/hrm_sensor.cpp | 1 + src/server/physical_sensor.cpp | 5 +++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/sensor/hrm/hrm_sensor.cpp b/src/sensor/hrm/hrm_sensor.cpp index aa48965..d29ff49 100644 --- a/src/sensor/hrm/hrm_sensor.cpp +++ b/src/sensor/hrm/hrm_sensor.cpp @@ -30,4 +30,5 @@ hrm_sensor::hrm_sensor() hrm_sensor::~hrm_sensor() { + _I("hrm_sensor is destroyed : %#x", this); } diff --git a/src/server/physical_sensor.cpp b/src/server/physical_sensor.cpp index 689ffed..b16561d 100644 --- a/src/server/physical_sensor.cpp +++ b/src/server/physical_sensor.cpp @@ -32,6 +32,7 @@ physical_sensor::physical_sensor() physical_sensor::~physical_sensor() { + _I("physical sensor is destroyed"); } void physical_sensor::set_sensor_info(const sensor_info_t *info) @@ -56,8 +57,8 @@ unsigned int physical_sensor::get_event_type(void) const char* physical_sensor::get_name(void) { - if (!m_info->name) - return UNKNOWN_NAME; + retv_if(!m_info, UNKNOWN_NAME); + retv_if(!m_info->name, UNKNOWN_NAME); return m_info->name; } -- 2.7.4 From a4d0c2a85e73a0fdba0b46a61aa5926c16bcca94 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 18 May 2016 20:56:26 +0900 Subject: [PATCH 08/16] sensord: add stop() in event_dispatcher thread - because sensord can be shutdown, event_dispatcher can be stopped. Change-Id: Idd59e0f34dad468c3f4a32d3a47e4329cdec0936 Signed-off-by: kibak.yoon --- src/server/sensor_event_dispatcher.cpp | 26 ++++++++++++++++++-------- src/server/sensor_event_dispatcher.h | 2 ++ 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/src/server/sensor_event_dispatcher.cpp b/src/server/sensor_event_dispatcher.cpp index 200cc95..c1fc3cb 100644 --- a/src/server/sensor_event_dispatcher.cpp +++ b/src/server/sensor_event_dispatcher.cpp @@ -30,6 +30,7 @@ using std::pair; #define MAX_PENDING_CONNECTION 32 sensor_event_dispatcher::sensor_event_dispatcher() +: m_running(false) { } @@ -45,12 +46,21 @@ sensor_event_dispatcher& sensor_event_dispatcher::get_instance() bool sensor_event_dispatcher::run(void) { + m_running = true; + thread dispatcher(&sensor_event_dispatcher::dispatch_event, this); dispatcher.detach(); return true; } +bool sensor_event_dispatcher::stop(void) +{ + m_running = false; + + return true; +} + void sensor_event_dispatcher::accept_event_channel(csocket client_socket) { int client_id; @@ -99,7 +109,7 @@ void sensor_event_dispatcher::dispatch_event(void) _I("Event Dispatcher started"); - while (true) { + while (m_running) { void *seed_event = get_event_queue().pop(); vector sensor_events; @@ -273,6 +283,9 @@ bool sensor_event_dispatcher::add_active_virtual_sensor(virtual_sensor *sensor) { AUTOLOCK(m_active_virtual_sensors_mutex); + if (!m_running) + return true; + if (has_active_virtual_sensor(sensor)) { _E("[%s] sensor is already added on active virtual sensors", sensor->get_name()); return false; @@ -287,14 +300,11 @@ bool sensor_event_dispatcher::delete_active_virtual_sensor(virtual_sensor *senso { AUTOLOCK(m_active_virtual_sensors_mutex); - auto it_v_sensor = find(m_active_virtual_sensors.begin(), m_active_virtual_sensors.end(), sensor); - - if (it_v_sensor == m_active_virtual_sensors.end()) { - _E("Fail to delete non-existent [%s] sensor on active virtual sensors", sensor->get_name()); - return false; - } + if (!m_running) + return true; - m_active_virtual_sensors.erase(it_v_sensor); + if (sensor) + m_active_virtual_sensors.remove(sensor); return true; } diff --git a/src/server/sensor_event_dispatcher.h b/src/server/sensor_event_dispatcher.h index 243fb70..ee7e630 100644 --- a/src/server/sensor_event_dispatcher.h +++ b/src/server/sensor_event_dispatcher.h @@ -37,6 +37,7 @@ public: static sensor_event_dispatcher& get_instance(); bool run(void); + bool stop(void); void accept_event_connections(csocket client_socket); void request_last_event(int client_id, sensor_id_t sensor_id); @@ -51,6 +52,7 @@ private: event_type_last_event_map m_last_events; virtual_sensors m_active_virtual_sensors; cmutex m_active_virtual_sensors_mutex; + bool m_running; sensor_event_dispatcher(); ~sensor_event_dispatcher(); -- 2.7.4 From bbdde42c7d0095ae7aa0da3622bdb30fd474d7b6 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 18 May 2016 21:04:48 +0900 Subject: [PATCH 09/16] sensord: set signal fd to poller for knowing what signal is received - delete fds which is added to poller when event_poller is destroyed. Change-Id: I098326f609b215e448c92420d88cfa7616d8f9c9 Signed-off-by: kibak.yoon --- src/server/sensor_event_poller.cpp | 24 ++++++++++++++++++++++-- src/server/sensor_event_poller.h | 10 +++++++--- src/shared/poller.cpp | 32 +++++++++++++++++++++++++++++--- src/shared/poller.h | 8 ++++++++ 4 files changed, 66 insertions(+), 8 deletions(-) diff --git a/src/server/sensor_event_poller.cpp b/src/server/sensor_event_poller.cpp index 1327b44..994c1f0 100644 --- a/src/server/sensor_event_poller.cpp +++ b/src/server/sensor_event_poller.cpp @@ -17,21 +17,27 @@ * */ -#include +#include +#include #include #include #include #include #include +#include sensor_event_poller::sensor_event_poller() { init_sensor_map(); init_fd(); + init_signal_fd(); } sensor_event_poller::~sensor_event_poller() { + fd_sensors_t::iterator it; + for (it = m_fd_sensors.begin(); it != m_fd_sensors.end(); it = m_fd_sensors.upper_bound(it->first)) + m_poller.del_fd(it->first); } void sensor_event_poller::init_sensor_map() @@ -58,7 +64,7 @@ void sensor_event_poller::init_sensor_map() } } -void sensor_event_poller::init_fd() +void sensor_event_poller::init_fd(void) { fd_sensors_t::iterator it; for (it = m_fd_sensors.begin(); it != m_fd_sensors.end(); it = m_fd_sensors.upper_bound(it->first)) { @@ -67,6 +73,20 @@ void sensor_event_poller::init_fd() } } +void sensor_event_poller::init_signal_fd(void) +{ + int sfd; + sigset_t mask; + + sigemptyset(&mask); + sigaddset(&mask, SIGTERM); + sigaddset(&mask, SIGABRT); + sigaddset(&mask, SIGINT); + + sfd = signalfd(-1, &mask, 0); + m_poller.add_signal_fd(sfd); +} + bool sensor_event_poller::add_poll_fd(int fd) { return m_poller.add_fd(fd); diff --git a/src/server/sensor_event_poller.h b/src/server/sensor_event_poller.h index 03e5a59..c6c9ade 100644 --- a/src/server/sensor_event_poller.h +++ b/src/server/sensor_event_poller.h @@ -31,13 +31,17 @@ public: sensor_event_poller(); virtual ~sensor_event_poller(); - bool poll(); + bool poll(void); + private: poller m_poller; fd_sensors_t m_fd_sensors; - void init_fd(); - void init_sensor_map(); +private: + void init_sensor_map(void); + void init_fd(void); + void init_signal_fd(void); + bool add_poll_fd(int fd); bool read_fd(int fd, std::vector &ids); bool process_event(int fd, const std::vector &ids); diff --git a/src/shared/poller.cpp b/src/shared/poller.cpp index b1a3ded..8e05af8 100644 --- a/src/shared/poller.cpp +++ b/src/shared/poller.cpp @@ -25,6 +25,7 @@ poller::poller() : m_epfd(-1) +, sfd(-1) { init_poll_fd(); } @@ -38,8 +39,8 @@ poller::poller(int fd) poller::~poller() { - if (m_epfd) - close(m_epfd); + if (m_epfd >= 0) + ::close(m_epfd); } void poller::init_poll_fd(void) @@ -47,6 +48,12 @@ void poller::init_poll_fd(void) m_epfd = epoll_create(EPOLL_MAX); } +bool poller::add_signal_fd(int fd) +{ + sfd = fd; + return add_fd(fd); +} + bool poller::add_fd(int fd) { struct epoll_event event; @@ -62,6 +69,20 @@ bool poller::add_fd(int fd) return true; } +bool poller::del_fd(int fd) +{ + struct epoll_event event; + + event.data.fd = fd; + + if (epoll_ctl(m_epfd, EPOLL_CTL_DEL, fd, &event)) { + _ERRNO(errno, _E, "Failed to del fd[%d]", fd); + return false; + } + + return true; +} + bool poller::fill_event_queue(void) { const int EPOLL_MAX_EVENT = 1; @@ -82,7 +103,7 @@ bool poller::fill_event_queue(void) return false; } - for (int i = 0; i < nr_events; i++) + for (int i = 0; i < nr_events; i++) m_event_queue.push(event_items[i]); return true; @@ -110,6 +131,11 @@ bool poller::poll(struct epoll_event &event) return false; } + if (event.data.fd == sfd) { + _E("received signal"); + return false; + } + return true; } } diff --git a/src/shared/poller.h b/src/shared/poller.h index b43d491..e61408d 100644 --- a/src/shared/poller.h +++ b/src/shared/poller.h @@ -31,12 +31,20 @@ public: poller(int fd); virtual ~poller(); + bool add_signal_fd(int fd); + bool add_fd(int fd); + bool del_fd(int fd); + bool poll(struct epoll_event &event); + private: int m_epfd; + int sfd; + std::queue m_event_queue; +private: void init_poll_fd(void); bool fill_event_queue(void); }; -- 2.7.4 From d028b7e131090c0693a4b0093cc223dd5c448b8f Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 18 May 2016 21:17:24 +0900 Subject: [PATCH 10/16] sensord: change raw pointer to shared pointer for managing device(HAL) handles - erase items in map and dlclose so handle in destructor explicitly Change-Id: I8c7e8359fe9741b31f857e127c3a8e2a192df1c7 Signed-off-by: kibak.yoon --- src/server/sensor_loader.cpp | 22 +++++++++++++--------- src/server/sensor_loader.h | 2 +- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/server/sensor_loader.cpp b/src/server/sensor_loader.cpp index d5961c8..f82856f 100644 --- a/src/server/sensor_loader.cpp +++ b/src/server/sensor_loader.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include @@ -58,8 +59,14 @@ sensor_loader::sensor_loader() sensor_loader::~sensor_loader() { - for (auto it = m_handles.begin(); it != m_handles.end(); ++it) - dlclose(*it); + sensor_device_map_t::iterator it_device; + std::vector::iterator it_handle; + + for (it_device = m_devices.begin(); it_device != m_devices.end();) + it_device = m_devices.erase(it_device); + + for (it_handle = m_handles.begin(); it_handle != m_handles.end(); ++it_handle) + dlclose(*it_handle); m_handles.clear(); } @@ -136,10 +143,11 @@ bool sensor_loader::load_sensor_devices(const string &path, void* &handle) for (int i = 0; i < device_size; ++i) { device = static_cast(_devices[i]); + std::shared_ptr device_ptr(device); - int info_size = device->get_sensors(&infos); + int info_size = device_ptr->get_sensors(&infos); for (int j = 0; j < info_size; ++j) - m_devices[&infos[j]] = device; + m_devices[&infos[j]] = device_ptr; } handle = _handle; @@ -187,9 +195,7 @@ void sensor_loader::create_physical_sensors(sensor_type_t type) for (it = m_devices.begin(); it != m_devices.end(); ++it) { info = it->first; - device = it->second; - if (m_devices[info] == NULL) - continue; + device = it->second.get(); if (type != UNKNOWN_SENSOR) { if (type != (sensor_type_t)(info->type)) @@ -214,8 +220,6 @@ void sensor_loader::create_physical_sensors(sensor_type_t type) m_sensors.insert(std::make_pair(_type, sensor_ptr)); _I("created [%s] sensor", sensor->get_name()); - - m_devices[info] = NULL; } } diff --git a/src/server/sensor_loader.h b/src/server/sensor_loader.h index ef659e3..486effe 100644 --- a/src/server/sensor_loader.h +++ b/src/server/sensor_loader.h @@ -36,7 +36,7 @@ class sensor_base; typedef std::multimap> sensor_map_t; -typedef std::map sensor_device_map_t; +typedef std::map> sensor_device_map_t; class sensor_loader { private: -- 2.7.4 From c79d9e3bae93607767ad89c6e627dcd6bb9bda79 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Thu, 19 May 2016 11:54:06 +0900 Subject: [PATCH 11/16] sensord: auto_rotation: fix interval setting in auto_rotation - use interval instead of m_interval Change-Id: I8ae7d1371f8b9e4c7110f1caf4b7541f45c239ea Signed-off-by: kibak.yoon --- src/sensor/auto_rotation/auto_rotation_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensor/auto_rotation/auto_rotation_sensor.cpp b/src/sensor/auto_rotation/auto_rotation_sensor.cpp index 335d41e..b3f0784 100644 --- a/src/sensor/auto_rotation/auto_rotation_sensor.cpp +++ b/src/sensor/auto_rotation/auto_rotation_sensor.cpp @@ -178,7 +178,7 @@ int auto_rotation_sensor::get_data(sensor_data_t **data, int *length) bool auto_rotation_sensor::set_interval(unsigned long interval) { - m_accel_sensor->add_interval((intptr_t)this , m_interval, true); + m_accel_sensor->add_interval((intptr_t)this , interval, true); m_interval = interval; return false; -- 2.7.4 From f3957f2da642de9571cce3c18b7242c22684707a Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Thu, 19 May 2016 11:54:50 +0900 Subject: [PATCH 12/16] sensord: terminate sensor daemon safely - checking event by using select() before accept() if event is accepted when sensord is terminating, restoring connection has a problem that client cannot connect to sensord. - in server, quit g_main_loop and close socket safely. - sensord.service : off restart option Change-Id: Ibb2c9eca3cdc26b9782e93d3a3e0337b0a28f1a0 Signed-off-by: kibak.yoon --- packaging/sensord.service | 2 - src/server/main.cpp | 12 ++-- src/server/server.cpp | 136 ++++++++++++++++++++++++++++++++++------------ src/server/server.h | 21 ++++++- src/shared/csocket.cpp | 48 +++++++++++++--- src/shared/csocket.h | 1 + 6 files changed, 168 insertions(+), 52 deletions(-) diff --git a/packaging/sensord.service b/packaging/sensord.service index 50dc0ad..5cb2e72 100644 --- a/packaging/sensord.service +++ b/packaging/sensord.service @@ -6,8 +6,6 @@ User=sensor Group=input Type=notify ExecStart=/usr/bin/sensord -Restart=always -RestartSec=0 MemoryLimit=20M Sockets=sensord_command.socket Sockets=sensord_event.socket diff --git a/src/server/main.cpp b/src/server/main.cpp index 2ef0bfa..b136d69 100644 --- a/src/server/main.cpp +++ b/src/server/main.cpp @@ -34,8 +34,7 @@ static void sig_term_handler(int signo, siginfo_t *info, void *data) _E("Received SIGTERM(%d) from %s(%d)\n", signo, proc_name, info->si_pid); - /* TODO: Refactoring */ - raise(SIGKILL); + server::get_instance().stop(); } static void signal_init(void) @@ -51,6 +50,8 @@ static void signal_init(void) sig_act.sa_sigaction = sig_term_handler; sig_act.sa_flags = SA_SIGINFO; sigaction(SIGTERM, &sig_act, NULL); + sigaction(SIGABRT, &sig_act, NULL); + sigaction(SIGINT, &sig_act, NULL); } static void set_cal_data(void) @@ -63,7 +64,9 @@ static void set_cal_data(void) } fprintf(fp, "%d", SET_CAL); - fclose(fp); + + if (fp) + fclose(fp); _I("Succeeded to set calibration data"); @@ -78,12 +81,13 @@ int main(int argc, char *argv[]) set_cal_data(); + /* TODO: loader has to be moved to server */ sensor_loader::get_instance().load(); server::get_instance().run(); - server::get_instance().stop(); _I("Sensord terminated"); + return 0; } diff --git a/src/server/server.cpp b/src/server/server.cpp index 7a5e416..184df3d 100644 --- a/src/server/server.cpp +++ b/src/server/server.cpp @@ -17,14 +17,17 @@ * */ +#include +#include + #include #include #include #include #include #include -#include #include +#include #define SYSTEMD_SOCKET_MAX 2 @@ -32,12 +35,12 @@ using std::thread; server::server() : m_mainloop(NULL) +, m_running(false) { } server::~server() { - stop(); } int server::get_systemd_socket(const char *name) @@ -73,46 +76,77 @@ int server::get_systemd_socket(const char *name) void server::accept_command_channel(void) { command_worker *cmd_worker; + _I("Command channel acceptor is started"); - while (true) { + while (m_running) { csocket client_command_socket; + if (!m_command_channel_accept_socket.is_valid()) { + _E("Failed to accept, event_channel_accept_socket is closed"); + break; + } + if (!m_command_channel_accept_socket.accept(client_command_socket)) { _E("Failed to accept command channel from a client"); continue; } + if (!m_running) { + _E("server die"); + break; + } + _D("New client (socket_fd : %d) connected", client_command_socket.get_socket_fd()); + /* TODO: if socket is closed, it should be erased */ + client_command_sockets.push_back(client_command_socket); + cmd_worker = new(std::nothrow) command_worker(client_command_socket); if (!cmd_worker) { _E("Failed to allocate memory"); - continue; + break; } if (!cmd_worker->start()) delete cmd_worker; } + + _I("Command channel acceptor is terminated"); } void server::accept_event_channel(void) { - _I("Event channel acceptor is started"); + _I("Event channel acceptor is started!"); - while (true) { + while (m_running) { csocket client_event_socket; + if (!m_event_channel_accept_socket.is_valid()) { + _E("Failed to accept, event_channel_accept_socket is closed"); + break; + } + if (!m_event_channel_accept_socket.accept(client_event_socket)) { _E("Failed to accept event channel from a client"); continue; } + if (!m_running) { + _E("server die"); + break; + } + + /* TODO: if socket is closed, it should be erased */ + client_event_sockets.push_back(client_event_socket); + _D("New client(socket_fd : %d) connected", client_event_socket.get_socket_fd()); sensor_event_dispatcher::get_instance().accept_event_connections(client_event_socket); } + + _I("Event channel acceptor is terminated"); } void server::poll_event(void) @@ -127,32 +161,6 @@ void server::poll_event(void) } } -void server::run(void) -{ - m_mainloop = g_main_loop_new(NULL, false); - - sensor_event_dispatcher::get_instance().run(); - - listen_command_channel(); - listen_event_channel(); - - thread event_channel_accepter(&server::accept_event_channel, this); - event_channel_accepter.detach(); - - thread command_channel_accepter(&server::accept_command_channel, this); - command_channel_accepter.detach(); - - thread event_poller(&server::poll_event, this); - event_poller.detach(); - - sd_notify(0, "READY=1"); - - g_main_loop_run(m_mainloop); - g_main_loop_unref(m_mainloop); - - return; -} - bool server::listen_command_channel(void) { int sock_fd = -1; @@ -221,13 +229,69 @@ bool server::listen_event_channel(void) return true; } -void server::stop(void) +void server::close_socket(void) { - if (m_mainloop) - g_main_loop_quit(m_mainloop); - m_command_channel_accept_socket.close(); m_event_channel_accept_socket.close(); + + for (int i = 0; i < client_command_sockets.size(); ++i) + client_command_sockets[i].close(); + + for (int i = 0; i < client_event_sockets.size(); ++i) + client_event_sockets[i].close(); + + client_command_sockets.clear(); + client_event_sockets.clear(); +} + +void server::initialize(void) +{ + m_running = true; + m_mainloop = g_main_loop_new(NULL, false); + + sensor_event_dispatcher::get_instance().run(); + + listen_command_channel(); + listen_event_channel(); + + std::thread event_channel_accepter(&server::accept_event_channel, this); + event_channel_accepter.detach(); + + std::thread command_channel_accepter(&server::accept_command_channel, this); + command_channel_accepter.detach(); + + std::thread event_poller(&server::poll_event, this); + event_poller.detach(); + + sd_notify(0, "READY=1"); + + g_main_loop_run(m_mainloop); +} + +void server::terminate(void) +{ + sensor_event_dispatcher::get_instance().stop(); + + close_socket(); +} + +void server::run(void) +{ + initialize(); + terminate(); +} + +void server::stop(void) +{ + _I("Sensord server stopped"); + + m_running = false; + + if (m_mainloop) { + g_main_loop_quit(m_mainloop); + g_main_loop_unref(m_mainloop); + m_mainloop = NULL; + } } server& server::get_instance() diff --git a/src/server/server.h b/src/server/server.h index 3f32b3f..3dbd0d4 100644 --- a/src/server/server.h +++ b/src/server/server.h @@ -22,23 +22,35 @@ #include #include +#include +#include class server { public: + static server& get_instance(); + +public: void run(void); void stop(void); - static server& get_instance(); private: GMainLoop *m_mainloop; csocket m_command_channel_accept_socket; csocket m_event_channel_accept_socket; + std::vector client_command_sockets; + std::vector client_event_sockets; + + bool m_running; + +private: server(); - ~server(); + virtual ~server(); + + void initialize(void); + void terminate(void); void poll_event(void); - void accept_client(void); bool listen_command_channel(void); bool listen_event_channel(void); @@ -46,6 +58,9 @@ private: void accept_command_channel(void); void accept_event_channel(void); + void close_socket(void); + + /* TODO: move to socket class */ int get_systemd_socket(const char *name); }; diff --git a/src/shared/csocket.cpp b/src/shared/csocket.cpp index 8481333..4a01a38 100644 --- a/src/shared/csocket.cpp +++ b/src/shared/csocket.cpp @@ -130,9 +130,46 @@ bool csocket::listen(const int max_connections) bool csocket::accept(csocket& client_socket) const { + const int TIMEOUT = 1; + struct timeval tv; int addr_length = sizeof(m_addr); int err = 0; + fd_set read_fds; + + while (true) { + FD_ZERO(&read_fds); + FD_SET(m_sock_fd, &read_fds); + + tv.tv_sec = TIMEOUT; + tv.tv_usec = 0; + + err = ::select(m_sock_fd + 1, &read_fds, NULL, NULL, &tv); + if (err == -1) { + _ERRNO(errno, _E, "Failed to select(), m_sock_fd : %d", m_sock_fd); + return false; + } + + if (!is_valid()) { + _E("socket is closed, m_sock_fd : %d", m_sock_fd); + return false; + } + + /* timeout */ + if (!err) + continue; + + if (FD_ISSET(m_sock_fd, &read_fds)) + break; + + _ERRNO(errno, _E, "Failed to select(), msock_fd : %d", m_sock_fd); + } + + if (!is_valid()) { + _E("socket is closed, m_sock_fd : %d", m_sock_fd); + return false; + } + do { client_socket.m_sock_fd = ::accept(m_sock_fd, (sockaddr *)&m_addr, (socklen_t *)&addr_length); if (!client_socket.is_valid()) @@ -169,7 +206,7 @@ ssize_t csocket::recv_for_seqpacket(void* buffer, size_t size) const ssize_t err, len; do { - len = ::recv(m_sock_fd, buffer, size, m_recv_flags); + len = ::recv(m_sock_fd, buffer, size, m_recv_flags); if (len > 0) { err = 0; @@ -180,20 +217,17 @@ ssize_t csocket::recv_for_seqpacket(void* buffer, size_t size) const } else { err = errno; } - } while (err == EINTR); + } while (err == EINTR); - if ((err == EAGAIN) || (err == EWOULDBLOCK)) { - _ERRNO(err, _D, "Failed to recv(%d, %#x, %d, %#x) = %d", - m_socket_fd, buffer, size, m_recv_flags, len); + if ((err == EAGAIN) || (err == EWOULDBLOCK)) return 0; - } if (err) { _ERRNO(err, _E, "Failed to recv(%d, %#x, %d, %#x) = %d", m_sock_fd, buffer, size, m_recv_flags, len); } - return err == 0 ? len : -err; + return err == 0 ? len : -err; } ssize_t csocket::send_for_stream(const void *buffer, size_t size) const diff --git a/src/shared/csocket.h b/src/shared/csocket.h index 55582cf..1391531 100644 --- a/src/shared/csocket.h +++ b/src/shared/csocket.h @@ -69,6 +69,7 @@ private: ssize_t recv_for_seqpacket(void* buffer, size_t size) const; ssize_t recv_for_stream(void* buffer, size_t size) const; +private: int m_sock_fd; int m_sock_type; sockaddr_un m_addr; -- 2.7.4 From 4df48dbdb373f3b02bb9a1e05858f26847d0d18e Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Thu, 19 May 2016 13:11:32 +0900 Subject: [PATCH 13/16] sensord: set cynara configuration for cache size - cynara cache size : 16 Change-Id: Ia28f3b0da49a731ff2fa7493800482cdd08e77ad Signed-off-by: kibak.yoon --- src/server/permission_checker.cpp | 29 +++++++++++++++++++++++++++-- src/server/permission_checker.h | 6 ++++-- 2 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/server/permission_checker.cpp b/src/server/permission_checker.cpp index e811217..b8ece6e 100644 --- a/src/server/permission_checker.cpp +++ b/src/server/permission_checker.cpp @@ -25,6 +25,8 @@ #include #include +#define CACHE_SIZE 16 + static cynara *cynara_env = NULL; static bool check_privilege_by_sockfd(int sock_fd, const char *priv) @@ -90,10 +92,33 @@ void permission_checker::init() _I("Permission Set = %d", m_permission_set); - if (cynara_initialize(&cynara_env, NULL) != CYNARA_API_SUCCESS) { + init_cynara(); +} + +void permission_checker::init_cynara(void) +{ + cynara_configuration *conf; + + int err = cynara_configuration_create(&conf); + retm_if(err != CYNARA_API_SUCCESS, "Failed to create cynara configuration"); + + err = cynara_configuration_set_cache_size(conf, CACHE_SIZE); + if (err != CYNARA_API_SUCCESS) { + _E("Failed to set cynara cache"); + cynara_configuration_destroy(conf); + return; + } + + err = cynara_initialize(&cynara_env, conf); + cynara_configuration_destroy(conf); + + if (err != CYNARA_API_SUCCESS) { + _E("Failed to initialize cynara"); cynara_env = NULL; - _E("Cynara initialization failed"); + return; } + + _I("Cynara initialized"); } void permission_checker::deinit() diff --git a/src/server/permission_checker.h b/src/server/permission_checker.h index cf9f444..c7ccbb0 100644 --- a/src/server/permission_checker.h +++ b/src/server/permission_checker.h @@ -52,13 +52,15 @@ private: permission_checker(permission_checker const&) {}; permission_checker& operator=(permission_checker const&); - void init(); - void deinit(); + void init(void); + void deinit(void); private: permission_info_vector m_permission_infos; int m_permission_set; cmutex m_mutex; + + void init_cynara(void); }; #endif /* _PERMISSION_CHECKER_H_ */ -- 2.7.4 From 623b2709f08ef52a518cdc4b5ed68ce059d8b215 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Mon, 23 May 2016 15:46:30 +0900 Subject: [PATCH 14/16] sensord:fix compiler warnings due to the comparison of the signed and unsigned int -Wsign-compare Change-Id: I0f1b785c38aa3972a0b3fc249cf02f532d05d3e3 Signed-off-by: kibak.yoon --- src/server/server.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/server/server.cpp b/src/server/server.cpp index 184df3d..fb5882d 100644 --- a/src/server/server.cpp +++ b/src/server/server.cpp @@ -234,10 +234,10 @@ void server::close_socket(void) m_command_channel_accept_socket.close(); m_event_channel_accept_socket.close(); - for (int i = 0; i < client_command_sockets.size(); ++i) + for (unsigned int i = 0; i < client_command_sockets.size(); ++i) client_command_sockets[i].close(); - for (int i = 0; i < client_event_sockets.size(); ++i) + for (unsigned int i = 0; i < client_event_sockets.size(); ++i) client_event_sockets[i].close(); client_command_sockets.clear(); -- 2.7.4 From 6bf96b7ef2285273a6b546b47efa5a221ffc81a8 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Mon, 23 May 2016 15:44:33 +0900 Subject: [PATCH 15/16] sensord: version up 2.0.5 Change-Id: I7502e5b53f45ccfdd35a25b11df337f242730c37 Signed-off-by: kibak.yoon --- packaging/sensord.spec | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index c3f0488..d4eb4ef 100644 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -1,6 +1,6 @@ Name: sensord Summary: Sensor daemon -Version: 2.0.4 +Version: 2.0.5 Release: 0 Group: System/Sensor Framework License: Apache-2.0 -- 2.7.4 From c7bcb3067de0ce4d7039cb3e417d1089516bc894 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Mon, 23 May 2016 21:47:05 +0900 Subject: [PATCH 16/16] sensord: fix default interval when there is no client which registers event - POLL_MAX_HZ_MS is too big, so it has to be changed to POLL_10HZ_MS(default) Change-Id: I611c0983c601c02ab822b700efda4153dcb6a420 Signed-off-by: kibak.yoon --- src/client/sensor_handle_info.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/client/sensor_handle_info.cpp b/src/client/sensor_handle_info.cpp index 99647d7..69ccac4 100644 --- a/src/client/sensor_handle_info.cpp +++ b/src/client/sensor_handle_info.cpp @@ -130,7 +130,7 @@ void sensor_handle_info::get_batch(unsigned int &interval, unsigned int &latency { if (m_reg_event_infos.empty()) { _D("No events are registered for client %s", get_client_name()); - interval = POLL_MAX_HZ_MS; + interval = POLL_10HZ_MS; latency = 0; return; } -- 2.7.4