From 7fee15c18ac4ce536e2487196f63acc5ca5726aa Mon Sep 17 00:00:00 2001 From: Ankur Date: Wed, 12 Aug 2015 11:57:47 +0530 Subject: [PATCH] Modifying Gaming RV to use a separate instance of sensor fusion - separate from fusion sensor Independent of Fusion Sensor. Tested on rd-pq signed-off-by:"Ankur Garg " Change-Id: I7c65b00866e9c92fbc9e171f265a83ed232db95a --- src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp | 157 ++++++++++++++++----- src/rotation_vector/gaming_rv/gaming_rv_sensor.h | 14 +- virtual_sensors.xml | 18 +++ 3 files changed, 154 insertions(+), 35 deletions(-) diff --git a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp index 6982608..fa359a0 100755 --- a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp +++ b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp @@ -35,17 +35,36 @@ #define SENSOR_NAME "GAMING_RV_SENSOR" #define SENSOR_TYPE_GAMING_RV "GAMING_ROTATION_VECTOR" -#define MIN_DELIVERY_DIFF_FACTOR 0.75f +#define ACCELEROMETER_ENABLED 0x01 +#define GYROSCOPE_ENABLED 0x02 +#define GAMING_RV_ENABLED 3 + +#define INITIAL_VALUE -1 #define MS_TO_US 1000 +#define ELEMENT_NAME "NAME" #define ELEMENT_VENDOR "VENDOR" +#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT" #define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME" +#define ELEMENT_ACCEL_STATIC_BIAS "ACCEL_STATIC_BIAS" +#define ELEMENT_GYRO_STATIC_BIAS "GYRO_STATIC_BIAS" +#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION "GYRO_ROTATION_DIRECTION_COMPENSATION" +#define ELEMENT_ACCEL_SCALE "ACCEL_SCALE" +#define ELEMENT_GYRO_SCALE "GYRO_SCALE" + +void pre_process_data(sensor_data &data_out, const float *data_in, float *bias, int *sign, float scale) +{ + data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale; + data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale; + data_out.m_data.m_vec[2] = sign[2] * (data_in[2] - bias[2]) / scale; +} gaming_rv_sensor::gaming_rv_sensor() : m_accel_sensor(NULL) , m_gyro_sensor(NULL) -, m_fusion_sensor(NULL) +, m_accuracy(-1) , m_time(0) { cvirtual_sensor_config &config = cvirtual_sensor_config::get_instance(); @@ -56,8 +75,11 @@ gaming_rv_sensor::gaming_rv_sensor() else m_hardware_fusion = true; + INFO("m_hardware_fusion = %d", m_hardware_fusion); + m_name = string(SENSOR_NAME); register_supported_event(GAMING_RV_RAW_DATA_EVENT); + m_enable_gaming_rv = 0; if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_VENDOR, m_vendor)) { ERR("[VENDOR] is empty\n"); @@ -73,6 +95,48 @@ gaming_rv_sensor::gaming_rv_sensor() INFO("m_default_sampling_time = %d", m_default_sampling_time); + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) { + ERR("[ACCEL_STATIC_BIAS] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_static_bias = (%f, %f, %f)", m_accel_static_bias[0], m_accel_static_bias[1], m_accel_static_bias[2]); + + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_GYRO_STATIC_BIAS, m_gyro_static_bias,3)) { + ERR("[GYRO_STATIC_BIAS] is empty\n"); + throw ENXIO; + } + + INFO("m_gyro_static_bias = (%f, %f, %f)", m_gyro_static_bias[0], m_gyro_static_bias[1], m_gyro_static_bias[2]); + + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) { + ERR("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_rotation_direction_compensation = (%d, %d, %d)", m_accel_rotation_direction_compensation[0], m_accel_rotation_direction_compensation[1], m_accel_rotation_direction_compensation[2]); + + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION, m_gyro_rotation_direction_compensation, 3)) { + ERR("[GYRO_ROTATION_DIRECTION_COMPENSATION] is empty\n"); + throw ENXIO; + } + + INFO("m_gyro_rotation_direction_compensation = (%d, %d, %d)", m_gyro_rotation_direction_compensation[0], m_gyro_rotation_direction_compensation[1], m_gyro_rotation_direction_compensation[2]); + + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_ACCEL_SCALE, &m_accel_scale)) { + ERR("[ACCEL_SCALE] is empty\n"); + throw ENXIO; + } + + INFO("m_accel_scale = %f", m_accel_scale); + + if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_GYRO_SCALE, &m_gyro_scale)) { + ERR("[GYRO_SCALE] is empty\n"); + throw ENXIO; + } + + INFO("m_gyro_scale = %f", m_gyro_scale); + m_interval = m_default_sampling_time * MS_TO_US; } @@ -86,11 +150,9 @@ bool gaming_rv_sensor::init() m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); - m_fusion_sensor = sensor_plugin_loader::get_instance().get_sensor(FUSION_SENSOR); - - if (!m_accel_sensor || !m_gyro_sensor || !m_fusion_sensor) { - ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x, fusion: 0x%x", - m_accel_sensor, m_gyro_sensor, m_fusion_sensor); + if (!m_accel_sensor || !m_gyro_sensor) { + ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x", + m_accel_sensor, m_gyro_sensor); return false; } @@ -117,12 +179,6 @@ bool gaming_rv_sensor::on_start(void) m_gyro_sensor->start(); } - m_fusion_sensor->register_supported_event(FUSION_EVENT); - m_fusion_sensor->register_supported_event(FUSION_GAMING_ROTATION_VECTOR_ENABLED); - m_fusion_sensor->add_client(FUSION_EVENT); - m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_fusion_sensor->start(); - activate(); return true; } @@ -140,12 +196,6 @@ bool gaming_rv_sensor::on_stop(void) m_gyro_sensor->stop(); } - m_fusion_sensor->delete_client(FUSION_EVENT); - m_fusion_sensor->delete_interval((intptr_t)this, false); - m_fusion_sensor->unregister_supported_event(FUSION_EVENT); - m_fusion_sensor->unregister_supported_event(FUSION_GAMING_ROTATION_VECTOR_ENABLED); - m_fusion_sensor->stop(); - deactivate(); return true; } @@ -159,8 +209,6 @@ bool gaming_rv_sensor::add_interval(int client_id, unsigned int interval) m_gyro_sensor->add_interval(client_id, interval, false); } - m_fusion_sensor->add_interval(client_id, interval, false); - return sensor_base::add_interval(client_id, interval, false); } @@ -173,33 +221,58 @@ bool gaming_rv_sensor::delete_interval(int client_id) m_gyro_sensor->delete_interval(client_id, false); } - m_fusion_sensor->delete_interval(client_id, false); - return sensor_base::delete_interval(client_id, false); } void gaming_rv_sensor::synthesize(const sensor_event_t& event, vector &outs) { + const float MIN_DELIVERY_DIFF_FACTOR = 0.75f; unsigned long long diff_time; sensor_event_t rv_event; + quaternion quaternion_gaming_rv; + + if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) { + diff_time = event.data.timestamp - m_time; + + if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) + return; + + pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); - if (event.event_type == FUSION_EVENT) { + m_accel.m_time_stamp = event.data.timestamp; + + m_enable_gaming_rv |= ACCELEROMETER_ENABLED; + } else if (event.event_type == GYROSCOPE_RAW_DATA_EVENT) { diff_time = event.data.timestamp - m_time; if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) return; + pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale); + + m_gyro.m_time_stamp = event.data.timestamp; + + m_enable_gaming_rv |= GYROSCOPE_ENABLED; + } + + if (m_enable_gaming_rv == GAMING_RV_ENABLED) { + m_enable_gaming_rv = 0; + + m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, NULL); + + quaternion_gaming_rv = m_orientation_filter.m_quat_gaming_rv; + m_time = get_timestamp(); rv_event.sensor_id = get_id(); rv_event.event_type = GAMING_RV_RAW_DATA_EVENT; rv_event.data.accuracy = SENSOR_ACCURACY_GOOD; rv_event.data.timestamp = m_time; rv_event.data.value_count = 4; - rv_event.data.values[0] = event.data.values[1]; - rv_event.data.values[1] = event.data.values[2]; - rv_event.data.values[2] = event.data.values[3]; - rv_event.data.values[3] = event.data.values[0]; + rv_event.data.values[0] = quaternion_gaming_rv.m_quat.m_vec[1]; + rv_event.data.values[1] = quaternion_gaming_rv.m_quat.m_vec[2]; + rv_event.data.values[2] = quaternion_gaming_rv.m_quat.m_vec[3]; + rv_event.data.values[3] = quaternion_gaming_rv.m_quat.m_vec[0]; push(rv_event); } @@ -209,20 +282,36 @@ void gaming_rv_sensor::synthesize(const sensor_event_t& event, vector accel; + sensor_data gyro; + + sensor_data_t accel_data; + sensor_data_t gyro_data; + + quaternion quaternion_gaming_rv; if (event_type != GAMING_RV_RAW_DATA_EVENT) return -1; - m_fusion_sensor->get_sensor_data(FUSION_GAMING_ROTATION_VECTOR_ENABLED, fusion_data); + m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); + m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data); + + pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale); + pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale); + accel.m_time_stamp = accel_data.timestamp; + gyro.m_time_stamp = gyro_data.timestamp; + + m_orientation_filter_poll.get_device_orientation(&m_accel, &m_gyro, NULL); + + quaternion_gaming_rv = m_orientation_filter_poll.m_quat_gaming_rv; data.accuracy = SENSOR_ACCURACY_GOOD; data.timestamp = get_timestamp(); data.value_count = 4; - data.values[0] = fusion_data.values[1]; - data.values[1] = fusion_data.values[2]; - data.values[2] = fusion_data.values[3]; - data.values[3] = fusion_data.values[0]; + data.values[0] = quaternion_gaming_rv.m_quat.m_vec[1]; + data.values[1] = quaternion_gaming_rv.m_quat.m_vec[2]; + data.values[2] = quaternion_gaming_rv.m_quat.m_vec[3]; + data.values[3] = quaternion_gaming_rv.m_quat.m_vec[0]; return 0; } diff --git a/src/rotation_vector/gaming_rv/gaming_rv_sensor.h b/src/rotation_vector/gaming_rv/gaming_rv_sensor.h index 6572907..db9dd47 100755 --- a/src/rotation_vector/gaming_rv/gaming_rv_sensor.h +++ b/src/rotation_vector/gaming_rv/gaming_rv_sensor.h @@ -43,13 +43,18 @@ public: private: sensor_base *m_accel_sensor; sensor_base *m_gyro_sensor; - sensor_base *m_fusion_sensor; sensor_data m_accel; sensor_data m_gyro; cmutex m_value_mutex; + orientation_filter m_orientation_filter; + orientation_filter m_orientation_filter_poll; + + unsigned int m_enable_gaming_rv; + + int m_accuracy; unsigned long long m_time; unsigned int m_interval; @@ -57,6 +62,13 @@ private: string m_raw_data_unit; int m_default_sampling_time; + float m_accel_static_bias[3]; + float m_gyro_static_bias[3]; + int m_accel_rotation_direction_compensation[3]; + int m_gyro_rotation_direction_compensation[3]; + float m_accel_scale; + float m_gyro_scale; + bool on_start(void); bool on_stop(void); }; diff --git a/virtual_sensors.xml b/virtual_sensors.xml index 52ce6fb..4b2c3e0 100755 --- a/virtual_sensors.xml +++ b/virtual_sensors.xml @@ -63,6 +63,12 @@ + + + + + + @@ -152,6 +158,12 @@ + + + + + + @@ -241,6 +253,12 @@ + + + + + + -- 2.7.4