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

diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp
index 48c48de..2aacf39 100755 (executable)
--- 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<sensor_event_t> &
                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 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -107,27 +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_mag_data(const sensor_data<TYPE> accel,
-               const sensor_data<TYPE> gyro, const sensor_data<TYPE> 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 <typename TYPE>
 inline void orientation_filter<TYPE>::init_accel_mag_data(const sensor_data<TYPE> accel,
                const sensor_data<TYPE> magnetic)
 {
@@ -456,11 +435,11 @@ rotation_matrix<TYPE> orientation_filter<TYPE>::get_rotation_matrix(const sensor
 }
 
 template <typename TYPE>
-quaternion<TYPE> orientation_filter<TYPE>::get_9axis_quaternion(const sensor_data<TYPE> accel,
-               const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
+quaternion<TYPE> orientation_filter<TYPE>::get_9axis_quaternion(const sensor_data<TYPE> *accel,
+               const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *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 (file)
--- a/src/sensor_fusion/orientation_filter.h
+++ b/src/sensor_fusion/orientation_filter.h
@@ -93,8 +93,8 @@ public:
                        const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
        rotation_matrix<TYPE> get_rotation_matrix(const sensor_data<TYPE> *accel,
                        const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
-       quaternion<TYPE> get_9axis_quaternion(const sensor_data<TYPE> accel,
-                       const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic);
+       quaternion<TYPE> get_9axis_quaternion(const sensor_data<TYPE> *accel,
+                       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,

diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp
index 57b6393..6c417ce 100644 (file)
--- a/src/sensor_fusion/test/orientation_sensor.cpp
+++ b/src/sensor_fusion/test/orientation_sensor.cpp
@@ -78,14 +78,14 @@ rotation_matrix<float> orientation_sensor::get_rotation_matrix(sensor_data<float
        return orien_filter.get_rotation_matrix(accel_data, gyro_data, magnetic_data);
 }
 
-quaternion<float> orientation_sensor::get_9axis_quaternion(sensor_data<float> accel_data,
-               sensor_data<float> gyro_data, sensor_data<float> magnetic_data)
+quaternion<float> orientation_sensor::get_9axis_quaternion(sensor_data<float> *accel_data,
+               sensor_data<float> *gyro_data, sensor_data<float> *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 (file)
--- a/src/sensor_fusion/test/orientation_sensor.h
+++ b/src/sensor_fusion/test/orientation_sensor.h
@@ -31,8 +31,8 @@ public:
                        sensor_data<float> *gyro, sensor_data<float> *magnetic);
        rotation_matrix<float> get_rotation_matrix(sensor_data<float> *accel,
                        sensor_data<float> *gyro, sensor_data<float> *magnetic);
-       quaternion<float> get_9axis_quaternion(sensor_data<float> accel,
-                       sensor_data<float> gyro, sensor_data<float> magnetic);
+       quaternion<float> get_9axis_quaternion(sensor_data<float> *accel,
+                       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,

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 (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
@@ -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";