From 3f412f894d4a7fc9f928423d743c346224cf5c3a Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 12 Mar 2015 09:02:45 +0530 Subject: [PATCH 01/16] Updating sensor plugin loader and cmake files - Adding sensor_fusion module to sensor plugin loader. - Adding fusion folder to master cmake file. Change-Id: I3ef1329b6c79d2123c458645c2fa6f9532a67988 --- sensor_plugins.xml.in | 1 + src/CMakeLists.txt | 3 +++ 2 files changed, 4 insertions(+) diff --git a/sensor_plugins.xml.in b/sensor_plugins.xml.in index eb469c1..9fde9d6 100755 --- a/sensor_plugins.xml.in +++ b/sensor_plugins.xml.in @@ -16,6 +16,7 @@ + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3b0b245..556256a 100755 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -58,6 +58,9 @@ 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") add_subdirectory(orientation) ENDIF() -- 2.7.4 From 3785cff17ee6070881bfc94e8bf1f20e62500758 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 12 Mar 2015 09:05:18 +0530 Subject: [PATCH 02/16] Updating orientation sensor after fusion restructuring - Works in combination with the new fusion sensor - supports new configuration xml - cleanup Change-Id: I0694ff6c2cc5c4ab8d9000d53359eefc39198302 --- src/orientation/orientation_sensor.cpp | 232 ++++++++------------------------- src/orientation/orientation_sensor.h | 13 +- 2 files changed, 58 insertions(+), 187 deletions(-) diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index d5fe72d..aee027a 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -35,11 +35,6 @@ #define SENSOR_NAME "ORIENTATION_SENSOR" #define SENSOR_TYPE_ORIENTATION "ORIENTATION" -#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 @@ -53,16 +48,6 @@ #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" #define ELEMENT_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION" #define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION" #define ELEMENT_AZIMUTH_ROTATION_COMPENSATION "AZIMUTH_ROTATION_COMPENSATION" @@ -107,76 +92,6 @@ orientation_sensor::orientation_sensor() INFO("m_default_sampling_time = %d", m_default_sampling_time); - if (!config.get(SENSOR_TYPE_ORIENTATION, 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_ORIENTATION, 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_ORIENTATION, 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_ORIENTATION, 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_ORIENTATION, 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_ORIENTATION, 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_ORIENTATION, 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_ORIENTATION, 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_ORIENTATION, 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_ORIENTATION, 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); - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) { ERR("[AZIMUTH_ROTATION_COMPENSATION] is empty\n"); throw ENXIO; @@ -199,7 +114,6 @@ orientation_sensor::orientation_sensor() INFO("m_roll_rotation_compensation = %d", m_roll_rotation_compensation); m_interval = m_default_sampling_time * MS_TO_US; - } orientation_sensor::~orientation_sensor() @@ -213,13 +127,16 @@ bool orientation_sensor::init(void) 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); + m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_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; } - INFO("%s is created!", sensor_base::get_name()); + INFO("%s is created!\n", sensor_base::get_name()); + return true; } @@ -242,6 +159,12 @@ bool orientation_sensor::on_start(void) 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; } @@ -260,6 +183,12 @@ bool orientation_sensor::on_stop(void) 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; } @@ -272,6 +201,8 @@ bool orientation_sensor::add_interval(int client_id, unsigned int interval) 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); } @@ -283,68 +214,30 @@ bool orientation_sensor::delete_interval(int client_id) 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); } void orientation_sensor::synthesize(const sensor_event_t &event, vector &outs) { - unsigned long long diff_time; - sensor_event_t orientation_event; - euler_angles euler_orientation; + unsigned long long diff_time; float azimuth_offset; - if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) { - diff_time = event.data.timestamp - m_time; - - if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) - return; - - pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); - - m_accel.m_time_stamp = event.data.timestamp; - - m_enable_orientation |= ACCELEROMETER_ENABLED; - } - else if (event.event_type == GYROSCOPE_RAW_DATA_EVENT) { - diff_time = event.data.timestamp - m_time; - - if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) - return; - - pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale); - - m_gyro.m_time_stamp = event.data.timestamp; - - m_enable_orientation |= GYROSCOPE_ENABLED; - } - else if (event.event_type == GEOMAGNETIC_RAW_DATA_EVENT) { + if (event.event_type == FUSION_EVENT) { diff_time = event.data.timestamp - m_time; if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) return; - pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, 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; + quaternion quat(event.data.values[0], event.data.values[1], + event.data.values[2], event.data.values[3]); - m_orientation_filter.m_pitch_phase_compensation = m_pitch_rotation_compensation; - m_orientation_filter.m_roll_phase_compensation = m_roll_rotation_compensation; - m_orientation_filter.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; - m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - - m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, &m_magnetic); - - euler_orientation = m_orientation_filter.m_orientation; + euler_angles euler = quat2euler(quat); if(m_raw_data_unit == "DEGREES") { - euler_orientation = rad2deg(euler_orientation); + euler = rad2deg(euler); azimuth_offset = AZIMUTH_OFFSET_DEGREES; } else { @@ -354,15 +247,19 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector= 0) - orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[2]; + orientation_event.data.values[1] = euler.m_ang.m_vec[0]; + orientation_event.data.values[2] = euler.m_ang.m_vec[1]; + if (euler.m_ang.m_vec[2] >= 0) + orientation_event.data.values[0] = euler.m_ang.m_vec[2]; else - orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[2] + azimuth_offset; + orientation_event.data.values[0] = euler.m_ang.m_vec[2] + azimuth_offset; + + orientation_event.data.values[1] *= m_pitch_rotation_compensation; + orientation_event.data.values[2] *= m_roll_rotation_compensation; + orientation_event.data.values[0] *= m_azimuth_rotation_compensation; push(orientation_event); } @@ -372,57 +269,40 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector accel; - sensor_data gyro; - sensor_data magnetic; - - sensor_data_t accel_data; - sensor_data_t gyro_data; - sensor_data_t magnetic_data; - - euler_angles euler_orientation; + sensor_data_t fusion_data; float azimuth_offset; if (event_type != ORIENTATION_RAW_DATA_EVENT) return -1; - m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); - m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data); - m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_RAW_DATA_EVENT, magnetic_data); + m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_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; + quaternion quat(fusion_data.values[0], fusion_data.values[1], + fusion_data.values[2], fusion_data.values[3]); - m_orientation_filter_poll.m_pitch_phase_compensation = m_pitch_rotation_compensation; - m_orientation_filter_poll.m_roll_phase_compensation = m_roll_rotation_compensation; - m_orientation_filter_poll.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; - m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - - m_orientation_filter_poll.get_device_orientation(&m_accel, &m_gyro, &m_magnetic); - - euler_orientation = m_orientation_filter_poll.m_orientation; + euler_angles euler = quat2euler(quat); if(m_raw_data_unit == "DEGREES") { - euler_orientation = rad2deg(euler_orientation); + euler = rad2deg(euler); azimuth_offset = AZIMUTH_OFFSET_DEGREES; } else { azimuth_offset = AZIMUTH_OFFSET_RADIANS; } - data.accuracy = SENSOR_ACCURACY_GOOD; + data.accuracy = fusion_data.accuracy; data.timestamp = get_timestamp(); - data.values[1] = euler_orientation.m_ang.m_vec[0]; - data.values[2] = euler_orientation.m_ang.m_vec[1]; - if (euler_orientation.m_ang.m_vec[2] >= 0) - data.values[0] = euler_orientation.m_ang.m_vec[2]; - else - data.values[0] = euler_orientation.m_ang.m_vec[2] + azimuth_offset; data.value_count = 3; + data.values[1] = euler.m_ang.m_vec[0]; + data.values[2] = euler.m_ang.m_vec[1]; + if (euler.m_ang.m_vec[2] >= 0) + data.values[0] = euler.m_ang.m_vec[2]; + else + data.values[0] = euler.m_ang.m_vec[2] + azimuth_offset; + + data.values[1] *= m_pitch_rotation_compensation; + data.values[2] *= m_roll_rotation_compensation; + data.values[0] *= m_azimuth_rotation_compensation; return 0; } diff --git a/src/orientation/orientation_sensor.h b/src/orientation/orientation_sensor.h index baa783b..1128541 100755 --- a/src/orientation/orientation_sensor.h +++ b/src/orientation/orientation_sensor.h @@ -38,12 +38,13 @@ public: bool get_properties(sensor_properties_s &properties); sensor_type_t get_type(void); - int get_sensor_data(const unsigned int data_id, sensor_data_t &data); + int get_sensor_data(const unsigned int event_type, sensor_data_t &data); private: 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; @@ -62,16 +63,6 @@ private: 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; -- 2.7.4 From e4c7102201c9bacc73c7402f3c74c06cb50335f9 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 12 Mar 2015 09:07:54 +0530 Subject: [PATCH 03/16] Updating Gaming RV sensor after fusion restructuring - Works in combination with the new fusion sensor - supports new configuration xml - cleanup Change-Id: Icdacfa0bda9a247123eea07669a61c8e3d87bc1a --- src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp | 143 +++++---------------- src/rotation_vector/gaming_rv/gaming_rv_sensor.h | 10 +- 2 files changed, 35 insertions(+), 118 deletions(-) diff --git a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp index 7e8b005..997bc7b 100755 --- a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp +++ b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp @@ -35,9 +35,7 @@ #define SENSOR_NAME "GAMING_RV_SENSOR" #define SENSOR_TYPE_GAMING_RV "GAMING_ROTATION_VECTOR" -#define ACCELEROMETER_ENABLED 0x01 -#define GYROSCOPE_ENABLED 0x02 -#define GAMING_RV_ENABLED 3 +#define MIN_DELIVERY_DIFF_FACTOR 0.75f #define INITIAL_VALUE -1 @@ -47,12 +45,6 @@ #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_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" -#define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION "GYRO_ROTATION_DIRECTION_COMPENSATION" -#define ELEMENT_ACCEL_SCALE "ACCEL_SCALE" -#define ELEMENT_GYRO_SCALE "GYRO_SCALE" void pre_process_data(sensor_data &data_out, const float *data_in, float *bias, int *sign, float scale) { @@ -64,7 +56,6 @@ 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_accuracy(-1) , m_time(0) { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); @@ -87,48 +78,6 @@ gaming_rv_sensor::gaming_rv_sensor() INFO("m_default_sampling_time = %d", m_default_sampling_time); - if (!config.get(SENSOR_TYPE_GAMING_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_GAMING_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_GAMING_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_GAMING_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_GAMING_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_GAMING_RV, ELEMENT_GYRO_SCALE, &m_gyro_scale)) { - ERR("[GYRO_SCALE] is empty\n"); - throw ENXIO; - } - - INFO("m_gyro_scale = %f", m_gyro_scale); - m_interval = m_default_sampling_time * MS_TO_US; } @@ -142,9 +91,11 @@ bool gaming_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); - if (!m_accel_sensor || !m_gyro_sensor) { - ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x", - m_accel_sensor, m_gyro_sensor); + m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_SENSOR); + + if (!m_accel_sensor || !m_gyro_sensor || !m_fusion_sensor) { + ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x, fusion: 0x%x", + m_accel_sensor, m_gyro_sensor, m_fusion_sensor); return false; } @@ -169,6 +120,12 @@ bool gaming_rv_sensor::on_start(void) 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); + 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; } @@ -184,6 +141,12 @@ bool gaming_rv_sensor::on_stop(void) 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); + m_fusion_sensor->unregister_supported_event(FUSION_EVENT); + m_fusion_sensor->unregister_supported_event(FUSION_GAMING_ROTATION_VECTOR_ENABLED); + m_fusion_sensor->stop(); + deactivate(); return true; } @@ -195,6 +158,8 @@ bool gaming_rv_sensor::add_interval(int client_id, unsigned int interval) 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); + return sensor_base::add_interval(client_id, interval, false); } @@ -205,59 +170,33 @@ bool gaming_rv_sensor::delete_interval(int client_id) m_accel_sensor->delete_interval(client_id, false); m_gyro_sensor->delete_interval(client_id, false); + m_fusion_sensor->delete_interval(client_id, false); + return sensor_base::delete_interval(client_id, false); } void gaming_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_gaming_rv; - - if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) { - diff_time = event.data.timestamp - m_time; - if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) - return; - - pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); - - m_accel.m_time_stamp = event.data.timestamp; - - m_enable_gaming_rv |= ACCELEROMETER_ENABLED; - } - else if (event.event_type == GYROSCOPE_RAW_DATA_EVENT) { + if (event.event_type == FUSION_EVENT) { diff_time = event.data.timestamp - m_time; if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) return; - pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale); - - m_gyro.m_time_stamp = event.data.timestamp; - - m_enable_gaming_rv |= GYROSCOPE_ENABLED; - } - - if (m_enable_gaming_rv == GAMING_RV_ENABLED) { - m_enable_gaming_rv = 0; - - m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, NULL); - - quaternion_gaming_rv = m_orientation_filter.m_quat_gaming_rv; - m_time = get_timestamp(); rv_event.sensor_id = get_id(); rv_event.event_type = GAMING_RV_RAW_DATA_EVENT; rv_event.data.accuracy = SENSOR_ACCURACY_GOOD; rv_event.data.timestamp = m_time; rv_event.data.value_count = 4; - rv_event.data.values[0] = quaternion_gaming_rv.m_quat.m_vec[1]; - rv_event.data.values[1] = quaternion_gaming_rv.m_quat.m_vec[2]; - rv_event.data.values[2] = quaternion_gaming_rv.m_quat.m_vec[3]; - rv_event.data.values[3] = quaternion_gaming_rv.m_quat.m_vec[0]; + rv_event.data.values[0] = event.data.values[1]; + rv_event.data.values[1] = event.data.values[2]; + rv_event.data.values[2] = event.data.values[3]; + rv_event.data.values[3] = event.data.values[0]; push(rv_event); } @@ -267,36 +206,20 @@ void gaming_rv_sensor::synthesize(const sensor_event_t& event, vector accel; - sensor_data gyro; - - sensor_data_t accel_data; - sensor_data_t gyro_data; - - quaternion quaternion_gaming_rv; + sensor_data_t fusion_data; if (event_type != GAMING_RV_RAW_DATA_EVENT) return -1; - m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); - m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_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); - accel.m_time_stamp = accel_data.timestamp; - gyro.m_time_stamp = gyro_data.timestamp; - - m_orientation_filter_poll.get_device_orientation(&m_accel, &m_gyro, NULL); - - quaternion_gaming_rv = m_orientation_filter_poll.m_quat_gaming_rv; + m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); data.value_count = 4; - data.values[0] = quaternion_gaming_rv.m_quat.m_vec[1]; - data.values[1] = quaternion_gaming_rv.m_quat.m_vec[2]; - data.values[2] = quaternion_gaming_rv.m_quat.m_vec[3]; - data.values[3] = quaternion_gaming_rv.m_quat.m_vec[0]; + data.values[0] = data.values[1]; + data.values[1] = data.values[2]; + data.values[2] = data.values[3]; + data.values[3] = data.values[0]; return 0; } diff --git a/src/rotation_vector/gaming_rv/gaming_rv_sensor.h b/src/rotation_vector/gaming_rv/gaming_rv_sensor.h index 6fe4d28..9a2a7e7 100755 --- a/src/rotation_vector/gaming_rv/gaming_rv_sensor.h +++ b/src/rotation_vector/gaming_rv/gaming_rv_sensor.h @@ -38,11 +38,12 @@ public: bool get_properties(sensor_properties_s &properties); sensor_type_t get_type(void); - int get_sensor_data(const unsigned int data_id, sensor_data_t &data); + int get_sensor_data(const unsigned int event_type, sensor_data_t &data); private: sensor_base *m_accel_sensor; sensor_base *m_gyro_sensor; + sensor_base *m_fusion_sensor; sensor_data m_accel; sensor_data m_gyro; @@ -54,19 +55,12 @@ private: unsigned int m_enable_gaming_rv; - 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]; - int m_accel_rotation_direction_compensation[3]; - int m_gyro_rotation_direction_compensation[3]; - float m_accel_scale; - float m_gyro_scale; bool on_start(void); bool on_stop(void); -- 2.7.4 From 27863743e26f30ce39cfd0609a88ac719c7f4d79 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 12 Mar 2015 09:09:30 +0530 Subject: [PATCH 04/16] Updating Geomagnetic RV sensor after fusion restructuring - Works in combination with the new fusion sensor - supports new configuration xml - cleanup Change-Id: I60fec833776d3c5d3cbfc76f8fdaf3c7a198a708 --- .../geomagnetic_rv/geomagnetic_rv_sensor.cpp | 158 +++++---------------- .../geomagnetic_rv/geomagnetic_rv_sensor.h | 11 +- 2 files changed, 36 insertions(+), 133 deletions(-) diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp index ef56350..381dbe2 100755 --- a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp +++ b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp @@ -35,9 +35,7 @@ #define SENSOR_NAME "GEOMAGNETIC_RV_SENSOR" #define SENSOR_TYPE_GEOMAGNETIC_RV "GEOMAGNETIC_ROTATION_VECTOR" -#define ACCELEROMETER_ENABLED 0x01 -#define GEOMAGNETIC_ENABLED 0x02 -#define GEOMAGNETIC_RV_ENABLED 3 +#define MIN_DELIVERY_DIFF_FACTOR 0.75f #define INITIAL_VALUE -1 @@ -47,13 +45,6 @@ #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_GEOMAGNETIC_STATIC_BIAS "GEOMAGNETIC_STATIC_BIAS" -#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" -#define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION" -#define ELEMENT_ACCEL_SCALE "ACCEL_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) { @@ -65,7 +56,6 @@ 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_accuracy(-1) , m_time(0) { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); @@ -88,57 +78,7 @@ geomagnetic_rv_sensor::geomagnetic_rv_sensor() INFO("m_default_sampling_time = %d", m_default_sampling_time); - if (!config.get(SENSOR_TYPE_GEOMAGNETIC_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_GEOMAGNETIC_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_GEOMAGNETIC_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_GEOMAGNETIC_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_GEOMAGNETIC_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_GEOMAGNETIC_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_GEOMAGNETIC_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; - } geomagnetic_rv_sensor::~geomagnetic_rv_sensor() @@ -151,9 +91,11 @@ bool geomagnetic_rv_sensor::init() m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); - if (!m_accel_sensor || !m_magnetic_sensor) { - ERR("Failed to load sensors, accel: 0x%x, mag: 0x%x", - m_accel_sensor, m_magnetic_sensor); + m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_SENSOR); + + if (!m_accel_sensor || !m_magnetic_sensor || !m_fusion_sensor) { + ERR("Failed to load sensors, accel: 0x%x, mag: 0x%x, fusion: 0x%x", + m_accel_sensor, m_magnetic_sensor, m_fusion_sensor); return false; } @@ -178,6 +120,12 @@ bool geomagnetic_rv_sensor::on_start(void) 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); + 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; } @@ -193,6 +141,12 @@ bool geomagnetic_rv_sensor::on_stop(void) 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_GEOMAGNETIC_ROTATION_VECTOR_ENABLED); + m_fusion_sensor->stop(); + deactivate(); return true; } @@ -204,6 +158,8 @@ bool geomagnetic_rv_sensor::add_interval(int client_id, unsigned int interval) 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); + return sensor_base::add_interval(client_id, interval, false); } @@ -214,61 +170,33 @@ bool geomagnetic_rv_sensor::delete_interval(int client_id) m_accel_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); } -void geomagnetic_rv_sensor::synthesize(const sensor_event_t& event, vector &outs) +void geomagnetic_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_geo_rv; - - if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) { - diff_time = event.data.timestamp - m_time; - - if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) - return; - pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); - - m_accel.m_time_stamp = event.data.timestamp; - - m_enable_geomagnetic_rv |= ACCELEROMETER_ENABLED; - } - else if (event.event_type == GEOMAGNETIC_RAW_DATA_EVENT) { + if (event.event_type == FUSION_EVENT) { diff_time = event.data.timestamp - m_time; if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) return; - pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale); - - m_magnetic.m_time_stamp = event.data.timestamp; - - m_enable_geomagnetic_rv |= GEOMAGNETIC_ENABLED; - } - - if (m_enable_geomagnetic_rv == GEOMAGNETIC_RV_ENABLED) { - m_enable_geomagnetic_rv = 0; - - m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - - m_orientation_filter.get_device_orientation(&m_accel, NULL, &m_magnetic); - - quaternion_geo_rv = m_orientation_filter.m_quat_aid; - m_time = get_timestamp(); rv_event.sensor_id = get_id(); rv_event.event_type = GEOMAGNETIC_RV_RAW_DATA_EVENT; rv_event.data.accuracy = SENSOR_ACCURACY_GOOD; rv_event.data.timestamp = m_time; rv_event.data.value_count = 4; - rv_event.data.values[0] = quaternion_geo_rv.m_quat.m_vec[1]; - rv_event.data.values[1] = quaternion_geo_rv.m_quat.m_vec[2]; - rv_event.data.values[2] = quaternion_geo_rv.m_quat.m_vec[3]; - rv_event.data.values[3] = quaternion_geo_rv.m_quat.m_vec[0]; + rv_event.data.values[0] = event.data.values[1]; + rv_event.data.values[1] = event.data.values[2]; + rv_event.data.values[2] = event.data.values[3]; + rv_event.data.values[3] = event.data.values[0]; push(rv_event); } @@ -278,38 +206,20 @@ void geomagnetic_rv_sensor::synthesize(const sensor_event_t& event, vector accel; - sensor_data magnetic; - - sensor_data_t accel_data; - sensor_data_t magnetic_data; - - quaternion quaternion_geo_rv; + sensor_data_t fusion_data; if (event_type != GEOMAGNETIC_RV_RAW_DATA_EVENT) return -1; - m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); - m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_RAW_DATA_EVENT, magnetic_data); - - pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_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; - magnetic.m_time_stamp = magnetic_data.timestamp; - - m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - - m_orientation_filter_poll.get_device_orientation(&m_accel, NULL, &m_magnetic); - - quaternion_geo_rv = m_orientation_filter_poll.m_quat_aid; + m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); data.value_count = 4; - data.values[0] = quaternion_geo_rv.m_quat.m_vec[1]; - data.values[1] = quaternion_geo_rv.m_quat.m_vec[2]; - data.values[2] = quaternion_geo_rv.m_quat.m_vec[3]; - data.values[3] = quaternion_geo_rv.m_quat.m_vec[0]; + data.values[0] = data.values[1]; + data.values[1] = data.values[2]; + data.values[2] = data.values[3]; + data.values[3] = data.values[0]; return 0; } diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h index 8b54705..3cf1999 100755 --- a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h +++ b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.h @@ -38,11 +38,12 @@ public: bool get_properties(sensor_properties_s &properties); sensor_type_t get_type(void); - int get_sensor_data(const unsigned int data_id, sensor_data_t &data); + int get_sensor_data(const unsigned int event_type, sensor_data_t &data); private: sensor_base *m_accel_sensor; sensor_base *m_magnetic_sensor; + sensor_base *m_fusion_sensor; sensor_data m_accel; sensor_data m_magnetic; @@ -54,20 +55,12 @@ private: unsigned int m_enable_geomagnetic_rv; - 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_geomagnetic_static_bias[3]; - int m_accel_rotation_direction_compensation[3]; - int m_geomagnetic_rotation_direction_compensation[3]; - float m_accel_scale; - float m_geomagnetic_scale; - int m_magnetic_alignment_factor; bool on_start(void); bool on_stop(void); -- 2.7.4 From caba4f34cafa591f25bd5d8a46d3b4b0b3dd7650 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 12 Mar 2015 09:15:32 +0530 Subject: [PATCH 05/16] Updating Rotation Vector sensor after fusion restructuring - Works in combination with the new fusion sensor - supports new configuration xml - cleanup Change-Id: I0670885257fe0d5c607f1de87da74811985ab03f --- src/rotation_vector/rv/rv_sensor.cpp | 206 ++++++----------------------------- src/rotation_vector/rv/rv_sensor.h | 15 +-- 2 files changed, 35 insertions(+), 186 deletions(-) diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp index 7abe036..b3330f1 100755 --- a/src/rotation_vector/rv/rv_sensor.cpp +++ b/src/rotation_vector/rv/rv_sensor.cpp @@ -35,10 +35,7 @@ #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 MIN_DELIVERY_DIFF_FACTOR 0.75f #define INITIAL_VALUE -1 @@ -48,16 +45,6 @@ #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) { @@ -70,7 +57,6 @@ rv_sensor::rv_sensor() : m_accel_sensor(NULL) , m_gyro_sensor(NULL) , m_magnetic_sensor(NULL) -, m_accuracy(-1) , m_time(0) { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); @@ -93,78 +79,7 @@ rv_sensor::rv_sensor() 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() @@ -178,9 +93,11 @@ bool rv_sensor::init() 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); + m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_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; } @@ -208,6 +125,12 @@ bool rv_sensor::on_start(void) 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); + 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; } @@ -226,6 +149,12 @@ bool rv_sensor::on_stop(void) 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_ROTATION_VECTOR_ENABLED); + m_fusion_sensor->stop(); + deactivate(); return true; } @@ -238,6 +167,8 @@ bool rv_sensor::add_interval(int client_id, unsigned int interval) 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); } @@ -249,76 +180,33 @@ bool rv_sensor::delete_interval(int client_id) 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); } -void rv_sensor::synthesize(const sensor_event_t& event, vector &outs) +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_RAW_DATA_EVENT) { - diff_time = event.data.timestamp - m_time; - - if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) - return; - - pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); - - m_accel.m_time_stamp = event.data.timestamp; - - m_enable_orientation |= ACCELEROMETER_ENABLED; - } - else if (event.event_type == GYROSCOPE_RAW_DATA_EVENT) { - diff_time = event.data.timestamp - m_time; - - if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) - return; - - pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale); - m_gyro.m_time_stamp = event.data.timestamp; - - m_enable_orientation |= GYROSCOPE_ENABLED; - } - else if (event.event_type == GEOMAGNETIC_RAW_DATA_EVENT) { + if (event.event_type == FUSION_EVENT) { diff_time = event.data.timestamp - m_time; if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) return; - pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, 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_filter.m_pitch_phase_compensation = m_pitch_rotation_compensation; - m_orientation_filter.m_roll_phase_compensation = m_roll_rotation_compensation; - m_orientation_filter.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; - m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - - m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, &m_magnetic); - - quaternion_orientation = m_orientation_filter.m_quat_9axis; - m_time = get_timestamp(); 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 = m_time; 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]; + rv_event.data.values[0] = event.data.values[1]; + rv_event.data.values[1] = event.data.values[2]; + rv_event.data.values[2] = event.data.values[3]; + rv_event.data.values[3] = event.data.values[0]; push(rv_event); } @@ -328,46 +216,20 @@ void rv_sensor::synthesize(const sensor_event_t& event, vector & int rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) { - sensor_data accel; - sensor_data gyro; - sensor_data magnetic; - - sensor_data_t accel_data; - sensor_data_t gyro_data; - sensor_data_t magnetic_data; - - quaternion quaternion_orientation; + sensor_data_t fusion_data; if (event_type != ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME) return -1; - m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); - m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data); - m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_RAW_DATA_EVENT, 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_filter_poll.m_pitch_phase_compensation = m_pitch_rotation_compensation; - m_orientation_filter_poll.m_roll_phase_compensation = m_roll_rotation_compensation; - m_orientation_filter_poll.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; - m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - - m_orientation_filter_poll.get_device_orientation(&m_accel, &m_gyro, &m_magnetic); - - quaternion_orientation = m_orientation_filter_poll.m_quat_9axis; + m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); 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]; + data.values[0] = data.values[1]; + data.values[1] = data.values[2]; + data.values[2] = data.values[3]; + data.values[3] = data.values[0]; return 0; } diff --git a/src/rotation_vector/rv/rv_sensor.h b/src/rotation_vector/rv/rv_sensor.h index e3d2158..3e5de3d 100755 --- a/src/rotation_vector/rv/rv_sensor.h +++ b/src/rotation_vector/rv/rv_sensor.h @@ -44,6 +44,7 @@ private: 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; @@ -56,26 +57,12 @@ private: unsigned int m_enable_orientation; - 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); -- 2.7.4 From 4577ab62ac35105dcd38390c5ed49d2edb262ac1 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 16 Mar 2015 09:06:22 +0530 Subject: [PATCH 06/16] Updating spec file after fusion framework restructuring - updating the spec file to support new fusion framework structure as this was not committed earlier. Change-Id: If85a6286d23c2c656a0bb81570872fc0782526b1 --- packaging/sensord.spec | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 583da23..e311020 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -30,6 +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 build_test_suite OFF %description @@ -81,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} \ - -DTEST_SUITE=%{build_test_suite} \ + -DFUSION=%{fusion_state} -DTEST_SUITE=%{build_test_suite} \ -DLIBDIR=%{_libdir} -DINCLUDEDIR=%{_includedir} %build -- 2.7.4 From e7d4b9ab7bf5eaa6ed2b679d4aacfc676e38b8ac Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 16 Mar 2015 19:15:36 +0530 Subject: [PATCH 07/16] Moving azimuth offset after rotation compensation - moving computation of azimuth to range 0-360 after compensating for azimuth rotation. - initializing fusion sensor pointer. Change-Id: Ie13af01dd3e76f2a3dd0bc1f2ae68726bcf3073d --- src/orientation/orientation_sensor.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index aee027a..b9291e8 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -63,6 +63,7 @@ orientation_sensor::orientation_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(); @@ -244,6 +245,10 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector Date: Mon, 16 Mar 2015 19:20:01 +0530 Subject: [PATCH 08/16] Fixing value assignment to m_orientation - fixing value assignment for m_orientation in orientation_filter - cleanup of fusion_sensor Change-Id: I3442dd275adc3952ff92b5911bcab2b6751ccf79 --- src/fusion/fusion_sensor.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/fusion/fusion_sensor.h b/src/fusion/fusion_sensor.h index 010c786..8b4013f 100755 --- a/src/fusion/fusion_sensor.h +++ b/src/fusion/fusion_sensor.h @@ -76,9 +76,6 @@ private: 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); -- 2.7.4 From ad424c3610364192c9704965295e75c42dc3ae07 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 18 Mar 2015 13:32:51 +0530 Subject: [PATCH 09/16] Improved rotation matrix to quaternion conversion - This has better azimuth computation response over previous implementation. Change-Id: Ie631f1f3e3815c4553d4bb51efdabf23738f680a --- src/sensor_fusion/rotation_matrix.cpp | 54 +++++++++++------------------------ 1 file changed, 16 insertions(+), 38 deletions(-) diff --git a/src/sensor_fusion/rotation_matrix.cpp b/src/sensor_fusion/rotation_matrix.cpp index 83ff5d3..fd3e6b5 100644 --- a/src/sensor_fusion/rotation_matrix.cpp +++ b/src/sensor_fusion/rotation_matrix.cpp @@ -95,44 +95,22 @@ template quaternion rot_mat2quat(rotation_matrix rm) { T q0, q1, q2, q3; - - T diag_sum = rm.m_rot_mat.m_mat[0][0] + rm.m_rot_mat.m_mat[1][1] + rm.m_rot_mat.m_mat[2][2]; - - if ( diag_sum > 0 ) - { - T val = (T) 0.5 / sqrt(diag_sum + (T) 1.0); - q0 = (T) 0.25 / val; - q1 = ( rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) * val; - q2 = ( rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) * val; - q3 = ( rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) * val; - } - else - { - if ( rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[1][1] && rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[2][2] ) - { - T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[2][2]); - q0 = (rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) / val; - q1 = (T) 0.25 * val; - q2 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val; - q3 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val; - } - else if (rm.m_rot_mat.m_mat[1][1] > rm.m_rot_mat.m_mat[2][2]) - { - T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[2][2]); - q0 = (rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) / val; - q1 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val; - q2 = (T) 0.25 * val; - q3 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val; - } - else - { - T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[2][2] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] ); - q0 = (rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) / val; - q1 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val; - q2 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val; - q3 = (T) 0.25 * val; - } - } + T phi, theta, psi; + + phi = atan2(rm.m_rot_mat.m_mat[2][1], rm.m_rot_mat.m_mat[2][2]); + theta = atan2(-rm.m_rot_mat.m_mat[2][0], + sqrt((rm.m_rot_mat.m_mat[2][1] * rm.m_rot_mat.m_mat[2][1]) + + (rm.m_rot_mat.m_mat[2][2] * rm.m_rot_mat.m_mat[2][2]))); + psi = atan2(rm.m_rot_mat.m_mat[1][0], rm.m_rot_mat.m_mat[0][0]); + + q0 = (cos(phi/2) * cos(theta/2) * cos(psi/2)) + + (sin(phi/2) * sin(theta/2) * sin(psi/2)); + q1 = (-cos(phi/2) * sin(theta/2) * sin(psi/2)) + + (sin(phi/2) * cos(theta/2) * cos(psi/2)); + q2 = (cos(phi/2) * sin(theta/2) * cos(psi/2)) + + (sin(phi/2) * cos(theta/2) * sin(psi/2)); + q3 = (cos(phi/2) * cos(theta/2) * sin(psi/2)) - + (sin(phi/2) * sin(theta/2) * cos(psi/2)); quaternion q(q0, q1, q2, q3); -- 2.7.4 From cd5d6ad07f9a3fd3f51553ed5bb97bafe5835ded Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 18 Mar 2015 14:16:45 +0530 Subject: [PATCH 10/16] Fixing gyro data handling and cleanup - Fixing code related to input gyro data handling. - cleanup of orientation filter code. Change-Id: I9efd0c69ed7c2ab0dcc334e460723d9f8cdea668 --- src/sensor_fusion/orientation_filter.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index 47c7b34..5aa4968 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -88,9 +88,9 @@ inline void orientation_filter::initialize_sensor_data(const sensor_datam_time_stamp; - m_gyro.m_data = m_gyro.m_data * (TYPE) PI; + m_gyro.m_data = gyro->m_data * (TYPE) PI; - m_gyro.m_data = gyro->m_data - m_bias_correction; + m_gyro.m_data = m_gyro.m_data - m_bias_correction; } if (magnetic != NULL) { @@ -226,7 +226,7 @@ inline void orientation_filter::time_update() m_quat_9axis = quat_output; m_quat_gaming_rv = m_quat_9axis; - orientation = quat2euler(quat_output); + m_orientation = quat2euler(quat_output); m_rot_matrix = quat2rot_mat(m_quat_driv); @@ -292,6 +292,8 @@ inline void orientation_filter::time_update_gaming_rv() quat_output = phase_correction(m_quat_driv, m_quat_aid); + m_orientation = quat2euler(quat_output); + quat_error = m_quat_aid * m_quat_driv; euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI; -- 2.7.4 From 8465531093b0eadd1041680b9b2ae4e3c9cc469b Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 18 Mar 2015 14:27:50 +0530 Subject: [PATCH 11/16] Updating orientation sensor standalone test files - Updating orientation sensor standalone test files after restructuring or sensor fusion Change-Id: I58519ba358290142d585f2d5aea9e667c61a4f33 --- src/sensor_fusion/test/orientation_sensor.cpp | 7 ------- .../orientation_sensor_main.cpp | 18 ++++++++++++++++-- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp index b9ff4bb..784ac3b 100644 --- a/src/sensor_fusion/test/orientation_sensor.cpp +++ b/src/sensor_fusion/test/orientation_sensor.cpp @@ -28,10 +28,6 @@ int sign_magnetic[] = {+1, +1, +1}; float scale_accel = 1; float scale_gyro = 1150; float scale_magnetic = 1; - -int pitch_phase_compensation = -1; -int roll_phase_compensation = -1; -int azimuth_phase_compensation = -1; int magnetic_alignment_factor = -1; void pre_process_data(sensor_data *data_out, sensor_data *data_in, float *bias, int *sign, float scale) @@ -53,9 +49,6 @@ void orientation_sensor::get_device_orientation(sensor_data *accel_data, 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; orien_filter.get_device_orientation(accel_data, gyro_data, magnetic_data); diff --git a/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp b/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp index 53599d2..a35a6ce 100644 --- a/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp +++ b/src/sensor_fusion/test/test_projects/orientation_sensor_test/orientation_sensor_main.cpp @@ -26,6 +26,9 @@ using namespace std; #define ORIENTATION_DATA_PATH "../../../design/data/100ms/orientation/roll_pitch_yaw/" #define ORIENTATION_DATA_SIZE 1095 +int pitch_phase_compensation = -1; +int roll_phase_compensation = -1; +int azimuth_phase_compensation = -1; int main() { @@ -81,9 +84,20 @@ int main() orien_sensor.get_device_orientation(&accel_data, &gyro_data, &magnetic_data); - orien_file << orien_sensor.orien_filter.m_orientation.m_ang; + orientation = orien_sensor.orien_filter.m_orientation; - cout << "Orientation angles\t" << orien_sensor.orien_filter.m_orientation.m_ang << "\n\n"; + orientation = rad2deg(orientation); + + orientation.m_ang.m_vec[0] *= pitch_phase_compensation; + orientation.m_ang.m_vec[1] *= roll_phase_compensation; + orientation.m_ang.m_vec[2] *= azimuth_phase_compensation; + + if (orientation.m_ang.m_vec[2] < 0) + orientation.m_ang.m_vec[2] += 360; + + orien_file << orientation.m_ang; + + cout << "Orientation angles\t" << orientation.m_ang << "\n\n"; cout << "Orientation matrix\t" << orien_sensor.orien_filter.m_rot_matrix.m_rot_mat << "\n\n"; -- 2.7.4 From 08775ae0af1b0ca77342f741b807507b22c0a249 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 18 Mar 2015 14:34:29 +0530 Subject: [PATCH 12/16] Updating octave design files for orientation - Updating octave design files based on latest sensor fusion C/C++ library code. - Matching the functionalities between the octave code and C/C++ library code. Change-Id: I24f26381fc07c5237d3965ddddb01a9d8c2013c3 --- .../design/lib/estimate_orientation.m | 4 ++-- src/sensor_fusion/design/sf_orientation.m | 24 ++++++++++++++++++---- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m index 6f0e852..d7f023a 100755 --- a/src/sensor_fusion/design/lib/estimate_orientation.m +++ b/src/sensor_fusion/design/lib/estimate_orientation.m @@ -24,8 +24,8 @@ % - 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) - PLOT_SCALED_SENSOR_COMPARISON_DATA = 0; - PLOT_INDIVIDUAL_SENSOR_INPUT_DATA = 0; + PLOT_SCALED_SENSOR_COMPARISON_DATA = 1; + PLOT_INDIVIDUAL_SENSOR_INPUT_DATA = 1; MAGNETIC_ALIGNMENT_FACTOR = -1; GYRO_DATA_DISABLED = 0; diff --git a/src/sensor_fusion/design/sf_orientation.m b/src/sensor_fusion/design/sf_orientation.m index 140487a..3a97580 100755 --- a/src/sensor_fusion/design/sf_orientation.m +++ b/src/sensor_fusion/design/sf_orientation.m @@ -48,6 +48,14 @@ Bias_Mx = 0; Bias_My = 0; Bias_Mz = 0; +Sign_Ax = 1; +Sign_Ay = 1; +Sign_Az = 1; + +Sign_Gx = 1; +Sign_Gy = 1; +Sign_Gz = 1; + Sign_Mx = 1; Sign_My = 1; Sign_Mz = 1; @@ -80,15 +88,15 @@ Gyro_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:, Gyro_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,3))') - Bias_Gz)(1:BUFFER_SIZE); Gyro_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/gyro.txt")(:,4))')(1:BUFFER_SIZE); -scale_Gyro = 575; +scale_Gyro = 1150; Gyro_data(1,:) = Gyro_data(1,:)/scale_Gyro; Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro; Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro; % get magnetometer x,y,z axis data from stored file -Mag_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx; -Mag_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My; -Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz; +Mag_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,1))') - Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx; +Mag_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,2))') - Bias_My)(1:BUFFER_SIZE) * Sign_My; +Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,3))') - Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz; Mag_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,4))')(1:BUFFER_SIZE); % estimate orientation @@ -104,6 +112,14 @@ 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 + + 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; -- 2.7.4 From f58d91769d81840fc53a931ff48f9a8554e22146 Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Fri, 20 Mar 2015 15:43:37 +0530 Subject: [PATCH 13/16] Updating event representation for rotation vector virtual sensor -Updating rpresentation from ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME to ROTATION_VECTOR_RAW_DATA_EVENT Change-Id: Iaee8da2a4523f48a08a251ab931e4fc82ccb0911 --- src/libsensord/client_common.cpp | 2 +- src/libsensord/sensor_rv.h | 2 +- src/rotation_vector/rv/rv_sensor.cpp | 6 +++--- src/server/command_worker.cpp | 2 +- test/src/auto_test.c | 2 +- test/src/tc-common.c | 4 ++-- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/libsensord/client_common.cpp b/src/libsensord/client_common.cpp index dc00b87..c051c8c 100755 --- a/src/libsensord/client_common.cpp +++ b/src/libsensord/client_common.cpp @@ -63,7 +63,7 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_EVENT, ORIENTATION_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, PRESSURE_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, TEMPERATURE_RAW_DATA_EVENT, 0, 10), - FILL_LOG_ELEMENT(LOG_ID_EVENT, ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME, 0, 10), + FILL_LOG_ELEMENT(LOG_ID_EVENT, ROTATION_VECTOR_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_RV_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, GAMING_RV_RAW_DATA_EVENT, 0, 10), FILL_LOG_ELEMENT(LOG_ID_EVENT, FUSION_EVENT, 0, 10), diff --git a/src/libsensord/sensor_rv.h b/src/libsensord/sensor_rv.h index a7d6a99..3e65acc 100755 --- a/src/libsensord/sensor_rv.h +++ b/src/libsensord/sensor_rv.h @@ -37,7 +37,7 @@ extern "C" */ enum rot_event_type { - ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME = (ROTATION_VECTOR_SENSOR << 16) | 0x0001, + ROTATION_VECTOR_RAW_DATA_EVENT = (ROTATION_VECTOR_SENSOR << 16) | 0x0001, }; /** diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp index b3330f1..f647ef0 100755 --- a/src/rotation_vector/rv/rv_sensor.cpp +++ b/src/rotation_vector/rv/rv_sensor.cpp @@ -62,7 +62,7 @@ rv_sensor::rv_sensor() 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); + register_supported_event(ROTATION_VECTOR_RAW_DATA_EVENT); m_enable_orientation = 0; if (!config.get(SENSOR_TYPE_RV, ELEMENT_VENDOR, m_vendor)) { @@ -199,7 +199,7 @@ void rv_sensor::synthesize(const sensor_event_t &event, vector & m_time = get_timestamp(); rv_event.sensor_id = get_id(); - rv_event.event_type = ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME; + rv_event.event_type = ROTATION_VECTOR_RAW_DATA_EVENT; rv_event.data.accuracy = SENSOR_ACCURACY_GOOD; rv_event.data.timestamp = m_time; rv_event.data.value_count = 4; @@ -218,7 +218,7 @@ int rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) { sensor_data_t fusion_data; - if (event_type != ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME) + if (event_type != ROTATION_VECTOR_RAW_DATA_EVENT) return -1; m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); diff --git a/src/server/command_worker.cpp b/src/server/command_worker.cpp index 19965a0..7a8c59e 100755 --- a/src/server/command_worker.cpp +++ b/src/server/command_worker.cpp @@ -852,7 +852,7 @@ void insert_priority_list(unsigned int event_type) if (event_type == ORIENTATION_RAW_DATA_EVENT || event_type == LINEAR_ACCEL_RAW_DATA_EVENT || event_type == GRAVITY_RAW_DATA_EVENT || - event_type == ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME) { + event_type == ROTATION_VECTOR_RAW_DATA_EVENT) { priority_list.insert(ACCELEROMETER_RAW_DATA_EVENT); priority_list.insert(GYROSCOPE_RAW_DATA_EVENT); priority_list.insert(GEOMAGNETIC_RAW_DATA_EVENT); diff --git a/test/src/auto_test.c b/test/src/auto_test.c index 6bb0d29..67f5319 100644 --- a/test/src/auto_test.c +++ b/test/src/auto_test.c @@ -223,7 +223,7 @@ int main(int argc, char **argv) result = check_sensor_api(PRESSURE_RAW_DATA_EVENT, interval); fprintf(fp, "Pressure - RAW_DATA_REPORT_ON_TIME - %d\n", result); - result = check_sensor_api(ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME, interval); + result = check_sensor_api(ROTATION_VECTOR_RAW_DATA_EVENT, interval); fprintf(fp, "Rotation Vector - RAW_DATA_REPORT_ON_TIME - %d\n", result); result = check_sensor_api(GEOMAGNETIC_RV_RAW_DATA_EVENT, interval); diff --git a/test/src/tc-common.c b/test/src/tc-common.c index 8606165..634af34 100644 --- a/test/src/tc-common.c +++ b/test/src/tc-common.c @@ -104,7 +104,7 @@ int get_event_driven(sensor_type_t sensor_type, char str[]) break; case ROTATION_VECTOR_SENSOR: if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) - return ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME; + return ROTATION_VECTOR_RAW_DATA_EVENT; break; case GEOMAGNETIC_RV_SENSOR: if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) @@ -217,7 +217,7 @@ int main(int argc, char **argv) } else if (strcmp(argv[1], "rotation_vector") == 0) { sensor_type = ROTATION_VECTOR_SENSOR; - event = ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME; + event = ROTATION_VECTOR_RAW_DATA_EVENT; } else if (strcmp(argv[1], "geomagnetic_rv") == 0) { sensor_type = GEOMAGNETIC_RV_SENSOR; -- 2.7.4 From 88fefc026292eeb306ad16a2bd288aa9d79d3c42 Mon Sep 17 00:00:00 2001 From: Ankur Date: Fri, 20 Mar 2015 16:08:08 +0530 Subject: [PATCH 14/16] Restructuring test folder to include parallel sensor test case Restructured the test folder to separate the functions from tc-common because they will also be used by a new test case (to be added in the next patch) for testing multiple test cases in parallel. Change-Id: If36de3b1123bd3811516fa859be73a753dc686e7 --- packaging/sensord.spec | 4 +- test/CMakeLists.txt | 16 ++-- test/src/{auto_test.c => api-test.c} | 0 test/src/{tc-common.c => check-sensor.c} | 135 +------------------------- test/src/check-sensor.h | 28 ++++++ test/src/sensor-test.c | 156 +++++++++++++++++++++++++++++++ 6 files changed, 199 insertions(+), 140 deletions(-) rename test/src/{auto_test.c => api-test.c} (100%) rename test/src/{tc-common.c => check-sensor.c} (61%) create mode 100644 test/src/check-sensor.h create mode 100644 test/src/sensor-test.c diff --git a/packaging/sensord.spec b/packaging/sensord.spec index e311020..af660f2 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -138,7 +138,7 @@ systemctl daemon-reload %if %{build_test_suite} == "ON" %files -n sensor-test %defattr(-,root,root,-) -%{_bindir}/auto_test -%{_bindir}/tc-common +%{_bindir}/api-test +%{_bindir}/sensor-test %license LICENSE.APLv2 %endif diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index e81b9c0..0f6c8d4 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -25,14 +25,14 @@ SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${EXTRA_CFLAGS}") link_directories(../src/libsensord/) -add_executable(auto_test src/auto_test.c) -add_executable(tc-common src/tc-common.c) +add_executable(api-test src/api-test.c) +add_executable(sensor-test src/sensor-test.c src/check-sensor.c) -SET_TARGET_PROPERTIES(auto_test PROPERTIES LINKER_LANGUAGE C) -SET_TARGET_PROPERTIES(tc-common PROPERTIES LINKER_LANGUAGE C) +SET_TARGET_PROPERTIES(api-test PROPERTIES LINKER_LANGUAGE C) +SET_TARGET_PROPERTIES(sensor-test PROPERTIES LINKER_LANGUAGE C) -target_link_libraries(auto_test glib-2.0 dlog sensor) -target_link_libraries(tc-common glib-2.0 dlog sensor) +target_link_libraries(api-test glib-2.0 dlog sensor) +target_link_libraries(sensor-test glib-2.0 dlog sensor) -INSTALL(TARGETS auto_test DESTINATION /usr/bin/) -INSTALL(TARGETS tc-common DESTINATION /usr/bin/) +INSTALL(TARGETS api-test DESTINATION /usr/bin/) +INSTALL(TARGETS sensor-test DESTINATION /usr/bin/) diff --git a/test/src/auto_test.c b/test/src/api-test.c similarity index 100% rename from test/src/auto_test.c rename to test/src/api-test.c diff --git a/test/src/tc-common.c b/test/src/check-sensor.c similarity index 61% rename from test/src/tc-common.c rename to test/src/check-sensor.c index 634af34..3c28feb 100644 --- a/test/src/tc-common.c +++ b/test/src/check-sensor.c @@ -26,39 +26,10 @@ #include #include -#define DEFAULT_EVENT_INTERVAL 100 +#include "check-sensor.h" static GMainLoop *mainloop; -void usage() -{ - printf("Usage : ./tc-common (optional) (optional)\n\n"); - - printf("Sensor_type: "); - printf("[accelerometer] "); - printf("[gyroscope] "); - printf("[pressure] "); - printf("[temperature] "); - printf("[geomagnetic] "); - printf("[orientation] "); - printf("[gravity] "); - printf("[linear_accel] "); - printf("[rotation_vector] "); - printf("[geomagnetic_rv] "); - printf("[gaming_rv] "); - printf("[light]\n"); - printf("event:"); - printf("[RAW_DATA_REPORT_ON_TIME]\n"); - - printf("Sensor_type: "); - printf("[proximity]\n"); - printf("event:"); - printf("[EVENT_CHANGE_STATE]\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"); -} - int get_event_driven(sensor_type_t sensor_type, char str[]) { switch (sensor_type) { @@ -168,107 +139,11 @@ void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, voi } } -int main(int argc, char **argv) +int check_sensor(sensor_type_t sensor_type, unsigned int event, int interval) { - int result, handle, start_handle, stop_handle, interval; - char *end1, *end2; - int event; - bool EVENT_NOT_ENTERED = TRUE; - sensor_type_t sensor_type; - mainloop = g_main_loop_new(NULL, FALSE); - - if (argc < 2 || argc > 4) { - printf("Wrong number of arguments\n"); - usage(); - return 0; - } + int handle, result, start_handle, stop_handle; - if (strcmp(argv[1], "accelerometer") == 0) { - sensor_type = ACCELEROMETER_SENSOR; - event = ACCELEROMETER_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "gyroscope") == 0) { - sensor_type = GYROSCOPE_SENSOR; - event = GYROSCOPE_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "pressure") == 0) { - sensor_type = PRESSURE_SENSOR; - event = PRESSURE_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "temperature") == 0) { - sensor_type = TEMPERATURE_SENSOR; - event = TEMPERATURE_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "geomagnetic") == 0) { - sensor_type = GEOMAGNETIC_SENSOR; - event = GEOMAGNETIC_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "orientation") == 0) { - sensor_type = ORIENTATION_SENSOR; - event = ORIENTATION_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "gravity") == 0) { - sensor_type = GRAVITY_SENSOR; - event = GRAVITY_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "linear_accel") == 0) { - sensor_type = LINEAR_ACCEL_SENSOR; - event = LINEAR_ACCEL_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "rotation_vector") == 0) { - sensor_type = ROTATION_VECTOR_SENSOR; - event = ROTATION_VECTOR_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "geomagnetic_rv") == 0) { - sensor_type = GEOMAGNETIC_RV_SENSOR; - event = GEOMAGNETIC_RV_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "gaming_rv") == 0) { - sensor_type = GAMING_RV_SENSOR; - event = GAMING_RV_RAW_DATA_EVENT; - } - else if (strcmp(argv[1], "light") == 0) { - sensor_type = LIGHT_SENSOR; - event = LIGHT_LUX_DATA_EVENT; - } - else if (strcmp(argv[1], "proximity") == 0) { - sensor_type = PROXIMITY_SENSOR; - event = PROXIMITY_CHANGE_STATE_EVENT; - } - else { - usage(); - } - - interval = DEFAULT_EVENT_INTERVAL; - - if (argc > 2) { - event = get_event_driven(sensor_type, argv[2]); - - if (event == -1) { - usage(); - return -1; - } - - EVENT_NOT_ENTERED = FALSE; - } - - if (argc == 4) { - interval = strtol(argv[3], &end1, 10); - - if (*end1) { - printf("Conversion error, non-convertible part: %s\n", end1); - return -1; - } - } - - if (argc == 3 && EVENT_NOT_ENTERED) { - interval = strtol(argv[2], &end2, 10); - - if (*end2) { - printf("Conversion error, non-convertible part: %s\n", end2); - return -1; - } - } + mainloop = g_main_loop_new(NULL, FALSE); sensor_t sensor = sensord_get_sensor(sensor_type); handle = sensord_connect(sensor); @@ -276,7 +151,7 @@ int main(int argc, char **argv) result = sensord_register_event(handle, event, interval, 0, callback, NULL); if (result < 0) { - printf("Can't register %s\n", argv[1]); + printf("Can't register sensor\n"); return -1; } diff --git a/test/src/check-sensor.h b/test/src/check-sensor.h new file mode 100644 index 0000000..097a770 --- /dev/null +++ b/test/src/check-sensor.h @@ -0,0 +1,28 @@ +/* + * 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. + * + */ +#ifndef CHECK_SENSOR_H +#define CHECK_SENSOR_H + +#define DEFAULT_EVENT_INTERVAL 100 + +int get_event_driven(sensor_type_t sensor_type, char str[]); +void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data); +int check_sensor(sensor_type_t sensor_type, unsigned int event, int interval); + +#endif diff --git a/test/src/sensor-test.c b/test/src/sensor-test.c new file mode 100644 index 0000000..0b55112 --- /dev/null +++ b/test/src/sensor-test.c @@ -0,0 +1,156 @@ +/* + * 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 "check-sensor.h" + +void usage() +{ + printf("Usage : ./sensor-test (optional) (optional)\n\n"); + + printf("Sensor_type: "); + printf("[accelerometer] "); + printf("[gyroscope] "); + printf("[pressure] "); + printf("[temperature] "); + printf("[geomagnetic] "); + printf("[orientation] "); + printf("[gravity] "); + printf("[linear_accel] "); + printf("[rotation_vector] "); + printf("[geomagnetic_rv] "); + printf("[gaming_rv] "); + printf("[light]\n"); + printf("event:"); + printf("[RAW_DATA_REPORT_ON_TIME]\n"); + + printf("Sensor_type: "); + printf("[proximity]\n"); + printf("event:"); + printf("[EVENT_CHANGE_STATE]\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"); +} + +int main(int argc, char **argv) +{ + int interval; + unsigned int event; + sensor_type_t sensor_type; + + char *end1; + + if (argc < 2 || argc > 4) { + printf("Wrong number of arguments\n"); + usage(); + return 0; + } + + if (strcmp(argv[1], "accelerometer") == 0) { + sensor_type = ACCELEROMETER_SENSOR; + event = ACCELEROMETER_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "gyroscope") == 0) { + sensor_type = GYROSCOPE_SENSOR; + event = GYROSCOPE_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "pressure") == 0) { + sensor_type = PRESSURE_SENSOR; + event = PRESSURE_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "temperature") == 0) { + sensor_type = TEMPERATURE_SENSOR; + event = TEMPERATURE_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "geomagnetic") == 0) { + sensor_type = GEOMAGNETIC_SENSOR; + event = GEOMAGNETIC_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "orientation") == 0) { + sensor_type = ORIENTATION_SENSOR; + event = ORIENTATION_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "gravity") == 0) { + sensor_type = GRAVITY_SENSOR; + event = GRAVITY_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "linear_accel") == 0) { + sensor_type = LINEAR_ACCEL_SENSOR; + event = LINEAR_ACCEL_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "rotation_vector") == 0) { + sensor_type = ROTATION_VECTOR_SENSOR; + event = ROTATION_VECTOR_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "geomagnetic_rv") == 0) { + sensor_type = GEOMAGNETIC_RV_SENSOR; + event = GEOMAGNETIC_RV_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "gaming_rv") == 0) { + sensor_type = GAMING_RV_SENSOR; + event = GAMING_RV_RAW_DATA_EVENT; + } + else if (strcmp(argv[1], "light") == 0) { + sensor_type = LIGHT_SENSOR; + event = LIGHT_LUX_DATA_EVENT; + } + else if (strcmp(argv[1], "proximity") == 0) { + sensor_type = PROXIMITY_SENSOR; + event = PROXIMITY_CHANGE_STATE_EVENT; + } + else { + usage(); + } + + interval = DEFAULT_EVENT_INTERVAL; + + if (argc == 3) { + int temp_event = get_event_driven(sensor_type, argv[2]); + + if (temp_event == -1) { + interval = atoi(argv[2]); + if (interval == 0){ + usage(); + return -1; + } + } + else { + event = temp_event; + } + } + else if (argc == 4) { + event = get_event_driven(sensor_type, argv[2]); + interval = strtol(argv[3], &end1, 10); + + if (*end1) { + printf("Conversion error, non-convertible part: %s\n", end1); + return -1; + } + } + + return check_sensor(sensor_type, event, interval); +} -- 2.7.4 From ab1a0791801b6c91316a3e032b6dcfea5bbbae77 Mon Sep 17 00:00:00 2001 From: Ankur Date: Mon, 23 Mar 2015 12:35:03 +0530 Subject: [PATCH 15/16] Adding new test case for testing sensors in parallel This test case runs multiple sensor tests in parallel. It can be used for performance analysis of the various modules. Change-Id: I10a3e7ec98140ce6ecdcb6a29a0b8d6052e3287e --- packaging/sensord.spec | 1 + test/CMakeLists.txt | 4 ++ test/src/performance-test.c | 110 ++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 115 insertions(+) create mode 100644 test/src/performance-test.c diff --git a/packaging/sensord.spec b/packaging/sensord.spec index af660f2..0f3c296 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -140,5 +140,6 @@ systemctl daemon-reload %defattr(-,root,root,-) %{_bindir}/api-test %{_bindir}/sensor-test +%{_bindir}/performance-test %license LICENSE.APLv2 %endif diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 0f6c8d4..8716991 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -27,12 +27,16 @@ 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) 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) 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) INSTALL(TARGETS api-test DESTINATION /usr/bin/) INSTALL(TARGETS sensor-test DESTINATION /usr/bin/) +INSTALL(TARGETS performance-test DESTINATION /usr/bin/) diff --git a/test/src/performance-test.c b/test/src/performance-test.c new file mode 100644 index 0000000..207b6fc --- /dev/null +++ b/test/src/performance-test.c @@ -0,0 +1,110 @@ +/* + * 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 "check-sensor.h" + +void usage() +{ + printf("Usage : ./performance-test (optional)\n\n"); + + printf("TIMEOUT:\n"); + printf("time for which the parallel sensor test cases should run\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.\n"); + printf("If no value for sensor is entered default value by the driver will be used.\n"); +} + +int main(int argc, char** argv) +{ + pid_t b = 1; + + int i = 0; + int interval = DEFAULT_EVENT_INTERVAL; + int TIMEOUT = 10; //in seconds for which all the sensor tests should run + + if(argc < 2) { + usage(); + return -1; + } + else if(argc == 2){ + TIMEOUT = atoi(argv[1]); + if (TIMEOUT == 0) { + usage(); + return -1; + } + } + else { + TIMEOUT = atoi(argv[1]); + interval = atoi(argv[2]); + if (TIMEOUT == 0 || interval == 0) { + usage(); + return -1; + } + } + + //make an array of size MAX and fill it with all the sensors needed to run + int MAX = 6; + pid_t pids[MAX]; + sensor_type_t sensor[MAX]; + + //Update the value of MAX and add more sensors here to test more sensors in parallel + sensor[0] = ACCELEROMETER_SENSOR; + sensor[1] = GYROSCOPE_SENSOR; + sensor[2] = GEOMAGNETIC_SENSOR; + sensor[3] = PRESSURE_SENSOR; + sensor[4] = PROXIMITY_SENSOR; + sensor[MAX-1] = LIGHT_SENSOR; + + while (i < MAX) { + if (b > 0) { + b = fork(); + if (b == -1) perror("Fork failed\n"); + else if (b == 0) { + break; + } + pids[i] = b; + i++; + } + } + + if (i < MAX) { + // call the sensord test tc-common for a sensor. + int event = (sensor[i] << 16) | 0x0001; + check_sensor(sensor[i], event, interval); + } + else { + // Main Parent Child. Waits for TIMEOUT and then kills all child processes. + sleep (TIMEOUT); + int j = 0; + + for (j = 0; j < MAX; j++) { + char command[100]; + sprintf(command, "kill %d", pids[j]); + system(command); + } + } + + return 0; +} -- 2.7.4 From e1dcb072724002f72076fdf22e1f2ac590e4b009 Mon Sep 17 00:00:00 2001 From: Vibhor Gaur Date: Wed, 8 Apr 2015 17:36:30 +0530 Subject: [PATCH 16/16] Adding polling based test functionality for sensors -Adding functionality for getting sensor event logs using polling mechansim. -Change is being integrated in the test suite after there was amajor restructuring of the test folder to combine all tests into one. Change-Id: I23a817895a9e7db8f7c4284f0d0c6c697476f5e9 --- test/src/check-sensor.c | 109 +++++++++++++++++++++++++++++++++++++++++------- test/src/check-sensor.h | 4 +- test/src/sensor-test.c | 79 +++++++++++++++++++++++++---------- 3 files changed, 153 insertions(+), 39 deletions(-) diff --git a/test/src/check-sensor.c b/test/src/check-sensor.c index 3c28feb..aa15e34 100644 --- a/test/src/check-sensor.c +++ b/test/src/check-sensor.c @@ -30,63 +30,109 @@ static GMainLoop *mainloop; -int get_event_driven(sensor_type_t sensor_type, char str[]) +void printpollinglogs(sensor_type_t type,sensor_data_t data) +{ + switch(type) { + case(ACCELEROMETER_SENSOR): + printf("Accelerometer [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); + break; + case(GYROSCOPE_SENSOR): + printf("Gyroscope [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); + break; + case(PRESSURE_SENSOR): + printf("Pressure [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); + break; + case(GEOMAGNETIC_SENSOR): + printf("Geomagnetic [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); + break; + case(LIGHT_SENSOR): + printf("Light [%lld] [%6.6f]\n\n", data.timestamp, data.values[0]); + break; + case(TEMPERATURE_SENSOR): + printf("Temperature [%lld] [%6.6f]\n\n", data.timestamp, data.values[0]); + break; + case(PROXIMITY_SENSOR): + printf("Proximity [%lld] [%6.6f]\n\n", data.timestamp, data.values[0]); + break; + 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(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; + case(LINEAR_ACCEL_SENSOR): + printf("Linear Acceleration [%lld] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2]); + break; + case(ROTATION_VECTOR_SENSOR): + printf("Rotation Vector [%lld] [%6.6f] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2], data.values[3]); + break; + case(GEOMAGNETIC_RV_SENSOR): + printf("Geomagnetic Rv [%lld] [%6.6f] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2], data.values[3]); + break; + case(GAMING_RV_SENSOR): + printf("Gaming Rv [%lld] [%6.6f] [%6.6f] [%6.6f] [%6.6f]\n\n", data.timestamp, data.values[0], data.values[1], data.values[2], data.values[3]); + break; + default: + return; + } +} + +int get_event(sensor_type_t sensor_type, char str[]) { switch (sensor_type) { case ACCELEROMETER_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return ACCELEROMETER_RAW_DATA_EVENT; break; case GYROSCOPE_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return GYROSCOPE_RAW_DATA_EVENT; break; case PRESSURE_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return PRESSURE_RAW_DATA_EVENT; break; case GEOMAGNETIC_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return GEOMAGNETIC_RAW_DATA_EVENT; break; case LIGHT_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return LIGHT_LUX_DATA_EVENT; break; case TEMPERATURE_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return TEMPERATURE_RAW_DATA_EVENT; break; case PROXIMITY_SENSOR: - if (strcmp(str, "EVENT_CHANGE_STATE") == 0) + if (strcmp(str, "CHANGE_STATE_EVENT") == 0) return PROXIMITY_CHANGE_STATE_EVENT; break; case ORIENTATION_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return ORIENTATION_RAW_DATA_EVENT; break; case GRAVITY_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return GRAVITY_RAW_DATA_EVENT; break; case LINEAR_ACCEL_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return LINEAR_ACCEL_RAW_DATA_EVENT; break; case ROTATION_VECTOR_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return ROTATION_VECTOR_RAW_DATA_EVENT; break; case GEOMAGNETIC_RV_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return GEOMAGNETIC_RV_RAW_DATA_EVENT; break; case GAMING_RV_SENSOR: - if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) + if (strcmp(str, "RAW_DATA_EVENT") == 0) return GAMING_RV_RAW_DATA_EVENT; break; } - return -1; } @@ -185,3 +231,36 @@ int check_sensor(sensor_type_t sensor_type, unsigned int event, int interval) return 0; } + +int polling_sensor(sensor_type_t sensor_type, unsigned int event) +{ + int result, handle; + printf("Polling based\n"); + + handle = sf_connect(sensor_type); + result = sf_start(handle, 1); + + if (result < 0) { + printf("Can't start the sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } + + sensor_data_t data; + + while(1) { + result = sf_get_data(handle, event , &data); + printpollinglogs(sensor_type, data); + usleep(100000); + } + + result = sf_disconnect(handle); + + if (result < 0) { + printf("Can't disconnect sensor\n"); + printf("Error\n\n\n\n"); + return -1; + } + + return 0; +} diff --git a/test/src/check-sensor.h b/test/src/check-sensor.h index 097a770..86b407b 100644 --- a/test/src/check-sensor.h +++ b/test/src/check-sensor.h @@ -21,8 +21,10 @@ #define DEFAULT_EVENT_INTERVAL 100 -int get_event_driven(sensor_type_t sensor_type, char str[]); +int get_event(sensor_type_t sensor_type, char str[]); void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data); int check_sensor(sensor_type_t sensor_type, unsigned int event, int interval); +void printpollinglogs(sensor_type_t type, sensor_data_t data); +int polling_sensor(sensor_type_t sensor_type, unsigned int event); #endif diff --git a/test/src/sensor-test.c b/test/src/sensor-test.c index 0b55112..5c5c3c1 100644 --- a/test/src/sensor-test.c +++ b/test/src/sensor-test.c @@ -29,7 +29,7 @@ void usage() { - printf("Usage : ./sensor-test (optional) (optional)\n\n"); + printf("Usage : ./sensor-test -p(optional) (optional) (optional)\n\n"); printf("Sensor_type: "); printf("[accelerometer] "); @@ -45,13 +45,13 @@ void usage() printf("[gaming_rv] "); printf("[light]\n"); printf("event:"); - printf("[RAW_DATA_REPORT_ON_TIME]\n"); - + printf("[RAW_DATA_EVENT]\n"); + printf("-p: [polling]\n"); printf("Sensor_type: "); printf("[proximity]\n"); printf("event:"); - printf("[EVENT_CHANGE_STATE]\n"); - + printf("[CHANGE_STATE_EVENT]\n"); + printf("-p: [polling]\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"); } @@ -61,10 +61,11 @@ int main(int argc, char **argv) int interval; unsigned int event; sensor_type_t sensor_type; + bool is_polling; char *end1; - if (argc < 2 || argc > 4) { + if (argc < 2 || argc > 5) { printf("Wrong number of arguments\n"); usage(); return 0; @@ -128,29 +129,61 @@ int main(int argc, char **argv) interval = DEFAULT_EVENT_INTERVAL; - if (argc == 3) { - int temp_event = get_event_driven(sensor_type, argv[2]); + is_polling = FALSE; - if (temp_event == -1) { - interval = atoi(argv[2]); - if (interval == 0){ - usage(); - return -1; + if(argc >= 3 && strcmp(argv[2], "-p") == 0) { + is_polling = TRUE; + } + + if (is_polling) { + if (argc == 4) { + int temp_event = get_event(sensor_type, argv[3]); + if (temp_event == -1) { + interval = atoi(argv[3]); + if (interval == 0){ + usage(); + return -1; + } + } + else { + event = temp_event; } } - else { - event = temp_event; + else if (argc == 5) { + event = get_event(sensor_type, argv[3]); + interval = strtol(argv[4], &end1, 10); + + if (*end1) { + printf("Conversion error, non-convertible part: %s\n", end1); + return -1; + } } + return polling_sensor(sensor_type, event); } - else if (argc == 4) { - event = get_event_driven(sensor_type, argv[2]); - interval = strtol(argv[3], &end1, 10); + else { + if (argc == 3) { + int temp_event = get_event(sensor_type, argv[2]); - if (*end1) { - printf("Conversion error, non-convertible part: %s\n", end1); - return -1; + if (temp_event == -1) { + interval = atoi(argv[2]); + if (interval == 0){ + usage(); + return -1; + } + } + else { + event = temp_event; + } } - } + else if (argc == 4) { + event = get_event(sensor_type, argv[2]); + interval = strtol(argv[3], &end1, 10); - return check_sensor(sensor_type, event, interval); + if (*end1) { + printf("Conversion error, non-convertible part: %s\n", end1); + return -1; + } + } + return check_sensor(sensor_type, event, interval); + } } -- 2.7.4