From 3a5a30495a45f8dc6780081cfc1a90dd38220dd1 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 27 Feb 2015 15:03:57 +0530 Subject: [PATCH 01/16] Removing mutex lock that was affecting performance - Removing mutex lock used during entry into orientation_filter - lock is not needed as each virtual sensor has a separate orientation_filter object - separate objects in virtual sensor for both event driven and polling based modes. Change-Id: Ie2d2aa41a664c858fc1305213c3f9bb2309a7e67 --- src/orientation/orientation_sensor.cpp | 28 +++++++++------------- src/orientation/orientation_sensor.h | 3 ++- src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp | 10 ++------ src/rotation_vector/gaming_rv/gaming_rv_sensor.h | 1 + .../geomagnetic_rv/geomagnetic_rv_sensor.cpp | 12 +++------- .../geomagnetic_rv/geomagnetic_rv_sensor.h | 1 + src/rotation_vector/rv/rv_sensor.cpp | 26 ++++++++------------ src/rotation_vector/rv/rv_sensor.h | 3 ++- src/shared/virtual_sensor.h | 1 - 9 files changed, 32 insertions(+), 53 deletions(-) diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index 500217a..0c8bed3 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -334,15 +334,12 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector m_orientation; + orientation_filter m_orientation_filter; + orientation_filter m_orientation_filter_poll; unsigned int m_enable_orientation; diff --git a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp index 51a0a4b..fb44e0c 100755 --- a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp +++ b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp @@ -244,10 +244,7 @@ void gaming_rv_sensor::synthesize(const sensor_event_t& event, vector m_orientation_filter; + orientation_filter m_orientation_filter_poll; unsigned int m_enable_gaming_rv; diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp index ae0348f..6649333 100755 --- a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp +++ b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp @@ -255,10 +255,7 @@ void geomagnetic_rv_sensor::synthesize(const sensor_event_t& event, vector m_orientation_filter; + orientation_filter m_orientation_filter_poll; unsigned int m_enable_geomagnetic_rv; diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp index 958042e..48c48de 100755 --- a/src/rotation_vector/rv/rv_sensor.cpp +++ b/src/rotation_vector/rv/rv_sensor.cpp @@ -300,15 +300,12 @@ void rv_sensor::synthesize(const sensor_event_t& event, vector & if (m_enable_orientation == ORIENTATION_ENABLED) { m_enable_orientation = 0; - m_orientation.m_pitch_phase_compensation = m_pitch_rotation_compensation; - m_orientation.m_roll_phase_compensation = m_roll_rotation_compensation; - m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; - m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor; + 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; - { - AUTOLOCK(m_fusion_mutex); - quaternion_orientation = m_orientation.get_9axis_quaternion(m_accel, m_gyro, m_magnetic); - } + quaternion_orientation = m_orientation_filter.get_9axis_quaternion(m_accel, m_gyro, m_magnetic); m_time = get_timestamp(); rv_event.sensor_id = get_id(); @@ -353,15 +350,12 @@ int rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) gyro.m_time_stamp = gyro_data.timestamp; magnetic.m_time_stamp = magnetic_data.timestamp; - m_orientation.m_pitch_phase_compensation = m_pitch_rotation_compensation; - m_orientation.m_roll_phase_compensation = m_roll_rotation_compensation; - m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; - m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor; + 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; - { - AUTOLOCK(m_fusion_mutex); - quaternion_orientation = m_orientation.get_9axis_quaternion(m_accel, m_gyro, m_magnetic); - } + quaternion_orientation = m_orientation_filter_poll.get_9axis_quaternion(m_accel, m_gyro, m_magnetic); data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); diff --git a/src/rotation_vector/rv/rv_sensor.h b/src/rotation_vector/rv/rv_sensor.h index 003c3fa..e3d2158 100755 --- a/src/rotation_vector/rv/rv_sensor.h +++ b/src/rotation_vector/rv/rv_sensor.h @@ -51,7 +51,8 @@ private: cmutex m_value_mutex; - orientation_filter m_orientation; + orientation_filter m_orientation_filter; + orientation_filter m_orientation_filter_poll; unsigned int m_enable_orientation; diff --git a/src/shared/virtual_sensor.h b/src/shared/virtual_sensor.h index 3cb79fb..7a3b70c 100755 --- a/src/shared/virtual_sensor.h +++ b/src/shared/virtual_sensor.h @@ -33,7 +33,6 @@ public: bool is_virtual(void); protected: - cmutex m_fusion_mutex; bool activate(void); bool deactivate(void); -- 2.7.4 From 380bc64e391f36f4050d3cd8509ec0ea1cd948c4 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 2 Mar 2015 12:57:59 +0530 Subject: [PATCH 02/16] Adding code to install sensor_gaming_rv.h to CMakelist Adding missed cmake code to install sensor_gaming_rv.h Change-Id: I4f60e0cebddad7176814e6d96aefd77da9ecf53b --- src/libsensord/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/libsensord/CMakeLists.txt b/src/libsensord/CMakeLists.txt index 2c5032e..2a8291f 100755 --- a/src/libsensord/CMakeLists.txt +++ b/src/libsensord/CMakeLists.txt @@ -56,6 +56,7 @@ install(FILES sensor_linear_accel.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sens install(FILES sensor_orientation.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_rv.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_geomagnetic_rv.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) +install(FILES sensor_gaming_rv.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_temperature.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_motion.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_deprecated.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) -- 2.7.4 From 9e537f46b7a6efee960bd17d4a06e19c738bec60 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 2 Mar 2015 13:00:08 +0530 Subject: [PATCH 03/16] Updating documentation code for gaming_rv and geomagnetic_rv Updating/correcting documentation code in api for gaming_rv and geomagnetic_rv sensors Change-Id: I286b82a8931a6922c4c8d9aeeca9749c23b4e51c --- src/libsensord/sensor_gaming_rv.h | 2 +- src/libsensord/sensor_geomagnetic_rv.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/libsensord/sensor_gaming_rv.h b/src/libsensord/sensor_gaming_rv.h index e290313..a02f738 100755 --- a/src/libsensord/sensor_gaming_rv.h +++ b/src/libsensord/sensor_gaming_rv.h @@ -11,7 +11,7 @@ extern "C" #endif /** - * @defgroup SENSOR_GEOMAGNETIC_RV Rotation Vector Sensor + * @defgroup SENSOR_GAMING_RV Gaming Rotation Vector Sensor * @ingroup SENSOR_FRAMEWORK * * These APIs are used to control the Gaming Rotation Vector sensor. diff --git a/src/libsensord/sensor_geomagnetic_rv.h b/src/libsensord/sensor_geomagnetic_rv.h index 8343a42..c0fc830 100755 --- a/src/libsensord/sensor_geomagnetic_rv.h +++ b/src/libsensord/sensor_geomagnetic_rv.h @@ -29,7 +29,7 @@ extern "C" #endif /** - * @defgroup SENSOR_GEOMAGNETIC_RV Rotation Vector Sensor + * @defgroup SENSOR_GEOMAGNETIC_RV Geomagnetic Rotation Vector Sensor * @ingroup SENSOR_FRAMEWORK * * These APIs are used to control the Geomagnetic Rotation Vector sensor. -- 2.7.4 From cd6e49282798d3c643e488c9bc9e2b869fbe3d8a Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 2 Mar 2015 15:57:24 +0530 Subject: [PATCH 04/16] Adding methods for common sensor_fusion flow - Adding new methods for a common orientation computation flow irrespective of the sensors used in the inputs - Changing sensor inputs to pointers for checking if inputs are enabled. - Tested on rotation_matrix functionality which is not used in any virtual sensor and using standalone test code. Change-Id: I763305f06346db2cce87c61e870c0a43f0d1e0e0 --- src/sensor_fusion/orientation_filter.cpp | 66 +++++++++++++++++++++- src/sensor_fusion/orientation_filter.h | 8 ++- src/sensor_fusion/test/orientation_sensor.cpp | 38 ++++++------- src/sensor_fusion/test/orientation_sensor.h | 4 +- .../orientation_sensor_main.cpp | 2 +- 5 files changed, 91 insertions(+), 27 deletions(-) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index 68f0431..03e50d6 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -75,6 +75,38 @@ orientation_filter::~orientation_filter() } template +inline void orientation_filter::initialize_sensor_data(const sensor_data *accel, + const sensor_data *gyro, const sensor_data *magnetic) +{ + if (accel != NULL) { + m_accel.m_data = accel->m_data; + m_accel.m_time_stamp = accel->m_time_stamp; + + normalize(m_accel); + } + + if (gyro != NULL) { + unsigned long long sample_interval_gyro = SAMPLE_INTV; + + if (m_gyro.m_time_stamp != 0 && gyro->m_time_stamp != 0) + sample_interval_gyro = gyro->m_time_stamp - m_gyro.m_time_stamp; + + m_gyro_dt = sample_interval_gyro * US2S; + m_gyro.m_time_stamp = gyro->m_time_stamp; + + m_gyro.m_data = m_gyro.m_data * (TYPE) PI; + + m_gyro.m_data = gyro->m_data - m_bias_correction; + } + + if (magnetic != NULL) { + m_magnetic.m_data = magnetic->m_data; + m_magnetic.m_time_stamp = magnetic->m_time_stamp; + } + +} + +template inline void orientation_filter::init_accel_gyro_mag_data(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic) { @@ -104,6 +136,8 @@ inline void orientation_filter::init_accel_mag_data(const sensor_data @@ -381,6 +415,32 @@ inline void orientation_filter::measurement_update() } template +euler_angles orientation_filter::get_device_rotation(const sensor_data *accel, + const sensor_data *gyro, const sensor_data *magnetic) +{ + initialize_sensor_data(accel, gyro, magnetic); + + const sensor_data accel_in, gyro_in, magnetic_in; + euler_angles cor_euler_ang; + + if (magnetic != NULL) + orientation_triad_algorithm(); + + if (gyro != NULL) { + compute_covariance(); + + if(magnetic != NULL) + time_update(); + else + time_update_gaming_rv(); + + measurement_update(); + } + + return m_orientation; +} + +template euler_angles orientation_filter::get_orientation(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic) { @@ -404,10 +464,10 @@ euler_angles orientation_filter::get_orientation(const sensor_data -rotation_matrix orientation_filter::get_rotation_matrix(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic) +rotation_matrix orientation_filter::get_rotation_matrix(const sensor_data *accel, + const sensor_data *gyro, const sensor_data *magnetic) { - get_orientation(accel, gyro, magnetic); + get_device_rotation(accel, gyro, magnetic); return m_rot_matrix; } diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index a04f3f7..97d372f 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -74,6 +74,8 @@ public: orientation_filter(); ~orientation_filter(); + inline void initialize_sensor_data(const sensor_data *accel, + const sensor_data *gyro, const sensor_data *magnetic); inline void init_accel_gyro_mag_data(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic); inline void init_accel_mag_data(const sensor_data accel, @@ -89,14 +91,16 @@ public: euler_angles get_orientation(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic); - rotation_matrix get_rotation_matrix(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic); + rotation_matrix get_rotation_matrix(const sensor_data *accel, + const sensor_data *gyro, const sensor_data *magnetic); quaternion get_9axis_quaternion(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic); quaternion get_geomagnetic_quaternion(const sensor_data accel, const sensor_data magnetic); quaternion get_gaming_quaternion(const sensor_data accel, const sensor_data gyro); + euler_angles get_device_rotation(const sensor_data *accel, + const sensor_data *gyro, const sensor_data *magnetic); }; #include "orientation_filter.cpp" diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp index 70746f5..30c74c7 100644 --- a/src/sensor_fusion/test/orientation_sensor.cpp +++ b/src/sensor_fusion/test/orientation_sensor.cpp @@ -34,23 +34,23 @@ 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) +void pre_process_data(sensor_data *data_out, sensor_data *data_in, float *bias, int *sign, float scale) { - data_out.m_data.m_vec[0] = sign[0] * (data_in.m_data.m_vec[0] - bias[0]) / scale; - data_out.m_data.m_vec[1] = sign[1] * (data_in.m_data.m_vec[1] - bias[1]) / scale; - data_out.m_data.m_vec[2] = sign[2] * (data_in.m_data.m_vec[2] - bias[2]) / scale; + data_out->m_data.m_vec[0] = sign[0] * (data_in->m_data.m_vec[0] - bias[0]) / scale; + data_out->m_data.m_vec[1] = sign[1] * (data_in->m_data.m_vec[1] - bias[1]) / scale; + data_out->m_data.m_vec[2] = sign[2] * (data_in->m_data.m_vec[2] - bias[2]) / scale; - data_out.m_time_stamp = data_in.m_time_stamp; + data_out->m_time_stamp = data_in->m_time_stamp; } euler_angles orientation_sensor::get_orientation(sensor_data accel_data, sensor_data gyro_data, sensor_data magnetic_data) { - pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + pre_process_data(&accel_data, &accel_data, bias_accel, sign_accel, scale_accel); normalize(accel_data); - pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); - pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); + pre_process_data(&gyro_data, &gyro_data, bias_gyro, sign_gyro, scale_gyro); + pre_process_data(&magnetic_data, &magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); normalize(magnetic_data); orien_filter.m_pitch_phase_compensation = pitch_phase_compensation; @@ -61,14 +61,14 @@ euler_angles orientation_sensor::get_orientation(sensor_data accel return orien_filter.get_orientation(accel_data, gyro_data, magnetic_data); } -rotation_matrix orientation_sensor::get_rotation_matrix(sensor_data accel_data, - sensor_data gyro_data, sensor_data magnetic_data) +rotation_matrix orientation_sensor::get_rotation_matrix(sensor_data *accel_data, + sensor_data *gyro_data, sensor_data *magnetic_data) { pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); - normalize(accel_data); + normalize(*accel_data); pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); - normalize(magnetic_data); + normalize(*magnetic_data); orien_filter.m_pitch_phase_compensation = pitch_phase_compensation; orien_filter.m_roll_phase_compensation = roll_phase_compensation; @@ -81,10 +81,10 @@ rotation_matrix orientation_sensor::get_rotation_matrix(sensor_data orientation_sensor::get_9axis_quaternion(sensor_data accel_data, sensor_data gyro_data, sensor_data magnetic_data) { - pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + pre_process_data(&accel_data, &accel_data, bias_accel, sign_accel, scale_accel); normalize(accel_data); - pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); - pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); + pre_process_data(&gyro_data, &gyro_data, bias_gyro, sign_gyro, scale_gyro); + pre_process_data(&magnetic_data, &magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); normalize(magnetic_data); orien_filter.m_pitch_phase_compensation = pitch_phase_compensation; @@ -98,9 +98,9 @@ quaternion orientation_sensor::get_9axis_quaternion(sensor_data ac quaternion orientation_sensor::get_geomagnetic_quaternion(sensor_data accel_data, sensor_data magnetic_data) { - pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + pre_process_data(&accel_data, &accel_data, bias_accel, sign_accel, scale_accel); normalize(accel_data); - pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); + pre_process_data(&magnetic_data, &magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); normalize(magnetic_data); return orien_filter.get_geomagnetic_quaternion(accel_data, magnetic_data); @@ -110,9 +110,9 @@ quaternion orientation_sensor::get_geomagnetic_quaternion(sensor_data orientation_sensor::get_gaming_quaternion(sensor_data accel_data, sensor_data gyro_data) { - pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + pre_process_data(&accel_data, &accel_data, bias_accel, sign_accel, scale_accel); normalize(accel_data); - pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); + pre_process_data(&gyro_data, &gyro_data, bias_gyro, sign_gyro, scale_gyro); return orien_filter.get_gaming_quaternion(accel_data, gyro_data); } diff --git a/src/sensor_fusion/test/orientation_sensor.h b/src/sensor_fusion/test/orientation_sensor.h index 358c476..5afbff4 100644 --- a/src/sensor_fusion/test/orientation_sensor.h +++ b/src/sensor_fusion/test/orientation_sensor.h @@ -29,8 +29,8 @@ public: euler_angles get_orientation(sensor_data accel, sensor_data gyro, sensor_data magnetic); - rotation_matrix get_rotation_matrix(sensor_data accel, - sensor_data gyro, sensor_data magnetic); + rotation_matrix get_rotation_matrix(sensor_data *accel, + sensor_data *gyro, sensor_data *magnetic); quaternion get_9axis_quaternion(sensor_data accel, sensor_data gyro, sensor_data magnetic); quaternion get_geomagnetic_quaternion(sensor_data accel, 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 c5593e9..8e6830b 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 @@ -85,7 +85,7 @@ int main() cout << "Orientation angles\t" << orientation.m_ang << "\n\n"; - orientation_mat = orien_sensor2.get_rotation_matrix(accel_data, gyro_data, magnetic_data); + orientation_mat = orien_sensor2.get_rotation_matrix(&accel_data, &gyro_data, &magnetic_data); cout << "Orientation matrix\t" << orientation_mat.m_rot_mat << "\n\n"; -- 2.7.4 From f93997c0b689240dd732d717c738f82554400534 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 2 Mar 2015 17:05:23 +0530 Subject: [PATCH 05/16] Fixing return type issue in tc-common - Changing unsigned int to int for event variables and fixing warning. - cleanup Change-Id: I76c30ee886352e194f78ef40c245ab2989244972 --- test/src/tc-common.c | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/test/src/tc-common.c b/test/src/tc-common.c index ce39702..8606165 100644 --- a/test/src/tc-common.c +++ b/test/src/tc-common.c @@ -59,7 +59,7 @@ void usage() 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"); } -unsigned int get_event_driven(sensor_type_t sensor_type, char str[]) +int get_event_driven(sensor_type_t sensor_type, char str[]) { switch (sensor_type) { case ACCELEROMETER_SENSOR: @@ -114,9 +114,9 @@ unsigned int get_event_driven(sensor_type_t sensor_type, char str[]) if (strcmp(str, "RAW_DATA_REPORT_ON_TIME") == 0) return GAMING_RV_RAW_DATA_EVENT; break; - default: - return -1; } + + return -1; } void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data) @@ -125,43 +125,43 @@ void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, voi switch (sensor_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]); + printf("Accelerometer [%lld] [%6.6f] [%6.6f] [%6.6f]\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]); + printf("Gyroscope [%lld] [%6.6f] [%6.6f] [%6.6f]\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]); + printf("Pressure [%lld] [%6.6f] [%6.6f] [%6.6f]\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]); + printf("Geomagnetic [%lld] [%6.6f] [%6.6f] [%6.6f]\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]); + printf("Light [%lld] [%6.6f]\n", data->timestamp, data->values[0]); break; case TEMPERATURE_SENSOR : - printf("Temperature [%lld] [%6.6f]\n\n", data->timestamp, data->values[0]); + printf("Temperature [%lld] [%6.6f]\n", data->timestamp, data->values[0]); break; case PROXIMITY_SENSOR: - printf("Proximity [%lld] [%6.6f]\n\n", data->timestamp, data->values[0]); + printf("Proximity [%lld] [%6.6f]\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]); + printf("Orientation [%lld] [%6.6f] [%6.6f] [%6.6f]\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]); + printf("Gravity [%lld] [%6.6f] [%6.6f] [%6.6f]\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]); + printf("Linear acceleration [%lld] [%6.6f] [%6.6f] [%6.6f]\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]); + printf("Rotation vector [%lld] [%6.6f] [%6.6f] [%6.6f] [%6.6f]\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]); + printf("Geomagnetic RV [%lld] [%6.6f] [%6.6f] [%6.6f] [%6.6f]\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]); + printf("Gaming RV [%lld] [%6.6f] [%6.6f] [%6.6f] [%6.6f]\n", data->timestamp, data->values[0], data->values[1], data->values[2], data->values[3]); break; default: return; @@ -172,7 +172,7 @@ int main(int argc, char **argv) { int result, handle, start_handle, stop_handle, interval; char *end1, *end2; - unsigned int event; + int event; bool EVENT_NOT_ENTERED = TRUE; sensor_type_t sensor_type; mainloop = g_main_loop_new(NULL, FALSE); -- 2.7.4 From f5c62e1096393da4ea88a603705a22e4e77a09a0 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 2 Mar 2015 17:26:40 +0530 Subject: [PATCH 06/16] Changing orientation sensor to follow single sensor fusion flow - Updating orientation sensor to work on common single sensor fusion code flow. Change-Id: I5581e833e33757a0e24d56a90717473bded3e172 --- src/orientation/orientation_sensor.cpp | 4 ++-- src/sensor_fusion/orientation_filter.cpp | 25 ++++------------------ src/sensor_fusion/orientation_filter.h | 4 ++-- src/sensor_fusion/test/orientation_sensor.cpp | 14 ++++++------ src/sensor_fusion/test/orientation_sensor.h | 4 ++-- .../orientation_sensor_main.cpp | 2 +- 6 files changed, 18 insertions(+), 35 deletions(-) diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index 0c8bed3..900a7c0 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -339,7 +339,7 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector orientation_filter::get_device_rotation(const sensor_da { initialize_sensor_data(accel, gyro, magnetic); - const sensor_data accel_in, gyro_in, magnetic_in; - euler_angles cor_euler_ang; - if (magnetic != NULL) orientation_triad_algorithm(); @@ -441,24 +438,10 @@ euler_angles orientation_filter::get_device_rotation(const sensor_da } template -euler_angles orientation_filter::get_orientation(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic) +euler_angles orientation_filter::get_orientation(const sensor_data *accel, + const sensor_data *gyro, const sensor_data *magnetic) { - euler_angles cor_euler_ang; - - init_accel_gyro_mag_data(accel, gyro, magnetic); - - normalize(m_accel); - m_gyro.m_data = m_gyro.m_data * (TYPE) PI; - normalize(m_magnetic); - - orientation_triad_algorithm(); - - compute_covariance(); - - time_update(); - - measurement_update(); + get_device_rotation(accel, gyro, magnetic); return m_orientation; } @@ -477,7 +460,7 @@ quaternion orientation_filter::get_9axis_quaternion(const sensor_dat const sensor_data gyro, const sensor_data magnetic) { - get_orientation(accel, gyro, magnetic); + get_orientation(&accel, &gyro, &magnetic); return m_quat_9axis; } diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index 97d372f..08add8a 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -89,8 +89,8 @@ public: inline void time_update_gaming_rv(); inline void measurement_update(); - euler_angles get_orientation(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic); + euler_angles get_orientation(const sensor_data *accel, + const sensor_data *gyro, const sensor_data *magnetic); rotation_matrix get_rotation_matrix(const sensor_data *accel, const sensor_data *gyro, const sensor_data *magnetic); quaternion get_9axis_quaternion(const sensor_data accel, diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp index 30c74c7..57b6393 100644 --- a/src/sensor_fusion/test/orientation_sensor.cpp +++ b/src/sensor_fusion/test/orientation_sensor.cpp @@ -43,15 +43,15 @@ void pre_process_data(sensor_data *data_out, sensor_data *data_in, data_out->m_time_stamp = data_in->m_time_stamp; } -euler_angles orientation_sensor::get_orientation(sensor_data accel_data, - sensor_data gyro_data, sensor_data magnetic_data) +euler_angles orientation_sensor::get_orientation(sensor_data *accel_data, + sensor_data *gyro_data, sensor_data *magnetic_data) { - pre_process_data(&accel_data, &accel_data, bias_accel, sign_accel, scale_accel); - normalize(accel_data); - pre_process_data(&gyro_data, &gyro_data, bias_gyro, sign_gyro, scale_gyro); - pre_process_data(&magnetic_data, &magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); - normalize(magnetic_data); + pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + normalize(*accel_data); + pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); + pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); + normalize(*magnetic_data); orien_filter.m_pitch_phase_compensation = pitch_phase_compensation; orien_filter.m_roll_phase_compensation = roll_phase_compensation; diff --git a/src/sensor_fusion/test/orientation_sensor.h b/src/sensor_fusion/test/orientation_sensor.h index 5afbff4..dab0677 100644 --- a/src/sensor_fusion/test/orientation_sensor.h +++ b/src/sensor_fusion/test/orientation_sensor.h @@ -27,8 +27,8 @@ class orientation_sensor public: orientation_filter orien_filter; - euler_angles get_orientation(sensor_data accel, - sensor_data gyro, sensor_data magnetic); + euler_angles get_orientation(sensor_data *accel, + sensor_data *gyro, sensor_data *magnetic); rotation_matrix get_rotation_matrix(sensor_data *accel, sensor_data *gyro, sensor_data *magnetic); quaternion get_9axis_quaternion(sensor_data accel, 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 8e6830b..a4e7da1 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 @@ -79,7 +79,7 @@ int main() cout << "Magnetic Data\t" << magnetic_data.m_data << "\t Time Stamp\t" << magnetic_data.m_time_stamp << "\n\n"; - orientation = orien_sensor1.get_orientation(accel_data, gyro_data, magnetic_data); + orientation = orien_sensor1.get_orientation(&accel_data, &gyro_data, &magnetic_data); orien_file << orientation.m_ang; -- 2.7.4 From b4e4a65a2930535b83f691c30f4ea6684a5fb2df Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 3 Mar 2015 15:14:23 +0530 Subject: [PATCH 07/16] Updating rotation vector sensor to follow single sensor fusion flow - updating rotation vector virtual sensor and related orientation_filter code to follow single sensor fusion flow. - cleanup of initialization code and common code with orientation sensor - updating orientation sensor code after code cleanup Change-Id: I28593ec06711575b0646153b3f1fcc0cc43bf932 --- src/rotation_vector/rv/rv_sensor.cpp | 4 ++-- src/sensor_fusion/orientation_filter.cpp | 27 +++------------------- src/sensor_fusion/orientation_filter.h | 4 ++-- src/sensor_fusion/test/orientation_sensor.cpp | 14 +++++------ src/sensor_fusion/test/orientation_sensor.h | 4 ++-- .../orientation_sensor_main.cpp | 2 +- 6 files changed, 17 insertions(+), 38 deletions(-) diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp index 48c48de..2aacf39 100755 --- a/src/rotation_vector/rv/rv_sensor.cpp +++ b/src/rotation_vector/rv/rv_sensor.cpp @@ -305,7 +305,7 @@ void rv_sensor::synthesize(const sensor_event_t& event, vector & m_orientation_filter.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - quaternion_orientation = m_orientation_filter.get_9axis_quaternion(m_accel, m_gyro, m_magnetic); + quaternion_orientation = m_orientation_filter.get_9axis_quaternion(&m_accel, &m_gyro, &m_magnetic); m_time = get_timestamp(); rv_event.sensor_id = get_id(); @@ -355,7 +355,7 @@ int rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) m_orientation_filter_poll.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - quaternion_orientation = m_orientation_filter_poll.get_9axis_quaternion(m_accel, m_gyro, m_magnetic); + quaternion_orientation = m_orientation_filter_poll.get_9axis_quaternion(&m_accel, &m_gyro, &m_magnetic); data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index 298e9aa..840bd09 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -107,27 +107,6 @@ inline void orientation_filter::initialize_sensor_data(const sensor_data -inline void orientation_filter::init_accel_gyro_mag_data(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic) -{ - unsigned long long sample_interval_gyro = SAMPLE_INTV; - - m_accel.m_data = accel.m_data; - m_magnetic.m_data = magnetic.m_data; - - if (m_gyro.m_time_stamp != 0 && gyro.m_time_stamp != 0) - sample_interval_gyro = gyro.m_time_stamp - m_gyro.m_time_stamp; - - m_gyro_dt = sample_interval_gyro * US2S; - - m_accel.m_time_stamp = accel.m_time_stamp; - m_gyro.m_time_stamp = gyro.m_time_stamp; - m_magnetic.m_time_stamp = magnetic.m_time_stamp; - - m_gyro.m_data = gyro.m_data - m_bias_correction; -} - -template inline void orientation_filter::init_accel_mag_data(const sensor_data accel, const sensor_data magnetic) { @@ -456,11 +435,11 @@ rotation_matrix orientation_filter::get_rotation_matrix(const sensor } template -quaternion orientation_filter::get_9axis_quaternion(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic) +quaternion orientation_filter::get_9axis_quaternion(const sensor_data *accel, + const sensor_data *gyro, const sensor_data *magnetic) { - get_orientation(&accel, &gyro, &magnetic); + get_device_rotation(accel, gyro, magnetic); return m_quat_9axis; } diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index 08add8a..ea4abc2 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -93,8 +93,8 @@ public: const sensor_data *gyro, const sensor_data *magnetic); rotation_matrix get_rotation_matrix(const sensor_data *accel, const sensor_data *gyro, const sensor_data *magnetic); - quaternion get_9axis_quaternion(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic); + quaternion get_9axis_quaternion(const sensor_data *accel, + const sensor_data *gyro, const sensor_data *magnetic); quaternion get_geomagnetic_quaternion(const sensor_data accel, const sensor_data magnetic); quaternion get_gaming_quaternion(const sensor_data accel, diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp index 57b6393..6c417ce 100644 --- a/src/sensor_fusion/test/orientation_sensor.cpp +++ b/src/sensor_fusion/test/orientation_sensor.cpp @@ -78,14 +78,14 @@ rotation_matrix orientation_sensor::get_rotation_matrix(sensor_data orientation_sensor::get_9axis_quaternion(sensor_data accel_data, - sensor_data gyro_data, sensor_data magnetic_data) +quaternion orientation_sensor::get_9axis_quaternion(sensor_data *accel_data, + sensor_data *gyro_data, sensor_data *magnetic_data) { - pre_process_data(&accel_data, &accel_data, bias_accel, sign_accel, scale_accel); - normalize(accel_data); - pre_process_data(&gyro_data, &gyro_data, bias_gyro, sign_gyro, scale_gyro); - pre_process_data(&magnetic_data, &magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); - normalize(magnetic_data); + pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + normalize(*accel_data); + pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); + pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); + normalize(*magnetic_data); orien_filter.m_pitch_phase_compensation = pitch_phase_compensation; orien_filter.m_roll_phase_compensation = roll_phase_compensation; diff --git a/src/sensor_fusion/test/orientation_sensor.h b/src/sensor_fusion/test/orientation_sensor.h index dab0677..80a12e4 100644 --- a/src/sensor_fusion/test/orientation_sensor.h +++ b/src/sensor_fusion/test/orientation_sensor.h @@ -31,8 +31,8 @@ public: sensor_data *gyro, sensor_data *magnetic); rotation_matrix get_rotation_matrix(sensor_data *accel, sensor_data *gyro, sensor_data *magnetic); - quaternion get_9axis_quaternion(sensor_data accel, - sensor_data gyro, sensor_data magnetic); + quaternion get_9axis_quaternion(sensor_data *accel, + sensor_data *gyro, sensor_data *magnetic); quaternion get_geomagnetic_quaternion(sensor_data accel, sensor_data magnetic); quaternion get_gaming_quaternion(sensor_data accel, 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 a4e7da1..b1fef99 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 @@ -89,7 +89,7 @@ int main() cout << "Orientation matrix\t" << orientation_mat.m_rot_mat << "\n\n"; - orientation_9axis_quat = orien_sensor3.get_9axis_quaternion(accel_data, gyro_data, magnetic_data); + orientation_9axis_quat = orien_sensor3.get_9axis_quaternion(&accel_data, &gyro_data, &magnetic_data); cout << "Orientation 9-axis quaternion\t" << orientation_9axis_quat.m_quat << "\n\n"; -- 2.7.4 From e8181b770adce11985243a744722022677e2da35 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 3 Mar 2015 15:33:29 +0530 Subject: [PATCH 08/16] Updating geomagnetic_rv sensor to follow single sensor fusion flow - The gyro input pointer is passed a value NULL - Cleaning up unused initialization code and old flow code Change-Id: I438d8dc1615d2e9dcc3188c9f06ca7d8aa042d4c --- .../geomagnetic_rv/geomagnetic_rv_sensor.cpp | 4 ++-- src/sensor_fusion/orientation_filter.cpp | 24 +++------------------- src/sensor_fusion/orientation_filter.h | 8 ++------ src/sensor_fusion/test/orientation_sensor.cpp | 12 +++++------ src/sensor_fusion/test/orientation_sensor.h | 4 ++-- .../orientation_sensor_main.cpp | 2 +- 6 files changed, 16 insertions(+), 38 deletions(-) diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp index 6649333..4a77526 100755 --- a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp +++ b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp @@ -255,7 +255,7 @@ void geomagnetic_rv_sensor::synthesize(const sensor_event_t& event, vector::initialize_sensor_data(const sensor_data -inline void orientation_filter::init_accel_mag_data(const sensor_data accel, - const sensor_data magnetic) -{ - m_accel.m_data = accel.m_data; - m_magnetic.m_data = magnetic.m_data; - - m_accel.m_time_stamp = accel.m_time_stamp; - m_magnetic.m_time_stamp = magnetic.m_time_stamp; - - normalize(m_magnetic); -} - -template inline void orientation_filter::init_accel_gyro_data(const sensor_data accel, const sensor_data gyro) { @@ -445,15 +432,10 @@ quaternion orientation_filter::get_9axis_quaternion(const sensor_dat } template -quaternion orientation_filter::get_geomagnetic_quaternion(const sensor_data accel, - const sensor_data magnetic) +quaternion orientation_filter::get_geomagnetic_quaternion(const sensor_data *accel, + const sensor_data *magnetic) { - init_accel_mag_data(accel, magnetic); - - normalize(m_accel); - normalize(m_magnetic); - - orientation_triad_algorithm(); + get_device_rotation(accel, NULL, magnetic); return m_quat_aid; } diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index ea4abc2..c36fb7c 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -76,10 +76,6 @@ public: inline void initialize_sensor_data(const sensor_data *accel, const sensor_data *gyro, const sensor_data *magnetic); - inline void init_accel_gyro_mag_data(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic); - inline void init_accel_mag_data(const sensor_data accel, - const sensor_data magnetic); inline void init_accel_gyro_data(const sensor_data accel, const sensor_data gyro); inline void orientation_triad_algorithm(); @@ -95,8 +91,8 @@ public: const sensor_data *gyro, const sensor_data *magnetic); quaternion get_9axis_quaternion(const sensor_data *accel, const sensor_data *gyro, const sensor_data *magnetic); - quaternion get_geomagnetic_quaternion(const sensor_data accel, - const sensor_data magnetic); + quaternion get_geomagnetic_quaternion(const sensor_data *accel, + const sensor_data *magnetic); quaternion get_gaming_quaternion(const sensor_data accel, const sensor_data gyro); euler_angles get_device_rotation(const sensor_data *accel, diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp index 6c417ce..0d8d17e 100644 --- a/src/sensor_fusion/test/orientation_sensor.cpp +++ b/src/sensor_fusion/test/orientation_sensor.cpp @@ -95,13 +95,13 @@ quaternion orientation_sensor::get_9axis_quaternion(sensor_data *a return orien_filter.get_9axis_quaternion(accel_data, gyro_data, magnetic_data); } -quaternion orientation_sensor::get_geomagnetic_quaternion(sensor_data accel_data, - sensor_data magnetic_data) +quaternion orientation_sensor::get_geomagnetic_quaternion(sensor_data *accel_data, + sensor_data *magnetic_data) { - pre_process_data(&accel_data, &accel_data, bias_accel, sign_accel, scale_accel); - normalize(accel_data); - pre_process_data(&magnetic_data, &magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); - normalize(magnetic_data); + pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + normalize(*accel_data); + pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); + normalize(*magnetic_data); return orien_filter.get_geomagnetic_quaternion(accel_data, magnetic_data); } diff --git a/src/sensor_fusion/test/orientation_sensor.h b/src/sensor_fusion/test/orientation_sensor.h index 80a12e4..09e383b 100644 --- a/src/sensor_fusion/test/orientation_sensor.h +++ b/src/sensor_fusion/test/orientation_sensor.h @@ -33,8 +33,8 @@ public: sensor_data *gyro, sensor_data *magnetic); quaternion get_9axis_quaternion(sensor_data *accel, sensor_data *gyro, sensor_data *magnetic); - quaternion get_geomagnetic_quaternion(sensor_data accel, - sensor_data magnetic); + quaternion get_geomagnetic_quaternion(sensor_data *accel, + sensor_data *magnetic); quaternion get_gaming_quaternion(sensor_data accel, sensor_data gyro); }; 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 b1fef99..ebeb7b4 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 @@ -93,7 +93,7 @@ int main() cout << "Orientation 9-axis quaternion\t" << orientation_9axis_quat.m_quat << "\n\n"; - orientation_geomagnetic_quat = orien_sensor4.get_geomagnetic_quaternion(accel_data, magnetic_data); + orientation_geomagnetic_quat = orien_sensor4.get_geomagnetic_quaternion(&accel_data, &magnetic_data); cout << "Orientation geomagnetic quaternion\t" << orientation_geomagnetic_quat.m_quat << "\n\n"; -- 2.7.4 From a2613e6ff819ae50c78b86f358a7149b41d6c060 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 3 Mar 2015 15:42:05 +0530 Subject: [PATCH 09/16] Updating gaming_rv sensor to follow single sensor fusion flow - The geomagnetic sensor input pointer is passed a value NULL - Cleaning up unused initialization code and old flow code Change-Id: I96e90b6a19e89b80071b8b821b486691640ce0d5 --- src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp | 4 +-- src/sensor_fusion/orientation_filter.cpp | 40 +++------------------- src/sensor_fusion/orientation_filter.h | 6 ++-- src/sensor_fusion/test/orientation_sensor.cpp | 10 +++--- src/sensor_fusion/test/orientation_sensor.h | 4 +-- .../orientation_sensor_main.cpp | 2 +- 6 files changed, 17 insertions(+), 49 deletions(-) diff --git a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp index fb44e0c..86ec74f 100755 --- a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp +++ b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp @@ -244,7 +244,7 @@ void gaming_rv_sensor::synthesize(const sensor_event_t& event, vector::initialize_sensor_data(const sensor_data -inline void orientation_filter::init_accel_gyro_data(const sensor_data accel, - const sensor_data gyro) -{ - unsigned long long sample_interval_gyro = SAMPLE_INTV; - - m_accel.m_data = accel.m_data; - - if (m_gyro.m_time_stamp != 0 && gyro.m_time_stamp != 0) - sample_interval_gyro = gyro.m_time_stamp - m_gyro.m_time_stamp; - - m_gyro_dt = sample_interval_gyro * US2S; - - m_accel.m_time_stamp = accel.m_time_stamp; - m_gyro.m_time_stamp = gyro.m_time_stamp; - - m_gyro.m_data = gyro.m_data - m_bias_correction; -} - -template inline void orientation_filter::orientation_triad_algorithm() { TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0}; @@ -388,6 +369,8 @@ euler_angles orientation_filter::get_device_rotation(const sensor_da if (magnetic != NULL) orientation_triad_algorithm(); + else if(gyro != NULL) + compute_accel_orientation(); if (gyro != NULL) { compute_covariance(); @@ -441,23 +424,10 @@ quaternion orientation_filter::get_geomagnetic_quaternion(const sens } template -quaternion orientation_filter::get_gaming_quaternion(const sensor_data accel, - const sensor_data gyro) +quaternion orientation_filter::get_gaming_quaternion(const sensor_data *accel, + const sensor_data *gyro) { - euler_angles cor_euler_ang; - - init_accel_gyro_data(accel, gyro); - - normalize(m_accel); - m_gyro.m_data = m_gyro.m_data * (TYPE) PI; - - compute_accel_orientation(); - - compute_covariance(); - - time_update_gaming_rv(); - - measurement_update(); + get_device_rotation(accel, gyro, NULL); return m_quat_gaming_rv; } diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index c36fb7c..0b8af39 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -76,8 +76,6 @@ public: inline void initialize_sensor_data(const sensor_data *accel, const sensor_data *gyro, const sensor_data *magnetic); - inline void init_accel_gyro_data(const sensor_data accel, - const sensor_data gyro); inline void orientation_triad_algorithm(); inline void compute_accel_orientation(); inline void compute_covariance(); @@ -93,8 +91,8 @@ public: const sensor_data *gyro, const sensor_data *magnetic); quaternion get_geomagnetic_quaternion(const sensor_data *accel, const sensor_data *magnetic); - quaternion get_gaming_quaternion(const sensor_data accel, - const sensor_data gyro); + quaternion get_gaming_quaternion(const sensor_data *accel, + const sensor_data *gyro); euler_angles get_device_rotation(const sensor_data *accel, const sensor_data *gyro, const sensor_data *magnetic); }; diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp index 0d8d17e..c8c8950 100644 --- a/src/sensor_fusion/test/orientation_sensor.cpp +++ b/src/sensor_fusion/test/orientation_sensor.cpp @@ -107,12 +107,12 @@ quaternion orientation_sensor::get_geomagnetic_quaternion(sensor_data orientation_sensor::get_gaming_quaternion(sensor_data accel_data, - sensor_data gyro_data) +quaternion orientation_sensor::get_gaming_quaternion(sensor_data *accel_data, + sensor_data *gyro_data) { - pre_process_data(&accel_data, &accel_data, bias_accel, sign_accel, scale_accel); - normalize(accel_data); - pre_process_data(&gyro_data, &gyro_data, bias_gyro, sign_gyro, scale_gyro); + pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); + normalize(*accel_data); + pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); return orien_filter.get_gaming_quaternion(accel_data, gyro_data); } diff --git a/src/sensor_fusion/test/orientation_sensor.h b/src/sensor_fusion/test/orientation_sensor.h index 09e383b..ebf9aea 100644 --- a/src/sensor_fusion/test/orientation_sensor.h +++ b/src/sensor_fusion/test/orientation_sensor.h @@ -35,8 +35,8 @@ public: sensor_data *gyro, sensor_data *magnetic); quaternion get_geomagnetic_quaternion(sensor_data *accel, sensor_data *magnetic); - quaternion get_gaming_quaternion(sensor_data accel, - sensor_data gyro); + quaternion get_gaming_quaternion(sensor_data *accel, + sensor_data *gyro); }; #include "orientation_sensor.cpp" 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 ebeb7b4..6f6ebb3 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 @@ -97,7 +97,7 @@ int main() cout << "Orientation geomagnetic quaternion\t" << orientation_geomagnetic_quat.m_quat << "\n\n"; - orientation_gaming_quat = orien_sensor5.get_gaming_quaternion(accel_data, gyro_data); + orientation_gaming_quat = orien_sensor5.get_gaming_quaternion(&accel_data, &gyro_data); cout << "Orientation gaming quaternion\t" << orientation_gaming_quat.m_quat << "\n\n"; } -- 2.7.4 From 6c1b260a108ce2d2e60383f5bc1eb53e6cbd8000 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 3 Mar 2015 17:22:55 +0530 Subject: [PATCH 10/16] Restructuring all virtual sensors and sensor fusion for single flow - Removed separate methods for computing orientation/RVs and replaced with single method - Updated virtual sensors to use the single sensor fusion method - Updated test files for using single method - Cleanup. Change-Id: Iacd555215cf1c146e2aa75451baa92751e67a2e1 --- src/orientation/orientation_sensor.cpp | 8 ++- src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp | 8 ++- .../geomagnetic_rv/geomagnetic_rv_sensor.cpp | 8 ++- src/rotation_vector/rv/rv_sensor.cpp | 8 ++- src/sensor_fusion/orientation_filter.cpp | 64 +++------------------- src/sensor_fusion/orientation_filter.h | 12 +--- src/sensor_fusion/test/orientation_sensor.cpp | 61 +-------------------- src/sensor_fusion/test/orientation_sensor.h | 10 +--- .../orientation_sensor_main.cpp | 24 +++----- 9 files changed, 44 insertions(+), 159 deletions(-) diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index 900a7c0..d5fe72d 100755 --- a/src/orientation/orientation_sensor.cpp +++ b/src/orientation/orientation_sensor.cpp @@ -339,7 +339,9 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector & m_orientation_filter.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - quaternion_orientation = m_orientation_filter.get_9axis_quaternion(&m_accel, &m_gyro, &m_magnetic); + 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(); @@ -355,7 +357,9 @@ int rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data) m_orientation_filter_poll.m_azimuth_phase_compensation = m_azimuth_rotation_compensation; m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor; - quaternion_orientation = m_orientation_filter_poll.get_9axis_quaternion(&m_accel, &m_gyro, &m_magnetic); + m_orientation_filter_poll.get_device_orientation(&m_accel, &m_gyro, &m_magnetic); + + quaternion_orientation = m_orientation_filter_poll.m_quat_9axis; data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index e5b4f72..ae43948 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -78,12 +78,9 @@ template inline void orientation_filter::initialize_sensor_data(const sensor_data *accel, const sensor_data *gyro, const sensor_data *magnetic) { - if (accel != NULL) { - m_accel.m_data = accel->m_data; - m_accel.m_time_stamp = accel->m_time_stamp; - - normalize(m_accel); - } + m_accel.m_data = accel->m_data; + m_accel.m_time_stamp = accel->m_time_stamp; + normalize(m_accel); if (gyro != NULL) { unsigned long long sample_interval_gyro = SAMPLE_INTV; @@ -103,7 +100,6 @@ inline void orientation_filter::initialize_sensor_data(const sensor_datam_data; m_magnetic.m_time_stamp = magnetic->m_time_stamp; } - } template @@ -231,6 +227,7 @@ inline void orientation_filter::time_update() quat_output = phase_correction(m_quat_driv, m_quat_aid); m_quat_9axis = quat_output; + m_quat_gaming_rv = m_quat_9axis; orientation = quat2euler(quat_output); @@ -362,73 +359,26 @@ inline void orientation_filter::measurement_update() } template -euler_angles orientation_filter::get_device_rotation(const sensor_data *accel, +void orientation_filter::get_device_orientation(const sensor_data *accel, const sensor_data *gyro, const sensor_data *magnetic) { initialize_sensor_data(accel, gyro, magnetic); if (magnetic != NULL) orientation_triad_algorithm(); - else if(gyro != NULL) + else if (gyro != NULL) compute_accel_orientation(); if (gyro != NULL) { compute_covariance(); - if(magnetic != NULL) + if (magnetic != NULL) time_update(); else time_update_gaming_rv(); measurement_update(); } - - return m_orientation; -} - -template -euler_angles orientation_filter::get_orientation(const sensor_data *accel, - const sensor_data *gyro, const sensor_data *magnetic) -{ - get_device_rotation(accel, gyro, magnetic); - - return m_orientation; -} - -template -rotation_matrix orientation_filter::get_rotation_matrix(const sensor_data *accel, - const sensor_data *gyro, const sensor_data *magnetic) -{ - get_device_rotation(accel, gyro, magnetic); - - return m_rot_matrix; } -template -quaternion orientation_filter::get_9axis_quaternion(const sensor_data *accel, - const sensor_data *gyro, const sensor_data *magnetic) -{ - - get_device_rotation(accel, gyro, magnetic); - - return m_quat_9axis; -} - -template -quaternion orientation_filter::get_geomagnetic_quaternion(const sensor_data *accel, - const sensor_data *magnetic) -{ - get_device_rotation(accel, NULL, magnetic); - - return m_quat_aid; -} - -template -quaternion orientation_filter::get_gaming_quaternion(const sensor_data *accel, - const sensor_data *gyro) -{ - get_device_rotation(accel, gyro, NULL); - - return m_quat_gaming_rv; -} #endif //_ORIENTATION_FILTER_H_ diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index 0b8af39..8a1351b 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -83,17 +83,7 @@ public: inline void time_update_gaming_rv(); inline void measurement_update(); - euler_angles get_orientation(const sensor_data *accel, - const sensor_data *gyro, const sensor_data *magnetic); - rotation_matrix get_rotation_matrix(const sensor_data *accel, - const sensor_data *gyro, const sensor_data *magnetic); - quaternion get_9axis_quaternion(const sensor_data *accel, - const sensor_data *gyro, const sensor_data *magnetic); - quaternion get_geomagnetic_quaternion(const sensor_data *accel, - const sensor_data *magnetic); - quaternion get_gaming_quaternion(const sensor_data *accel, - const sensor_data *gyro); - euler_angles get_device_rotation(const sensor_data *accel, + void get_device_orientation(const sensor_data *accel, const sensor_data *gyro, const sensor_data *magnetic); }; diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp index c8c8950..b9ff4bb 100644 --- a/src/sensor_fusion/test/orientation_sensor.cpp +++ b/src/sensor_fusion/test/orientation_sensor.cpp @@ -26,7 +26,7 @@ int sign_accel[] = {+1, +1, +1}; int sign_gyro[] = {+1, +1, +1}; int sign_magnetic[] = {+1, +1, +1}; float scale_accel = 1; -float scale_gyro = 575; +float scale_gyro = 1150; float scale_magnetic = 1; int pitch_phase_compensation = -1; @@ -43,7 +43,7 @@ void pre_process_data(sensor_data *data_out, sensor_data *data_in, data_out->m_time_stamp = data_in->m_time_stamp; } -euler_angles orientation_sensor::get_orientation(sensor_data *accel_data, +void orientation_sensor::get_device_orientation(sensor_data *accel_data, sensor_data *gyro_data, sensor_data *magnetic_data) { @@ -58,62 +58,7 @@ euler_angles orientation_sensor::get_orientation(sensor_data *acce orien_filter.m_azimuth_phase_compensation = azimuth_phase_compensation; orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor; - return orien_filter.get_orientation(accel_data, gyro_data, magnetic_data); + orien_filter.get_device_orientation(accel_data, gyro_data, magnetic_data); } -rotation_matrix orientation_sensor::get_rotation_matrix(sensor_data *accel_data, - sensor_data *gyro_data, sensor_data *magnetic_data) -{ - pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); - normalize(*accel_data); - pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); - pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); - normalize(*magnetic_data); - - orien_filter.m_pitch_phase_compensation = pitch_phase_compensation; - orien_filter.m_roll_phase_compensation = roll_phase_compensation; - orien_filter.m_azimuth_phase_compensation = azimuth_phase_compensation; - orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor; - - return orien_filter.get_rotation_matrix(accel_data, gyro_data, magnetic_data); -} - -quaternion orientation_sensor::get_9axis_quaternion(sensor_data *accel_data, - sensor_data *gyro_data, sensor_data *magnetic_data) -{ - pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); - normalize(*accel_data); - pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); - pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); - normalize(*magnetic_data); - - orien_filter.m_pitch_phase_compensation = pitch_phase_compensation; - orien_filter.m_roll_phase_compensation = roll_phase_compensation; - orien_filter.m_azimuth_phase_compensation = azimuth_phase_compensation; - orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor; - - return orien_filter.get_9axis_quaternion(accel_data, gyro_data, magnetic_data); -} - -quaternion orientation_sensor::get_geomagnetic_quaternion(sensor_data *accel_data, - sensor_data *magnetic_data) -{ - pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); - normalize(*accel_data); - pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); - normalize(*magnetic_data); - - return orien_filter.get_geomagnetic_quaternion(accel_data, magnetic_data); -} - - -quaternion orientation_sensor::get_gaming_quaternion(sensor_data *accel_data, - sensor_data *gyro_data) -{ - pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); - normalize(*accel_data); - pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); - - return orien_filter.get_gaming_quaternion(accel_data, gyro_data); -} #endif diff --git a/src/sensor_fusion/test/orientation_sensor.h b/src/sensor_fusion/test/orientation_sensor.h index ebf9aea..c7aff2f 100644 --- a/src/sensor_fusion/test/orientation_sensor.h +++ b/src/sensor_fusion/test/orientation_sensor.h @@ -27,16 +27,8 @@ class orientation_sensor public: orientation_filter orien_filter; - euler_angles get_orientation(sensor_data *accel, + void get_device_orientation(sensor_data *accel, sensor_data *gyro, sensor_data *magnetic); - rotation_matrix get_rotation_matrix(sensor_data *accel, - sensor_data *gyro, sensor_data *magnetic); - quaternion get_9axis_quaternion(sensor_data *accel, - sensor_data *gyro, sensor_data *magnetic); - quaternion get_geomagnetic_quaternion(sensor_data *accel, - sensor_data *magnetic); - quaternion get_gaming_quaternion(sensor_data *accel, - sensor_data *gyro); }; #include "orientation_sensor.cpp" 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 6f6ebb3..53599d2 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 @@ -40,7 +40,7 @@ int main() quaternion orientation_9axis_quat; quaternion orientation_geomagnetic_quat; quaternion orientation_gaming_quat; - orientation_sensor orien_sensor1, orien_sensor2, orien_sensor3, orien_sensor4, orien_sensor5; + orientation_sensor orien_sensor; accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str()); gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str()); @@ -79,27 +79,19 @@ int main() cout << "Magnetic Data\t" << magnetic_data.m_data << "\t Time Stamp\t" << magnetic_data.m_time_stamp << "\n\n"; - orientation = orien_sensor1.get_orientation(&accel_data, &gyro_data, &magnetic_data); + orien_sensor.get_device_orientation(&accel_data, &gyro_data, &magnetic_data); - orien_file << orientation.m_ang; + orien_file << orien_sensor.orien_filter.m_orientation.m_ang; - cout << "Orientation angles\t" << orientation.m_ang << "\n\n"; + cout << "Orientation angles\t" << orien_sensor.orien_filter.m_orientation.m_ang << "\n\n"; - orientation_mat = orien_sensor2.get_rotation_matrix(&accel_data, &gyro_data, &magnetic_data); + cout << "Orientation matrix\t" << orien_sensor.orien_filter.m_rot_matrix.m_rot_mat << "\n\n"; - cout << "Orientation matrix\t" << orientation_mat.m_rot_mat << "\n\n"; + cout << "Orientation 9-axis quaternion\t" << orien_sensor.orien_filter.m_quat_9axis.m_quat << "\n\n"; - orientation_9axis_quat = orien_sensor3.get_9axis_quaternion(&accel_data, &gyro_data, &magnetic_data); + cout << "Orientation geomagnetic quaternion\t" << orien_sensor.orien_filter.m_quat_aid.m_quat << "\n\n"; - cout << "Orientation 9-axis quaternion\t" << orientation_9axis_quat.m_quat << "\n\n"; - - orientation_geomagnetic_quat = orien_sensor4.get_geomagnetic_quaternion(&accel_data, &magnetic_data); - - cout << "Orientation geomagnetic quaternion\t" << orientation_geomagnetic_quat.m_quat << "\n\n"; - - orientation_gaming_quat = orien_sensor5.get_gaming_quaternion(&accel_data, &gyro_data); - - cout << "Orientation gaming quaternion\t" << orientation_gaming_quat.m_quat << "\n\n"; + cout << "Orientation gaming quaternion\t" << orien_sensor.orien_filter.m_quat_gaming_rv.m_quat << "\n\n"; } accel_in.close(); -- 2.7.4 From 614a1b616f63ef5862f8ed54083626425098fd2f Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 4 Mar 2015 08:53:18 +0530 Subject: [PATCH 11/16] Removing orientation_filter standalone test - orientation_sensor standalone test performs all testing functionality for orientation_filter - removing redundant standalone test for orientation_filter Change-Id: Ia688cb7f8334d6885709f5f47c614279fb988f74 --- .../orientation_filter_test/.cproject | 112 ------------------- .../test_projects/orientation_filter_test/.project | 89 --------------- .../orientation_filter_main.cpp | 124 --------------------- 3 files changed, 325 deletions(-) delete mode 100644 src/sensor_fusion/test/test_projects/orientation_filter_test/.cproject delete mode 100644 src/sensor_fusion/test/test_projects/orientation_filter_test/.project delete mode 100644 src/sensor_fusion/test/test_projects/orientation_filter_test/orientation_filter_main.cpp diff --git a/src/sensor_fusion/test/test_projects/orientation_filter_test/.cproject b/src/sensor_fusion/test/test_projects/orientation_filter_test/.cproject deleted file mode 100644 index 4ebc742..0000000 --- a/src/sensor_fusion/test/test_projects/orientation_filter_test/.cproject +++ /dev/null @@ -1,112 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/sensor_fusion/test/test_projects/orientation_filter_test/.project b/src/sensor_fusion/test/test_projects/orientation_filter_test/.project deleted file mode 100644 index a106375..0000000 --- a/src/sensor_fusion/test/test_projects/orientation_filter_test/.project +++ /dev/null @@ -1,89 +0,0 @@ - - - orientation_filter_test - - - - - - org.eclipse.cdt.managedbuilder.core.genmakebuilder - clean,full,incremental, - - - ?name? - - - - org.eclipse.cdt.make.core.append_environment - true - - - org.eclipse.cdt.make.core.autoBuildTarget - all - - - org.eclipse.cdt.make.core.buildArguments - - - - org.eclipse.cdt.make.core.buildCommand - make - - - org.eclipse.cdt.make.core.buildLocation - ${workspace_loc:/orientation_filter_test/Debug} - - - org.eclipse.cdt.make.core.cleanBuildTarget - clean - - - org.eclipse.cdt.make.core.contents - org.eclipse.cdt.make.core.activeConfigSettings - - - org.eclipse.cdt.make.core.enableAutoBuild - false - - - org.eclipse.cdt.make.core.enableCleanBuild - true - - - org.eclipse.cdt.make.core.enableFullBuild - true - - - org.eclipse.cdt.make.core.fullBuildTarget - all - - - org.eclipse.cdt.make.core.stopOnError - true - - - org.eclipse.cdt.make.core.useDefaultBuildCmd - true - - - - - org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder - full,incremental, - - - - - org.tizen.nativecpp.apichecker.core.builder - - - - - - org.eclipse.cdt.core.cnature - org.eclipse.cdt.core.ccnature - org.eclipse.cdt.managedbuilder.core.managedBuildNature - org.eclipse.cdt.managedbuilder.core.ScannerConfigNature - org.tizen.nativecpp.apichecker.core.tizenCppNature - - diff --git a/src/sensor_fusion/test/test_projects/orientation_filter_test/orientation_filter_main.cpp b/src/sensor_fusion/test/test_projects/orientation_filter_test/orientation_filter_main.cpp deleted file mode 100644 index d9c7f2b..0000000 --- a/src/sensor_fusion/test/test_projects/orientation_filter_test/orientation_filter_main.cpp +++ /dev/null @@ -1,124 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#include "../../../orientation_filter.h" -#include -#include -#include -#include -using namespace std; - -#define ORIENTATION_DATA_PATH "../../../design/data/100ms/orientation/roll_pitch_yaw/" -#define ORIENTATION_DATA_SIZE 1095 - -float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)}; -float bias_gyro[] = {-5.3539, 0.24325, 2.3391}; -float bias_magnetic[] = {0, 0, 0}; -int sign_accel[] = {+1, +1, +1}; -int sign_gyro[] = {+1, +1, +1}; -int sign_magnetic[] = {+1, +1, +1}; -float scale_accel = 1; -float scale_gyro = 575; -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) -{ - data_out.m_data.m_vec[0] = sign[0] * (data_in.m_data.m_vec[0] - bias[0]) / scale; - data_out.m_data.m_vec[1] = sign[1] * (data_in.m_data.m_vec[1] - bias[1]) / scale; - data_out.m_data.m_vec[2] = sign[2] * (data_in.m_data.m_vec[2] - bias[2]) / scale; - - data_out.m_time_stamp = data_in.m_time_stamp; -} - -int main() -{ - int data_available = ORIENTATION_DATA_SIZE; - ifstream accel_in, gyro_in, mag_in; - ofstream orien_file; - string line_accel, line_gyro, line_magnetic; - float sdata[3]; - unsigned long long time_stamp; - euler_angles orientation; - orientation_filter orien_filter; - - accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str()); - gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str()); - mag_in.open(((string)ORIENTATION_DATA_PATH + (string)"magnetic.txt").c_str()); - - orien_file.open(((string)"orientation.txt").c_str()); - - char *token = NULL; - - while (data_available-- > 0) - { - getline(accel_in, line_accel); - sdata[0] = strtof(line_accel.c_str(), &token); - sdata[1] = strtof(token, &token); - sdata[2] = strtof(token, &token); - time_stamp = strtoull (token, NULL, 10); - sensor_data accel_data(sdata[0], sdata[1], sdata[2], time_stamp); - - getline(gyro_in, line_gyro); - sdata[0] = strtof(line_gyro.c_str(), &token); - sdata[1] = strtof(token, &token); - sdata[2] = strtof(token, &token); - time_stamp = strtoull (token, NULL, 10); - sensor_data gyro_data(sdata[0], sdata[1], sdata[2], time_stamp); - - getline(mag_in, line_magnetic); - sdata[0] = strtof(line_magnetic.c_str(), &token); - sdata[1] = strtof(token, &token); - sdata[2] = strtof(token, &token); - time_stamp = strtoull (token, NULL, 10); - sensor_data magnetic_data(sdata[0], sdata[1], sdata[2], time_stamp); - - pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel); - normalize(accel_data); - pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro); - pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic); - normalize(magnetic_data); - - cout << "Accel Data\t" << accel_data.m_data << "\t Time Stamp\t" << accel_data.m_time_stamp << "\n\n"; - cout << "Gyro Data\t" << gyro_data.m_data << "\t Time Stamp\t" << gyro_data.m_time_stamp << "\n\n"; - cout << "Magnetic Data\t" << magnetic_data.m_data << "\t Time Stamp\t" << magnetic_data.m_time_stamp << "\n\n"; - - 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; - - orientation = orien_filter.get_orientation(accel_data, gyro_data, magnetic_data); - - orien_file << orientation.m_ang; - - cout << "Orientation Data\t" << orientation.m_ang << "\n\n"; - } - - accel_in.close(); - gyro_in.close(); - mag_in.close(); - orien_file.close(); - - return 0; -} -- 2.7.4 From 63e4795662d48011af06dd5df495287600f44107 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 4 Mar 2015 12:00:37 +0530 Subject: [PATCH 12/16] Adding new fusion sensor for single flow - Adding new fusion sensor which executes orientation_filter only once for all virtual sensors. - Generates multiple events for orientation sensor, rv sensor, gaming_rv sensor and geomagnetic_rv sensor based on event type registration - Will work in complementary fashion to virtual sensors orientation, rv, gaming_rv and geomagnetic_rv - maintains minimum operation interval. - Sends only fusion events that are registered by applications Change-Id: I1dfecc545a0f25647e7ec299c5f0a2bae05e6456 --- src/fusion/CMakeLists.txt | 24 +++ src/fusion/fusion_sensor.cpp | 436 +++++++++++++++++++++++++++++++++++++++++++ src/fusion/fusion_sensor.h | 87 +++++++++ 3 files changed, 547 insertions(+) create mode 100755 src/fusion/CMakeLists.txt create mode 100755 src/fusion/fusion_sensor.cpp create mode 100755 src/fusion/fusion_sensor.h diff --git a/src/fusion/CMakeLists.txt b/src/fusion/CMakeLists.txt new file mode 100755 index 0000000..9c6b3f4 --- /dev/null +++ b/src/fusion/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 2.6) +project(fusion CXX) + +SET(SENSOR_NAME fusion_sensor) + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +include_directories(${CMAKE_SOURCE_DIR}/src/libsensord) +include_directories(${CMAKE_SOURCE_DIR}/src/sensor_fusion) + +FOREACH(flag ${fusion_pkgs_LDFLAGS}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") +ENDFOREACH(flag) + +FOREACH(flag ${fusion_pkgs_CFLAGS}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") +ENDFOREACH(flag) + +add_library(${SENSOR_NAME} SHARED + fusion_sensor.cpp + ) + +target_link_libraries(${SENSOR_NAME} ${fusion_pkgs_LDFLAGS} "-lm") + +install(TARGETS ${SENSOR_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/sensord) diff --git a/src/fusion/fusion_sensor.cpp b/src/fusion/fusion_sensor.cpp new file mode 100755 index 0000000..fa80416 --- /dev/null +++ b/src/fusion/fusion_sensor.cpp @@ -0,0 +1,436 @@ +/* + * sensord + * + * Copyright (c) 2015 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define SENSOR_NAME "FUSION_SENSOR" +#define SENSOR_TYPE_FUSION "FUSION" + +#define ACCELEROMETER_ENABLED 0x01 +#define GYROSCOPE_ENABLED 0x02 +#define GEOMAGNETIC_ENABLED 0x04 +#define GAMING_RV_ENABLED 3 +#define GEOMAGNETIC_RV_ENABLED 5 +#define ORIENTATION_ENABLED 7 +#define ROTATION_VECTOR_ENABLED 7 + +#define INITIAL_VALUE -1 + +#define MS_TO_US 1000 +#define MIN_DELIVERY_DIFF_FACTOR 0.75f + +#define PI 3.141593 +#define AZIMUTH_OFFSET_DEGREES 360 +#define AZIMUTH_OFFSET_RADIANS (2 * PI) + +#define ELEMENT_NAME "NAME" +#define ELEMENT_VENDOR "VENDOR" +#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT" +#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME" +#define ELEMENT_ACCEL_STATIC_BIAS "ACCEL_STATIC_BIAS" +#define ELEMENT_GYRO_STATIC_BIAS "GYRO_STATIC_BIAS" +#define ELEMENT_GEOMAGNETIC_STATIC_BIAS "GEOMAGNETIC_STATIC_BIAS" +#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION "GYRO_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_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" + +void pre_process_data(sensor_data &data_out, const float *data_in, float *bias, int *sign, float scale) +{ + data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale; + data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale; + data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale; +} + +fusion_sensor::fusion_sensor() +: m_accel_sensor(NULL) +, m_gyro_sensor(NULL) +, m_magnetic_sensor(NULL) +, m_time(0) +{ + cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); + m_name = string(SENSOR_NAME); + m_enable_fusion = 0; + + if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_VENDOR, m_vendor)) { + ERR("[VENDOR] is empty\n"); + throw ENXIO; + } + + INFO("m_vendor = %s", m_vendor.c_str()); + + if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) { + ERR("[RAW_DATA_UNIT] is empty\n"); + throw ENXIO; + } + + INFO("m_raw_data_unit = %s", m_raw_data_unit.c_str()); + + if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) { + ERR("[DEFAULT_SAMPLING_TIME] is empty\n"); + throw ENXIO; + } + + INFO("m_default_sampling_time = %d", m_default_sampling_time); + + if (!config.get(SENSOR_TYPE_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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_FUSION, 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; + + m_accel_ptr = m_gyro_ptr = m_magnetic_ptr = NULL; +} + +fusion_sensor::~fusion_sensor() +{ + INFO("fusion_sensor is destroyed!\n"); +} + +bool fusion_sensor::init(void) +{ + m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); + m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); + m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); + + if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor) { + ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x", + m_accel_sensor, m_gyro_sensor, m_magnetic_sensor); + return false; + } + + INFO("%s is created!", sensor_base::get_name()); + return true; +} + +sensor_type_t fusion_sensor::get_type(void) +{ + return FUSION_SENSOR; +} + +bool fusion_sensor::on_start(void) +{ + AUTOLOCK(m_mutex); + activate(); + return true; +} + +bool fusion_sensor::on_stop(void) +{ + AUTOLOCK(m_mutex); + deactivate(); + return true; +} + +bool fusion_sensor::add_interval(int client_id, unsigned int interval) +{ + bool retval; + + AUTOLOCK(m_mutex); + retval = sensor_base::add_interval(client_id, interval, false); + + m_interval = sensor_base::get_interval(client_id, false); + + if (m_interval != 0) + retval = true; + + return retval; +} + +bool fusion_sensor::delete_interval(int client_id) +{ + bool retval; + + AUTOLOCK(m_mutex); + retval = sensor_base::delete_interval(client_id, false); + + m_interval = sensor_base::get_interval(client_id, false); + + if (m_interval != 0) + retval = true; + + return retval; +} + +void fusion_sensor::synthesize(const sensor_event_t &event, vector &outs) +{ + unsigned long long diff_time; + euler_angles euler_orientation; + + if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); + + m_accel.m_time_stamp = event.data.timestamp; + + m_accel_ptr = &m_accel; + + m_enable_fusion |= ACCELEROMETER_ENABLED; + } + + if (sensor_base::is_supported(FUSION_ORIENTATION_ENABLED) || + sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED) || + sensor_base::is_supported(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED)) { + if (event.event_type == GEOMAGNETIC_RAW_DATA_EVENT) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale); + + m_magnetic.m_time_stamp = event.data.timestamp; + + m_magnetic_ptr = &m_magnetic; + + m_enable_fusion |= GEOMAGNETIC_ENABLED; + } + } + + if (sensor_base::is_supported(FUSION_ORIENTATION_ENABLED) || + sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED) || + sensor_base::is_supported(FUSION_GAMING_ROTATION_VECTOR_ENABLED)) { + 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_gyro_ptr = &m_gyro; + + m_enable_fusion |= GYROSCOPE_ENABLED; + } + } + + if ((m_enable_fusion == ORIENTATION_ENABLED && sensor_base::is_supported(FUSION_ORIENTATION_ENABLED)) || + (m_enable_fusion == ROTATION_VECTOR_ENABLED && sensor_base::is_supported(FUSION_ROTATION_VECTOR_ENABLED)) || + (m_enable_fusion == GAMING_RV_ENABLED && sensor_base::is_supported(FUSION_GAMING_ROTATION_VECTOR_ENABLED)) || + (m_enable_fusion == GEOMAGNETIC_RV_ENABLED && sensor_base::is_supported(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED))) { + sensor_event_t fusion_event; + m_enable_fusion = 0; + + m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor; + + m_orientation_filter.get_device_orientation(m_accel_ptr, m_gyro_ptr, m_magnetic_ptr); + + m_time = get_timestamp(); + fusion_event.sensor_id = get_id(); + fusion_event.event_type = FUSION_EVENT; + fusion_event.data.accuracy = SENSOR_ACCURACY_GOOD; + fusion_event.data.timestamp = m_time; + fusion_event.data.value_count = 4; + fusion_event.data.values[0] = m_orientation_filter.m_quaternion.m_quat.m_vec[0]; + fusion_event.data.values[1] = m_orientation_filter.m_quaternion.m_quat.m_vec[1]; + fusion_event.data.values[2] = m_orientation_filter.m_quaternion.m_quat.m_vec[2]; + fusion_event.data.values[3] = m_orientation_filter.m_quaternion.m_quat.m_vec[3]; + + m_accel_ptr = m_gyro_ptr = m_magnetic_ptr = NULL; + + push(fusion_event); + } + + return; +} + +int fusion_sensor::get_sensor_data(const 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; + + euler_angles euler_orientation; + float azimuth_offset; + + if (event_type != FUSION_ORIENTATION_ENABLED || + event_type != FUSION_ROTATION_VECTOR_ENABLED || + event_type != FUSION_GAMING_ROTATION_VECTOR_ENABLED || + event_type != FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) + return -1; + + m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); + pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); + accel.m_time_stamp = accel_data.timestamp; + + if (event_type == FUSION_ORIENTATION_ENABLED || + event_type == FUSION_ROTATION_VECTOR_ENABLED || + event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED) + { + m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data); + pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale); + gyro.m_time_stamp = gyro_data.timestamp; + } + + if (event_type == FUSION_ORIENTATION_ENABLED || + event_type == FUSION_ROTATION_VECTOR_ENABLED || + event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) + { + m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_RAW_DATA_EVENT, magnetic_data); + pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale); + magnetic.m_time_stamp = magnetic_data.timestamp; + } + + m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor; + + if (event_type == FUSION_ORIENTATION_ENABLED || event_type == FUSION_ROTATION_VECTOR_ENABLED) + m_orientation_filter_poll.get_device_orientation(&accel, &gyro, &magnetic); + else if (event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED) + m_orientation_filter_poll.get_device_orientation(&accel, &gyro, NULL); + else if (event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) + m_orientation_filter_poll.get_device_orientation(&accel, NULL, &magnetic); + + data.accuracy = SENSOR_ACCURACY_GOOD; + data.timestamp = get_timestamp(); + data.value_count = 4; + data.values[0] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[0]; + data.values[1] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[1]; + data.values[2] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[2]; + data.values[3] = m_orientation_filter_poll.m_quaternion.m_quat.m_vec[3]; + + return 0; +} + +bool fusion_sensor::get_properties(sensor_properties_s &properties) +{ + properties.min_range = 0; + properties.max_range = 0; + properties.resolution = 0; + properties.vendor = m_vendor; + properties.name = SENSOR_NAME; + properties.min_interval = 0; + properties.fifo_count = 0; + properties.max_batch_count = 0; + + return true; +} + +extern "C" sensor_module* create(void) +{ + fusion_sensor *sensor; + + try { + sensor = new(std::nothrow) fusion_sensor; + } catch (int err) { + ERR("Failed to create module, err: %d, cause: %s", err, strerror(err)); + return NULL; + } + + sensor_module *module = new(std::nothrow) sensor_module; + retvm_if(!module || !sensor, NULL, "Failed to allocate memory"); + + module->sensors.push_back(sensor); + return module; +} diff --git a/src/fusion/fusion_sensor.h b/src/fusion/fusion_sensor.h new file mode 100755 index 0000000..010c786 --- /dev/null +++ b/src/fusion/fusion_sensor.h @@ -0,0 +1,87 @@ +/* + * sensord + * + * Copyright (c) 2015 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef _FUSION_SENSOR_H_ +#define _FUSION_SENSOR_H_ + +#include +#include +#include + +class fusion_sensor : public virtual_sensor { +public: + fusion_sensor(); + virtual ~fusion_sensor(); + + bool init(void); + + void synthesize(const sensor_event_t &event, vector &outs); + + bool add_interval(int client_id, unsigned int interval); + bool delete_interval(int client_id); + bool get_properties(sensor_properties_s &properties); + sensor_type_t get_type(void); + + int get_sensor_data(const unsigned int data_id, sensor_data_t &data); + +private: + sensor_base *m_accel_sensor; + sensor_base *m_gyro_sensor; + sensor_base *m_magnetic_sensor; + + sensor_data m_accel; + sensor_data m_gyro; + sensor_data m_magnetic; + + sensor_data *m_accel_ptr; + sensor_data *m_gyro_ptr; + sensor_data *m_magnetic_ptr; + + cmutex m_value_mutex; + + orientation_filter m_orientation_filter; + orientation_filter m_orientation_filter_poll; + + unsigned int m_enable_fusion; + + unsigned long long m_time; + unsigned int m_interval; + + string m_vendor; + string m_raw_data_unit; + int m_default_sampling_time; + float m_accel_static_bias[3]; + float m_gyro_static_bias[3]; + float m_geomagnetic_static_bias[3]; + int m_accel_rotation_direction_compensation[3]; + int m_gyro_rotation_direction_compensation[3]; + int m_geomagnetic_rotation_direction_compensation[3]; + float m_accel_scale; + float m_gyro_scale; + float m_geomagnetic_scale; + int m_magnetic_alignment_factor; + int m_azimuth_rotation_compensation; + int m_pitch_rotation_compensation; + int m_roll_rotation_compensation; + + bool on_start(void); + bool on_stop(void); +}; + +#endif -- 2.7.4 From ad8578db54b3e93ea0bf260fdd370f6f0ab080a3 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 11 Mar 2015 17:12:47 +0530 Subject: [PATCH 13/16] Adding method to unregister supported events for sensor_base - method is needed in virtual sensors to dynamically add and remove virtual sensor events used in fusion sensor. Change-Id: I1265808d67f79d240c2e2239c50ff5c02a098d16 --- src/shared/sensor_base.cpp | 5 +++++ src/shared/sensor_base.h | 1 + 2 files changed, 6 insertions(+) diff --git a/src/shared/sensor_base.cpp b/src/shared/sensor_base.cpp index 6fe3b9b..60c1b2f 100755 --- a/src/shared/sensor_base.cpp +++ b/src/shared/sensor_base.cpp @@ -306,6 +306,11 @@ void sensor_base::register_supported_event(unsigned int event_type) m_supported_event_info.push_back(event_type); } +void sensor_base::unregister_supported_event(unsigned int event_type) +{ + m_supported_event_info.erase(std::remove(m_supported_event_info.begin(), + m_supported_event_info.end(), event_type), m_supported_event_info.end()); +} unsigned int sensor_base::get_client_cnt(unsigned int event_type) { AUTOLOCK(m_client_info_mutex); diff --git a/src/shared/sensor_base.h b/src/shared/sensor_base.h index f9e6184..0206201 100755 --- a/src/shared/sensor_base.h +++ b/src/shared/sensor_base.h @@ -85,6 +85,7 @@ public: virtual int get_sensor_data(unsigned int type, sensor_data_t &data); void register_supported_event(unsigned int event_type); + void unregister_supported_event(unsigned int event_type); protected: typedef lock_guard lock; typedef lock_guard rlock; -- 2.7.4 From 9f49d9fa090d200d3c9172c491b6008a06e1ac26 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 11 Mar 2015 17:17:27 +0530 Subject: [PATCH 14/16] Updating virtual sensor configuration based on fusion sensor - Adding new fusion sensor configuration - Cleaning up existing fusion related configuration from orientation, rotation vector, geomagnetic rotation vector and gaming rotation vector Change-Id: Icb2284875fc84782a5df2321c49411e59b333390 --- virtual_sensors.xml | 103 ++++++++++++++-------------------------------------- 1 file changed, 28 insertions(+), 75 deletions(-) diff --git a/virtual_sensors.xml b/virtual_sensors.xml index 42ea53d..8371d60 100755 --- a/virtual_sensors.xml +++ b/virtual_sensors.xml @@ -1,8 +1,8 @@ - - + + @@ -16,6 +16,13 @@ + + + + + + + @@ -44,47 +51,24 @@ - - - - - - - - - - - - - - - - - - - - - - - - - + + @@ -98,6 +82,13 @@ + + + + + + + @@ -126,46 +117,24 @@ - - - - - - - - - - - - - - - - - - - - - - - + - - + + @@ -179,6 +148,13 @@ + + + + + + + @@ -207,41 +183,18 @@ - - - - - - - - - - - - - - - - - - - - - - - -- 2.7.4 From 07328c396f489052d61aa38838aaa43a8404dbfc Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 11 Mar 2015 17:31:30 +0530 Subject: [PATCH 15/16] Adding API for fusion sensor - Added new fusion sensor events and included to sensor API - Adding cmake support for API Change-Id: Ib3acb6bb7329b84a54abac8fad0373be40dc631f --- src/libsensord/CMakeLists.txt | 1 + src/libsensord/client_common.cpp | 2 ++ src/libsensord/sensor_fusion.h | 56 +++++++++++++++++++++++++++++ src/libsensord/sensor_internal.h | 1 + src/libsensord/sensor_internal_deprecated.h | 1 + 5 files changed, 61 insertions(+) create mode 100755 src/libsensord/sensor_fusion.h diff --git a/src/libsensord/CMakeLists.txt b/src/libsensord/CMakeLists.txt index 2a8291f..4949c36 100755 --- a/src/libsensord/CMakeLists.txt +++ b/src/libsensord/CMakeLists.txt @@ -59,5 +59,6 @@ install(FILES sensor_geomagnetic_rv.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/se install(FILES sensor_gaming_rv.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_temperature.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_motion.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) +install(FILES sensor_fusion.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES sensor_deprecated.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sensor/) install(FILES ${PROJECT_NAME}.pc DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig) diff --git a/src/libsensord/client_common.cpp b/src/libsensord/client_common.cpp index d1f132f..dc00b87 100755 --- a/src/libsensord/client_common.cpp +++ b/src/libsensord/client_common.cpp @@ -43,6 +43,7 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, ROTATION_VECTOR_SENSOR, 0, 1), FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, GEOMAGNETIC_RV_SENSOR, 0, 1), FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, GAMING_RV_SENSOR, 0, 1), + FILL_LOG_ELEMENT(LOG_ID_SENSOR_TYPE, FUSION_SENSOR, 0, 1), FILL_LOG_ELEMENT(LOG_ID_EVENT, GEOMAGNETIC_CALIBRATION_NEEDED_EVENT, 0, 1), FILL_LOG_ELEMENT(LOG_ID_EVENT, PROXIMITY_CHANGE_STATE_EVENT, 0,1), @@ -65,6 +66,7 @@ log_element g_log_elements[] = { FILL_LOG_ELEMENT(LOG_ID_EVENT, ROTATION_VECTOR_EVENT_RAW_DATA_REPORT_ON_TIME, 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), }; typedef unordered_map log_map; diff --git a/src/libsensord/sensor_fusion.h b/src/libsensord/sensor_fusion.h new file mode 100755 index 0000000..7539be1 --- /dev/null +++ b/src/libsensord/sensor_fusion.h @@ -0,0 +1,56 @@ +/* + * libsensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef __SENSOR_FUSION_H__ +#define __SENSOR_FUSION_H__ + +//! Pre-defined events for the fusion sensor +//! Sensor Plugin developer can add more event to their own headers + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** + * @defgroup SENSOR_FUSION Fusion Sensor + * @ingroup SENSOR_FRAMEWORK + * + * These APIs are used to control the Fusion sensor. + * @{ + */ +enum fusion_event_type { + FUSION_ORIENTATION_ENABLED = (FUSION_SENSOR << 16) | 0x0001, + FUSION_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0002, + FUSION_GAMING_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0003, + FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0004, + FUSION_EVENT = (FUSION_SENSOR << 16) | 0x0005, + FUSION_CALIBRATION_NEEDED_EVENT = (FUSION_SENSOR << 16) | 0x0006, +}; + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif +//! End of a file diff --git a/src/libsensord/sensor_internal.h b/src/libsensord/sensor_internal.h index 4fcb859..844e7af 100755 --- a/src/libsensord/sensor_internal.h +++ b/src/libsensord/sensor_internal.h @@ -54,6 +54,7 @@ extern "C" #include #include #include +#include typedef void (*sensor_cb_t)(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data); diff --git a/src/libsensord/sensor_internal_deprecated.h b/src/libsensord/sensor_internal_deprecated.h index b7b06fe..4420044 100755 --- a/src/libsensord/sensor_internal_deprecated.h +++ b/src/libsensord/sensor_internal_deprecated.h @@ -53,6 +53,7 @@ extern "C" #include #include #include +#include #define MAX_KEY_LEN 30 -- 2.7.4 From b1ceb13a2aa80b121e81ab0d406ff45a90514f99 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 11 Mar 2015 17:36:41 +0530 Subject: [PATCH 16/16] Updating orientation filter to support new configuration handling - Updated/cleanup of orientation filter to support new configuration handling Change-Id: I5a8b7001db7edbf48d88945937b7eede1f31fa37 --- src/sensor_fusion/orientation_filter.cpp | 16 +++++++++------- src/sensor_fusion/orientation_filter.h | 4 +--- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index ae43948..47c7b34 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -61,9 +61,6 @@ orientation_filter::orientation_filter() m_var_pitch = vec; m_var_azimuth = vec; - m_pitch_phase_compensation = 1; - m_roll_phase_compensation = 1; - m_azimuth_phase_compensation = 1; m_magnetic_alignment_factor = 1; m_gyro.m_time_stamp = 0; @@ -231,10 +228,6 @@ inline void orientation_filter::time_update() orientation = quat2euler(quat_output); - m_orientation.m_ang.m_vec[0] = orientation.m_ang.m_vec[0] * m_pitch_phase_compensation; - m_orientation.m_ang.m_vec[1] = orientation.m_ang.m_vec[1] * m_roll_phase_compensation; - m_orientation.m_ang.m_vec[2] = orientation.m_ang.m_vec[2] * m_azimuth_phase_compensation; - m_rot_matrix = quat2rot_mat(m_quat_driv); quat_error = m_quat_aid * m_quat_driv; @@ -378,6 +371,15 @@ void orientation_filter::get_device_orientation(const sensor_data *a time_update_gaming_rv(); measurement_update(); + + if (magnetic == NULL) { + m_quaternion = m_quat_gaming_rv; + } else { + m_quaternion = m_quat_9axis; + } + + } else { + m_quaternion = m_quat_aid; } } diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index 8a1351b..c89a6b8 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -64,11 +64,9 @@ public: euler_angles m_orientation; quaternion m_quat_9axis; quaternion m_quat_gaming_rv; + quaternion m_quaternion; TYPE m_gyro_dt; - int m_pitch_phase_compensation; - int m_roll_phase_compensation; - int m_azimuth_phase_compensation; int m_magnetic_alignment_factor; orientation_filter(); -- 2.7.4