Modifying Gaming RV to use a separate instance of sensor fusion - separate from fusio...

Independent of Fusion Sensor.
Tested on rd-pq

signed-off-by:"Ankur Garg <ankur29.garg@samsung.com>"
Change-Id: I7c65b00866e9c92fbc9e171f265a83ed232db95a

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 (executable)
--- 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<float> &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<sensor_event_t> &outs)
 {
+       const float MIN_DELIVERY_DIFF_FACTOR = 0.75f;
        unsigned long long diff_time;
 
        sensor_event_t rv_event;
+       quaternion<float> 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<sensor_eve
 
 int gaming_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &data)
 {
-       sensor_data_t fusion_data;
+       sensor_data<float> accel;
+       sensor_data<float> gyro;
+
+       sensor_data_t accel_data;
+       sensor_data_t gyro_data;
+
+       quaternion<float> 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 (executable)
--- 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<float> m_accel;
        sensor_data<float> m_gyro;
 
        cmutex m_value_mutex;
 
+       orientation_filter<float> m_orientation_filter;
+       orientation_filter<float> 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 (executable)
--- a/virtual_sensors.xml
+++ b/virtual_sensors.xml
@@ -63,6 +63,12 @@
                        <NAME value="GAMING_RV_SENSOR" />
                        <VENDOR value="SAMSUNG" />
                        <DEFAULT_SAMPLING_TIME value="100" />
+                       <ACCEL_STATIC_BIAS value1="0" value2="0" value3="0" />
+                       <GYRO_STATIC_BIAS value1="0" value2="0" value3="0" />
+                       <ACCEL_ROTATION_DIRECTION_COMPENSATION value1="-1" value2="-1" value3="-1" />
+                       <GYRO_ROTATION_DIRECTION_COMPENSATION value1="1" value2="1" value3="1" />
+                       <ACCEL_SCALE value="1" />
+                       <GYRO_SCALE value="1146" />
                </GAMING_ROTATION_VECTOR>
 
                <TILT>
@@ -152,6 +158,12 @@
                        <NAME value="GAMING_RV_SENSOR" />
                        <VENDOR value="SAMSUNG" />
                        <DEFAULT_SAMPLING_TIME value="100" />
+                       <ACCEL_STATIC_BIAS value1="0.064444" value2="0.15789" value3="-0.36865" />
+                       <GYRO_STATIC_BIAS value1="3.5638" value2="-5.8288" value3="-0.79750" />
+                       <ACCEL_ROTATION_DIRECTION_COMPENSATION value1="1" value2="1" value3="1" />
+                       <GYRO_ROTATION_DIRECTION_COMPENSATION value1="1" value2="1" value3="1" />
+                       <ACCEL_SCALE value="1" />
+                       <GYRO_SCALE value="1150" />
                </GAMING_ROTATION_VECTOR>
 
                <TILT>
@@ -241,6 +253,12 @@
                        <NAME value="GAMING_RV_SENSOR" />
                        <VENDOR value="SAMSUNG" />
                        <DEFAULT_SAMPLING_TIME value="100" />
+                       <ACCEL_STATIC_BIAS value1="0.046091" value2="0.127486" value3="0.10787" />
+                       <GYRO_STATIC_BIAS value1="-0.54385" value2="0.24325" value3="0.87875" />
+                       <ACCEL_ROTATION_DIRECTION_COMPENSATION value1="1" value2="-1" value3="1" />
+                       <GYRO_ROTATION_DIRECTION_COMPENSATION value1="-1" value2="1" value3="-1" />
+                       <ACCEL_SCALE value="1" />
+                       <GYRO_SCALE value="1150" />
                </GAMING_ROTATION_VECTOR>
 
                <TILT>