From: akhilkedia94 Date: Fri, 29 Jul 2016 06:27:17 +0000 (+0900) Subject: Porting Sensor Fusion to Tizen 3.0 X-Git-Tag: accepted/tizen/3.0/ivi/20161011.062327^2~22 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=refs%2Fchanges%2F00%2F81900%2F9;p=platform%2Fcore%2Fsystem%2Fsensord.git Porting Sensor Fusion to Tizen 3.0 Change-Id: I8607fea652431cf277d7f9a577686c77318052bc Signed-off-by: akhilkedia94 --- diff --git a/.gitignore b/.gitignore index 8b1529d..ca9cbd7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ .project .cproject .*.swp +nbproject diff --git a/src/sensor/CMakeLists.txt b/src/sensor/CMakeLists.txt index 7328a1d..0547bc8 100644 --- a/src/sensor/CMakeLists.txt +++ b/src/sensor/CMakeLists.txt @@ -14,7 +14,7 @@ ELSE() SET(RV "OFF") SET(ORIENTATION "OFF") ENDIF() -SET(FUSION "OFF") +SET(FUSION "ON") SET(MOTION "OFF") INCLUDE_DIRECTORIES( @@ -64,7 +64,8 @@ IF("${RV}" STREQUAL "ON") SET(SENSOR_DEFINITIONS ${SENSOR_DEFINITIONS} "-DENABLE_ROTATION_VECTOR") ENDIF() IF("${FUSION}" STREQUAL "ON") -add_subdirectory(fusion) +FILE(GLOB SENSOR_SRCS ${SENSOR_SRCS} ${CMAKE_CURRENT_SOURCE_DIR}/sensor_fusion/*.cpp) +SET(SENSOR_HEADERS ${SENSOR_HEADERS} ${CMAKE_CURRENT_SOURCE_DIR}/sensor_fusion) ENDIF() IF("${MOTION}" STREQUAL "ON") add_subdirectory(motion) diff --git a/src/sensor/fusion/fusion_sensor.cpp b/src/sensor/fusion/fusion_sensor.cpp deleted file mode 100644 index ff56ead..0000000 --- a/src/sensor/fusion/fusion_sensor.cpp +++ /dev/null @@ -1,440 +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 -#include - -using std::string; -using std::vector; - -#define SENSOR_NAME "FUSION_SENSOR" -#define SENSOR_TYPE_FUSION "FUSION" - -#define ACCELEROMETER_ENABLED 0x01 -#define GYROSCOPE_ENABLED 0x02 -#define GEOMAGNETIC_ENABLED 0x04 -#define TILT_ENABLED 1 -#define GAMING_RV_ENABLED 3 -#define GEOMAGNETIC_RV_ENABLED 5 -#define ORIENTATION_ENABLED 7 -#define ROTATION_VECTOR_ENABLED 7 -#define GYROSCOPE_UNCAL_ENABLED 7 - -#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_ACCEL_STATIC_BIAS "ACCEL_STATIC_BIAS" -#define ELEMENT_GYRO_STATIC_BIAS "GYRO_STATIC_BIAS" -#define ELEMENT_GEOMAGNETIC_STATIC_BIAS "GEOMAGNETIC_STATIC_BIAS" -#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" -#define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION "GYRO_ROTATION_DIRECTION_COMPENSATION" -#define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION" -#define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR "MAGNETIC_ALIGNMENT_FACTOR" -#define ELEMENT_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION" -#define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION" -#define ELEMENT_AZIMUTH_ROTATION_COMPENSATION "AZIMUTH_ROTATION_COMPENSATION" - -fusion_sensor::fusion_sensor() -: m_accel_sensor(NULL) -, m_gyro_sensor(NULL) -, m_magnetic_sensor(NULL) -, m_time(0) -{ - virtual_sensor_config &config = virtual_sensor_config::get_instance(); - m_name = string(SENSOR_NAME); - m_enable_fusion = 0; - - if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_VENDOR, m_vendor)) { - _E("[VENDOR] is empty\n"); - throw ENXIO; - } - - _I("m_vendor = %s", m_vendor.c_str()); - - if (!config.get(SENSOR_TYPE_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, ELEMENT_GEOMAGNETIC_STATIC_BIAS, m_geomagnetic_static_bias, 3)) { - _E("[GEOMAGNETIC_STATIC_BIAS] is empty\n"); - throw ENXIO; - } - - _I("m_geomagnetic_static_bias = (%f, %f, %f)", m_geomagnetic_static_bias[0], m_geomagnetic_static_bias[1], m_geomagnetic_static_bias[2]); - - if (!config.get(SENSOR_TYPE_FUSION, 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_FUSION, 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]); - - if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION, m_geomagnetic_rotation_direction_compensation, 3)) { - _E("[GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_geomagnetic_rotation_direction_compensation = (%d, %d, %d)", m_geomagnetic_rotation_direction_compensation[0], m_geomagnetic_rotation_direction_compensation[1], m_geomagnetic_rotation_direction_compensation[2]); - - if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_MAGNETIC_ALIGNMENT_FACTOR, &m_magnetic_alignment_factor)) { - _E("[MAGNETIC_ALIGNMENT_FACTOR] is empty\n"); - throw ENXIO; - } - - _I("m_magnetic_alignment_factor = %d", m_magnetic_alignment_factor); - - m_interval = m_default_sampling_time * MS_TO_US; - - m_accel_ptr = m_gyro_ptr = m_magnetic_ptr = NULL; -} - -fusion_sensor::~fusion_sensor() -{ - _I("fusion_sensor is destroyed!\n"); -} - -bool fusion_sensor::init(void) -{ - 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); - - if (!m_accel_sensor) { - _E("Failed to load accel sensor: %#x", m_accel_sensor); - return false; - } - - if (!m_gyro_sensor) - _I("Failed to load gyro sensor: %#x", m_gyro_sensor); - - if (!m_magnetic_sensor) - _I("Failed to load geomagnetic sensor: %#x", m_magnetic_sensor); - - _I("%s is created!", sensor_base::get_name()); - return true; -} - -void fusion_sensor::get_types(vector &types) -{ - types.push_back(FUSION_SENSOR); -} - -bool fusion_sensor::on_start(void) -{ - AUTOLOCK(m_mutex); - activate(); - return true; -} - -bool fusion_sensor::on_stop(void) -{ - AUTOLOCK(m_mutex); - deactivate(); - return true; -} - -bool fusion_sensor::add_interval(int client_id, unsigned int interval) -{ - bool retval; - - AUTOLOCK(m_mutex); - retval = sensor_base::add_interval(client_id, interval, false); - - m_interval = sensor_base::get_interval(client_id, false); - - if (m_interval != 0) - retval = true; - - return retval; -} - -bool fusion_sensor::delete_interval(int client_id) -{ - bool retval; - - AUTOLOCK(m_mutex); - retval = sensor_base::delete_interval(client_id, false); - - m_interval = sensor_base::get_interval(client_id, false); - - if (m_interval != 0) - retval = true; - - return retval; -} - -void fusion_sensor::synthesize(const sensor_event_t &event, vector &outs) -{ - unsigned long long diff_time; - euler_angles euler_orientation; - - 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_accel_ptr = &m_accel; - - m_enable_fusion |= ACCELEROMETER_ENABLED; - } - - if (sensor_base::is_supported(FUSION_ORIENTATION_ENABLED) || - sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED) || - sensor_base::is_supported(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) || - sensor_base::is_supported(FUSION_GYROSCOPE_UNCAL_ENABLED)) { - if (event.event_type == GEOMAGNETIC_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_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, GEOMAGNETIC_SCALE); - - m_magnetic.m_time_stamp = event.data.timestamp; - - m_magnetic_ptr = &m_magnetic; - - m_enable_fusion |= GEOMAGNETIC_ENABLED; - } - } - - if (sensor_base::is_supported(FUSION_ORIENTATION_ENABLED) || - sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED) || - sensor_base::is_supported(FUSION_GAMING_ROTATION_VECTOR_ENABLED) || - sensor_base::is_supported(FUSION_GYROSCOPE_UNCAL_ENABLED)) { - 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_gyro_ptr = &m_gyro; - - m_enable_fusion |= GYROSCOPE_ENABLED; - } - } - - if ((m_enable_fusion == TILT_ENABLED && sensor_base::is_supported(FUSION_TILT_ENABLED)) || - (m_enable_fusion == ORIENTATION_ENABLED && sensor_base::is_supported(FUSION_ORIENTATION_ENABLED)) || - (m_enable_fusion == ROTATION_VECTOR_ENABLED && sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED)) || - (m_enable_fusion == GAMING_RV_ENABLED && sensor_base::is_supported(FUSION_GAMING_ROTATION_VECTOR_ENABLED)) || - (m_enable_fusion == GEOMAGNETIC_RV_ENABLED && sensor_base::is_supported(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED)) || - (m_enable_fusion == GYROSCOPE_UNCAL_ENABLED && sensor_base::is_supported(FUSION_GYROSCOPE_UNCAL_ENABLED))) { - sensor_event_t fusion_event; - - m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - - m_orientation_filter.get_device_orientation(m_accel_ptr, m_gyro_ptr, m_magnetic_ptr); - - if (m_enable_fusion == GYROSCOPE_UNCAL_ENABLED && sensor_base::is_supported(FUSION_GYROSCOPE_UNCAL_ENABLED)) { - m_time = get_timestamp(); - fusion_event.sensor_id = get_id(); - fusion_event.data.timestamp = m_time; - fusion_event.data.accuracy = SENSOR_ACCURACY_GOOD; - fusion_event.event_type = FUSION_GYROSCOPE_UNCAL_EVENT; - fusion_event.data.value_count = 3; - fusion_event.data.values[0] = m_orientation_filter.m_gyro_bias.m_vec[0]; - fusion_event.data.values[1] = m_orientation_filter.m_gyro_bias.m_vec[1]; - fusion_event.data.values[2] = m_orientation_filter.m_gyro_bias.m_vec[2]; - - push(fusion_event); - } - - if ((m_enable_fusion == TILT_ENABLED && sensor_base::is_supported(FUSION_TILT_ENABLED)) || - (m_enable_fusion == ORIENTATION_ENABLED && sensor_base::is_supported(FUSION_ORIENTATION_ENABLED)) || - (m_enable_fusion == ROTATION_VECTOR_ENABLED && sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED)) || - (m_enable_fusion == GAMING_RV_ENABLED && sensor_base::is_supported(FUSION_GAMING_ROTATION_VECTOR_ENABLED)) || - (m_enable_fusion == GEOMAGNETIC_RV_ENABLED && sensor_base::is_supported(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED))) { - m_time = get_timestamp(); - fusion_event.sensor_id = get_id(); - fusion_event.data.timestamp = m_time; - fusion_event.data.accuracy = SENSOR_ACCURACY_GOOD; - fusion_event.event_type = FUSION_EVENT; - fusion_event.data.value_count = 4; - fusion_event.data.values[0] = m_orientation_filter.m_quaternion.m_quat.m_vec[0]; - fusion_event.data.values[1] = m_orientation_filter.m_quaternion.m_quat.m_vec[1]; - fusion_event.data.values[2] = m_orientation_filter.m_quaternion.m_quat.m_vec[2]; - fusion_event.data.values[3] = m_orientation_filter.m_quaternion.m_quat.m_vec[3]; - - push(fusion_event); - } - - m_enable_fusion = 0; - m_accel_ptr = m_gyro_ptr = m_magnetic_ptr = NULL; - } - - return; -} - -int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) -{ - sensor_data accel; - sensor_data gyro; - sensor_data magnetic; - - sensor_data_t accel_data; - sensor_data_t gyro_data; - sensor_data_t magnetic_data; - - euler_angles euler_orientation; - - if (event_type != FUSION_ORIENTATION_ENABLED && - event_type != FUSION_ROTATION_VECTOR_ENABLED && - event_type != FUSION_GAMING_ROTATION_VECTOR_ENABLED && - event_type != FUSION_TILT_ENABLED && - event_type != FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED && - event_type != FUSION_GYROSCOPE_UNCAL_ENABLED) - return -1; - - m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); - pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, ACCEL_SCALE); - accel.m_time_stamp = accel_data.timestamp; - - if (event_type == FUSION_ORIENTATION_ENABLED || - event_type == FUSION_ROTATION_VECTOR_ENABLED || - event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED || - event_type == FUSION_GYROSCOPE_UNCAL_ENABLED) - { - m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data); - pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, GYRO_SCALE); - gyro.m_time_stamp = gyro_data.timestamp; - } - - if (event_type == FUSION_ORIENTATION_ENABLED || - event_type == FUSION_ROTATION_VECTOR_ENABLED || - event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED || - event_type == FUSION_GYROSCOPE_UNCAL_ENABLED) - { - m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_RAW_DATA_EVENT, magnetic_data); - pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, GEOMAGNETIC_SCALE); - magnetic.m_time_stamp = magnetic_data.timestamp; - } - - m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - - if (event_type == FUSION_ORIENTATION_ENABLED || - event_type == FUSION_ROTATION_VECTOR_ENABLED || - event_type == FUSION_GYROSCOPE_UNCAL_ENABLED) - m_orientation_filter_poll.get_device_orientation(&accel, &gyro, &magnetic); - else if (event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED) - m_orientation_filter_poll.get_device_orientation(&accel, &gyro, NULL); - else if (event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) - m_orientation_filter_poll.get_device_orientation(&accel, NULL, &magnetic); - else if (event_type == FUSION_TILT_ENABLED) - m_orientation_filter_poll.get_device_orientation(&accel, NULL, NULL); - - if (event_type == FUSION_GYROSCOPE_UNCAL_ENABLED) { - data.accuracy = SENSOR_ACCURACY_GOOD; - data.timestamp = get_timestamp(); - data.value_count = 3; - data.values[0] = m_orientation_filter_poll.m_gyro_bias.m_vec[0]; - data.values[1] = m_orientation_filter_poll.m_gyro_bias.m_vec[1]; - data.values[2] = m_orientation_filter_poll.m_gyro_bias.m_vec[2]; - } else if (event_type == FUSION_ORIENTATION_ENABLED || - event_type == FUSION_ROTATION_VECTOR_ENABLED || - event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED || - event_type == FUSION_TILT_ENABLED || - event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) { - data.accuracy = SENSOR_ACCURACY_GOOD; - data.timestamp = get_timestamp(); - data.value_count = 4; - data.values[0] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[0]; - data.values[1] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[1]; - data.values[2] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[2]; - data.values[3] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[3]; - } - - return 0; -} - -bool fusion_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties) -{ - properties.min_range = 0; - properties.max_range = 0; - properties.resolution = 0; - properties.vendor = m_vendor; - properties.name = SENSOR_NAME; - properties.min_interval = 0; - properties.fifo_count = 0; - properties.max_batch_count = 0; - - return true; -} diff --git a/src/sensor/fusion/fusion_sensor.h b/src/sensor/fusion/fusion_sensor.h deleted file mode 100644 index 4985f15..0000000 --- a/src/sensor/fusion/fusion_sensor.h +++ /dev/null @@ -1,81 +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 _FUSION_SENSOR_H_ -#define _FUSION_SENSOR_H_ - -#include -#include -#include - -class fusion_sensor : public virtual_sensor { -public: - fusion_sensor(); - virtual ~fusion_sensor(); - - bool init(void); - - void synthesize(const sensor_event_t &event, 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 data_id, sensor_data_t &data); - -private: - sensor_base *m_accel_sensor; - sensor_base *m_gyro_sensor; - sensor_base *m_magnetic_sensor; - - sensor_data m_accel; - sensor_data m_gyro; - sensor_data m_magnetic; - - sensor_data *m_accel_ptr; - sensor_data *m_gyro_ptr; - sensor_data *m_magnetic_ptr; - - cmutex m_value_mutex; - - orientation_filter m_orientation_filter; - orientation_filter m_orientation_filter_poll; - - unsigned int m_enable_fusion; - - 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]; - float m_geomagnetic_static_bias[3]; - int m_accel_rotation_direction_compensation[3]; - int m_gyro_rotation_direction_compensation[3]; - int m_geomagnetic_rotation_direction_compensation[3]; - int m_magnetic_alignment_factor; - - bool on_start(void); - bool on_stop(void); -}; - -#endif diff --git a/src/sensor/fusion/hardware_fusion_sensor.html b/src/sensor/sensor_fusion/design/documentation/hardware_fusion_sensor.html similarity index 100% rename from src/sensor/fusion/hardware_fusion_sensor.html rename to src/sensor/sensor_fusion/design/documentation/hardware_fusion_sensor.html diff --git a/src/sensor/sensor_fusion/fusion.h b/src/sensor/sensor_fusion/fusion.h new file mode 100644 index 0000000..d53f800 --- /dev/null +++ b/src/sensor/sensor_fusion/fusion.h @@ -0,0 +1,37 @@ +/* + * 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 __FUSION_H__ +#define __FUSION_H__ + +#include + +class fusion { +public: + fusion() {}; + virtual ~fusion() {} ; + + virtual void push_accel(sensor_data_t &data) = 0; + virtual void push_gyro(sensor_data_t &data) = 0; + virtual void push_mag(sensor_data_t &data) = 0; + virtual bool get_rv(unsigned long long timestamp, float &w, float &x, float &y, float &z) = 0; +}; + + + +#endif /* __FUSION_H__ */ diff --git a/src/sensor/sensor_fusion/fusion_base.cpp b/src/sensor/sensor_fusion/fusion_base.cpp new file mode 100644 index 0000000..92e02f6 --- /dev/null +++ b/src/sensor/sensor_fusion/fusion_base.cpp @@ -0,0 +1,105 @@ +/* + * 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 "fusion_base.h" +#include "orientation_filter.h" + +#define ACCEL_COMPENSATION -1 +#define GYRO_COMPENSATION 1 +#define MAG_COMPENSATION -1 + +fusion_base::fusion_base() +: m_enable_accel(false) +, m_enable_gyro(false) +, m_enable_magnetic(false) +{ + _I("fusion_base is created!"); +} + +fusion_base::~fusion_base() +{ + _I("fusion_sensor is destroyed!"); +} + +void fusion_base::clear(void) +{ + m_enable_accel = false; + m_enable_gyro = false; + m_enable_magnetic = false; +} + +void fusion_base::push_accel(sensor_data_t &data) +{ + //_I("[fusion_sensor] : Pushing accel"); + pre_process_data(m_accel, data.values, ACCEL_COMPENSATION, ACCEL_SCALE); + m_accel.m_time_stamp = data.timestamp; + m_enable_accel = true; + if (get_orientation()) + store_orientation(); +} + +void fusion_base::push_gyro(sensor_data_t &data) +{ + //_I("[fusion_sensor] : Pushing mag"); + pre_process_data(m_gyro, data.values, GYRO_COMPENSATION, GYRO_SCALE); + m_gyro.m_time_stamp = data.timestamp; + m_enable_gyro = true; + if (get_orientation()) + store_orientation(); +} + +void fusion_base::push_mag(sensor_data_t &data) +{ + //_I("[fusion_sensor] : Pushing gyro"); + pre_process_data(m_magnetic, data.values, MAG_COMPENSATION, GEOMAGNETIC_SCALE); + m_magnetic.m_time_stamp = data.timestamp; + m_enable_magnetic = true; + if (get_orientation()) + store_orientation(); + +} + +bool fusion_base::get_rv(unsigned long long timestamp, float &x, float &y, float &z, float &w) +{ + if (m_timestamp == 0) + return false; + timestamp = m_timestamp; + x = m_x; + y = m_y; + z = m_z; + w = m_w; + return true; +} + +void fusion_base::store_orientation(void) +{ + m_x = m_orientation_filter.m_quaternion.m_quat.m_vec[0]; + m_y = m_orientation_filter.m_quaternion.m_quat.m_vec[1]; + m_z = m_orientation_filter.m_quaternion.m_quat.m_vec[2]; + m_w = m_orientation_filter.m_quaternion.m_quat.m_vec[3]; + clear(); +} \ No newline at end of file diff --git a/src/sensor/sensor_fusion/fusion_base.h b/src/sensor/sensor_fusion/fusion_base.h new file mode 100644 index 0000000..2428712 --- /dev/null +++ b/src/sensor/sensor_fusion/fusion_base.h @@ -0,0 +1,60 @@ +/* + * 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 __FUSION_BASE_H__ +#define __FUSION_BASE_H__ + +#include +#include + +class fusion_base : public virtual fusion { +public: + fusion_base(); + virtual ~fusion_base(); + + virtual void push_accel(sensor_data_t &data); + virtual void push_gyro(sensor_data_t &data); + virtual void push_mag(sensor_data_t &data); + virtual bool get_rv(unsigned long long timestamp, float &w, float &x, float &y, float &z); + +protected: + + sensor_data m_accel; + sensor_data m_gyro; + sensor_data m_magnetic; + + orientation_filter m_orientation_filter; + + bool m_enable_accel; + bool m_enable_gyro; + bool m_enable_magnetic; + + float m_x; + float m_y; + float m_z; + float m_w; + float m_timestamp; + + void clear(); + void store_orientation(void); + virtual bool get_orientation(void) = 0; +}; + + + +#endif /* __FUSION_BASE_H__ */ diff --git a/src/sensor/sensor_fusion/gyro_fusion.cpp b/src/sensor/sensor_fusion/gyro_fusion.cpp new file mode 100644 index 0000000..a668e65 --- /dev/null +++ b/src/sensor/sensor_fusion/gyro_fusion.cpp @@ -0,0 +1,39 @@ +/* + * 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 "gyro_fusion.h" + +gyro_fusion::gyro_fusion() +{ +} + +gyro_fusion::~gyro_fusion() +{ +} + +bool gyro_fusion::get_orientation(void) +{ + //_I("[fusion_sensor] : enable values are %d %d %d", m_enable_accel, m_enable_gyro, m_enable_magnetic); + if (!m_enable_accel || !m_enable_gyro) + return false; + + m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, NULL); + m_timestamp = fmax(m_accel.m_time_stamp, m_gyro.m_time_stamp); + return true; +} diff --git a/src/sensor/sensor_fusion/gyro_fusion.h b/src/sensor/sensor_fusion/gyro_fusion.h new file mode 100644 index 0000000..0089e0d --- /dev/null +++ b/src/sensor/sensor_fusion/gyro_fusion.h @@ -0,0 +1,33 @@ +/* + * 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 __GYRO_FUSION_H__ +#define __GYRO_FUSION_H__ + +#include + +class gyro_fusion : public fusion_base { +public: + gyro_fusion(); + ~gyro_fusion(); +private: + bool get_orientation(); +}; + +#endif /* __GYRO_FUSION_H__ */ diff --git a/src/sensor/sensor_fusion/gyro_magnetic_fusion.cpp b/src/sensor/sensor_fusion/gyro_magnetic_fusion.cpp new file mode 100644 index 0000000..0d1f32d --- /dev/null +++ b/src/sensor/sensor_fusion/gyro_magnetic_fusion.cpp @@ -0,0 +1,40 @@ +/* + * 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 "gyro_magnetic_fusion.h" + +gyro_magnetic_fusion::gyro_magnetic_fusion() +{ +} + +gyro_magnetic_fusion::~gyro_magnetic_fusion() +{ +} + +bool gyro_magnetic_fusion::get_orientation(void) +{ + //_I("[fusion_sensor] : enable values are %d %d %d", m_enable_accel, m_enable_gyro, m_enable_magnetic); + if (!m_enable_accel || !m_enable_gyro || !m_enable_magnetic) + return false; + + m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, &m_magnetic); + m_timestamp = fmax(m_accel.m_time_stamp, m_gyro.m_time_stamp); + m_timestamp = fmax(m_timestamp, m_magnetic.m_time_stamp); + return true; +} diff --git a/src/sensor/sensor_fusion/gyro_magnetic_fusion.h b/src/sensor/sensor_fusion/gyro_magnetic_fusion.h new file mode 100644 index 0000000..54bf75c --- /dev/null +++ b/src/sensor/sensor_fusion/gyro_magnetic_fusion.h @@ -0,0 +1,33 @@ +/* + * 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 __GYRO_MAGNETIC_FUSION_H__ +#define __GYRO_MAGNETIC_FUSION_H__ + +#include + +class gyro_magnetic_fusion : public fusion_base { +public: + gyro_magnetic_fusion(); + ~gyro_magnetic_fusion(); +private: + bool get_orientation(); +}; + +#endif /* __GYRO_MAGNETIC_FUSION_H__ */ diff --git a/src/sensor/sensor_fusion/magnetic_fusion.cpp b/src/sensor/sensor_fusion/magnetic_fusion.cpp new file mode 100644 index 0000000..89050e5 --- /dev/null +++ b/src/sensor/sensor_fusion/magnetic_fusion.cpp @@ -0,0 +1,39 @@ +/* + * 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 "magnetic_fusion.h" + +magnetic_fusion::magnetic_fusion() +{ +} + +magnetic_fusion::~magnetic_fusion() +{ +} + +bool magnetic_fusion::get_orientation(void) +{ + //_I("[fusion_sensor] : enable values are %d %d %d", m_enable_accel, m_enable_magnetic, m_enable_magnetic); + if (!m_enable_accel || !m_enable_magnetic) + return false; + + m_orientation_filter.get_device_orientation(&m_accel, &m_magnetic, NULL); + m_timestamp = fmax(m_accel.m_time_stamp, m_magnetic.m_time_stamp); + return true; +} diff --git a/src/sensor/sensor_fusion/magnetic_fusion.h b/src/sensor/sensor_fusion/magnetic_fusion.h new file mode 100644 index 0000000..41758ea --- /dev/null +++ b/src/sensor/sensor_fusion/magnetic_fusion.h @@ -0,0 +1,33 @@ +/* + * 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 __MAGNETIC_FUSION_H__ +#define __MAGNETIC_FUSION_H__ + +#include + +class magnetic_fusion: public fusion_base { +public: + magnetic_fusion(); + ~magnetic_fusion(); +private: + bool get_orientation(); +}; + +#endif /* __MAGNETIC_FUSION_H__ */ diff --git a/src/sensor/sensor_fusion/orientation_filter.cpp b/src/sensor/sensor_fusion/orientation_filter.cpp index 7e1cd8d..be41328 100644 --- a/src/sensor/sensor_fusion/orientation_filter.cpp +++ b/src/sensor/sensor_fusion/orientation_filter.cpp @@ -64,8 +64,6 @@ orientation_filter::orientation_filter() m_var_pitch = vec; m_var_azimuth = vec; - m_magnetic_alignment_factor = 1; - m_gyro.m_time_stamp = 0; } @@ -106,7 +104,7 @@ template inline void orientation_filter::orientation_triad_algorithm() { TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0}; - TYPE arr_mag_e[V1x3S] = {0.0, (TYPE) m_magnetic_alignment_factor, 0.0}; + TYPE arr_mag_e[V1x3S] = {0.0, 1.0, 0.0}; vect acc_e(arr_acc_e); vect mag_e(arr_mag_e); diff --git a/src/sensor/sensor_fusion/orientation_filter.h b/src/sensor/sensor_fusion/orientation_filter.h index d55b7c1..eaea224 100644 --- a/src/sensor/sensor_fusion/orientation_filter.h +++ b/src/sensor/sensor_fusion/orientation_filter.h @@ -73,8 +73,6 @@ public: euler_angles m_euler_error; TYPE m_gyro_dt; - int m_magnetic_alignment_factor; - orientation_filter(void); ~orientation_filter(void); diff --git a/src/sensor/sensor_fusion/sensor_data.cpp b/src/sensor/sensor_fusion/sensor_data.cpp index 9f730cc..abd1181 100644 --- a/src/sensor/sensor_fusion/sensor_data.cpp +++ b/src/sensor/sensor_fusion/sensor_data.cpp @@ -117,12 +117,11 @@ quaternion sensor_data2quat(const sensor_data data, const vect -void pre_process_data(sensor_data &data_out, const T *data_in, T *bias, int *sign, int scale) +void pre_process_data(sensor_data &data_out, const T *data_in, int sign, int scale) { - data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale; - data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale; - data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale; + data_out.m_data.m_vec[0] = sign * data_in[0] / scale; + data_out.m_data.m_vec[1] = sign * data_in[1] / scale; + data_out.m_data.m_vec[2] = sign * data_in[2] / scale; } #endif /* _SENSOR_DATA_H_ */ - diff --git a/src/sensor/sensor_fusion/sensor_data.h b/src/sensor/sensor_fusion/sensor_data.h index 6841369..f266760 100644 --- a/src/sensor/sensor_fusion/sensor_data.h +++ b/src/sensor/sensor_fusion/sensor_data.h @@ -50,7 +50,7 @@ public: template friend quaternion sensor_data2quat(const sensor_data data, const vect ref_vec); template friend void pre_process_data(sensor_data &data_out, - const T *data_in, T *bias, int *sign, int scale); + const T *data_in, int sign, int scale); }; #include "sensor_data.cpp" diff --git a/src/sensor/sensor_fusion/test/orientation_sensor.cpp b/src/sensor/sensor_fusion/test/orientation_sensor.cpp index a67627e..25099e2 100644 --- a/src/sensor/sensor_fusion/test/orientation_sensor.cpp +++ b/src/sensor/sensor_fusion/test/orientation_sensor.cpp @@ -45,8 +45,6 @@ void orientation_sensor::get_device_orientation(sensor_data *accel_data, if (magnetic_data != NULL) { pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); normalize(*magnetic_data); - - orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor; } orien_filter.get_device_orientation(accel_data, gyro_data, magnetic_data);