Adding support for uncal_gyro sensor in fusion_sensor

Modifying the fusion sensor to support uncallibrated gyroscope sensor

Change-Id: I223e58181b0cbcd3d152548cc2c6401826b35348

diff --git a/src/fusion/fusion_sensor.cpp b/src/fusion/fusion_sensor.cpp
index 42a8e41..48ba309 100755 (executable)
--- a/src/fusion/fusion_sensor.cpp
+++ b/src/fusion/fusion_sensor.cpp
@@ -44,6 +44,7 @@
 #define GEOMAGNETIC_RV_ENABLED 5
 #define ORIENTATION_ENABLED 7
 #define ROTATION_VECTOR_ENABLED 7
+#define UNCAL_GYRO_ENABLED 7
 
 #define INITIAL_VALUE -1
 
@@ -282,7 +283,8 @@ void fusion_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_
 
        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)) {
+                       sensor_base::is_supported(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED) ||
+                       sensor_base::is_supported(FUSION_UNCAL_GYRO_ENABLED)) {
                if (event.event_type == GEOMAGNETIC_RAW_DATA_EVENT) {
                        diff_time = event.data.timestamp - m_time;
 
@@ -301,7 +303,8 @@ void fusion_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_
 
        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)) {
+                       sensor_base::is_supported(FUSION_GAMING_ROTATION_VECTOR_ENABLED) ||
+                       sensor_base::is_supported(FUSION_UNCAL_GYRO_ENABLED)) {
                if (event.event_type == GYROSCOPE_RAW_DATA_EVENT) {
                                diff_time = event.data.timestamp - m_time;
 
@@ -322,28 +325,52 @@ void fusion_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_
                        (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))) {
+                       (m_enable_fusion == GEOMAGNETIC_RV_ENABLED && sensor_base::is_supported(FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED)) ||
+                       (m_enable_fusion == UNCAL_GYRO_ENABLED && sensor_base::is_supported(FUSION_UNCAL_GYRO_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);
+               if (m_enable_fusion == UNCAL_GYRO_ENABLED && sensor_base::is_supported(FUSION_UNCAL_GYRO_ENABLED)) {
+                       m_time = get_timestamp();
+                       fusion_event.sensor_id = get_id();
+                       fusion_event.data.timestamp = m_time;
+                       fusion_event.data.accuracy = SENSOR_ACCURACY_GOOD;
+                       fusion_event.event_type = FUSION_UNCAL_GYRO_EVENT;
+                       fusion_event.data.value_count = 3;
+                       fusion_event.data.values[0] = m_orientation_filter.m_gyro_bias.m_vec[0];
+                       fusion_event.data.values[1] = m_orientation_filter.m_gyro_bias.m_vec[1];
+                       fusion_event.data.values[2] = m_orientation_filter.m_gyro_bias.m_vec[2];
+
+                       push(fusion_event);
+               }
+
+               if ((m_enable_fusion == TILT_ENABLED && sensor_base::is_supported(FUSION_TILT_ENABLED)) ||
+                               (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))) {
+                       m_time = get_timestamp();
+                       fusion_event.sensor_id = get_id();
+                       fusion_event.data.timestamp = m_time;
+                       fusion_event.data.accuracy = SENSOR_ACCURACY_GOOD;
+                       fusion_event.event_type = FUSION_EVENT;
+                       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];
+
+                       push(fusion_event);
+               }
+
+               m_enable_fusion = 0;
+
+               m_accel_ptr = m_gyro_ptr = m_magnetic_ptr = NULL;
        }
 
        return;
@@ -364,7 +391,8 @@ int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t
        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)
+                       event_type != FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED ||
+                       event_type != FUSION_UNCAL_GYRO_ENABLED)
                return -1;
 
        m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data);
@@ -373,7 +401,8 @@ int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t
 
        if (event_type == FUSION_ORIENTATION_ENABLED ||
                        event_type == FUSION_ROTATION_VECTOR_ENABLED ||
-                       event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED)
+                       event_type == FUSION_GAMING_ROTATION_VECTOR_ENABLED ||
+                       event_type == FUSION_UNCAL_GYRO_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);
@@ -382,7 +411,8 @@ int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t
 
        if (event_type == FUSION_ORIENTATION_ENABLED ||
                        event_type == FUSION_ROTATION_VECTOR_ENABLED ||
-                       event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED)
+                       event_type == FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED ||
+                       event_type == FUSION_UNCAL_GYRO_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);
@@ -391,20 +421,35 @@ int fusion_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t
 
        m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 
-       if (event_type == FUSION_ORIENTATION_ENABLED || event_type == FUSION_ROTATION_VECTOR_ENABLED)
+       if (event_type == FUSION_ORIENTATION_ENABLED ||
+                       event_type == FUSION_ROTATION_VECTOR_ENABLED ||
+                       event_type == FUSION_UNCAL_GYRO_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];
+       if (event_type == FUSION_UNCAL_GYRO_ENABLED) {
+               data.accuracy = SENSOR_ACCURACY_GOOD;
+               data.timestamp = get_timestamp();
+               data.value_count = 3;
+               data.values[0] = m_orientation_filter_poll.m_gyro_bias.m_vec[0];
+               data.values[1] = m_orientation_filter_poll.m_gyro_bias.m_vec[1];
+               data.values[2] = m_orientation_filter_poll.m_gyro_bias.m_vec[2];
+       }
+       else 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) {
+               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;
 }

diff --git a/src/libsensord/sensor_fusion.h b/src/libsensord/sensor_fusion.h
index 8f80499..1a65ea3 100755 (executable)
--- a/src/libsensord/sensor_fusion.h
+++ b/src/libsensord/sensor_fusion.h
@@ -37,12 +37,14 @@ extern "C"
  */
 enum fusion_event_type {
        FUSION_EVENT = (FUSION_SENSOR << 16) | 0x0001,
-       FUSION_CALIBRATION_NEEDED_EVENT = (FUSION_SENSOR << 16) | 0x0002,
-       FUSION_ORIENTATION_ENABLED = (FUSION_SENSOR << 16) | 0x0003,
-       FUSION_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0004,
-       FUSION_GAMING_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0005,
-       FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0006,
-       FUSION_TILT_ENABLED = (FUSION_SENSOR << 16) | 0x0007,
+       FUSION_UNCAL_GYRO_EVENT = (FUSION_SENSOR << 16) | 0x0002,
+       FUSION_CALIBRATION_NEEDED_EVENT = (FUSION_SENSOR << 16) | 0x0003,
+       FUSION_ORIENTATION_ENABLED = (FUSION_SENSOR << 16) | 0x0004,
+       FUSION_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0005,
+       FUSION_GAMING_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0006,
+       FUSION_GEOMAGNETIC_ROTATION_VECTOR_ENABLED = (FUSION_SENSOR << 16) | 0x0007,
+       FUSION_TILT_ENABLED = (FUSION_SENSOR << 16) | 0x0008,
+       FUSION_UNCAL_GYRO_ENABLED = (FUSION_SENSOR << 16) | 0x0009,
 };
 
 /**