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

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 (executable)
--- 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<sensor_eve
        if (m_enable_gaming_rv == GAMING_RV_ENABLED) {
                m_enable_gaming_rv = 0;
 
-               quaternion_gaming_rv = m_orientation_filter.get_gaming_quaternion(m_accel, m_gyro);
+               quaternion_gaming_rv = m_orientation_filter.get_gaming_quaternion(&m_accel, &m_gyro);
 
                m_time = get_timestamp();
                rv_event.sensor_id = get_id();
@@ -284,7 +284,7 @@ int gaming_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &da
        accel.m_time_stamp = accel_data.timestamp;
        gyro.m_time_stamp = gyro_data.timestamp;
 
-       quaternion_gaming_rv = m_orientation_filter_poll.get_gaming_quaternion(m_accel, m_gyro);
+       quaternion_gaming_rv = m_orientation_filter_poll.get_gaming_quaternion(&m_accel, &m_gyro);
 
        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 c74ae4c..e5b4f72 100644 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -107,25 +107,6 @@ inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<T
 }
 
 template <typename TYPE>
-inline void orientation_filter<TYPE>::init_accel_gyro_data(const sensor_data<TYPE> accel,
-               const sensor_data<TYPE> 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 <typename TYPE>
 inline void orientation_filter<TYPE>::orientation_triad_algorithm()
 {
        TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
@@ -388,6 +369,8 @@ euler_angles<TYPE> orientation_filter<TYPE>::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<TYPE> orientation_filter<TYPE>::get_geomagnetic_quaternion(const sens
 }
 
 template <typename TYPE>
-quaternion<TYPE> orientation_filter<TYPE>::get_gaming_quaternion(const sensor_data<TYPE> accel,
-               const sensor_data<TYPE> gyro)
+quaternion<TYPE> orientation_filter<TYPE>::get_gaming_quaternion(const sensor_data<TYPE> *accel,
+               const sensor_data<TYPE> *gyro)
 {
-       euler_angles<TYPE> 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 (file)
--- 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<TYPE> *accel,
                        const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
-       inline void init_accel_gyro_data(const sensor_data<TYPE> accel,
-                       const sensor_data<TYPE> gyro);
        inline void orientation_triad_algorithm();
        inline void compute_accel_orientation();
        inline void compute_covariance();
@@ -93,8 +91,8 @@ public:
                        const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
        quaternion<TYPE> get_geomagnetic_quaternion(const sensor_data<TYPE> *accel,
                        const sensor_data<TYPE> *magnetic);
-       quaternion<TYPE> get_gaming_quaternion(const sensor_data<TYPE> accel,
-                       const sensor_data<TYPE> gyro);
+       quaternion<TYPE> get_gaming_quaternion(const sensor_data<TYPE> *accel,
+                       const sensor_data<TYPE> *gyro);
        euler_angles<TYPE> get_device_rotation(const sensor_data<TYPE> *accel,
                        const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
 };

diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp
index 0d8d17e..c8c8950 100644 (file)
--- a/src/sensor_fusion/test/orientation_sensor.cpp
+++ b/src/sensor_fusion/test/orientation_sensor.cpp
@@ -107,12 +107,12 @@ quaternion<float> orientation_sensor::get_geomagnetic_quaternion(sensor_data<flo
 }
 
 
-quaternion<float> orientation_sensor::get_gaming_quaternion(sensor_data<float> accel_data,
-               sensor_data<float> gyro_data)
+quaternion<float> orientation_sensor::get_gaming_quaternion(sensor_data<float> *accel_data,
+               sensor_data<float> *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 (file)
--- a/src/sensor_fusion/test/orientation_sensor.h
+++ b/src/sensor_fusion/test/orientation_sensor.h
@@ -35,8 +35,8 @@ public:
                        sensor_data<float> *gyro, sensor_data<float> *magnetic);
        quaternion<float> get_geomagnetic_quaternion(sensor_data<float> *accel,
                        sensor_data<float> *magnetic);
-       quaternion<float> get_gaming_quaternion(sensor_data<float> accel,
-                       sensor_data<float> gyro);
+       quaternion<float> get_gaming_quaternion(sensor_data<float> *accel,
+                       sensor_data<float> *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 (file)
--- 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";
        }