From d57be8cc5575106d79022feb5549dc918605f53d Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Mon, 22 Dec 2014 14:32:22 +0530 Subject: [PATCH 01/16] Adding polling based support for testing of temperature sensor Change-Id: I3ff0750bd160c3259062a4f91542b69947597b53 --- packaging/sensord.spec | 1 + test/CMakeLists.txt | 4 ++ test/src/temperature.c | 140 +++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 145 insertions(+) create mode 100644 test/src/temperature.c diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 1cab59a..1eaccdf 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -144,6 +144,7 @@ systemctl daemon-reload /usr/bin/gyro /usr/bin/proxi /usr/bin/pressure +/usr/bin/temperature %license LICENSE.APLv2 %{_datadir}/license/test diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 24b24ce..5c89818 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -35,6 +35,7 @@ add_executable(linear_acceleration src/linear_acceleration.c) add_executable(gyro src/gyro.c) add_executable(proxi src/proxi.c) add_executable(pressure src/pressure.c) +add_executable(temperature src/temperature.c) SET_TARGET_PROPERTIES(accelerometer PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(geomagnetic PROPERTIES LINKER_LANGUAGE C) @@ -44,6 +45,7 @@ SET_TARGET_PROPERTIES(linear_acceleration PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(gyro PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(proxi PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(pressure PROPERTIES LINKER_LANGUAGE C) +SET_TARGET_PROPERTIES(temperature PROPERTIES LINKER_LANGUAGE C) target_link_libraries(accelerometer glib-2.0 dlog sensor) target_link_libraries(geomagnetic glib-2.0 dlog sensor) @@ -53,6 +55,7 @@ target_link_libraries(linear_acceleration glib-2.0 dlog sensor) target_link_libraries(gyro glib-2.0 dlog sensor) target_link_libraries(proxi glib-2.0 dlog sensor) target_link_libraries(pressure glib-2.0 dlog sensor) +target_link_libraries(temperature glib-2.0 dlog sensor) INSTALL(TARGETS accelerometer DESTINATION /usr/bin/) INSTALL(TARGETS geomagnetic DESTINATION /usr/bin/) @@ -62,4 +65,5 @@ INSTALL(TARGETS linear_acceleration DESTINATION /usr/bin/) INSTALL(TARGETS gyro DESTINATION /usr/bin/) INSTALL(TARGETS proxi DESTINATION /usr/bin/) INSTALL(TARGETS pressure DESTINATION /usr/bin/) +INSTALL(TARGETS temperature DESTINATION /usr/bin/) diff --git a/test/src/temperature.c b/test/src/temperature.c new file mode 100644 index 0000000..ffdb2e9 --- /dev/null +++ b/test/src/temperature.c @@ -0,0 +1,140 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#include +#include +#include +#include +#include +#include +#include +#include + +static GMainLoop *mainloop; + +void callback(unsigned int event_type, sensor_event_data_t *event, void *user_data) +{ + sensor_data_t *data = (sensor_data_t *)event->event_data; + printf("Temperature [%lld] [%6.6f] \n\n", data->timestamp, data->values[0]); +} + +void printformat() +{ + printf("Usage : ./temperature (optional) (optional)\n\n"); + + printf("mode:"); + printf("[-p]\n"); + printf("p is for polling based,default mode is event driven\n"); + + printf("event:"); + printf("[RAW_DATA_REPORT_ON_TIME]\n"); + + printf("interval:\n"); + printf("The time interval should be entered based on the sampling frequency supported by temperature driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n"); +} + +int main(int argc,char **argv) +{ + int result, handle, start_handle, stop_handle; + unsigned int event; + mainloop = g_main_loop_new(NULL, FALSE); + event = TEMPERATURE_EVENT_RAW_DATA_REPORT_ON_TIME; + event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); + event_condition->cond_op = CONDITION_EQUAL; + + sensor_type_t type = TEMPERATURE_SENSOR; + + if (argc != 2 && argc != 3 && argc!=4) { + printformat(); + free(event_condition); + return 0; + } + + else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Polling based\n"); + handle = sf_connect(type); + result = sf_start(handle, 1); + + if (result < 0) { + printf("Can't start temperature SENSOR\n"); + printf("Error\n\n\n\n"); + return -1; + } + + sensor_data_t data; + + while(1) { + result = sf_get_data(handle, TEMPERATURE_BASE_DATA_SET , &data); + printf("Temperature [%6.6f] \n\n", data.values[0]); + usleep(100000); + } + + result = sf_disconnect(handle); + + if (result < 0) { + printf("Can't disconnect temperature sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } + } + + else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Event based\n"); + + event_condition->cond_value1 = 100; + if (argc == 3) + event_condition->cond_value1 = atof(argv[2]); + + handle = sf_connect(type); + result = sf_register_event(handle, event, event_condition, callback, NULL); + + if (result < 0) + printf("Can't register temperature\n"); + + start_handle = sf_start(handle,0); + + if (start_handle < 0) { + printf("Error\n\n\n\n"); + sf_unregister_event(handle, event); + sf_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); + + sf_unregister_event(handle, event); + + stop_handle = sf_stop(handle); + + if (stop_handle < 0) { + printf("Error\n\n"); + return -1; + } + + sf_disconnect(handle); + free(event_condition); + } + + else { + printformat(); + } + + return 0; +} + -- 2.7.4 From d9f0239a7425ff1d549104dbde260fa28624f19c Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 22 Dec 2014 15:12:25 +0530 Subject: [PATCH 02/16] Removed warnings related to comparison between signed and unsigned int -converted int variables to unsinged int to remove warnings for comparison between signed and unsigned integers. Change-Id: I3e0187d002b19b215a5d542d1206ba51ef663368 --- src/shared/csensor_event_dispatcher.cpp | 2 +- src/shared/sensor_info.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/shared/csensor_event_dispatcher.cpp b/src/shared/csensor_event_dispatcher.cpp index f9f90d5..cee0cf3 100755 --- a/src/shared/csensor_event_dispatcher.cpp +++ b/src/shared/csensor_event_dispatcher.cpp @@ -171,7 +171,7 @@ void csensor_event_dispatcher::dispatch_event(void) sort_sensor_events(sensor_events, event_cnt); - for (int i = 0; i < event_cnt; ++i) { + for (unsigned int i = 0; i < event_cnt; ++i) { if (is_record_event(sensor_events[i].event_type)) put_last_event(sensor_events[i].event_type, sensor_events[i]); } diff --git a/src/shared/sensor_info.cpp b/src/shared/sensor_info.cpp index 56da0c9..0521aea 100755 --- a/src/shared/sensor_info.cpp +++ b/src/shared/sensor_info.cpp @@ -211,8 +211,8 @@ void sensor_info::show(void) INFO("Fifo_count = %d", m_fifo_count); INFO("Max_batch_count = %d", m_max_batch_count); - for (int i = 0; i < m_supported_events.size(); ++i) - INFO("supported_events[%d] = 0x%x", i, m_supported_events[i]); + for (unsigned int i = 0; i < m_supported_events.size(); ++i) + INFO("supported_events[%u] = 0x%x", i, m_supported_events[i]); } -- 2.7.4 From 0258205da40c08ac1b7ff1e2f2041682e1edafd9 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 22 Dec 2014 15:46:46 +0530 Subject: [PATCH 03/16] Adding orientation filter to support quaternion output - orientation_filter would output device orientation as quaternion to support rotation_vector virtual sensor. Change-Id: I5e0cdcb1b218c59d32965deac76629f04c728899 --- src/sensor_fusion/orientation_filter.cpp | 13 +++++++++++++ src/sensor_fusion/orientation_filter.h | 3 +++ src/sensor_fusion/standalone/orientation_sensor.cpp | 18 +++++++++++++++++- src/sensor_fusion/standalone/orientation_sensor.h | 2 ++ .../orientation_sensor_main.cpp | 9 +++++++-- 5 files changed, 42 insertions(+), 3 deletions(-) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index a615217..05537a3 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -218,8 +218,11 @@ inline void orientation_filter::time_update() m_quat_driv = m_quat_driv + (quat_diff * (TYPE) m_gyro_dt * (TYPE) PI); m_quat_driv.quat_normalize(); + quat_output = phase_correction(m_quat_driv, m_quat_aid); + m_quaternion = quat_output; + orientation = quat2euler(quat_output); m_orientation.m_ang.m_vec[0] = orientation.m_ang.m_vec[0] * m_pitch_phase_compensation; @@ -313,4 +316,14 @@ rotation_matrix orientation_filter::get_rotation_matrix(const sensor return m_rot_matrix; } +template +quaternion orientation_filter::get_quaternion(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic) +{ + + get_orientation(accel, gyro, magnetic); + + return m_quaternion; +} + #endif //_ORIENTATION_FILTER_H_ diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index f7bc4d7..40e2c50 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -52,6 +52,7 @@ public: quaternion m_quat_driv; rotation_matrix m_rot_matrix; euler_angles m_orientation; + quaternion m_quaternion; TYPE m_gyro_dt; int m_pitch_phase_compensation; @@ -73,6 +74,8 @@ public: const sensor_data gyro, const sensor_data magnetic); rotation_matrix get_rotation_matrix(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic); + quaternion get_quaternion(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic); }; #include "orientation_filter.cpp" diff --git a/src/sensor_fusion/standalone/orientation_sensor.cpp b/src/sensor_fusion/standalone/orientation_sensor.cpp index c006922..82c227f 100644 --- a/src/sensor_fusion/standalone/orientation_sensor.cpp +++ b/src/sensor_fusion/standalone/orientation_sensor.cpp @@ -64,7 +64,6 @@ euler_angles orientation_sensor::get_orientation(sensor_data accel rotation_matrix orientation_sensor::get_rotation_matrix(sensor_data accel_data, sensor_data gyro_data, sensor_data magnetic_data) { - pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); normalize(accel_data); pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); @@ -79,4 +78,21 @@ rotation_matrix orientation_sensor::get_rotation_matrix(sensor_data orientation_sensor::get_quaternion(sensor_data accel_data, + sensor_data gyro_data, sensor_data magnetic_data) +{ + pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + normalize(accel_data); + pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); + pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); + normalize(magnetic_data); + + orien_filter.m_pitch_phase_compensation = pitch_phase_compensation; + orien_filter.m_roll_phase_compensation = roll_phase_compensation; + orien_filter.m_azimuth_phase_compensation = azimuth_phase_compensation; + orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor; + + return orien_filter.get_quaternion(accel_data, gyro_data, magnetic_data); +} + #endif diff --git a/src/sensor_fusion/standalone/orientation_sensor.h b/src/sensor_fusion/standalone/orientation_sensor.h index a02e2af..1ead53e 100644 --- a/src/sensor_fusion/standalone/orientation_sensor.h +++ b/src/sensor_fusion/standalone/orientation_sensor.h @@ -31,6 +31,8 @@ public: sensor_data gyro, sensor_data magnetic); rotation_matrix get_rotation_matrix(sensor_data accel, sensor_data gyro, sensor_data magnetic); + quaternion get_quaternion(sensor_data accel, + sensor_data gyro, sensor_data magnetic); }; #include "orientation_sensor.cpp" diff --git a/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp b/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp index b3ef1f5..12058e0 100644 --- a/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp +++ b/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp @@ -37,7 +37,8 @@ int main() unsigned long long time_stamp; euler_angles orientation; rotation_matrix orientation_mat; - orientation_sensor orien_sensor1, orien_sensor2; + quaternion orientation_quat; + orientation_sensor orien_sensor1, orien_sensor2, orien_sensor3; accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str()); gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str()); @@ -82,9 +83,13 @@ int main() cout << "Orientation angles\t" << orientation.m_ang << "\n\n"; - orientation_mat = orien_sensor1.get_rotation_matrix(accel_data, gyro_data, magnetic_data); + orientation_mat = orien_sensor2.get_rotation_matrix(accel_data, gyro_data, magnetic_data); cout << "Orientation matrix\t" << orientation_mat.m_rot_mat << "\n\n"; + + orientation_quat = orien_sensor3.get_quaternion(accel_data, gyro_data, magnetic_data); + + cout << "Orientation quaternion\t" << orientation_quat.m_quat << "\n\n"; } accel_in.close(); -- 2.7.4 From 11824fe5196cc1f71b42ef9d612dd9a83acd4b75 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 22 Dec 2014 16:37:30 +0530 Subject: [PATCH 04/16] Removed compiler warning for converting bool 'false' to pointer 'void *' -In the function sensor_get_sensor() in src/libsensord/client.cpp boolean 'false' was being converted to void * This was showing as a warning at compile time. For returning failed cases, NULL should be returned. The call to sensor_info_to_sensor in this function also returns NULL when failed. -Tested the code after change. Seems to be working fine. Change-Id: Id100370c9bc8b9f62c86acb6968f766b10370d7b --- src/libsensord/client.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libsensord/client.cpp b/src/libsensord/client.cpp index 9acf884..2f1e649 100755 --- a/src/libsensord/client.cpp +++ b/src/libsensord/client.cpp @@ -423,7 +423,7 @@ API bool sensord_get_sensor_list(sensor_type_t type, sensor_t **list, int *senso API sensor_t sensord_get_sensor(sensor_type_t type) { - retvm_if (!get_sensor_list(), false, "Fail to get sensor list from server"); + retvm_if (!get_sensor_list(), NULL, "Fail to get sensor list from server"); const sensor_info *info; -- 2.7.4 From a7cede0b57291d01d4c196ca6af9a32481241d62 Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Mon, 22 Dec 2014 17:28:25 +0530 Subject: [PATCH 05/16] Adding polling based support for testing accelerometer Change-Id: Id1a4343e8ad9ca44d9bb5a1027526ed50f88b6e2 --- test/src/accelerometer.c | 108 ++++++++++++++++++++++++++++++----------------- 1 file changed, 70 insertions(+), 38 deletions(-) diff --git a/test/src/accelerometer.c b/test/src/accelerometer.c index c7ea6e6..fe61f37 100644 --- a/test/src/accelerometer.c +++ b/test/src/accelerometer.c @@ -22,7 +22,8 @@ #include #include #include -#include +#include +#include static GMainLoop *mainloop; @@ -34,10 +35,14 @@ void callback(unsigned int event_type, sensor_event_data_t *event, void *user_da void printformat() { - printf("Usage : ./accelerometer (optional)\n\n"); - printf("event:\n"); + printf("Usage : ./accelerometer (optional) (optional)\n\n"); - printf("RAW_DATA_REPORT_ON_TIME\n"); + printf("mode:"); + printf("[-p]\n"); + printf("p is for polling based,default mode is event driven\n"); + + printf("event:"); + printf("[RAW_DATA_REPORT_ON_TIME]\n"); printf("interval:\n"); printf("The time interval should be entered based on the sampling frequency supported by accelerometer driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n"); @@ -47,61 +52,88 @@ int main(int argc,char **argv) { int result, handle, start_handle, stop_handle; unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - sensor_type_t type = ACCELEROMETER_SENSOR; + event = ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME; event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); event_condition->cond_op = CONDITION_EQUAL; - event_condition->cond_value1 = 100; - if (argc != 2 && argc != 3) { + sensor_type_t type = ACCELEROMETER_SENSOR; + + if (argc != 2 && argc != 3 && argc!=4) { printformat(); free(event_condition); return 0; } - if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { - event = ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME; - } + else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Polling based\n"); + handle = sf_connect(type); + result = sf_start(handle, 1); - else { - printformat(); - free(event_condition); - return 0; - } - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); + if (result < 0) { + printf("Can't start accelerometer SENSOR\n"); + printf("Error\n\n\n\n"); + return -1; + } - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); + sensor_data_t data; - if (result < 0) - printf("Can't register accelerometer\n"); + while(1) { + result = sf_get_data(handle, ACCELEROMETER_BASE_DATA_SET , &data); + printf("Accelerometer [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); + usleep(100000); + } - start_handle = sf_start(handle,0); + result = sf_disconnect(handle); - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); - sf_disconnect(handle); - return -1; + if (result < 0) { + printf("Can't disconnect Accelerometer sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } } - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); + else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Event based\n"); - sf_unregister_event(handle, event); + event_condition->cond_value1 = 100; + if (argc == 3) + event_condition->cond_value1 = atof(argv[2]); - stop_handle = sf_stop(handle); + handle = sf_connect(type); + result = sf_register_event(handle, event, event_condition, callback, NULL); - if (stop_handle < 0) { - printf("Error\n\n"); - return -1; - } + if (result < 0) + printf("Can't register accelerometer\n"); - sf_disconnect(handle); + start_handle = sf_start(handle,0); - free(event_condition); + if (start_handle < 0) { + printf("Error\n\n\n\n"); + sf_unregister_event(handle, event); + sf_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); + + sf_unregister_event(handle, event); + + stop_handle = sf_stop(handle); + + if (stop_handle < 0) { + printf("Error\n\n"); + return -1; + } + + sf_disconnect(handle); + free(event_condition); + } + + else { + printformat(); + } return 0; } -- 2.7.4 From 1efa8724e74e955ac8402718d58677c970c91c75 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 22 Dec 2014 17:35:34 +0530 Subject: [PATCH 06/16] Removed warnings related to unused variables -Removed two unused variables. Both variables were never being intialised or being assigned or being used anywhere in the code. The variables were just declared. Safe to remove such variables. Change-Id: I998a1c0c2978d48150c1b15e9b0cc083e430c73a --- src/libsensord/client.cpp | 2 +- src/orientation/orientation_sensor.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/libsensord/client.cpp b/src/libsensord/client.cpp index 2f1e649..79245d7 100755 --- a/src/libsensord/client.cpp +++ b/src/libsensord/client.cpp @@ -108,7 +108,7 @@ static void clean_up(void) static int get_power_save_state (void) { int state = 0; - int pm_state, ps_state; + int pm_state; vconf_get_int(VCONFKEY_PM_STATE, &pm_state); diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index 325c004..dd0f818 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -295,7 +295,6 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector euler_orientation; - float raw_data[3]; float azimuth_offset; if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { -- 2.7.4 From ddbc8c8396afda438ec3907db2004ea555584eb0 Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Mon, 22 Dec 2014 17:39:15 +0530 Subject: [PATCH 07/16] Adding polling based support for testing gyroscope sensor Change-Id: Ibff596783a3d5c69f8fedc284df2f46cea144315 --- test/src/gyro.c | 112 ++++++++++++++++++++++++++++++++++++-------------------- 1 file changed, 73 insertions(+), 39 deletions(-) diff --git a/test/src/gyro.c b/test/src/gyro.c index ae34d95..b74d98d 100644 --- a/test/src/gyro.c +++ b/test/src/gyro.c @@ -16,28 +16,34 @@ * limitations under the License. * */ - #include #include #include #include #include #include -#include +#include +#include static GMainLoop *mainloop; void callback(unsigned int event_type, sensor_event_data_t *event, void *user_data) { sensor_data_t *data = (sensor_data_t *)event->event_data; - printf("Gyroscope [%lld] [%6.6f] [%6.6f] [%6.6f] \n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); + printf("Gyroscope [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data->timestamp, data->values[0], data->values[1], data->values[2]); } void printformat() { - printf("Usage : ./gyro (optional)\n\n"); - printf("event:\n"); - printf("RAW_DATA_REPORT_ON_TIME\n"); + printf("Usage : ./gyro (optional) (optional)\n\n"); + + printf("mode:"); + printf("[-p]\n"); + printf("p is for polling based,default mode is event driven\n"); + + printf("event:"); + printf("[RAW_DATA_REPORT_ON_TIME]\n"); + printf("interval:\n"); printf("The time interval should be entered based on the sampling frequency supported by gyroscope driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n"); } @@ -46,61 +52,89 @@ int main(int argc,char **argv) { int result, handle, start_handle, stop_handle; unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - sensor_type_t type = GYROSCOPE_SENSOR; + event = GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME; event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); event_condition->cond_op = CONDITION_EQUAL; - event_condition->cond_value1 = 10.52; - if (argc != 2 && argc != 3) { + sensor_type_t type = GYROSCOPE_SENSOR; + + if (argc != 2 && argc != 3 && argc!=4) { printformat(); free(event_condition); return 0; } - else { - if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) - event = GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME; - else { - printformat(); - free(event_condition); - return 0; + else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Polling based\n"); + + handle = sf_connect(type); + result = sf_start(handle, 1); + + if (result < 0) { + printf("Can't start gyroscope SENSOR\n"); + printf("Error\n\n\n\n"); + return -1; } - if(argc == 3) - event_condition->cond_value1 = atof(argv[2]); + sensor_data_t data; + + while(1) { + result = sf_get_data(handle, GYRO_BASE_DATA_SET , &data); + printf("Gyroscope [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); + usleep(100000); + } + + result = sf_disconnect(handle); + + if (result < 0) { + printf("Can't disconnect gyroscope sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } } - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); + else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Event based\n"); + + event_condition->cond_value1 = 10.52; + if (argc == 3) + event_condition->cond_value1 = atof(argv[2]); + + handle = sf_connect(type); + result = sf_register_event(handle, event, event_condition, callback, NULL); - if (result < 0) - printf("Can't register gyroscope\n"); + if (result < 0) + printf("Can't register gyroscope\n"); - start_handle = sf_start(handle, 0); + start_handle = sf_start(handle,0); + + if (start_handle < 0) { + printf("Error\n\n\n\n"); + sf_unregister_event(handle, event); + sf_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); - if (start_handle < 0) { - printf("Error\n\n\n\n"); sf_unregister_event(handle, event); - sf_disconnect(handle); - return -1; - } - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); + stop_handle = sf_stop(handle); - sf_unregister_event(handle, event); - stop_handle = sf_stop(handle); + if (stop_handle < 0) { + printf("Error\n\n"); + return -1; + } - if (stop_handle < 0) { - printf("Error\n\n"); - return -1; + sf_disconnect(handle); + free(event_condition); } - sf_disconnect(handle); - - free(event_condition); + else { + printformat(); + } return 0; } -- 2.7.4 From a1ae1f819ee4830c3e565863b1fb4d75efdf3c7d Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Mon, 22 Dec 2014 17:50:40 +0530 Subject: [PATCH 08/16] Adding polling based support for testing pressure sensor Change-Id: I843b060db3f28a76cbac33c7deebb701055e8681 --- test/src/pressure.c | 111 ++++++++++++++++++++++++++++++++++------------------ 1 file changed, 73 insertions(+), 38 deletions(-) diff --git a/test/src/pressure.c b/test/src/pressure.c index e3ed4b4..a92ad59 100644 --- a/test/src/pressure.c +++ b/test/src/pressure.c @@ -16,14 +16,14 @@ * limitations under the License. * */ - #include #include #include #include #include #include -#include +#include +#include static GMainLoop *mainloop; @@ -35,71 +35,106 @@ void callback(unsigned int event_type, sensor_event_data_t *event, void *user_da void printformat() { - printf("Usage : ./pressure (optional)\n\n"); - printf("event:\n"); - printf("RAW_DATA_REPORT_ON_TIME\n"); + printf("Usage : ./pressure (optional) (optional)\n\n"); + + printf("mode:"); + printf("[-p]\n"); + printf("p is for polling based,default mode is event driven\n"); + + printf("event:"); + printf("[RAW_DATA_REPORT_ON_TIME]\n"); + printf("interval:\n"); printf("The time interval should be entered based on the sampling frequency supported by pressure driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n"); } -int main(int argc, char **argv) +int main(int argc,char **argv) { int result, handle, start_handle, stop_handle; unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - sensor_type_t type = PRESSURE_SENSOR; + event = PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME; event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); event_condition->cond_op = CONDITION_EQUAL; - event_condition->cond_value1 = 100; - if (argc != 2 && argc != 3) { - printformat(); - free(event_condition); - return 0; - } + sensor_type_t type = PRESSURE_SENSOR; - if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") != 0) { + if (argc != 2 && argc != 3 && argc!=4) { printformat(); free(event_condition); return 0; } - event = PRESSURE_EVENT_RAW_DATA_REPORT_ON_TIME; + else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Polling based\n"); - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); + handle = sf_connect(type); + result = sf_start(handle, 1); - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); + if (result < 0) { + printf("Can't start pressure SENSOR\n"); + printf("Error\n\n\n\n"); + return -1; + } - if (result < 0) - printf("Can't register pressure\n"); + sensor_data_t data; - start_handle = sf_start(handle,0); + while(1) { + result = sf_get_data(handle, PRESSURE_BASE_DATA_SET , &data); + printf("Pressure [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); + usleep(100000); + } - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); - sf_disconnect(handle); - return -1; + result = sf_disconnect(handle); + + if (result < 0) { + printf("Can't disconnect pressure sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } } - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); + else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Event based\n"); - sf_unregister_event(handle, event); + event_condition->cond_value1 = 10.52; + if (argc == 3) + event_condition->cond_value1 = atof(argv[2]); - stop_handle = sf_stop(handle); + handle = sf_connect(type); + result = sf_register_event(handle, event, event_condition, callback, NULL); - if (stop_handle < 0) { - printf("Error\n\n"); - return -1; - } + if (result < 0) + printf("Can't register pressure\n"); + + start_handle = sf_start(handle,0); + + if (start_handle < 0) { + printf("Error\n\n\n\n"); + sf_unregister_event(handle, event); + sf_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); + + sf_unregister_event(handle, event); - sf_disconnect(handle); + stop_handle = sf_stop(handle); - free(event_condition); + if (stop_handle < 0) { + printf("Error\n\n"); + return -1; + } + + sf_disconnect(handle); + free(event_condition); + } + + else { + printformat(); + } return 0; } -- 2.7.4 From 47655267960bc72d0b3be5fc495a991dbb982fb2 Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Mon, 22 Dec 2014 18:04:12 +0530 Subject: [PATCH 09/16] Adding polling based support for testing of light sensor Change-Id: Iab85d66ad648a2ecd5669938b448728ddfe898c7 --- packaging/sensord.spec | 1 + test/CMakeLists.txt | 4 ++ test/src/light.c | 140 +++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 145 insertions(+) create mode 100644 test/src/light.c diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 1eaccdf..2e1fff7 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -145,6 +145,7 @@ systemctl daemon-reload /usr/bin/proxi /usr/bin/pressure /usr/bin/temperature +/usr/bin/light %license LICENSE.APLv2 %{_datadir}/license/test diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 5c89818..3c103cc 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -36,6 +36,7 @@ add_executable(gyro src/gyro.c) add_executable(proxi src/proxi.c) add_executable(pressure src/pressure.c) add_executable(temperature src/temperature.c) +add_executable(light src/light.c) SET_TARGET_PROPERTIES(accelerometer PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(geomagnetic PROPERTIES LINKER_LANGUAGE C) @@ -46,6 +47,7 @@ SET_TARGET_PROPERTIES(gyro PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(proxi PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(pressure PROPERTIES LINKER_LANGUAGE C) SET_TARGET_PROPERTIES(temperature PROPERTIES LINKER_LANGUAGE C) +SET_TARGET_PROPERTIES(light PROPERTIES LINKER_LANGUAGE C) target_link_libraries(accelerometer glib-2.0 dlog sensor) target_link_libraries(geomagnetic glib-2.0 dlog sensor) @@ -56,6 +58,7 @@ target_link_libraries(gyro glib-2.0 dlog sensor) target_link_libraries(proxi glib-2.0 dlog sensor) target_link_libraries(pressure glib-2.0 dlog sensor) target_link_libraries(temperature glib-2.0 dlog sensor) +target_link_libraries(light glib-2.0 dlog sensor) INSTALL(TARGETS accelerometer DESTINATION /usr/bin/) INSTALL(TARGETS geomagnetic DESTINATION /usr/bin/) @@ -66,4 +69,5 @@ INSTALL(TARGETS gyro DESTINATION /usr/bin/) INSTALL(TARGETS proxi DESTINATION /usr/bin/) INSTALL(TARGETS pressure DESTINATION /usr/bin/) INSTALL(TARGETS temperature DESTINATION /usr/bin/) +INSTALL(TARGETS light DESTINATION /usr/bin/) diff --git a/test/src/light.c b/test/src/light.c new file mode 100644 index 0000000..454d5c2 --- /dev/null +++ b/test/src/light.c @@ -0,0 +1,140 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#include +#include +#include +#include +#include +#include +#include +#include + +static GMainLoop *mainloop; + +void callback(unsigned int event_type, sensor_event_data_t *event, void *user_data) +{ + sensor_data_t *data = (sensor_data_t *)event->event_data; + printf("Light :[%lld] [%6.6f]\n", data->timestamp, data->values[0]); +} + +void printformat() +{ + printf("Usage : ./light (optional) (optional)\n\n"); + + printf("mode:"); + printf("[-p]\n"); + printf("p is for polling based,default mode is event driven\n"); + + printf("event:"); + printf("[RAW_DATA_REPORT_ON_TIME]\n"); + + printf("interval:\n"); + printf("The time interval should be entered based on the sampling frequency supported by light driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n"); +} + +int main(int argc,char **argv) +{ + int result, handle, start_handle, stop_handle; + unsigned int event; + mainloop = g_main_loop_new(NULL, FALSE); + event = LIGHT_EVENT_LUX_DATA_REPORT_ON_TIME; + event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); + event_condition->cond_op = CONDITION_EQUAL; + + sensor_type_t type = LIGHT_SENSOR; + + if (argc != 2 && argc != 3 && argc!=4) { + printformat(); + free(event_condition); + return 0; + } + + else if (argc>=3 && strcmp(argv[1], "-p") == 0 && strcmp(argv[2], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Polling based\n"); + + handle = sf_connect(type); + result = sf_start(handle, 1); + + if (result < 0) { + printf("Can't start light SENSOR\n"); + printf("Error\n\n\n\n"); + return -1; + } + + sensor_data_t data; + + while(1) { + result = sf_get_data(handle, LIGHT_LUX_DATA_SET , &data); + printf("Light : [%6.6f]\n", data.values[0]); + usleep(100000); + } + + result = sf_disconnect(handle); + if (result < 0) { + printf("Can't disconnect light sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } + } + + else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0) { + printf("Event based\n"); + + event_condition->cond_value1 = 100; + if (argc == 3) + event_condition->cond_value1 = atof(argv[2]); + + handle = sf_connect(type); + result = sf_register_event(handle, event, event_condition, callback, NULL); + + if (result < 0) + printf("Can't register light\n"); + + start_handle = sf_start(handle,0); + + if (start_handle < 0) { + printf("Error\n\n\n\n"); + sf_unregister_event(handle, event); + sf_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); + + sf_unregister_event(handle, event); + + stop_handle = sf_stop(handle); + + if (stop_handle < 0) { + printf("Error\n\n"); + return -1; + } + + sf_disconnect(handle); + free(event_condition); + } + + else { + printformat(); + } + + return 0; +} + -- 2.7.4 From 2b036ab72d4c1988623977d4e6aaa382c110bf2f Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Mon, 22 Dec 2014 18:15:52 +0530 Subject: [PATCH 10/16] Adding polling based support for testing proximity sensor Change-Id: I0a5c82eb75a615348a348f56bf9254bc0d4f17c9 --- test/src/proxi.c | 154 +++++++++++++++++++++++++++++++++++++------------------ 1 file changed, 105 insertions(+), 49 deletions(-) diff --git a/test/src/proxi.c b/test/src/proxi.c index 9ac965f..70185bd 100644 --- a/test/src/proxi.c +++ b/test/src/proxi.c @@ -16,14 +16,14 @@ * limitations under the License. * */ - #include #include #include #include #include #include -#include +#include +#include static GMainLoop *mainloop; @@ -35,11 +35,17 @@ void callback(unsigned int event_type, sensor_event_data_t *event, void *user_da void printformat() { - printf("Usage : ./proxi (optional)\n\n"); - printf("event:\n"); - printf("EVENT_CHANGE_STATE\n"); - printf("EVENT_STATE_REPORT_ON_TIME\n"); - printf("EVENT_DISTANCE_DATA_REPORT_ON_TIME\n"); + printf("Usage : ./proxi (optional) (optional)\n\n"); + + printf("mode:"); + printf("[-p]\n"); + printf("p is for polling based,default mode is event driven\n"); + + printf("event:"); + printf("[EVENT_CHANGE_STATE] "); + printf("[EVENT_STATE_REPORT_ON_TIME] "); + printf("[EVENT_DISTANCE_DATA_REPORT_ON_TIME]\n"); + printf("interval:\n"); printf("The time interval should be entered based on the sampling frequency supported by proximity driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n"); } @@ -48,68 +54,118 @@ int main(int argc,char **argv) { int result, handle, start_handle, stop_handle; unsigned int event; - mainloop = g_main_loop_new(NULL, FALSE); - sensor_type_t type = PROXIMITY_SENSOR; event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t)); event_condition->cond_op = CONDITION_EQUAL; - event_condition->cond_value1 = 100; + sensor_type_t type = PROXIMITY_SENSOR; - if (argc != 2 && argc != 3) { + if (argc != 2 && argc != 3 && argc!=4) { printformat(); free(event_condition); return 0; } - if (strcmp(argv[1], "EVENT_CHANGE_STATE") == 0) { - event = PROXIMITY_EVENT_CHANGE_STATE; - } - else if (strcmp(argv[1], "EVENT_STATE_REPORT_ON_TIME") == 0) { - event = PROXIMITY_EVENT_STATE_REPORT_ON_TIME; - } - else if (strcmp(argv[1], "EVENT_DISTANCE_DATA_REPORT_ON_TIME") == 0) { - event = PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME; - } - else { - printformat(); - free(event_condition); - return 0; + else if (argc>=3 && strcmp(argv[1], "-p") == 0) { + printf("Polling based\n"); + + if (strcmp(argv[2], "EVENT_CHANGE_STATE") == 0) { + event = PROXIMITY_BASE_DATA_SET; + } + else if (strcmp(argv[2], "EVENT_STATE_REPORT_ON_TIME") == 0) { + event = PROXIMITY_DISTANCE_DATA_SET; + } + else if (strcmp(argv[2], "EVENT_DISTANCE_DATA_REPORT_ON_TIME") == 0) { + event = PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME; + } + else { + printformat(); + free(event_condition); + return 0; + } + + handle = sf_connect(type); + result = sf_start(handle, 1); + + if (result < 0) { + printf("Can't start proximity SENSOR\n"); + printf("Error\n\n\n\n"); + return -1; + } + + sensor_data_t data; + + while(1) { + result = sf_get_data(handle, event , &data); + printf("Proximity [%6.6f]\n\n", data.values[0]); + usleep(100000); + } + + result = sf_disconnect(handle); + + if (result < 0) { + printf("Can't disconnect proximity sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } } - if (argc == 3) - event_condition->cond_value1 = atof(argv[2]); + else if (argc == 2 || argc ==3) { + printf("Event based\n"); + + if (strcmp(argv[1], "EVENT_CHANGE_STATE") == 0) { + event = PROXIMITY_EVENT_CHANGE_STATE; + } + else if (strcmp(argv[1], "EVENT_STATE_REPORT_ON_TIME") == 0) { + event = PROXIMITY_EVENT_STATE_REPORT_ON_TIME; + } + else if (strcmp(argv[1], "EVENT_DISTANCE_DATA_REPORT_ON_TIME") == 0) { + event = PROXIMITY_EVENT_DISTANCE_DATA_REPORT_ON_TIME; + } + else { + printformat(); + free(event_condition); + return 0; + } + + event_condition->cond_value1 = 100; + if (argc == 3) + event_condition->cond_value1 = atof(argv[2]); + + handle = sf_connect(type); + result = sf_register_event(handle, event, event_condition, callback, NULL); + + if (result < 0) + printf("Can't register proximity\n"); + + start_handle = sf_start(handle,0); + + if (start_handle < 0) { + printf("Error\n\n\n\n"); + sf_unregister_event(handle, event); + sf_disconnect(handle); + return -1; + } + + g_main_loop_run(mainloop); + g_main_loop_unref(mainloop); - handle = sf_connect(type); - result = sf_register_event(handle, event, event_condition, callback, NULL); + sf_unregister_event(handle, event); - if (result < 0) - printf("Can't register proximity sensor\n"); + stop_handle = sf_stop(handle); - start_handle = sf_start(handle,0); + if (stop_handle < 0) { + printf("Error\n\n"); + return -1; + } - if (start_handle < 0) { - printf("Error\n\n\n\n"); - sf_unregister_event(handle, event); sf_disconnect(handle); - return -1; + free(event_condition); } - g_main_loop_run(mainloop); - g_main_loop_unref(mainloop); - - sf_unregister_event(handle, event); - - stop_handle = sf_stop(handle); - - if (stop_handle < 0) { - printf("Failed to stop proximity sensor\n\n"); - return -1; + else { + printformat(); } - sf_disconnect(handle); - - free(event_condition); - return 0; } -- 2.7.4 From 6749fe38425410aa54500081a4cae0f135ac3a0a Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 2 Jan 2015 08:50:16 +0530 Subject: [PATCH 11/16] Fix for orientation values being set to Nan - Fixing issue where low floating point values are rounded of to zero. - indexing issue. - Cleanup Change-Id: Ief4a37395b50f8946bf1d622c0895a881c3a7619 --- src/sensor_fusion/orientation_filter.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index 05537a3..5e0a354 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -40,6 +40,10 @@ #define QWB_CONST ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W) #define F_CONST (-1 / TAU_W) +#define NEGLIGIBLE_VAL 0.0000001 + +#define ABS(val) (((val) < 0) ? -(val) : (val)) + // M-matrix, V-vector, MxN=> matrix dimension, R-RowCount, C-Column count #define M3X3R 3 #define M3X3C 3 @@ -96,8 +100,6 @@ template inline void orientation_filter::initialize_sensor_data(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic) { - vect acc_data(V1x3S); - vect gyr_data(V1x3S); unsigned long long sample_interval_gyro = SAMPLE_INTV; m_accel.m_data = accel.m_data; @@ -258,10 +260,8 @@ inline void orientation_filter::measurement_update() matrix gain(M6X6R, M6X6C); TYPE iden = 0; - for (int j = 0; j < M6X6C; j++) - { - for (int i = 0; i < M6X6R; i++) - { + for (int j=0; j::measurement_update() else iden = 0; - m_pred_cov.m_mat[i][j] = (iden - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i])) * - m_pred_cov.m_mat[i][j]; + m_pred_cov.m_mat[j][i] = (iden - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i])) * + m_pred_cov.m_mat[j][i]; + + if (ABS(m_pred_cov.m_mat[j][i]) < NEGLIGIBLE_VAL) + m_pred_cov.m_mat[j][i] = NEGLIGIBLE_VAL; } } -- 2.7.4 From 537339baada31e06a10bc201e4ccdab002f0afc9 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 2 Jan 2015 09:05:11 +0530 Subject: [PATCH 12/16] Updating linear acceleration range in get_properties - Updating the min_range and max_range values in get_properties to to shown according to accelerometer range. - cleanup Change-Id: I2f2c68b8e29aad658d1cea1eed8ef7fc3bd0a2c4 --- src/linear_accel/linear_accel_sensor.cpp | 4 +--- src/orientation/orientation_sensor.cpp | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/linear_accel/linear_accel_sensor.cpp b/src/linear_accel/linear_accel_sensor.cpp index 1af20bc..fb2dabd 100755 --- a/src/linear_accel/linear_accel_sensor.cpp +++ b/src/linear_accel/linear_accel_sensor.cpp @@ -273,11 +273,9 @@ int linear_accel_sensor::get_sensor_data(const unsigned int event_type, sensor_d bool linear_accel_sensor::get_properties(sensor_properties_t &properties) { - m_gravity_sensor->get_properties(properties); + m_accel_sensor->get_properties(properties); properties.name = "Linear Acceleration Sensor"; properties.vendor = m_vendor; - properties.min_range = - 2 * GRAVITY; - properties.max_range = 2 * GRAVITY; properties.resolution = 0.000001; return true; diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index dd0f818..7653ef3 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -436,7 +436,7 @@ bool orientation_sensor::get_properties(sensor_properties_t &properties) properties.min_range = -PI; properties.max_range = 2 * PI; } - properties.resolution = 0.000001;; + properties.resolution = 0.000001; properties.vendor = m_vendor; properties.name = SENSOR_NAME; -- 2.7.4 From 7cd4ed8647a5a0b42fdb8eec6b3f289472b8a204 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 2 Jan 2015 09:35:02 +0530 Subject: [PATCH 13/16] Adding Rotation Vector virtual sensor - Added 9-axis rotation vector based on sensor fusion - Added xml configuration support for virtual sensor - Added xml configuration for loading rotation vector library - Updated build and spec files Change-Id: I540b277019aa9a946e267b89c39a15e168dbe9f0 --- packaging/sensord.spec | 3 +- sensor_plugins.xml.in | 1 + src/libsensord/sensor_internal.h | 1 + src/libsensord/sensor_internal_deprecated.h | 1 + src/rotation_vector/CMakeLists.txt | 4 + src/rotation_vector/rv/CMakeLists.txt | 47 ++++ src/rotation_vector/rv/rv_sensor.cpp | 390 ++++++++++++++++++++++++++++ src/rotation_vector/rv/rv_sensor.h | 87 +++++++ virtual_sensors.xml.in | 16 ++ 9 files changed, 549 insertions(+), 1 deletion(-) create mode 100644 src/rotation_vector/CMakeLists.txt create mode 100755 src/rotation_vector/rv/CMakeLists.txt create mode 100755 src/rotation_vector/rv/rv_sensor.cpp create mode 100755 src/rotation_vector/rv/rv_sensor.h diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 2e1fff7..1d305ff 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -18,6 +18,7 @@ Source2: sensord.socket %define orientation_state ON %define gravity_state ON %define linear_accel_state ON +%define rv_state ON %define build_test_suite OFF @@ -79,7 +80,7 @@ cmake . -DCMAKE_INSTALL_PREFIX=%{_prefix} -DACCEL=%{accel_state} \ -DGYRO=%{gyro_state} -DPROXI=%{proxi_state} -DLIGHT=%{light_state} \ -DGEO=%{geo_state} -DPRESSURE=%{pressure_state} -DTEMPERATURE=%{temperature_state} \ -DORIENTATION=%{orientation_state} -DGRAVITY=%{gravity_state} \ - -DLINEAR_ACCEL=%{linear_accel_state} \ + -DLINEAR_ACCEL=%{linear_accel_state} -DRV=%{rv_state} \ -DTEST_SUITE=%{build_test_suite} make %{?jobs:-j%jobs} diff --git a/sensor_plugins.xml.in b/sensor_plugins.xml.in index f40f79a..aca8a81 100755 --- a/sensor_plugins.xml.in +++ b/sensor_plugins.xml.in @@ -20,5 +20,6 @@ + diff --git a/src/libsensord/sensor_internal.h b/src/libsensord/sensor_internal.h index bec1487..3bc8311 100755 --- a/src/libsensord/sensor_internal.h +++ b/src/libsensord/sensor_internal.h @@ -49,6 +49,7 @@ extern "C" #include #include #include +#include #include diff --git a/src/libsensord/sensor_internal_deprecated.h b/src/libsensord/sensor_internal_deprecated.h index f0a1a74..dc4a8b3 100755 --- a/src/libsensord/sensor_internal_deprecated.h +++ b/src/libsensord/sensor_internal_deprecated.h @@ -48,6 +48,7 @@ extern "C" #include #include #include +#include typedef struct { condition_op_t cond_op; diff --git a/src/rotation_vector/CMakeLists.txt b/src/rotation_vector/CMakeLists.txt new file mode 100644 index 0000000..c111931 --- /dev/null +++ b/src/rotation_vector/CMakeLists.txt @@ -0,0 +1,4 @@ +IF("${RV}" STREQUAL "ON") +add_subdirectory(rv) +ENDIF() + diff --git a/src/rotation_vector/rv/CMakeLists.txt b/src/rotation_vector/rv/CMakeLists.txt new file mode 100755 index 0000000..33ac289 --- /dev/null +++ b/src/rotation_vector/rv/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 2.6) +project(rv CXX) + +# to install pkgconfig setup file. +SET(EXEC_PREFIX "\${prefix}") +SET(LIBDIR "\${prefix}/lib") +SET(INCLUDEDIR "\${prefix}/include") +SET(VERSION 1.0) + +SET(SENSOR_NAME rv_sensor) + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) +include_directories(${CMAKE_SOURCE_DIR}/src/sensor_fusion) + +include(FindPkgConfig) +pkg_check_modules(rpkgs REQUIRED vconf) +add_definitions(${rpkgs_CFLAGS} -DUSE_ONLY_ONE_MODULE -DUSE_LCD_TYPE_CHECK) + +set(PROJECT_MAJOR_VERSION "0") +set(PROJECT_MINOR_VERSION "0") +set(PROJECT_RELEASE_VERSION "1") +set(CMAKE_VERBOSE_MAKEFILE OFF) + + +FIND_PROGRAM(UNAME NAMES uname) +EXEC_PROGRAM("${UNAME}" ARGS "-m" OUTPUT_VARIABLE "ARCH") +IF("${ARCH}" MATCHES "^arm.*") + ADD_DEFINITIONS("-DTARGET") + MESSAGE("add -DTARGET") +ELSE("${ARCH}" MATCHES "^arm.*") + ADD_DEFINITIONS("-DSIMULATOR") + MESSAGE("add -DSIMULATOR") +ENDIF("${ARCH}" MATCHES "^arm.*") + +add_definitions(-Wall -O3 -omit-frame-pointer) +add_definitions(-DUSE_DLOG_LOG) +#add_definitions(-Wall -g -D_DEBUG) +add_definitions(-Iinclude) + +add_library(${SENSOR_NAME} SHARED + rv_sensor.cpp +) + +target_link_libraries(${SENSOR_NAME} ${rpkgs_LDFLAGS} ${GLES_LDFLAGS} "-lm") + +install(TARGETS ${SENSOR_NAME} DESTINATION lib/sensord) diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp new file mode 100755 index 0000000..cee3a94 --- /dev/null +++ b/src/rotation_vector/rv/rv_sensor.cpp @@ -0,0 +1,390 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define SENSOR_NAME "RV_SENSOR" +#define SENSOR_TYPE_RV "ROTATION_VECTOR" + +#define ACCELEROMETER_ENABLED 0x01 +#define GYROSCOPE_ENABLED 0x02 +#define GEOMAGNETIC_ENABLED 0x04 +#define ORIENTATION_ENABLED 7 + +#define INITIAL_VALUE -1 + +#define MS_TO_US 1000 + +#define ELEMENT_NAME "NAME" +#define ELEMENT_VENDOR "VENDOR" +#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT" +#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME" +#define ELEMENT_ACCEL_STATIC_BIAS "ACCEL_STATIC_BIAS" +#define ELEMENT_GYRO_STATIC_BIAS "GYRO_STATIC_BIAS" +#define ELEMENT_GEOMAGNETIC_STATIC_BIAS "GEOMAGNETIC_STATIC_BIAS" +#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION "GYRO_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_ACCEL_SCALE "ACCEL_SCALE" +#define ELEMENT_GYRO_SCALE "GYRO_SCALE" +#define ELEMENT_GEOMAGNETIC_SCALE "GEOMAGNETIC_SCALE" +#define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR "MAGNETIC_ALIGNMENT_FACTOR" + +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; +} + +rv_sensor::rv_sensor() +: m_accel_sensor(NULL) +, m_gyro_sensor(NULL) +, m_magnetic_sensor(NULL) +, m_x(-1) +, m_y(-1) +, m_z(-1) +, m_w(-1) +, m_accuracy(-1) +, m_time(0) +{ + cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + + m_name = string(SENSOR_NAME); + register_supported_event(ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME); + m_enable_orientation = 0; + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_VENDOR, m_vendor)) { + ERR("[VENDOR] is empty\n"); + throw ENXIO; + } + + INFO("m_vendor = %s", m_vendor.c_str()); + + if (!config.get(SENSOR_TYPE_RV, 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_RV, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) { + ERR("[ACCEL_STATIC_BIAS] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_GYRO_STATIC_BIAS, m_gyro_static_bias,3)) { + ERR("[GYRO_STATIC_BIAS] is empty\n"); + throw ENXIO; + } + + INFO("m_gyro_static_bias = (%f, %f, %f)", m_gyro_static_bias[0], m_gyro_static_bias[1], m_gyro_static_bias[2]); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_GEOMAGNETIC_STATIC_BIAS, m_geomagnetic_static_bias, 3)) { + ERR("[GEOMAGNETIC_STATIC_BIAS] is empty\n"); + throw ENXIO; + } + + INFO("m_geomagnetic_static_bias = (%f, %f, %f)", m_geomagnetic_static_bias[0], m_geomagnetic_static_bias[1], m_geomagnetic_static_bias[2]); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) { + ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_rotation_direction_compensation = (%d, %d, %d)", m_accel_rotation_direction_compensation[0], m_accel_rotation_direction_compensation[1], m_accel_rotation_direction_compensation[2]); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION, m_gyro_rotation_direction_compensation, 3)) { + ERR("[GYRO_ROTATION_DIRECTION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_gyro_rotation_direction_compensation = (%d, %d, %d)", m_gyro_rotation_direction_compensation[0], m_gyro_rotation_direction_compensation[1], m_gyro_rotation_direction_compensation[2]); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION, m_geomagnetic_rotation_direction_compensation, 3)) { + ERR("[GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_geomagnetic_rotation_direction_compensation = (%d, %d, %d)", m_geomagnetic_rotation_direction_compensation[0], m_geomagnetic_rotation_direction_compensation[1], m_geomagnetic_rotation_direction_compensation[2]); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_ACCEL_SCALE, &m_accel_scale)) { + ERR("[ACCEL_SCALE] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_scale = %f", m_accel_scale); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_GYRO_SCALE, &m_gyro_scale)) { + ERR("[GYRO_SCALE] is empty\n"); + throw ENXIO; + } + + INFO("m_gyro_scale = %f", m_gyro_scale); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_GEOMAGNETIC_SCALE, &m_geomagnetic_scale)) { + ERR("[GEOMAGNETIC_SCALE] is empty\n"); + throw ENXIO; + } + + INFO("m_geomagnetic_scale = %f", m_geomagnetic_scale); + + if (!config.get(SENSOR_TYPE_RV, ELEMENT_MAGNETIC_ALIGNMENT_FACTOR, &m_magnetic_alignment_factor)) { + ERR("[MAGNETIC_ALIGNMENT_FACTOR] is empty\n"); + throw ENXIO; + } + + INFO("m_magnetic_alignment_factor = %d", m_magnetic_alignment_factor); + + m_interval = m_default_sampling_time * MS_TO_US; + +} + +rv_sensor::~rv_sensor() +{ + INFO("rv_sensor is destroyed!\n"); +} + +bool rv_sensor::init() +{ + 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); + + if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor) { + ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x", + m_accel_sensor, m_gyro_sensor, m_magnetic_sensor); + return false; + } + + INFO("%s is created!\n", sensor_base::get_name()); + + return true; +} + +sensor_type_t rv_sensor::get_type(void) +{ + return ROTATION_VECTOR_SENSOR; +} + +bool rv_sensor::on_start(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->add_interval((int)this, (m_interval/MS_TO_US), false); + m_accel_sensor->start(); + m_gyro_sensor->add_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); + m_gyro_sensor->add_interval((int)this, (m_interval/MS_TO_US), false); + m_gyro_sensor->start(); + m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); + m_magnetic_sensor->add_interval((int)this, (m_interval/MS_TO_US), false); + m_magnetic_sensor->start(); + + activate(); + return true; +} + +bool rv_sensor::on_stop(void) +{ + AUTOLOCK(m_mutex); + + m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); + m_accel_sensor->delete_interval((int)this, false); + m_accel_sensor->stop(); + m_gyro_sensor->delete_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); + m_gyro_sensor->delete_interval((int)this, false); + m_gyro_sensor->stop(); + m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); + m_magnetic_sensor->delete_interval((int)this, false); + m_magnetic_sensor->stop(); + + deactivate(); + return true; +} + +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); + + return sensor_base::add_interval(client_id, interval, false); +} + +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); + + return sensor_base::delete_interval(client_id, false); +} + +void rv_sensor::synthesize(const sensor_event_t& event, vector &outs) +{ + const float MIN_DELIVERY_DIFF_FACTOR = 0.75f; + unsigned long long diff_time; + + sensor_event_t rv_event; + quaternion quaternion_orientation; + + if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); + + m_accel.m_time_stamp = event.data.timestamp; + + m_enable_orientation |= ACCELEROMETER_ENABLED; + } + else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale); + + m_gyro.m_time_stamp = event.data.timestamp; + + m_enable_orientation |= GYROSCOPE_ENABLED; + } + else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale); + + m_magnetic.m_time_stamp = event.data.timestamp; + + m_enable_orientation |= GEOMAGNETIC_ENABLED; + } + + if (m_enable_orientation == ORIENTATION_ENABLED) { + m_enable_orientation = 0; + + m_orientation.m_pitch_phase_compensation = m_pitch_rotation_compensation; + m_orientation.m_roll_phase_compensation = m_roll_rotation_compensation; + m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; + m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor; + + { + AUTOLOCK(m_fusion_mutex); + quaternion_orientation = m_orientation.get_quaternion(m_accel, m_gyro, m_magnetic); + } + + rv_event.sensor_id = get_id(); + rv_event.event_type = ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME; + rv_event.data.accuracy = SENSOR_ACCURACY_GOOD; + rv_event.data.timestamp = get_timestamp(); + rv_event.data.value_count = 4; + rv_event.data.values[0] = quaternion_orientation.m_quat.m_vec[1]; + rv_event.data.values[1] = quaternion_orientation.m_quat.m_vec[2]; + rv_event.data.values[2] = quaternion_orientation.m_quat.m_vec[3]; + rv_event.data.values[3] = quaternion_orientation.m_quat.m_vec[0]; + + push(rv_event); + + { + AUTOLOCK(m_value_mutex); + m_time = rv_event.data.value_count; + m_x = rv_event.data.values[0]; + m_y = rv_event.data.values[1]; + m_z = rv_event.data.values[2]; + m_w = rv_event.data.values[3]; + } + } + + return; +} + +int rv_sensor::get_sensor_data(unsigned int data_id, sensor_data_t &data) +{ + if (data_id != ROTATION_VECTOR_BASE_DATA_SET) + return -1; + + data.accuracy = SENSOR_ACCURACY_GOOD; + + AUTOLOCK(m_value_mutex); + data.timestamp = m_time; + data.values[0] = m_x; + data.values[1] = m_y; + data.values[2] = m_z; + data.values[3] = m_w; + data.value_count = 4; + + return 0; +} + +bool rv_sensor::get_properties(sensor_properties_t &properties) +{ + properties.vendor = m_vendor; + properties.name = SENSOR_NAME; + properties.min_range = 0; + properties.max_range = 1; + properties.resolution = 0.000001; + properties.fifo_count = 0; + properties.max_batch_count = 0; + properties.min_interval = 1; + + return true; +} + +extern "C" sensor_module* create(void) +{ + rv_sensor *sensor; + + try { + sensor = new(std::nothrow) rv_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/rotation_vector/rv/rv_sensor.h b/src/rotation_vector/rv/rv_sensor.h new file mode 100755 index 0000000..3d84a80 --- /dev/null +++ b/src/rotation_vector/rv/rv_sensor.h @@ -0,0 +1,87 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef _RV_SENSOR_H_ +#define _RV_SENSOR_H_ + +#include +#include +#include + +class rv_sensor : public virtual_sensor { +public: + rv_sensor(); + virtual ~rv_sensor(); + + bool init(void); + + void synthesize(const sensor_event_t &event, vector &outs); + + bool add_interval(int client_id, unsigned int interval); + bool delete_interval(int client_id); + bool get_properties(sensor_properties_t &properties); + sensor_type_t get_type(void); + + int get_sensor_data(const unsigned int data_id, sensor_data_t &data); + +private: + sensor_base *m_accel_sensor; + sensor_base *m_gyro_sensor; + sensor_base *m_magnetic_sensor; + + sensor_data m_accel; + sensor_data m_gyro; + sensor_data m_magnetic; + + cmutex m_value_mutex; + + orientation_filter m_orientation; + + unsigned int m_enable_orientation; + + float m_x; + float m_y; + float m_z; + float m_w; + int m_accuracy; + unsigned long long m_time; + unsigned int m_interval; + + string m_vendor; + string m_raw_data_unit; + int m_default_sampling_time; + float m_accel_static_bias[3]; + float m_gyro_static_bias[3]; + float m_geomagnetic_static_bias[3]; + int m_accel_rotation_direction_compensation[3]; + int m_gyro_rotation_direction_compensation[3]; + int m_geomagnetic_rotation_direction_compensation[3]; + float m_accel_scale; + float m_gyro_scale; + float m_geomagnetic_scale; + int m_magnetic_alignment_factor; + int m_azimuth_rotation_compensation; + int m_pitch_rotation_compensation; + int m_roll_rotation_compensation; + + bool on_start(void); + bool on_stop(void); +}; + +#endif /*_RV_SENSOR_H_*/ diff --git a/virtual_sensors.xml.in b/virtual_sensors.xml.in index 42c9d72..680745e 100644 --- a/virtual_sensors.xml.in +++ b/virtual_sensors.xml.in @@ -38,4 +38,20 @@ + + + + + + + + + + + + + + + + -- 2.7.4 From 4612a07cb5158eeaadd089e18524260eaf942977 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 2 Jan 2015 09:53:28 +0530 Subject: [PATCH 14/16] Updating virtual sensor xml configuration for multiple devices - Updated virtual sensor xml configuration file to support multiple devices and emulators - Updated virtual sensor xml parser to support additional functionality Change-Id: I647958a6010e7ecebfc282ba72bf61ac6706d5cf --- src/shared/cvirtual_sensor_config.cpp | 117 ++++++++++++++++--------- src/shared/cvirtual_sensor_config.h | 13 ++- virtual_sensors.xml.in | 160 +++++++++++++++++++++++----------- 3 files changed, 195 insertions(+), 95 deletions(-) diff --git a/src/shared/cvirtual_sensor_config.cpp b/src/shared/cvirtual_sensor_config.cpp index c0d0f0b..a636e87 100755 --- a/src/shared/cvirtual_sensor_config.cpp +++ b/src/shared/cvirtual_sensor_config.cpp @@ -28,13 +28,13 @@ using std::string; using std::stringstream; -#define ROOT_ELEMENT "VIRTUAL_SENSOR" -#define TEXT_ELEMENT "text" -#define MODEL_ID_ATTR "id" -#define DEFAULT_ATTR "value" -#define DEFAULT_ATTR1 "value1" -#define DEFAULT_ATTR2 "value2" -#define DEFAULT_ATTR3 "value3" +#define ROOT_ELEMENT "VIRTUAL_SENSOR" +#define DEVICE_TYPE_ATTR "type" +#define TEXT_ELEMENT "text" +#define DEFAULT_ATTR "value" +#define DEFAULT_ATTR1 "value1" +#define DEFAULT_ATTR2 "value2" +#define DEFAULT_ATTR3 "value3" cvirtual_sensor_config::cvirtual_sensor_config() { @@ -85,57 +85,78 @@ bool cvirtual_sensor_config::load_config(const string& config_path) return false; } + xmlNodePtr device_node_ptr; xmlNodePtr virtual_sensor_node_ptr; xmlNodePtr element_node_ptr; xmlAttrPtr attr_ptr; char* prop = NULL; - virtual_sensor_node_ptr = cur->xmlChildrenNode; - - while (virtual_sensor_node_ptr != NULL) { + device_node_ptr = cur->xmlChildrenNode; + while (device_node_ptr != NULL){ //skip garbage element, [text] - if (!xmlStrcmp(virtual_sensor_node_ptr->name,(const xmlChar *)TEXT_ELEMENT)) { - virtual_sensor_node_ptr = virtual_sensor_node_ptr->next; + if (!xmlStrcmp(device_node_ptr->name,(const xmlChar *)TEXT_ELEMENT)) { + device_node_ptr = device_node_ptr->next; continue; } - //insert Model_list to config map - m_virtual_sensor_config[(const char*)virtual_sensor_node_ptr->name]; - DBG("<%s>\n",(const char*)virtual_sensor_node_ptr->name); - element_node_ptr = virtual_sensor_node_ptr->xmlChildrenNode; - while (element_node_ptr != NULL) { + string device_type; + prop = (char*)xmlGetProp(device_node_ptr,(const xmlChar*)DEVICE_TYPE_ATTR); + device_type = prop; + free(prop); + + //insert device to device_list + m_virtual_sensor_config[device_type]; + DBG("\n",device_type.c_str()); + + virtual_sensor_node_ptr = device_node_ptr->xmlChildrenNode; + + while (virtual_sensor_node_ptr != NULL) { //skip garbage element, [text] - if (!xmlStrcmp(element_node_ptr->name,(const xmlChar *)TEXT_ELEMENT)) { - element_node_ptr = element_node_ptr->next; + if (!xmlStrcmp(virtual_sensor_node_ptr->name,(const xmlChar *)TEXT_ELEMENT)) { + virtual_sensor_node_ptr = virtual_sensor_node_ptr->next; continue; } - //insert Element to Model - m_virtual_sensor_config[(const char*)virtual_sensor_node_ptr->name][(const char*)element_node_ptr->name]; - DBG("<%s><%s>\n",(const char*)virtual_sensor_node_ptr->name,(const char*)element_node_ptr->name); + m_virtual_sensor_config[device_type][(const char*)virtual_sensor_node_ptr->name]; + DBG("<%s>\n",device_type.c_str(),(const char*)virtual_sensor_node_ptr->name); - attr_ptr = element_node_ptr->properties; - while (attr_ptr != NULL) { + element_node_ptr = virtual_sensor_node_ptr->xmlChildrenNode; + while (element_node_ptr != NULL) { + //skip garbage element, [text] + if (!xmlStrcmp(element_node_ptr->name,(const xmlChar *)TEXT_ELEMENT)) { + element_node_ptr = element_node_ptr->next; + continue; + } - string key,value; - key = (char*)attr_ptr->name; - prop = (char*)xmlGetProp(element_node_ptr,attr_ptr->name); - value = prop; - free(prop); + //insert Element to Model + m_virtual_sensor_config[device_type][(const char*)virtual_sensor_node_ptr->name][(const char*)element_node_ptr->name]; + DBG("<%s><%s>\n",device_type.c_str(),(const char*)virtual_sensor_node_ptr->name,(const char*)element_node_ptr->name); + + attr_ptr = element_node_ptr->properties; + while (attr_ptr != NULL) { + + string key,value; + key = (char*)attr_ptr->name; + prop = (char*)xmlGetProp(element_node_ptr,attr_ptr->name); + value = prop; + free(prop); + + //insert attribute to Element + m_virtual_sensor_config[device_type][(const char*)virtual_sensor_node_ptr->name][(const char*)element_node_ptr->name][key]=value; + DBG("<%s><%s \"%s\"=\"%s\">\n",device_type.c_str(),(const char*)virtual_sensor_node_ptr->name,(const char*)element_node_ptr->name,key.c_str(),value.c_str()); + attr_ptr = attr_ptr->next; + } - //insert attribute to Element - m_virtual_sensor_config[(const char*)virtual_sensor_node_ptr->name][(const char*)element_node_ptr->name][key]=value; - DBG("<%s><%s \"%s\"=\"%s\">\n",(const char*)virtual_sensor_node_ptr->name,(const char*)element_node_ptr->name,key.c_str(),value.c_str()); - attr_ptr = attr_ptr->next; - } + element_node_ptr = element_node_ptr->next; + } - element_node_ptr = element_node_ptr->next; + DBG("\n"); + virtual_sensor_node_ptr = virtual_sensor_node_ptr->next; } - DBG("\n"); - virtual_sensor_node_ptr = virtual_sensor_node_ptr->next; + device_node_ptr = device_node_ptr->next; } xmlFreeDoc(doc); @@ -144,10 +165,17 @@ bool cvirtual_sensor_config::load_config(const string& config_path) bool cvirtual_sensor_config::get(const string& sensor_type, const string& element, const string& attr, string& value) { - auto it_virtual_sensor_list = m_virtual_sensor_config.find(sensor_type); + auto it_device_list = m_virtual_sensor_config.find(m_device_id); + + if (it_device_list == m_virtual_sensor_config.end()) { + ERR("There is no <%s> device\n",m_device_id.c_str()); + return false; + } + + auto it_virtual_sensor_list = it_device_list->second.find(sensor_type); - if (it_virtual_sensor_list == m_virtual_sensor_config.end()) { - ERR("There is no <%s> element\n",sensor_type.c_str()); + if (it_virtual_sensor_list == it_device_list->second.end()) { + ERR("There is no <%s> sensor\n",sensor_type.c_str()); return false; } @@ -272,9 +300,14 @@ bool cvirtual_sensor_config::get(const string& sensor_type, const string& elemen bool cvirtual_sensor_config::is_supported(const string& sensor_type) { - auto it_virtual_sensor_list = m_virtual_sensor_config.find(sensor_type); + auto it_device_list = m_virtual_sensor_config.find(m_device_id); + + if (it_device_list == m_virtual_sensor_config.end()) + return false; + + auto it_virtual_sensor_list = it_device_list->second.find(sensor_type); - if (it_virtual_sensor_list == m_virtual_sensor_config.end()) + if (it_virtual_sensor_list == it_device_list->second.end()) return false; return true; diff --git a/src/shared/cvirtual_sensor_config.h b/src/shared/cvirtual_sensor_config.h index 7cfbcb6..b3c5f7d 100755 --- a/src/shared/cvirtual_sensor_config.h +++ b/src/shared/cvirtual_sensor_config.h @@ -40,7 +40,7 @@ typedef unordered_map Virtual_sensor; * ... */ -typedef unordered_map Virtual_sensor_config; +typedef unordered_map virtual_sensor_config; /* * a Virtual_sensor_config represents virtual_sensors.xml * @@ -49,6 +49,14 @@ typedef unordered_map Virtual_sensor_config; * */ +typedef unordered_map virtual_sensor_device_config; +/* +* a virtual_sensor_device_config represents virtual_sensors.xml +* +* +* +*/ + class cvirtual_sensor_config : public cconfig { private: @@ -58,7 +66,8 @@ private: bool load_config(const string& config_path); - Virtual_sensor_config m_virtual_sensor_config; + virtual_sensor_device_config m_virtual_sensor_config; + public: static cvirtual_sensor_config& get_instance(void); diff --git a/virtual_sensors.xml.in b/virtual_sensors.xml.in index 680745e..1c20c0d 100644 --- a/virtual_sensors.xml.in +++ b/virtual_sensors.xml.in @@ -1,57 +1,115 @@ - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -- 2.7.4 From 30340892dbba88cb6705648789fa69b2c3dddff5 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 2 Jan 2015 10:13:05 +0530 Subject: [PATCH 15/16] Adding lock in virtual sensor layer for sensor fusion library - Adding mutex lock in virtual sensor layer to restrict usage and access to sensor fusion library. - adding lock for event driven and polling based calls Change-Id: I2202d046544de4d8474606179a516a5e38b77548 --- src/orientation/orientation_sensor.cpp | 10 ++++++-- src/rotation_vector/rv/rv_sensor.cpp | 42 +++++++++++++++++++++++++++------- src/shared/virtual_sensor.h | 2 ++ 3 files changed, 44 insertions(+), 10 deletions(-) diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index 7653ef3..02afcd1 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -343,7 +343,10 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector & int rv_sensor::get_sensor_data(unsigned int data_id, sensor_data_t &data) { - if (data_id != ROTATION_VECTOR_BASE_DATA_SET) - return -1; + sensor_data accel; + sensor_data gyro; + sensor_data magnetic; - data.accuracy = SENSOR_ACCURACY_GOOD; + sensor_data_t accel_data; + sensor_data_t gyro_data; + sensor_data_t magnetic_data; + + quaternion quaternion_orientation; - AUTOLOCK(m_value_mutex); + m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data); + m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data); + m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data); + + pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); + pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale); + pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale); + accel.m_time_stamp = accel_data.timestamp; + gyro.m_time_stamp = gyro_data.timestamp; + magnetic.m_time_stamp = magnetic_data.timestamp; + + m_orientation.m_pitch_phase_compensation = m_pitch_rotation_compensation; + m_orientation.m_roll_phase_compensation = m_roll_rotation_compensation; + m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; + m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor; + + { + AUTOLOCK(m_fusion_mutex); + quaternion_orientation = m_orientation.get_quaternion(m_accel, m_gyro, m_magnetic); + } + + data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = m_time; - data.values[0] = m_x; - data.values[1] = m_y; - data.values[2] = m_z; - data.values[3] = m_w; data.value_count = 4; + data.values[0] = quaternion_orientation.m_quat.m_vec[1]; + data.values[1] = quaternion_orientation.m_quat.m_vec[2]; + data.values[2] = quaternion_orientation.m_quat.m_vec[3]; + data.values[3] = quaternion_orientation.m_quat.m_vec[0]; return 0; } diff --git a/src/shared/virtual_sensor.h b/src/shared/virtual_sensor.h index d592ce6..3cb79fb 100755 --- a/src/shared/virtual_sensor.h +++ b/src/shared/virtual_sensor.h @@ -33,6 +33,8 @@ public: bool is_virtual(void); protected: + cmutex m_fusion_mutex; + bool activate(void); bool deactivate(void); -- 2.7.4 From 55eab417e1a92e1beeaf21065fd957ff8c6d582d Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 2 Jan 2015 10:46:09 +0530 Subject: [PATCH 16/16] Adding Makelist support for virtual sensor configuration - Added xml configuration support for CMakelist - Updated spec file for virtual sensor xml Change-Id: I8b9be20f0f3d4ae728a172237640454e4880de36 --- CMakeLists.txt | 4 ++++ packaging/sensord.spec | 1 + src/libsensord/CMakeLists.txt | 1 + 3 files changed, 6 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index b555de8..edc7d9b 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -33,16 +33,20 @@ ENDIF("${ARCH}" MATCHES "^arm.*") IF("${ARCH}" MATCHES "^arm.*") CONFIGURE_FILE(sensor_plugins.xml.in sensor_plugins.xml @ONLY) CONFIGURE_FILE(sensors.xml.in sensors.xml @ONLY) + CONFIGURE_FILE(virtual_sensors.xml.in virtual_sensors.xml @ONLY) install(FILES sensor_plugins.xml sensors.xml + virtual_sensors.xml DESTINATION etc) ELSE("${ARCH}" MATCHES "^arm.*") CONFIGURE_FILE(sensor_plugins_sim.xml.in sensor_plugins.xml @ONLY) CONFIGURE_FILE(sensors.xml.in sensors.xml @ONLY) + CONFIGURE_FILE(virtual_sensors.xml.in virtual_sensors.xml @ONLY) install(FILES sensor_plugins.xml sensors.xml + virtual_sensors.xml DESTINATION etc) ENDIF("${ARCH}" MATCHES "^arm.*") diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 1d305ff..88d4f71 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -108,6 +108,7 @@ systemctl daemon-reload %{_bindir}/sensord %attr(0644,root,root)/usr/etc/sensor_plugins.xml %attr(0644,root,root)/usr/etc/sensors.xml +%attr(0644,root,root)/usr/etc/virtual_sensors.xml %{_unitdir}/sensord.service %{_unitdir}/sensord.socket %{_unitdir}/multi-user.target.wants/sensord.service diff --git a/src/libsensord/CMakeLists.txt b/src/libsensord/CMakeLists.txt index 6686be2..87dac4e 100755 --- a/src/libsensord/CMakeLists.txt +++ b/src/libsensord/CMakeLists.txt @@ -70,5 +70,6 @@ install(FILES sensor_context.h DESTINATION include/sensor/) install(FILES sensor_gravity.h DESTINATION include/sensor/) install(FILES sensor_linear_accel.h DESTINATION include/sensor/) install(FILES sensor_orientation.h DESTINATION include/sensor/) +install(FILES sensor_rv.h DESTINATION include/sensor/) install(FILES sensor_temperature.h DESTINATION include/sensor/) install(FILES ${PROJECT_NAME}.pc DESTINATION lib/pkgconfig) -- 2.7.4