.project
.cproject
.*.swp
+nbproject
SET(RV "OFF")
SET(ORIENTATION "OFF")
ENDIF()
-SET(FUSION "OFF")
+SET(FUSION "ON")
SET(MOTION "OFF")
INCLUDE_DIRECTORIES(
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)
+++ /dev/null
-/*
- * 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 <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <errno.h>
-#include <math.h>
-#include <time.h>
-#include <sys/types.h>
-#include <dlfcn.h>
-#include <sensor_log.h>
-#include <fusion_sensor.h>
-#include <sensor_loader.h>
-#include <orientation_filter.h>
-#include <virtual_sensor_config.h>
-#include <algorithm>
-
-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<sensor_type_t> &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<sensor_event_t> &outs)
-{
- unsigned long long diff_time;
- euler_angles<float> 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<float> accel;
- sensor_data<float> gyro;
- sensor_data<float> magnetic;
-
- sensor_data_t accel_data;
- sensor_data_t gyro_data;
- sensor_data_t magnetic_data;
-
- euler_angles<float> 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;
-}
+++ /dev/null
-/*
- * 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 <sensor_internal.h>
-#include <virtual_sensor.h>
-#include <orientation_filter.h>
-
-class fusion_sensor : public virtual_sensor {
-public:
- fusion_sensor();
- virtual ~fusion_sensor();
-
- bool init(void);
-
- void synthesize(const sensor_event_t &event, vector<sensor_event_t> &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<sensor_type_t> &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<float> m_accel;
- sensor_data<float> m_gyro;
- sensor_data<float> m_magnetic;
-
- sensor_data<float> *m_accel_ptr;
- sensor_data<float> *m_gyro_ptr;
- sensor_data<float> *m_magnetic_ptr;
-
- cmutex m_value_mutex;
-
- orientation_filter<float> m_orientation_filter;
- orientation_filter<float> 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
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef __FUSION_H__
+#define __FUSION_H__
+
+#include <sensor_hal_types.h>
+
+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__ */
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <sensor_log.h>
+#include <sensor_loader.h>
+#include <sensor_base.h>
+#include <cmath>
+#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
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef __FUSION_BASE_H__
+#define __FUSION_BASE_H__
+
+#include <fusion.h>
+#include <orientation_filter.h>
+
+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<float> m_accel;
+ sensor_data<float> m_gyro;
+ sensor_data<float> m_magnetic;
+
+ orientation_filter<float> 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__ */
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include "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;
+}
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#ifndef __GYRO_FUSION_H__
+#define __GYRO_FUSION_H__
+
+#include <fusion_base.h>
+
+class gyro_fusion : public fusion_base {
+public:
+ gyro_fusion();
+ ~gyro_fusion();
+private:
+ bool get_orientation();
+};
+
+#endif /* __GYRO_FUSION_H__ */
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include "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;
+}
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#ifndef __GYRO_MAGNETIC_FUSION_H__
+#define __GYRO_MAGNETIC_FUSION_H__
+
+#include <fusion_base.h>
+
+class gyro_magnetic_fusion : public fusion_base {
+public:
+ gyro_magnetic_fusion();
+ ~gyro_magnetic_fusion();
+private:
+ bool get_orientation();
+};
+
+#endif /* __GYRO_MAGNETIC_FUSION_H__ */
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include "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;
+}
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#ifndef __MAGNETIC_FUSION_H__
+#define __MAGNETIC_FUSION_H__
+
+#include <fusion_base.h>
+
+class magnetic_fusion: public fusion_base {
+public:
+ magnetic_fusion();
+ ~magnetic_fusion();
+private:
+ bool get_orientation();
+};
+
+#endif /* __MAGNETIC_FUSION_H__ */
m_var_pitch = vec;
m_var_azimuth = vec;
- m_magnetic_alignment_factor = 1;
-
m_gyro.m_time_stamp = 0;
}
inline void orientation_filter<TYPE>::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<TYPE, V1x3S> acc_e(arr_acc_e);
vect<TYPE, V1x3S> mag_e(arr_mag_e);
euler_angles<TYPE> m_euler_error;
TYPE m_gyro_dt;
- int m_magnetic_alignment_factor;
-
orientation_filter(void);
~orientation_filter(void);
}
template<typename T>
-void pre_process_data(sensor_data<T> &data_out, const T *data_in, T *bias, int *sign, int scale)
+void pre_process_data(sensor_data<T> &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_ */
-
template<typename T> friend quaternion<T> sensor_data2quat(const sensor_data<T> data,
const vect<T, REF_VEC_SIZE> ref_vec);
template<typename T> friend void pre_process_data(sensor_data<T> &data_out,
- const T *data_in, T *bias, int *sign, int scale);
+ const T *data_in, int sign, int scale);
};
#include "sensor_data.cpp"
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);
projects / platform / core / system / sensord.git / commitdiff