From b35f82121ecf4fac092e77eef13d547fcb6f1064 Mon Sep 17 00:00:00 2001 From: Ankur Date: Thu, 9 Apr 2015 17:27:40 +0530 Subject: [PATCH 01/16] Addition of Test case for fusion data collection This test case registers accelerometer, gyroscope, magnetometer and proximity sensors from one single client. This is useful for collecting data from these physical sensors for any particular motion. Change-Id: Ie2c866c5ec624290f9988b17ee33ea92bc8752ca --- packaging/sensord.spec | 1 + test/CMakeLists.txt | 4 ++ test/src/fusion-data-collection.c | 125 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 130 insertions(+) create mode 100644 test/src/fusion-data-collection.c diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 0f3c296..48c8cf1 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -141,5 +141,6 @@ systemctl daemon-reload %{_bindir}/api-test %{_bindir}/sensor-test %{_bindir}/performance-test +%{_bindir}/fusion-data-collection %license LICENSE.APLv2 %endif diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 8716991..81213f1 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -28,15 +28,19 @@ link_directories(../src/libsensord/) add_executable(api-test src/api-test.c) add_executable(sensor-test src/sensor-test.c src/check-sensor.c) add_executable(performance-test src/performance-test.c src/check-sensor.c) +add_executable(fusion-data-collection src/fusion-data-collection.c) SET_TARGET_PROPERTIES(api-test PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(sensor-test PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(performance-test PROPERTIES LINKER_LANGUAGE C) +SET_TARGET_PROPERTIES(fusion-data-collection PROPERTIES LINKER_LANGUAGE C) target_link_libraries(api-test glib-2.0 dlog sensor) target_link_libraries(sensor-test glib-2.0 dlog sensor) target_link_libraries(performance-test glib-2.0 dlog sensor) +target_link_libraries(fusion-data-collection glib-2.0 dlog sensor) INSTALL(TARGETS api-test DESTINATION /usr/bin/) INSTALL(TARGETS sensor-test DESTINATION /usr/bin/) INSTALL(TARGETS performance-test DESTINATION /usr/bin/) +INSTALL(TARGETS fusion-data-collection DESTINATION /usr/bin/) diff --git a/test/src/fusion-data-collection.c b/test/src/fusion-data-collection.c new file mode 100644 index 0000000..62e63f8 --- /dev/null +++ b/test/src/fusion-data-collection.c @@ -0,0 +1,125 @@ +/* + * sensord + * + * Copyright (c) 2014-15 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 + +#define MAXSIZE 4 + +static GMainLoop *mainloop; +FILE* file_output[MAXSIZE]; + +void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data) +{ + sensor_type_t sensor_type = event_type >> 16; + + switch (sensor_type) { + case ACCELEROMETER_SENSOR: + fprintf(file_output[0], "%6.6f %6.6f %6.6f %lld\n", data->values[0], data->values[1], data->values[2], data->timestamp); + break; + case GEOMAGNETIC_SENSOR: + fprintf(file_output[1], "%6.6f %6.6f %6.6f %lld\n", data->values[0], data->values[1], data->values[2], data->timestamp); + break; + case GYROSCOPE_SENSOR: + fprintf(file_output[2], "%6.6f %6.6f %6.6f %lld\n", data->values[0], data->values[1], data->values[2], data->timestamp); + break; + case PROXIMITY_SENSOR: + fprintf(file_output[MAXSIZE-1], "%6.6f %lld\n", data->values[0], data->timestamp); + break; + default: + return; + } +} + +void usage() +{ + printf("Usage : ./one-client-multiple-sensors \n\n"); + + printf("interval:\n"); + printf("The sampling interval in ms.\n"); + exit(-1); +} + +int main(int argc, char **argv) +{ + int interval; + + if (argc == 2) { + interval = atoi(argv[1]); + if (interval <= 0) + usage(); + } + else + usage(); + + int i; + + int handle[MAXSIZE]; + int result[MAXSIZE], start_handle[MAXSIZE], stop_handle[MAXSIZE]; + unsigned int event[MAXSIZE]; + int sensors[MAXSIZE]; + + sensors[0] = ACCELEROMETER_SENSOR; + sensors[1] = GEOMAGNETIC_SENSOR; + sensors[2] = GYROSCOPE_SENSOR; + sensors[MAXSIZE-1] = PROXIMITY_SENSOR; + + mainloop = g_main_loop_new(NULL, FALSE); + + char file_name[50]; + + for (i = 0; i < MAXSIZE; i++) { + sprintf(file_name, "output_%d", sensors[i]); + file_output[i] = fopen(file_name, "w+"); + sensor_t sensor = sensord_get_sensor(sensors[i]); + handle[i] = sensord_connect(sensor); + event[i] = (sensors[i] << 16) | 0x0001; + result[i] = sensord_register_event(handle[i], event[i], interval, 0, callback, NULL); + + if (result[i] < 0) { + printf("error: unable to register sensor\n"); + return -1; + } + start_handle[i] = sensord_start(handle[i], 1); + + if (start_handle[i] < 0) { + printf("error: unable to start handle\n"); + sensord_unregister_event(handle[i], event[i]); + sensord_disconnect(handle[i]); + return -1; + } + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); + + for (i = 0; i < MAXSIZE; i++) { + sensord_unregister_event(handle[i], event[i]); + sensord_stop(handle[i]); + sensord_disconnect(handle[i]); + } + + return 0; +} -- 2.7.4 From 5bfb197853878c3dd31072a612c1c64c6a7a4adc Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 15 May 2015 18:48:26 +0900 Subject: [PATCH 02/16] Adding support for computing tilt quaternion based on accelerometer - support for sensor fusion algorithm to compute tilt quaternion Change-Id: I32135f270acc4eb8de1811e6f29182614dd7ce54 --- src/sensor_fusion/orientation_filter.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index ae88a46..c3a6c92 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -371,6 +371,12 @@ void orientation_filter::get_device_orientation(const sensor_data *a m_quaternion = m_quat_9axis; + } else if (!gyro && !magnetic) { + + compute_accel_orientation(); + + m_quaternion = m_quat_aid; + } else if (!gyro) { orientation_triad_algorithm(); -- 2.7.4 From d104d0ac5b4772c105642ef6304809ce92808b59 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 15 May 2015 19:00:26 +0900 Subject: [PATCH 03/16] Adding Tilt Sensor and related files - Adding tilt sensor based on accelerometer only - uses intermediate quaternion output of sensor fusion - Used in Tizen Z1 for computing limited device orientation - Adding all api, build files related to tilt Change-Id: I7682da9238ca61e9d8fc1b392fb14afc33406d2d --- packaging/sensord.spec | 5 +- sensor_plugins.xml.in | 1 + src/CMakeLists.txt | 11 +- src/fusion/fusion_sensor.cpp | 4 +- src/libsensord/CMakeLists.txt | 1 + src/libsensord/client_common.cpp | 2 + src/libsensord/sensor_fusion.h | 13 +- src/libsensord/sensor_internal.h | 1 + src/libsensord/sensor_internal_deprecated.h | 1 + src/shared/sensor_common.h | 1 + src/tilt/CMakeLists.txt | 24 +++ src/tilt/tilt_sensor.cpp | 311 ++++++++++++++++++++++++++++ src/tilt/tilt_sensor.h | 69 ++++++ virtual_sensors.xml | 27 +++ 14 files changed, 459 insertions(+), 12 deletions(-) create mode 100755 src/tilt/CMakeLists.txt create mode 100755 src/tilt/tilt_sensor.cpp create mode 100755 src/tilt/tilt_sensor.h diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 48c8cf1..499c99e 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -30,7 +30,7 @@ BuildRequires: pkgconfig(capi-system-info) %define rv_state ON %define geomagnetic_rv_state ON %define gaming_rv_state ON -%define fusion_state ON +%define tilt_state ON %define build_test_suite OFF %description @@ -82,7 +82,7 @@ cmake . -DCMAKE_INSTALL_PREFIX=%{_prefix} -DACCEL=%{accel_state} \ -DORIENTATION=%{orientation_state} -DGRAVITY=%{gravity_state} \ -DLINEAR_ACCEL=%{linear_accel_state} -DRV=%{rv_state} \ -DGEOMAGNETIC_RV=%{geomagnetic_rv_state} -DGAMING_RV=%{gaming_rv_state} \ - -DFUSION=%{fusion_state} -DTEST_SUITE=%{build_test_suite} \ + -DTILT=%{tilt_state} -DTEST_SUITE=%{build_test_suite} \ -DLIBDIR=%{_libdir} -DINCLUDEDIR=%{_includedir} %build @@ -134,6 +134,7 @@ systemctl daemon-reload %{_libdir}/pkgconfig/sensor.pc %{_libdir}/pkgconfig/sf_common.pc %{_libdir}/pkgconfig/sensord-server.pc +%license LICENSE.APLv2 %if %{build_test_suite} == "ON" %files -n sensor-test diff --git a/sensor_plugins.xml.in b/sensor_plugins.xml.in index 9fde9d6..c1822ce 100755 --- a/sensor_plugins.xml.in +++ b/sensor_plugins.xml.in @@ -24,5 +24,6 @@ + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 556256a..65f0ec7 100755 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -44,6 +44,10 @@ ENDIF() IF("${GAMING_RV}" STREQUAL "ON") set(SENSOR_FUSION_ENABLE "1") ENDIF() +IF("${TILT}" STREQUAL "ON") +set(SENSOR_FUSION_ENABLE "1") +set(TILT_ENABLE "1") +ENDIF() IF("${GRAVITY}" STREQUAL "ON") set(SENSOR_FUSION_ENABLE "1") set(ORIENTATION_ENABLE "1") @@ -57,8 +61,6 @@ set(LINEAR_ACCELERATION_ENABLE "1") ENDIF() IF("${SENSOR_FUSION_ENABLE}" STREQUAL "1") add_subdirectory(sensor_fusion) -ENDIF() -IF("${FUSION}" STREQUAL "ON") add_subdirectory(fusion) ENDIF() IF("${ORIENTATION_ENABLE}" STREQUAL "1") @@ -70,8 +72,11 @@ ENDIF() IF("${LINEAR_ACCELERATION_ENABLE}" STREQUAL "1") add_subdirectory(linear_accel) ENDIF() -add_subdirectory(rotation_vector) +IF("${TILT_ENABLE}" STREQUAL "1") +add_subdirectory(tilt) +ENDIF() +add_subdirectory(rotation_vector) add_subdirectory(server) add_subdirectory(libsensord) add_subdirectory(shared) diff --git a/src/fusion/fusion_sensor.cpp b/src/fusion/fusion_sensor.cpp index 0bac151..42a8e41 100755 --- a/src/fusion/fusion_sensor.cpp +++ b/src/fusion/fusion_sensor.cpp @@ -39,6 +39,7 @@ #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 @@ -317,7 +318,8 @@ void fusion_sensor::synthesize(const sensor_event_t &event, vector log_map; diff --git a/src/libsensord/sensor_fusion.h b/src/libsensord/sensor_fusion.h index 7539be1..8f80499 100755 --- a/src/libsensord/sensor_fusion.h +++ b/src/libsensord/sensor_fusion.h @@ -36,12 +36,13 @@ extern "C" * @{ */ enum fusion_event_type { - FUSION_ORIENTATION_ENABLED = (FUSION_SENSOR << 16) | 0x0001, - FUSION_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0002, - FUSION_GAMING_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0003, - FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0004, - FUSION_EVENT = (FUSION_SENSOR << 16) | 0x0005, - FUSION_CALIBRATION_NEEDED_EVENT = (FUSION_SENSOR << 16) | 0x0006, + FUSION_EVENT = (FUSION_SENSOR << 16) | 0x0001, + FUSION_CALIBRATION_NEEDED_EVENT = (FUSION_SENSOR << 16) | 0x0002, + FUSION_ORIENTATION_ENABLED = (FUSION_SENSOR << 16) | 0x0003, + FUSION_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0004, + FUSION_GAMING_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0005, + FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0006, + FUSION_TILT_ENABLED = (FUSION_SENSOR << 16) | 0x0007, }; /** diff --git a/src/libsensord/sensor_internal.h b/src/libsensord/sensor_internal.h index 844e7af..7842cda 100755 --- a/src/libsensord/sensor_internal.h +++ b/src/libsensord/sensor_internal.h @@ -55,6 +55,7 @@ extern "C" #include #include #include +#include typedef void (*sensor_cb_t)(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data); diff --git a/src/libsensord/sensor_internal_deprecated.h b/src/libsensord/sensor_internal_deprecated.h index 4420044..fdcbab1 100755 --- a/src/libsensord/sensor_internal_deprecated.h +++ b/src/libsensord/sensor_internal_deprecated.h @@ -54,6 +54,7 @@ extern "C" #include #include #include +#include #define MAX_KEY_LEN 30 diff --git a/src/shared/sensor_common.h b/src/shared/sensor_common.h index 48346ac..d1df649 100755 --- a/src/shared/sensor_common.h +++ b/src/shared/sensor_common.h @@ -66,6 +66,7 @@ typedef enum { GEOMAGNETIC_RV_SENSOR, GAMING_RV_SENSOR, ORIENTATION_SENSOR, + TILT_SENSOR, PIR_SENSOR, PIR_LONG_SENSOR, TEMPERATURE_SENSOR, diff --git a/src/tilt/CMakeLists.txt b/src/tilt/CMakeLists.txt new file mode 100755 index 0000000..44e6a1a --- /dev/null +++ b/src/tilt/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 2.6) +project(tilt CXX) + +SET(SENSOR_NAME tilt_sensor) + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) +include_directories(${CMAKE_SOURCE_DIR}/src/sensor_fusion) + +FOREACH(flag ${tilt_pkgs_LDFLAGS}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") +ENDFOREACH(flag) + +FOREACH(flag ${tilt_pkgs_CFLAGS}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") +ENDFOREACH(flag) + +add_library(${SENSOR_NAME} SHARED + tilt_sensor.cpp + ) + +target_link_libraries(${SENSOR_NAME} ${tilt_pkgs_LDFLAGS} "-lm") + +install(TARGETS ${SENSOR_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/sensord) diff --git a/src/tilt/tilt_sensor.cpp b/src/tilt/tilt_sensor.cpp new file mode 100755 index 0000000..db40acf --- /dev/null +++ b/src/tilt/tilt_sensor.cpp @@ -0,0 +1,311 @@ +/* + * sensord + * + * Copyright (c) 2015 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define SENSOR_NAME "TILT_SENSOR" +#define SENSOR_TYPE_TILT "TILT" + +#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" +#define ELEMENT_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION" +#define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION" + +void pre_process_data(sensor_data &data_out, const float *data_in, float *bias, int *sign, float scale) +{ + data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale; + data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale; + data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale; +} + +tilt_sensor::tilt_sensor() +: m_accel_sensor(NULL) +, m_fusion_sensor(NULL) +, m_time(0) +{ + cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + + m_name = string(SENSOR_NAME); + register_supported_event(TILT_RAW_DATA_EVENT); + m_enable_tilt = 0; + + if (!config.get(SENSOR_TYPE_TILT, ELEMENT_VENDOR, m_vendor)) { + ERR("[VENDOR] is empty\n"); + throw ENXIO; + } + + INFO("m_vendor = %s", m_vendor.c_str()); + + if (!config.get(SENSOR_TYPE_TILT, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) { + ERR("[RAW_DATA_UNIT] is empty\n"); + throw ENXIO; + } + + INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str()); + + if (!config.get(SENSOR_TYPE_TILT, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) { + ERR("[DEFAULT_SAMPLING_TIME] is empty\n"); + throw ENXIO; + } + + INFO("m_default_sampling_time = %d", m_default_sampling_time); + + if (!config.get(SENSOR_TYPE_TILT, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) { + ERR("[PITCH_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation); + + if (!config.get(SENSOR_TYPE_TILT, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) { + ERR("[ROLL_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_roll_rotation_compensation = %d", m_roll_rotation_compensation); + + m_interval = m_default_sampling_time * MS_TO_US; + + INFO("ramasamy 1: Constructor completed"); +} + +tilt_sensor::~tilt_sensor() +{ + INFO("tilt_sensor is destroyed!\n"); +} + +bool tilt_sensor::init(void) +{ + m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_SENSOR); + + if (!m_accel_sensor || !m_fusion_sensor) { + ERR("Failed to load sensors, accel: 0x%x, fusion: 0x%x", + m_accel_sensor, m_fusion_sensor); + return false; + } + + INFO("%s is created!\n", sensor_base::get_name()); + + INFO("ramasamy 2: Init completed"); + + return true; +} + +sensor_type_t tilt_sensor::get_type(void) +{ + return TILT_SENSOR; +} + +bool tilt_sensor::on_start(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); + m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); + m_accel_sensor->start(); + + m_fusion_sensor->register_supported_event(FUSION_EVENT); + m_fusion_sensor->register_supported_event(FUSION_TILT_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(); + + INFO("ramasamy 3: ON_START completed"); + + activate(); + return true; +} + +bool tilt_sensor::on_stop(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); + m_accel_sensor->delete_interval((intptr_t)this, false); + m_accel_sensor->stop(); + + m_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_TILT_ENABLED); + m_fusion_sensor->stop(); + + INFO("ramasamy 4: ON_STOP completed"); + + deactivate(); + return true; +} + +bool tilt_sensor::add_interval(int client_id, unsigned int interval) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->add_interval(client_id, interval, false); + m_fusion_sensor->add_interval(client_id, interval, false); + + INFO("ramasamy 5: ADD_INTERVAL completed"); + + return sensor_base::add_interval(client_id, interval, false); +} + +bool tilt_sensor::delete_interval(int client_id) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_interval(client_id, false); + m_fusion_sensor->delete_interval(client_id, false); + + INFO("ramasamy 6: DELETE_INTERVAL completed"); + + return sensor_base::delete_interval(client_id, false); +} + +void tilt_sensor::synthesize(const sensor_event_t &event, vector &outs) +{ + sensor_event_t tilt_event; + unsigned long long diff_time; + + INFO("ramasamy 7: SYNTHESIZE STARTED"); + + if (event.event_type == FUSION_EVENT) { + + INFO("ramasamy 8: SYNTHESIZE FUSION EVENT RECEIVED"); + 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); + } + + euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation; + euler.m_ang.m_vec[1] *= m_roll_rotation_compensation; + + m_time = get_timestamp(); + tilt_event.sensor_id = get_id(); + tilt_event.event_type = TILT_RAW_DATA_EVENT; + tilt_event.data.accuracy = event.data.accuracy; + tilt_event.data.timestamp = m_time; + tilt_event.data.value_count = 2; + tilt_event.data.values[0] = euler.m_ang.m_vec[0]; + tilt_event.data.values[1] = euler.m_ang.m_vec[1]; + + INFO("ramasamy 8: SYNTHESIZE FUSION EVENT SENT"); + + push(tilt_event); + } + + return; +} + +int tilt_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) +{ + sensor_data_t fusion_data; + + if (event_type != TILT_RAW_DATA_EVENT) + return -1; + + m_fusion_sensor->get_sensor_data(FUSION_TILT_ENABLED, fusion_data); + + quaternion quat(fusion_data.values[0], fusion_data.values[1], + fusion_data.values[2], fusion_data.values[3]); + + euler_angles euler = quat2euler(quat); + + if(m_raw_data_unit == "DEGREES") { + euler = rad2deg(euler); + } + + data.accuracy = fusion_data.accuracy; + data.timestamp = get_timestamp(); + data.value_count = 2; + data.values[0] = euler.m_ang.m_vec[0]; + data.values[1] = euler.m_ang.m_vec[1]; + + data.values[0] *= m_pitch_rotation_compensation; + data.values[1] *= m_roll_rotation_compensation; + + return 0; +} + +bool tilt_sensor::get_properties(sensor_properties_s &properties) +{ + if(m_raw_data_unit == "DEGREES") { + properties.min_range = -180; + properties.max_range = 180; + } + else { + properties.min_range = -PI; + properties.max_range = 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; + + return true; +} + +extern "C" sensor_module* create(void) +{ + tilt_sensor *sensor; + + try { + sensor = new(std::nothrow) tilt_sensor; + } catch (int err) { + ERR("Failed to create module, err: %d, cause: %s", err, strerror(err)); + return NULL; + } + + sensor_module *module = new(std::nothrow) sensor_module; + retvm_if(!module || !sensor, NULL, "Failed to allocate memory"); + + module->sensors.push_back(sensor); + return module; +} diff --git a/src/tilt/tilt_sensor.h b/src/tilt/tilt_sensor.h new file mode 100755 index 0000000..758751f --- /dev/null +++ b/src/tilt/tilt_sensor.h @@ -0,0 +1,69 @@ +/* + * 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 _TILT_SENSOR_H_ +#define _TILT_SENSOR_H_ + +#include +#include +#include + +class tilt_sensor : public virtual_sensor { +public: + tilt_sensor(); + virtual ~tilt_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); + bool get_properties(sensor_properties_s &properties); + sensor_type_t get_type(void); + + int get_sensor_data(const unsigned int event_type, sensor_data_t &data); + +private: + sensor_base *m_accel_sensor; + sensor_base *m_fusion_sensor; + + sensor_data m_accel; + + cmutex m_value_mutex; + + orientation_filter m_orientation_filter; + orientation_filter m_orientation_filter_poll; + + unsigned int m_enable_tilt; + + unsigned long long m_time; + unsigned int m_interval; + + string m_vendor; + string m_raw_data_unit; + int m_default_sampling_time; + int m_pitch_rotation_compensation; + int m_roll_rotation_compensation; + + bool on_start(void); + bool on_stop(void); +}; + +#endif /*_TILT_SENSOR_H_*/ diff --git a/virtual_sensors.xml b/virtual_sensors.xml index 3210e4f..3005df6 100755 --- a/virtual_sensors.xml +++ b/virtual_sensors.xml @@ -64,6 +64,15 @@ + + + + + + + + + @@ -130,6 +139,15 @@ + + + + + + + + + @@ -196,5 +214,14 @@ + + + + + + + + + -- 2.7.4 From 99ad55233a4b1fbc9bf652e2d816860df59c7500 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 15 May 2015 19:07:10 +0900 Subject: [PATCH 04/16] Updating test files to support the tilt sensor - Adding support for tilt sensor in all test files Change-Id: I7107668a844204b7087a5a89d7ca21aa9ece3e93 --- test/src/api-test.c | 3 +++ test/src/check-sensor.c | 10 ++++++++++ test/src/sensor-test.c | 5 +++++ 3 files changed, 18 insertions(+) diff --git a/test/src/api-test.c b/test/src/api-test.c index 0ecea23..599041a 100644 --- a/test/src/api-test.c +++ b/test/src/api-test.c @@ -220,6 +220,9 @@ int main(int argc, char **argv) result = check_sensor_api(ORIENTATION_RAW_DATA_EVENT, interval); fprintf(fp, "Orientation - RAW_DATA_REPORT_ON_TIME - %d\n", result); + result = check_sensor_api(TILT_RAW_DATA_EVENT, interval); + fprintf(fp, "Tilt - RAW_DATA_REPORT_ON_TIME - %d\n", result); + result = check_sensor_api(PRESSURE_RAW_DATA_EVENT, interval); fprintf(fp, "Pressure - RAW_DATA_REPORT_ON_TIME - %d\n", result); diff --git a/test/src/check-sensor.c b/test/src/check-sensor.c index aa15e34..9bba712 100644 --- a/test/src/check-sensor.c +++ b/test/src/check-sensor.c @@ -57,6 +57,9 @@ void printpollinglogs(sensor_type_t type,sensor_data_t data) case(ORIENTATION_SENSOR): printf("Orientation [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); break; + case(TILT_SENSOR): + printf("Tilt [%lld] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1]); + break; case(GRAVITY_SENSOR): printf("Gravity [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); break; @@ -112,6 +115,10 @@ int get_event(sensor_type_t sensor_type, char str[]) if (strcmp(str, "RAW_DATA_EVENT") == 0) return ORIENTATION_RAW_DATA_EVENT; break; + case TILT_SENSOR: + if (strcmp(str, "RAW_DATA_EVENT") == 0) + return TILT_RAW_DATA_EVENT; + break; case GRAVITY_SENSOR: if (strcmp(str, "RAW_DATA_EVENT") == 0) return GRAVITY_RAW_DATA_EVENT; @@ -165,6 +172,9 @@ void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, voi case ORIENTATION_SENSOR : printf("Orientation [%lld] [%6.6f] [%6.6f] [%6.6f]\n", data->timestamp, data->values[0], data->values[1], data->values[2]); break; + case TILT_SENSOR : + printf("Tilt [%lld] [%6.6f] [%6.6f]\n", data->timestamp, data->values[0], data->values[1]); + break; case GRAVITY_SENSOR: printf("Gravity [%lld] [%6.6f] [%6.6f] [%6.6f]\n", data->timestamp, data->values[0], data->values[1], data->values[2]); break; diff --git a/test/src/sensor-test.c b/test/src/sensor-test.c index 5c5c3c1..99ea138 100644 --- a/test/src/sensor-test.c +++ b/test/src/sensor-test.c @@ -38,6 +38,7 @@ void usage() printf("[temperature] "); printf("[geomagnetic] "); printf("[orientation] "); + printf("[tilt] "); printf("[gravity] "); printf("[linear_accel] "); printf("[rotation_vector] "); @@ -95,6 +96,10 @@ int main(int argc, char **argv) sensor_type = ORIENTATION_SENSOR; event = ORIENTATION_RAW_DATA_EVENT; } + else if (strcmp(argv[1], "tilt") == 0) { + sensor_type = TILT_SENSOR; + event = TILT_RAW_DATA_EVENT; + } else if (strcmp(argv[1], "gravity") == 0) { sensor_type = GRAVITY_SENSOR; event = GRAVITY_RAW_DATA_EVENT; -- 2.7.4 From 63fcf151541422ae2c5c2820129783114f85ada9 Mon Sep 17 00:00:00 2001 From: Ankur Date: Fri, 15 May 2015 18:15:27 +0530 Subject: [PATCH 05/16] Removal of redundant variables and functions from orientation sensor: Code cleanup Cleanup of orientation sensor code: removal of unused variables and functions. Change-Id: I54f99cd2193c78f40012a23766851f86830b5ed3 --- src/orientation/orientation_sensor.cpp | 8 -------- src/orientation/orientation_sensor.h | 5 ----- 2 files changed, 13 deletions(-) diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index b9291e8..3b27a7b 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -52,13 +52,6 @@ #define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION" #define ELEMENT_AZIMUTH_ROTATION_COMPENSATION "AZIMUTH_ROTATION_COMPENSATION" -void pre_process_data(sensor_data &data_out, const float *data_in, float *bias, int *sign, float scale) -{ - data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale; - data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale; - data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale; -} - orientation_sensor::orientation_sensor() : m_accel_sensor(NULL) , m_gyro_sensor(NULL) @@ -70,7 +63,6 @@ orientation_sensor::orientation_sensor() m_name = string(SENSOR_NAME); register_supported_event(ORIENTATION_RAW_DATA_EVENT); - m_enable_orientation = 0; if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_VENDOR, m_vendor)) { ERR("[VENDOR] is empty\n"); diff --git a/src/orientation/orientation_sensor.h b/src/orientation/orientation_sensor.h index 1128541..86c39f9 100755 --- a/src/orientation/orientation_sensor.h +++ b/src/orientation/orientation_sensor.h @@ -52,11 +52,6 @@ private: 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; -- 2.7.4 From 9db2b77b478b50ee549a76614b6c07a52b7fa8a5 Mon Sep 17 00:00:00 2001 From: Ankur Date: Fri, 15 May 2015 17:42:23 +0530 Subject: [PATCH 06/16] Common plugin for Hardware/Software Rotation Vector Next Part: Support in fusion sensor to handle hal (replace rv_raw) Change-Id: I736bd022626eed5e9086b9040906b706130e161c --- src/rotation_vector/rv/rv_sensor.cpp | 67 ++++++++++++++++++++++-------------- src/shared/virtual_sensor.h | 1 + 2 files changed, 42 insertions(+), 26 deletions(-) diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp index f647ef0..5114cdc 100755 --- a/src/rotation_vector/rv/rv_sensor.cpp +++ b/src/rotation_vector/rv/rv_sensor.cpp @@ -57,10 +57,18 @@ rv_sensor::rv_sensor() : m_accel_sensor(NULL) , m_gyro_sensor(NULL) , m_magnetic_sensor(NULL) +, m_fusion_sensor(NULL) , m_time(0) { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + // Will check if fusion_sensor is in the list of hal sensors. + sensor_hal *fusion_sensor_hal = sensor_plugin_loader::get_instance().get_sensor_hal(FUSION_SENSOR); + 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; @@ -115,15 +123,17 @@ bool rv_sensor::on_start(void) { AUTOLOCK(m_mutex); - m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_accel_sensor->start(); - m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_gyro_sensor->start(); - 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(); + 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); @@ -139,15 +149,17 @@ bool rv_sensor::on_stop(void) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->delete_interval((intptr_t)this, false); - m_accel_sensor->stop(); - m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->delete_interval((intptr_t)this, false); - m_gyro_sensor->stop(); - m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT); - m_magnetic_sensor->delete_interval((intptr_t)this, false); - m_magnetic_sensor->stop(); + 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); @@ -163,9 +175,11 @@ bool rv_sensor::add_interval(int client_id, unsigned int interval) { AUTOLOCK(m_mutex); - m_accel_sensor->add_interval(client_id, interval, false); - m_gyro_sensor->add_interval(client_id, interval, false); - m_magnetic_sensor->add_interval(client_id, interval, false); + 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); @@ -176,10 +190,11 @@ bool rv_sensor::delete_interval(int client_id) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_interval(client_id, false); - m_gyro_sensor->delete_interval(client_id, false); - m_magnetic_sensor->delete_interval(client_id, false); - + 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); @@ -221,7 +236,7 @@ int rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) if (event_type != ROTATION_VECTOR_RAW_DATA_EVENT) return -1; - m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); + m_fusion_sensor->get_sensor_data(FUSION_ROTATION_VECTOR_ENABLED, fusion_data); data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); diff --git a/src/shared/virtual_sensor.h b/src/shared/virtual_sensor.h index 7a3b70c..d6414e3 100755 --- a/src/shared/virtual_sensor.h +++ b/src/shared/virtual_sensor.h @@ -31,6 +31,7 @@ public: virtual void synthesize(const sensor_event_t &event, vector &outs) = 0; virtual int get_sensor_data(const unsigned int event_type, sensor_data_t &data) = 0; bool is_virtual(void); + bool m_hardware_fusion; protected: -- 2.7.4 From 4fd5379bbb2182237c88cc625ef469a51afeb7c8 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 18 May 2015 13:30:53 +0530 Subject: [PATCH 07/16] Adding common plugin for hardware/software fusion - orientation sensor Support for common virtual sensor plugin for orientation sensor for both hardware and software based sensor fusion Tested on rd-pq device with tizen 2.4 repo Change-Id: I676cc594d167928508e3e82a585e2d5910d01e40 --- src/orientation/orientation_sensor.cpp | 62 +++++++++++++++++++++------------- 1 file changed, 38 insertions(+), 24 deletions(-) diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index b9291e8..8b55c7e 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -68,6 +68,12 @@ orientation_sensor::orientation_sensor() { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + sensor_hal *fusion_sensor_hal = sensor_plugin_loader::get_instance().get_sensor_hal(FUSION_SENSOR); + 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); m_enable_orientation = 0; @@ -150,15 +156,17 @@ bool orientation_sensor::on_start(void) { AUTOLOCK(m_mutex); - m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_accel_sensor->start(); - m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_gyro_sensor->start(); - 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(); + 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); @@ -174,15 +182,17 @@ bool orientation_sensor::on_stop(void) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->delete_interval((intptr_t)this, false); - m_accel_sensor->stop(); - m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->delete_interval((intptr_t)this, false); - m_gyro_sensor->stop(); - m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT); - m_magnetic_sensor->delete_interval((intptr_t)this, false); - m_magnetic_sensor->stop(); + 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); @@ -198,9 +208,11 @@ bool orientation_sensor::add_interval(int client_id, unsigned int interval) { AUTOLOCK(m_mutex); - m_accel_sensor->add_interval(client_id, interval, false); - m_gyro_sensor->add_interval(client_id, interval, false); - m_magnetic_sensor->add_interval(client_id, interval, false); + 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); @@ -211,9 +223,11 @@ bool orientation_sensor::delete_interval(int client_id) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_interval(client_id, false); - m_gyro_sensor->delete_interval(client_id, false); - m_magnetic_sensor->delete_interval(client_id, false); + 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); -- 2.7.4 From 7a859c75bbf1c8deaea69f25eb191566862a58b5 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 18 May 2015 15:50:19 +0530 Subject: [PATCH 08/16] Adding common plugin for hardware/sensor fusion - gravity sensor Tested on rd-pq device for 2.4 repo Change-Id: Ia6599ce7dac2de2ac1935668d38297466374ab08 --- src/gravity/gravity_sensor.cpp | 62 ++++++++++++++++++++++++++---------------- 1 file changed, 38 insertions(+), 24 deletions(-) diff --git a/src/gravity/gravity_sensor.cpp b/src/gravity/gravity_sensor.cpp index bf1c6d0..aa23a5e 100755 --- a/src/gravity/gravity_sensor.cpp +++ b/src/gravity/gravity_sensor.cpp @@ -66,6 +66,12 @@ gravity_sensor::gravity_sensor() { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + sensor_hal *fusion_sensor_hal = sensor_plugin_loader::get_instance().get_sensor_hal(FUSION_SENSOR); + if (!fusion_sensor_hal) + m_hardware_fusion = false; + else + m_hardware_fusion = true; + m_name = std::string(SENSOR_NAME); register_supported_event(GRAVITY_RAW_DATA_EVENT); @@ -160,15 +166,17 @@ bool gravity_sensor::on_start(void) { AUTOLOCK(m_mutex); - m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_accel_sensor->start(); - m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_gyro_sensor->start(); - 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(); + 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); @@ -184,15 +192,17 @@ bool gravity_sensor::on_stop(void) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->delete_interval((intptr_t)this, false); - m_accel_sensor->stop(); - m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->delete_interval((intptr_t)this, false); - m_gyro_sensor->stop(); - m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT); - m_magnetic_sensor->delete_interval((intptr_t)this, false); - m_magnetic_sensor->stop(); + 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); @@ -207,9 +217,11 @@ bool gravity_sensor::on_stop(void) bool gravity_sensor::add_interval(int client_id, unsigned int interval) { AUTOLOCK(m_mutex); - m_accel_sensor->add_interval(client_id, interval, false); - m_gyro_sensor->add_interval(client_id, interval, false); - m_magnetic_sensor->add_interval(client_id, interval, false); + 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); @@ -219,9 +231,11 @@ bool gravity_sensor::add_interval(int client_id, unsigned int interval) bool gravity_sensor::delete_interval(int client_id) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_interval(client_id, false); - m_gyro_sensor->delete_interval(client_id, false); - m_magnetic_sensor->delete_interval(client_id, false); + 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); -- 2.7.4 From 58467dec010951a78b277c79f2853f2257e14314 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 18 May 2015 15:31:59 +0530 Subject: [PATCH 09/16] Updating gravity sensor after fusion sensor restructuring Tested on rd-pq device with 2.4 repo Change-Id: I8881c5823507b3ef02567eb13c628af5ea8e62a4 --- src/gravity/CMakeLists.txt | 1 + src/gravity/gravity_sensor.cpp | 174 +++++++++++++++++++++++++++++++++-------- src/gravity/gravity_sensor.h | 14 +++- 3 files changed, 155 insertions(+), 34 deletions(-) diff --git a/src/gravity/CMakeLists.txt b/src/gravity/CMakeLists.txt index 910bf2e..9ebbb00 100755 --- a/src/gravity/CMakeLists.txt +++ b/src/gravity/CMakeLists.txt @@ -5,6 +5,7 @@ SET(SENSOR_NAME gravity_sensor) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) +include_directories(${CMAKE_SOURCE_DIR}/src/sensor_fusion) FOREACH(flag ${gravity_pkgs_LDFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") diff --git a/src/gravity/gravity_sensor.cpp b/src/gravity/gravity_sensor.cpp index 48fddf4..bf1c6d0 100755 --- a/src/gravity/gravity_sensor.cpp +++ b/src/gravity/gravity_sensor.cpp @@ -34,9 +34,12 @@ #define INITIAL_VALUE -1 #define GRAVITY 9.80665 -#define DEG2RAD (M_PI/180) #define DEVIATION 0.1 +#define PI 3.141593 +#define AZIMUTH_OFFSET_DEGREES 360 +#define AZIMUTH_OFFSET_RADIANS (2 * PI) + #define SENSOR_NAME "GRAVITY_SENSOR" #define SENSOR_TYPE_GRAVITY "GRAVITY" #define SENSOR_TYPE_ORIENTATION "ORIENTATION" @@ -50,9 +53,15 @@ #define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME" #define ELEMENT_GRAVITY_SIGN_COMPENSATION "GRAVITY_SIGN_COMPENSATION" #define ELEMENT_ORIENTATION_DATA_UNIT "RAW_DATA_UNIT" +#define ELEMENT_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION" +#define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION" +#define ELEMENT_AZIMUTH_ROTATION_COMPENSATION "AZIMUTH_ROTATION_COMPENSATION" gravity_sensor::gravity_sensor() -: m_orientation_sensor(NULL) +: m_accel_sensor(NULL) +, m_gyro_sensor(NULL) +, m_magnetic_sensor(NULL) +, m_fusion_sensor(NULL) , m_time(0) { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); @@ -95,6 +104,27 @@ gravity_sensor::gravity_sensor() INFO("m_gravity_sign_compensation = (%d, %d, %d)", m_gravity_sign_compensation[0], m_gravity_sign_compensation[1], m_gravity_sign_compensation[2]); + if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) { + ERR("[AZIMUTH_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation); + + if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) { + ERR("[PITCH_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation); + + if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) { + ERR("[ROLL_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_roll_rotation_compensation = %d", m_roll_rotation_compensation); + m_interval = m_default_sampling_time * MS_TO_US; } @@ -105,10 +135,15 @@ gravity_sensor::~gravity_sensor() bool gravity_sensor::init() { - m_orientation_sensor = sensor_plugin_loader::get_instance().get_sensor(ORIENTATION_SENSOR); + m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); + m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); + + m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_SENSOR); - if (!m_orientation_sensor) { - ERR("Failed to load orientation sensor: 0x%x", m_orientation_sensor); + if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) { + ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x, fusion: 0x%x", + m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor); return false; } @@ -125,9 +160,21 @@ bool gravity_sensor::on_start(void) { AUTOLOCK(m_mutex); - m_orientation_sensor->add_client(ORIENTATION_RAW_DATA_EVENT); - m_orientation_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_orientation_sensor->start(); + 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; @@ -137,9 +184,21 @@ bool gravity_sensor::on_stop(void) { AUTOLOCK(m_mutex); - m_orientation_sensor->delete_client(ORIENTATION_RAW_DATA_EVENT); - m_orientation_sensor->delete_interval((intptr_t)this, false); - m_orientation_sensor->stop(); + 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_ORIENTATION_ENABLED); + m_fusion_sensor->stop(); deactivate(); return true; @@ -148,7 +207,11 @@ bool gravity_sensor::on_stop(void) bool gravity_sensor::add_interval(int client_id, unsigned int interval) { AUTOLOCK(m_mutex); - m_orientation_sensor->add_interval(client_id , interval, false); + 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); } @@ -156,7 +219,11 @@ bool gravity_sensor::add_interval(int client_id, unsigned int interval) bool gravity_sensor::delete_interval(int client_id) { AUTOLOCK(m_mutex); - m_orientation_sensor->delete_interval(client_id , false); + 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); } @@ -165,28 +232,49 @@ void gravity_sensor::synthesize(const sensor_event_t &event, vector quat(event.data.values[0], event.data.values[1], + event.data.values[2], event.data.values[3]); + + euler_angles euler = quat2euler(quat); + + if(m_orientation_data_unit == "DEGREES") { + euler = rad2deg(euler); + azimuth_offset = AZIMUTH_OFFSET_DEGREES; + } + else { + azimuth_offset = AZIMUTH_OFFSET_RADIANS; + } + + euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation; + euler.m_ang.m_vec[1] *= m_roll_rotation_compensation; + euler.m_ang.m_vec[2] *= m_azimuth_rotation_compensation; + + pitch = euler.m_ang.m_vec[0]; + roll = euler.m_ang.m_vec[1]; + if (euler.m_ang.m_vec[2] >= 0) + azimuth = euler.m_ang.m_vec[2]; + else + azimuth = euler.m_ang.m_vec[2] + azimuth_offset; + + if(m_orientation_data_unit == "DEGREES") { + azimuth *= DEG2RAD; + pitch *= DEG2RAD; + roll *= DEG2RAD; + } + m_time = get_timestamp(); gravity_event.sensor_id = get_id(); gravity_event.event_type = GRAVITY_RAW_DATA_EVENT; + if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) || (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) { gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth); @@ -208,18 +296,41 @@ void gravity_sensor::synthesize(const sensor_event_t &event, vectorget_sensor_data(ORIENTATION_RAW_DATA_EVENT, orientation_data); + if (event_type != GRAVITY_RAW_DATA_EVENT) + return -1; - azimuth = orientation_data.values[0]; - pitch = orientation_data.values[1]; - roll = orientation_data.values[2]; + m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); + + quaternion quat(fusion_data.values[0], fusion_data.values[1], + fusion_data.values[2], fusion_data.values[3]); + + euler_angles euler = quat2euler(quat); + + if(m_orientation_data_unit == "DEGREES") { + euler = rad2deg(euler); + azimuth_offset = AZIMUTH_OFFSET_DEGREES; + } + else { + azimuth_offset = AZIMUTH_OFFSET_RADIANS; + } + + pitch = euler.m_ang.m_vec[0]; + roll = euler.m_ang.m_vec[1]; + + if (euler.m_ang.m_vec[2] >= 0) + azimuth = euler.m_ang.m_vec[2]; + else + azimuth = euler.m_ang.m_vec[2] + azimuth_offset; if(m_orientation_data_unit == "DEGREES") { azimuth *= DEG2RAD; @@ -227,9 +338,6 @@ int gravity_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t roll *= DEG2RAD; } - if (event_type != GRAVITY_RAW_DATA_EVENT) - return -1; - data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) || diff --git a/src/gravity/gravity_sensor.h b/src/gravity/gravity_sensor.h index 376a892..cd15b79 100755 --- a/src/gravity/gravity_sensor.h +++ b/src/gravity/gravity_sensor.h @@ -22,6 +22,7 @@ #include #include +#include class gravity_sensor : public virtual_sensor { public: @@ -39,7 +40,15 @@ public: int get_sensor_data(const unsigned int event_type, sensor_data_t &data); bool get_properties(sensor_properties_s &properties); private: - sensor_base *m_orientation_sensor; + sensor_base *m_accel_sensor; + sensor_base *m_gyro_sensor; + sensor_base *m_magnetic_sensor; + sensor_base *m_fusion_sensor; + + sensor_data m_accel; + sensor_data m_gyro; + sensor_data m_magnetic; + cmutex m_value_mutex; int m_accuracy; @@ -51,6 +60,9 @@ private: string m_orientation_data_unit; int m_default_sampling_time; int m_gravity_sign_compensation[3]; + int m_azimuth_rotation_compensation; + int m_pitch_rotation_compensation; + int m_roll_rotation_compensation; bool on_start(void); bool on_stop(void); -- 2.7.4 From 699ebee13fed98e9504db5f3e50287db07171426 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 18 May 2015 16:06:41 +0530 Subject: [PATCH 10/16] Adding common plugin for hardware/software sensor fusion - gaming_rv Tested on rd-pq device for tizen 2.4 repo Change-Id: I62436db7534a325515ae49594cde02b02ef3e51f --- src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp | 49 ++++++++++++++-------- 1 file changed, 32 insertions(+), 17 deletions(-) diff --git a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp index 997bc7b..0b3559f 100755 --- a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp +++ b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp @@ -56,10 +56,17 @@ void pre_process_data(sensor_data &data_out, const float *data_in, float gaming_rv_sensor::gaming_rv_sensor() : m_accel_sensor(NULL) , m_gyro_sensor(NULL) +, m_fusion_sensor(NULL) , m_time(0) { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + sensor_hal *fusion_sensor_hal = sensor_plugin_loader::get_instance().get_sensor_hal(FUSION_SENSOR); + if (!fusion_sensor_hal) + m_hardware_fusion = false; + else + m_hardware_fusion = true; + m_name = string(SENSOR_NAME); register_supported_event(GAMING_RV_RAW_DATA_EVENT); m_enable_gaming_rv = 0; @@ -113,12 +120,14 @@ bool gaming_rv_sensor::on_start(void) { AUTOLOCK(m_mutex); - m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_accel_sensor->start(); - m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_gyro_sensor->start(); + 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_fusion_sensor->register_supported_event(FUSION_EVENT); m_fusion_sensor->register_supported_event(FUSION_GAMING_ROTATION_VECTOR_ENABLED); @@ -134,12 +143,14 @@ bool gaming_rv_sensor::on_stop(void) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->delete_interval((intptr_t)this, false); - m_accel_sensor->stop(); - m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->delete_interval((intptr_t)this, false); - m_gyro_sensor->stop(); + 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_fusion_sensor->delete_client(FUSION_EVENT); m_fusion_sensor->delete_interval((intptr_t)this, false); @@ -155,8 +166,10 @@ bool gaming_rv_sensor::add_interval(int client_id, unsigned int interval) { AUTOLOCK(m_mutex); - m_accel_sensor->add_interval(client_id, interval, false); - m_gyro_sensor->add_interval(client_id, interval, false); + if (!m_hardware_fusion) { + m_accel_sensor->add_interval(client_id, interval, false); + m_gyro_sensor->add_interval(client_id, interval, false); + } m_fusion_sensor->add_interval(client_id, interval, false); @@ -167,8 +180,10 @@ bool gaming_rv_sensor::delete_interval(int client_id) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_interval(client_id, false); - m_gyro_sensor->delete_interval(client_id, false); + if (!m_hardware_fusion) { + m_accel_sensor->delete_interval(client_id, false); + m_gyro_sensor->delete_interval(client_id, false); + } m_fusion_sensor->delete_interval(client_id, false); @@ -211,7 +226,7 @@ int gaming_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &da if (event_type != GAMING_RV_RAW_DATA_EVENT) return -1; - m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); + m_fusion_sensor->get_sensor_data(FUSION_GAMING_ROTATION_VECTOR_ENABLED, fusion_data); data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); -- 2.7.4 From 5fe76a606713339ab90027546372565e148aef7e Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 18 May 2015 16:33:47 +0530 Subject: [PATCH 11/16] Adding common plugin for hardware/software sensor fusion - geomagnetic_rv Tested on rd-pq device with tizen 2.4 repo Change-Id: Ifa9a10a1ab60dcde364f7b26f4e8ee4ea66df509 --- .../geomagnetic_rv/geomagnetic_rv_sensor.cpp | 49 ++++++++++++++-------- 1 file changed, 32 insertions(+), 17 deletions(-) diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp index 381dbe2..d39f81a 100755 --- a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp +++ b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp @@ -56,10 +56,17 @@ void pre_process_data(sensor_data &data_out, const float *data_in, float geomagnetic_rv_sensor::geomagnetic_rv_sensor() : m_accel_sensor(NULL) , m_magnetic_sensor(NULL) +, m_fusion_sensor(NULL) , m_time(0) { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + sensor_hal *fusion_sensor_hal = sensor_plugin_loader::get_instance().get_sensor_hal(FUSION_SENSOR); + 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); m_enable_geomagnetic_rv = 0; @@ -113,12 +120,14 @@ bool geomagnetic_rv_sensor::on_start(void) { AUTOLOCK(m_mutex); - m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_accel_sensor->start(); - m_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(); + 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); @@ -134,12 +143,14 @@ bool geomagnetic_rv_sensor::on_stop(void) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->delete_interval((intptr_t)this, false); - m_accel_sensor->stop(); - m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT); - m_magnetic_sensor->delete_interval((intptr_t)this, false); - m_magnetic_sensor->stop(); + 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); @@ -155,8 +166,10 @@ bool geomagnetic_rv_sensor::add_interval(int client_id, unsigned int interval) { AUTOLOCK(m_mutex); - m_accel_sensor->add_interval(client_id, interval, false); - m_magnetic_sensor->add_interval(client_id, interval, false); + 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); @@ -167,8 +180,10 @@ bool geomagnetic_rv_sensor::delete_interval(int client_id) { AUTOLOCK(m_mutex); - m_accel_sensor->delete_interval(client_id, false); - m_magnetic_sensor->delete_interval(client_id, false); + 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); @@ -211,7 +226,7 @@ int geomagnetic_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_ if (event_type != GEOMAGNETIC_RV_RAW_DATA_EVENT) return -1; - m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); + m_fusion_sensor->get_sensor_data(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED, fusion_data); data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); -- 2.7.4 From 70b127df6edaff532aad29324f3659dd6ffc1998 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 18 May 2015 17:42:58 +0530 Subject: [PATCH 12/16] Updating Linear Acceleration sensor as per fusion sensor restructuring Tested on rd-pq device with tizen 2.4 repo Change-Id: I989940c47b8673e9f607c229e44188759659934f --- src/linear_accel/linear_accel_sensor.cpp | 183 ++++++++++++++++++++++++++----- src/linear_accel/linear_accel_sensor.h | 16 ++- 2 files changed, 170 insertions(+), 29 deletions(-) diff --git a/src/linear_accel/linear_accel_sensor.cpp b/src/linear_accel/linear_accel_sensor.cpp index fe2d608..c967ce2 100755 --- a/src/linear_accel/linear_accel_sensor.cpp +++ b/src/linear_accel/linear_accel_sensor.cpp @@ -33,6 +33,8 @@ #define SENSOR_NAME "LINEAR_ACCEL_SENSOR" #define SENSOR_TYPE_LINEAR_ACCEL "LINEAR_ACCEL" +#define SENSOR_TYPE_GRAVITY "GRAVITY" +#define SENSOR_TYPE_ORIENTATION "ORIENTATION" #define ELEMENT_NAME "NAME" #define ELEMENT_VENDOR "VENDOR" @@ -42,9 +44,19 @@ #define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" #define ELEMENT_ACCEL_SCALE "ACCEL_SCALE" #define ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION "LINEAR_ACCEL_SIGN_COMPENSATION" +#define ELEMENT_ORIENTATION_DATA_UNIT "RAW_DATA_UNIT" +#define ELEMENT_GRAVITY_SIGN_COMPENSATION "GRAVITY_SIGN_COMPENSATION" +#define ELEMENT_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION" +#define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION" +#define ELEMENT_AZIMUTH_ROTATION_COMPENSATION "AZIMUTH_ROTATION_COMPENSATION" #define INITIAL_VALUE -1 #define GRAVITY 9.80665 +#define DEVIATION 0.1 + +#define PI 3.141593 +#define AZIMUTH_OFFSET_DEGREES 360 +#define AZIMUTH_OFFSET_RADIANS (2 * PI) #define MS_TO_US 1000 #define MIN_DELIVERY_DIFF_FACTOR 0.75f @@ -55,7 +67,9 @@ linear_accel_sensor::linear_accel_sensor() : m_accel_sensor(NULL) -, m_gravity_sensor(NULL) +, m_gyro_sensor(NULL) +, m_magnetic_sensor(NULL) +, m_fusion_sensor(NULL) , m_time(0) { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); @@ -79,6 +93,13 @@ linear_accel_sensor::linear_accel_sensor() INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str()); + if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ORIENTATION_DATA_UNIT, m_orientation_data_unit)) { + ERR("[ORIENTATION_DATA_UNIT] is empty\n"); + throw ENXIO; + } + + INFO("m_orientation_data_unit = %s", m_orientation_data_unit.c_str()); + if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) { ERR("[DEFAULT_SAMPLING_TIME] is empty\n"); throw ENXIO; @@ -106,6 +127,12 @@ linear_accel_sensor::linear_accel_sensor() INFO("m_accel_scale = %f", m_accel_scale); + if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_GRAVITY_SIGN_COMPENSATION, m_gravity_sign_compensation, 3)) { + ERR("[GRAVITY_SIGN_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_gravity_sign_compensation = (%d, %d, %d)", m_gravity_sign_compensation[0], m_gravity_sign_compensation[1], m_gravity_sign_compensation[2]); if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION, m_linear_accel_sign_compensation, 3)) { ERR("[LINEAR_ACCEL_SIGN_COMPENSATION] is empty\n"); @@ -114,6 +141,27 @@ linear_accel_sensor::linear_accel_sensor() INFO("m_linear_accel_sign_compensation = (%d, %d, %d)", m_linear_accel_sign_compensation[0], m_linear_accel_sign_compensation[1], m_linear_accel_sign_compensation[2]); + if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) { + ERR("[AZIMUTH_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation); + + if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) { + ERR("[PITCH_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation); + + if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) { + ERR("[ROLL_ROTATION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_roll_rotation_compensation = %d", m_roll_rotation_compensation); + m_interval = m_default_sampling_time * MS_TO_US; } @@ -125,12 +173,15 @@ linear_accel_sensor::~linear_accel_sensor() bool linear_accel_sensor::init() { - m_gravity_sensor = sensor_plugin_loader::get_instance().get_sensor(GRAVITY_SENSOR); m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); + m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); + + m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_SENSOR); - if (!m_accel_sensor || !m_gravity_sensor) { - ERR("Failed to load sensors, accel: 0x%x, gravity: 0x%x", - m_accel_sensor, m_gravity_sensor); + if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) { + ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x, fusion: 0x%x", + m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor); return false; } @@ -146,13 +197,21 @@ sensor_type_t linear_accel_sensor::get_type(void) bool linear_accel_sensor::on_start(void) { AUTOLOCK(m_mutex); - m_gravity_sensor->add_client(GRAVITY_RAW_DATA_EVENT); - m_gravity_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_gravity_sensor->start(); - 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; @@ -161,13 +220,21 @@ bool linear_accel_sensor::on_start(void) bool linear_accel_sensor::on_stop(void) { AUTOLOCK(m_mutex); - m_gravity_sensor->delete_client(GRAVITY_RAW_DATA_EVENT); - m_gravity_sensor->delete_interval((intptr_t)this, false); - m_gravity_sensor->stop(); - 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_ORIENTATION_ENABLED); + m_fusion_sensor->stop(); deactivate(); return true; @@ -176,8 +243,11 @@ bool linear_accel_sensor::on_stop(void) bool linear_accel_sensor::add_interval(int client_id, unsigned int interval) { AUTOLOCK(m_mutex); - m_gravity_sensor->add_interval(client_id, interval, false); 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); } @@ -185,15 +255,78 @@ bool linear_accel_sensor::add_interval(int client_id, unsigned int interval) bool linear_accel_sensor::delete_interval(int client_id) { AUTOLOCK(m_mutex); - m_gravity_sensor->delete_interval(client_id, false); 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); } +sensor_data_t linear_accel_sensor::calculate_gravity(sensor_data_t data) +{ + sensor_data_t gravity_data; + float pitch, roll, azimuth; + float azimuth_offset; + + quaternion quat(data.values[0], data.values[1], + data.values[2], data.values[3]); + + euler_angles euler = quat2euler(quat); + + if(m_orientation_data_unit == "DEGREES") { + euler = rad2deg(euler); + azimuth_offset = AZIMUTH_OFFSET_DEGREES; + } + else { + azimuth_offset = AZIMUTH_OFFSET_RADIANS; + } + + euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation; + euler.m_ang.m_vec[1] *= m_roll_rotation_compensation; + euler.m_ang.m_vec[2] *= m_azimuth_rotation_compensation; + + pitch = euler.m_ang.m_vec[0]; + roll = euler.m_ang.m_vec[1]; + if (euler.m_ang.m_vec[2] >= 0) + azimuth = euler.m_ang.m_vec[2]; + else + azimuth = euler.m_ang.m_vec[2] + azimuth_offset; + + if(m_orientation_data_unit == "DEGREES") { + azimuth *= DEG2RAD; + pitch *= DEG2RAD; + roll *= DEG2RAD; + } + + + if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) || + (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) { + gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth); + gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth); + gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll); + } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) || + (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) { + gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth); + gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth); + gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch); + } else { + gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll); + gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch); + gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch); + } + gravity_data.value_count = 3; + gravity_data.timestamp = m_time; + gravity_data.accuracy = SENSOR_ACCURACY_GOOD; + + return gravity_data; +} + void linear_accel_sensor::synthesize(const sensor_event_t &event, vector &outs) { sensor_event_t lin_accel_event; + sensor_data_t gravity_data; unsigned long long diff_time; @@ -211,17 +344,13 @@ void linear_accel_sensor::synthesize(const sensor_event_t &event, vectorget_sensor_data(GRAVITY_RAW_DATA_EVENT, gravity_data); + sensor_data_t gravity_data, accel_data, fusion_data; + m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); + gravity_data = calculate_gravity(fusion_data); + accel_data.values[0] = m_accel_rotation_direction_compensation[0] * (accel_data.values[0] - m_accel_static_bias[0]) / m_accel_scale; accel_data.values[1] = m_accel_rotation_direction_compensation[1] * (accel_data.values[1] - m_accel_static_bias[1]) / m_accel_scale; accel_data.values[2] = m_accel_rotation_direction_compensation[2] * (accel_data.values[2] - m_accel_static_bias[2]) / m_accel_scale; diff --git a/src/linear_accel/linear_accel_sensor.h b/src/linear_accel/linear_accel_sensor.h index 4ef10ab..e8f8395 100755 --- a/src/linear_accel/linear_accel_sensor.h +++ b/src/linear_accel/linear_accel_sensor.h @@ -41,11 +41,15 @@ public: bool get_properties(sensor_properties_s &properties); private: sensor_base *m_accel_sensor; - sensor_base *m_gravity_sensor; - cmutex m_value_mutex; + sensor_base *m_gyro_sensor; + sensor_base *m_magnetic_sensor; + sensor_base *m_fusion_sensor; sensor_data m_accel; - sensor_data m_gravity; + sensor_data m_gyro; + sensor_data m_magnetic; + + cmutex m_value_mutex; unsigned long long m_time; unsigned int m_interval; @@ -54,14 +58,20 @@ private: string m_vendor; string m_raw_data_unit; + string m_orientation_data_unit; int m_default_sampling_time; float m_accel_static_bias[3]; int m_accel_rotation_direction_compensation[3]; float m_accel_scale; int m_linear_accel_sign_compensation[3]; + int m_gravity_sign_compensation[3]; + int m_azimuth_rotation_compensation; + int m_pitch_rotation_compensation; + int m_roll_rotation_compensation; bool on_start(void); bool on_stop(void); + sensor_data_t calculate_gravity(sensor_data_t data); }; #endif -- 2.7.4 From 57b8e1b2e9725c6652bd000247fbfe60ca488777 Mon Sep 17 00:00:00 2001 From: Ankur Date: Tue, 19 May 2015 16:39:57 +0530 Subject: [PATCH 13/16] Fixing Proximity state definitions Change-Id: I650b15614b923ed592061bb4bc393e300308480c --- src/proxi/proxi_sensor_hal.h | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/proxi/proxi_sensor_hal.h b/src/proxi/proxi_sensor_hal.h index ebb24b9..3b96b04 100755 --- a/src/proxi/proxi_sensor_hal.h +++ b/src/proxi/proxi_sensor_hal.h @@ -28,12 +28,19 @@ using std::string; class proxi_sensor_hal : public sensor_hal { public: - enum proxi_node_state_event_t { //changed as per IIO definitions - PROXIMITY_NODE_STATE_NEAR = 1, - PROXIMITY_NODE_STATE_FAR = 2, - PROXIMITY_NODE_STATE_UNKNOWN = 0, + enum proxi_node_state_event_t { //changed as per Input Event Method definitions + PROXIMITY_NODE_STATE_NEAR = 0, + PROXIMITY_NODE_STATE_FAR = 1, + PROXIMITY_NODE_STATE_UNKNOWN = -1, }; +// In case of IIO input method, use the following definitions as the values returned by sensor are different. +// enum proxi_node_state_event_t { //changed as per IIO Method definitions +// PROXIMITY_NODE_STATE_NEAR = 1, +// PROXIMITY_NODE_STATE_FAR = 2, +// PROXIMITY_NODE_STATE_UNKNOWN = 0, +// }; + proxi_sensor_hal(); virtual ~proxi_sensor_hal(); string get_model_id(void); -- 2.7.4 From 2210a3e617f0188e346961bf72df1a0cd3eadd40 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 18 May 2015 17:55:26 +0530 Subject: [PATCH 14/16] Adding common plugin for hardware/software sensor fusion - linear accel sensor Change-Id: Ie0a5eaf3607b14ced68d0cb545c29f0b442afff6 --- src/linear_accel/linear_accel_sensor.cpp | 51 ++++++++++++++++++++++---------- 1 file changed, 35 insertions(+), 16 deletions(-) diff --git a/src/linear_accel/linear_accel_sensor.cpp b/src/linear_accel/linear_accel_sensor.cpp index c967ce2..6e21ab2 100755 --- a/src/linear_accel/linear_accel_sensor.cpp +++ b/src/linear_accel/linear_accel_sensor.cpp @@ -78,6 +78,12 @@ linear_accel_sensor::linear_accel_sensor() m_enable_linear_accel = 0; register_supported_event(LINEAR_ACCEL_RAW_DATA_EVENT); + sensor_hal *fusion_sensor_hal = sensor_plugin_loader::get_instance().get_sensor_hal(FUSION_SENSOR); + if (!fusion_sensor_hal) + m_hardware_fusion = false; + else + m_hardware_fusion = true; + if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_VENDOR, m_vendor)) { ERR("[VENDOR] is empty\n"); @@ -197,15 +203,19 @@ sensor_type_t linear_accel_sensor::get_type(void) bool linear_accel_sensor::on_start(void) { AUTOLOCK(m_mutex); + m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); m_accel_sensor->start(); - m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_gyro_sensor->start(); - 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(); + + if (!m_hardware_fusion) { + m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT); + m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); + m_gyro_sensor->start(); + m_magnetic_sensor->add_client(GEOMAGNETIC_RAW_DATA_EVENT); + m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); + m_magnetic_sensor->start(); + } m_fusion_sensor->register_supported_event(FUSION_EVENT); m_fusion_sensor->register_supported_event(FUSION_ORIENTATION_ENABLED); @@ -223,12 +233,15 @@ bool linear_accel_sensor::on_stop(void) 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(); + + if (!m_hardware_fusion) { + m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT); + m_gyro_sensor->delete_interval((intptr_t)this, false); + m_gyro_sensor->stop(); + m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT); + m_magnetic_sensor->delete_interval((intptr_t)this, false); + m_magnetic_sensor->stop(); + } m_fusion_sensor->delete_client(FUSION_EVENT); m_fusion_sensor->delete_interval((intptr_t)this, false); @@ -244,8 +257,11 @@ bool linear_accel_sensor::add_interval(int client_id, unsigned int interval) { AUTOLOCK(m_mutex); m_accel_sensor->add_interval(client_id, interval, false); - m_gyro_sensor->add_interval(client_id, interval, false); - m_magnetic_sensor->add_interval(client_id, interval, false); + + if (!m_hardware_fusion) { + m_gyro_sensor->add_interval(client_id, interval, false); + m_magnetic_sensor->add_interval(client_id, interval, false); + } m_fusion_sensor->add_interval(client_id, interval, false); @@ -256,8 +272,11 @@ bool linear_accel_sensor::delete_interval(int client_id) { AUTOLOCK(m_mutex); m_accel_sensor->delete_interval(client_id, false); - m_gyro_sensor->delete_interval(client_id, false); - m_magnetic_sensor->delete_interval(client_id, false); + + if (!m_hardware_fusion) { + m_gyro_sensor->delete_interval(client_id, false); + m_magnetic_sensor->delete_interval(client_id, false); + } m_fusion_sensor->delete_interval(client_id, false); -- 2.7.4 From 8692956fc14e36fcd6f5319df1b99e2f35885881 Mon Sep 17 00:00:00 2001 From: Ankur Date: Fri, 22 May 2015 12:03:54 +0530 Subject: [PATCH 15/16] Adding documentation for adding hardware fusion sensor in virtual sensors Change-Id: I12d4b3a93a811709be80db62df5950cc98894cba --- src/fusion/hardware_fusion_sensor.html | 163 +++++++++++++++++++++++++++++++++ 1 file changed, 163 insertions(+) create mode 100644 src/fusion/hardware_fusion_sensor.html diff --git a/src/fusion/hardware_fusion_sensor.html b/src/fusion/hardware_fusion_sensor.html new file mode 100644 index 0000000..aae6fea --- /dev/null +++ b/src/fusion/hardware_fusion_sensor.html @@ -0,0 +1,163 @@ + + + + + + + + + + + + + + + +

Supporting +Virtual Sensors based on On-Chip(Hardware Based) Sensor Fusion +Solution

+


+

+

Software +Fusion Sensor:

+


+

+

The +fusion sensor registers for accelerometer, gyroscope and geomagnetic +sensors. Uses the data from these three sensors to fusion_event, +which is used by various sensors to generate their specific sensor +events.

+


+

+

->The +virtual sensor starts the hardware and fusion sensor.

+

->Fusion +sensor registers for hardware sensors. It received accelerometer, +gyroscope and/or geomagnetic sensor events.

+

->Based +on what all events are received, fusion sensor generates a fusion +sensor event.

+

->The +FUSION_EVENT is sent to all virtual sensors which have registered for +fusion sensor.

+

->The +virtual sensors then use the data from FUSION_EVENT to create their +specific virtual sensor events.

+


+

+

Hardware +Fusion Sensor:

+


+

+

In +case hardware based fusion sensor is present, individual sensors +won't be required by fusion sensor to generate FUSION_EVENT.

+

So, +virtual sensors won't have to register and start the individual +hardware sensors (accel, gyro or geomagnetic) for receiving +FUSION_EVENT. They will just have to register for hardware fusion +sensor event and they will receive the hardware sensor event.

+


+

+

Adding +hardware fusion sensor in virtual sensors:

+

In +case hardware fusion sensor is present, instead of registering of +individual hardware sensors and fusion sensor, virtual sensor will +register for just sensor hal fusion sensor. +

+


+

+

Also, +fusion sensor will register for just fusion_sensor_hal and send the +events to all virtual sensor registered for fusion sensor events.

+


+

+

For +this, the virtual sensor must register/start specific sensors in case +hardware fusion sensor is present or not.

+


+

+

In +any virtual sensor class, to add support for both hardware and +software sensor fusion, +

+


+

+

->In +the constructor use sensor_plugin_loader to check if the hal sensor +fusion is present or not:

+

sensor_plugin_loader::get_instance().get_sensor_hal(FUSION_SENSOR)

+


+

+

->If +it returns NULL, in that case, hardware fusion sensor is not present, +else the hardware fusion is present.

+

->Store +this result in virtual_sensor::m_hardware_fusion

+


+

+

sensor_hal +*fusion_sensor_hal = +sensor_plugin_loader::get_instance().get_sensor_hal(FUSION_SENSOR);

+

if +(!fusion_sensor_hal)

+

  m_hardware_fusion += false;

+

else

+

  m_hardware_fusion += true;

+


+

+


+

+

->Bases +on this, in on_start, on_stop, add_interval, delete_interval etc +functions, the physical sensors will be started in case hardware +fusion sensor is not present and only fusion sensor will be started +in case hardware fusion sensor is present.

+


+

+


+

+

bool +orientation_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_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;

+

}

+ + -- 2.7.4 From 850ec1aa7dd85d5c252c64a0cd3b8771bca422cc Mon Sep 17 00:00:00 2001 From: Ankur Date: Fri, 29 May 2015 14:33:13 +0530 Subject: [PATCH 16/16] Adding tilt sensor event definitions [sensor_tilt.h] to avoid build break the sensor_tilt.h file containing the tilt sensor event definitions was missing. This was causing the build to break. Added the tilt sensor event definitions with tilt_raw_data_event. signed-off-by: Ankur Garg Change-Id: I2d7768d70d05c7e4c1be08910276b958e76ef761 --- src/libsensord/sensor_tilt.h | 52 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 src/libsensord/sensor_tilt.h diff --git a/src/libsensord/sensor_tilt.h b/src/libsensord/sensor_tilt.h new file mode 100644 index 0000000..8b15d01 --- /dev/null +++ b/src/libsensord/sensor_tilt.h @@ -0,0 +1,52 @@ +/* + * libsensord + * + * 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 __SENSOR_TILT_H__ +#define __SENSOR_TILT_H__ + +//! Pre-defined events for the tilt sensor +//! Sensor Plugin developer can add more event to their own headers + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** + * @defgroup SENSOR_TILT tilt Sensor + * @ingroup SENSOR_FRAMEWORK + * + * These APIs are used to control the tilt sensor. + * @{ + */ + +enum tilt_event_type { + TILT_RAW_DATA_EVENT = (TILT_SENSOR << 16) | 0x0001, +}; + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif +//! End of a file -- 2.7.4