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 f3e7421d21da32a676e71acb458c57af91575705 Mon Sep 17 00:00:00 2001 From: Ankur Date: Fri, 29 May 2015 14:42:59 +0530 Subject: [PATCH 15/16] Code Cleanup: cleaning up unused functions and variables from gaming rotation vector removing unused functions, variables and macros from gaming rotation vector Change-Id: Ia59c6d171fc4ed01ffe5e670fbc61a9c3f67c05f --- src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp | 12 ------------ src/rotation_vector/gaming_rv/gaming_rv_sensor.h | 5 ----- 2 files changed, 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 0b3559f..36a8d4d 100755 --- a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp +++ b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp @@ -37,22 +37,11 @@ #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" -void pre_process_data(sensor_data &data_out, const float *data_in, float *bias, int *sign, float scale) -{ - data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale; - data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale; - data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale; -} - gaming_rv_sensor::gaming_rv_sensor() : m_accel_sensor(NULL) , m_gyro_sensor(NULL) @@ -69,7 +58,6 @@ gaming_rv_sensor::gaming_rv_sensor() m_name = string(SENSOR_NAME); register_supported_event(GAMING_RV_RAW_DATA_EVENT); - m_enable_gaming_rv = 0; if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_VENDOR, m_vendor)) { ERR("[VENDOR] is empty\n"); diff --git a/src/rotation_vector/gaming_rv/gaming_rv_sensor.h b/src/rotation_vector/gaming_rv/gaming_rv_sensor.h index 9a2a7e7..6572907 100755 --- a/src/rotation_vector/gaming_rv/gaming_rv_sensor.h +++ b/src/rotation_vector/gaming_rv/gaming_rv_sensor.h @@ -50,11 +50,6 @@ private: cmutex m_value_mutex; - orientation_filter m_orientation_filter; - orientation_filter m_orientation_filter_poll; - - unsigned int m_enable_gaming_rv; - unsigned long long m_time; unsigned int m_interval; -- 2.7.4 From 2ee554166327c80f89603b5e886a016d9c0932f2 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 5 Jun 2015 16:19:01 +0900 Subject: [PATCH 16/16] Adding octave code to compute gyro bias - gyro bias is needed for calibrated gyroscope sensor event - updated octave design code to simulate the gyroscope bias and plot it - substituted euler error code(which showed only drift) with gyroscope bias Change-Id: I3e411db520d6103326c4e2950e05d6930e34dd9b --- .../design/lib/estimate_orientation.m | 6 +++++- src/sensor_fusion/design/sf_orientation.m | 25 +++++++++++----------- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m index d7f023a..b2e3b73 100755 --- a/src/sensor_fusion/design/lib/estimate_orientation.m +++ b/src/sensor_fusion/design/lib/estimate_orientation.m @@ -23,7 +23,7 @@ % - Quaternion based approach % - Estimation and correction of orientation errors and bias errors for gyroscope using Kalman filter -function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, Gyro_data, Mag_data) +function [quat_driv, quat_aid, quat_error, gyro_bias] = estimate_orientation(Accel_data, Gyro_data, Mag_data) PLOT_SCALED_SENSOR_COMPARISON_DATA = 1; PLOT_INDIVIDUAL_SENSOR_INPUT_DATA = 1; @@ -42,6 +42,7 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G GRAVITY = 9.80665; PI = 3.141593; NON_ZERO_VAL = 0.1; + PI_DEG = 180; MOVING_AVERAGE_WINDOW_LENGTH = 20; RAD2DEG = 57.2957795; @@ -62,6 +63,8 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G mag_z = zeros(1,BUFFER_SIZE); MTime = zeros(1,BUFFER_SIZE); + gyro_bias = zeros(3,BUFFER_SIZE); + if MAG_DATA_DISABLED != 1 Mx = Mag_data(1,:); My = Mag_data(2,:); @@ -270,6 +273,7 @@ function [quat_driv, quat_aid, quat_error] = estimate_orientation(Accel_data, G Bx = x(4,i); By = x(5,i); Bz = x(6,i); + gyro_bias(:,i) = [x1; x2; x3] * PI_DEG + [Bx; By; Bz]; end end diff --git a/src/sensor_fusion/design/sf_orientation.m b/src/sensor_fusion/design/sf_orientation.m index 3a97580..1c181cb 100755 --- a/src/sensor_fusion/design/sf_orientation.m +++ b/src/sensor_fusion/design/sf_orientation.m @@ -100,7 +100,11 @@ Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt") Mag_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,4))')(1:BUFFER_SIZE); % estimate orientation -[Quat_driv, Quat_aid, Quat_err] = estimate_orientation(Accel_data, Gyro_data, Mag_data); +[Quat_driv, Quat_aid, Quat_err, Gyro_bias] = estimate_orientation(Accel_data, Gyro_data, Mag_data); + +Gyro_bias(1,:) = Gyro_bias(1,:) + Bias_Gx; +Gyro_bias(2,:) = Gyro_bias(2,:) + Bias_Gy; +Gyro_bias(3,:) = Gyro_bias(3,:) + Bias_Gz; for i = 1:BUFFER_SIZE euler_aid(i,:) = quat2euler(Quat_aid(i,:)); @@ -112,7 +116,7 @@ for i = 1:BUFFER_SIZE OR_driv(1,i) = ROLL_PHASE_CORRECTION * euler_driv(i,2)' * RAD2DEG; OR_driv(2,i) = PITCH_PHASE_CORRECTION * euler_driv(i,1)' * RAD2DEG; OR_driv(3,i) = YAW_PHASE_CORRECTION * euler_driv(i,3)' * RAD2DEG; - + if (OR_driv(3,i) < 0) OR_driv(3,i) = OR_driv(3,i) + 360; end @@ -120,11 +124,6 @@ for i = 1:BUFFER_SIZE if (OR_aid(3,i) < 0) OR_aid(3,i) = OR_aid(3,i) + 360; end - - euler_err(i,:) = quat2euler(Quat_err(i,:)); - OR_err(1,i) = euler_err(i,2)' * RAD2DEG; - OR_err(2,i) = euler_err(i,1)' * RAD2DEG; - OR_err(3,i) = euler_err(i,3)' * RAD2DEG; end % Rotation Plot Results @@ -140,10 +139,10 @@ UA = OR_driv(2,:); p2 = plot(1:length(UA),UA(1,1:length(UA)),'b'); hold on; grid on; -UA = OR_err(2,:); +UA = Gyro_bias(1,:); p3 = plot(1:length(UA),UA(1,1:length(UA)),'g'); title(['Pitch']); -legend([p1 p2 p3],'Aiding System', 'Driving System', 'Quaternion based error'); +legend([p1 p2 p3],'Aiding System', 'Driving System', 'Gyroscope Bias error'); subplot(3,1,2) UA = OR_aid(1,:); p1 = plot(1:length(UA),UA(1,1:length(UA)),'r'); @@ -153,10 +152,10 @@ UA = OR_driv(1,:); p2 = plot(1:length(UA),UA(1,1:length(UA)),'b'); hold on; grid on; -UA = OR_err(1,:); +UA = Gyro_bias(2,:); p3 = plot(1:length(UA),UA(1,1:length(UA)),'g'); title(['Roll']); -legend([p1 p2 p3],'Aiding System', 'Driving System', 'Quaternion based error'); +legend([p1 p2 p3],'Aiding System', 'Driving System', 'Gyroscope Bias error'); subplot(3,1,3) UA = OR_aid(3,:); p1 = plot(1:length(UA),UA(1,1:length(UA)),'r'); @@ -166,7 +165,7 @@ UA = OR_driv(3,:); p2 = plot(1:length(UA),UA(1,1:length(UA)),'b'); hold on; grid on; -UA = OR_err(3,:); +UA = Gyro_bias(3,:); p3 = plot(1:length(UA),UA(1,1:length(UA)),'g'); title(['Yaw']); -legend([p1 p2 p3],'Aiding System', 'Driving System', 'Quaternion based error'); +legend([p1 p2 p3],'Aiding System', 'Driving System', 'Gyroscope Bias error'); -- 2.7.4