Adding methods for common sensor_fusion flow

- Adding new methods for a common orientation computation flow
irrespective of the sensors used in the inputs
- Changing sensor inputs to pointers for checking if inputs are
enabled.
- Tested on rotation_matrix functionality which is not used in any
virtual sensor and using standalone test code.

Change-Id: I763305f06346db2cce87c61e870c0a43f0d1e0e0

diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp
index 68f0431..03e50d6 100644 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -75,6 +75,38 @@ orientation_filter<TYPE>::~orientation_filter()
 }
 
 template <typename TYPE>
+inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<TYPE> *accel,
+               const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic)
+{
+       if (accel != NULL) {
+               m_accel.m_data = accel->m_data;
+               m_accel.m_time_stamp = accel->m_time_stamp;
+
+               normalize(m_accel);
+       }
+
+       if (gyro != NULL) {
+               unsigned long long sample_interval_gyro = SAMPLE_INTV;
+
+               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_gyro.m_time_stamp = gyro->m_time_stamp;
+
+               m_gyro.m_data = m_gyro.m_data * (TYPE) PI;
+
+               m_gyro.m_data = gyro->m_data - m_bias_correction;
+       }
+
+       if (magnetic != NULL) {
+               m_magnetic.m_data = magnetic->m_data;
+               m_magnetic.m_time_stamp = magnetic->m_time_stamp;
+       }
+
+}
+
+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)
 {
@@ -104,6 +136,8 @@ inline void orientation_filter<TYPE>::init_accel_mag_data(const sensor_data<TYPE
 
        m_accel.m_time_stamp = accel.m_time_stamp;
        m_magnetic.m_time_stamp = magnetic.m_time_stamp;
+
+       normalize(m_magnetic);
 }
 
 template <typename TYPE>
@@ -381,6 +415,32 @@ inline void orientation_filter<TYPE>::measurement_update()
 }
 
 template <typename TYPE>
+euler_angles<TYPE> orientation_filter<TYPE>::get_device_rotation(const sensor_data<TYPE> *accel,
+               const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic)
+{
+       initialize_sensor_data(accel, gyro, magnetic);
+
+       const sensor_data<TYPE> accel_in, gyro_in, magnetic_in;
+       euler_angles<TYPE> cor_euler_ang;
+
+       if (magnetic != NULL)
+               orientation_triad_algorithm();
+
+       if (gyro != NULL) {
+               compute_covariance();
+
+               if(magnetic != NULL)
+                       time_update();
+               else
+                       time_update_gaming_rv();
+
+               measurement_update();
+       }
+
+       return m_orientation;
+}
+
+template <typename TYPE>
 euler_angles<TYPE> orientation_filter<TYPE>::get_orientation(const sensor_data<TYPE> accel,
                const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
 {
@@ -404,10 +464,10 @@ euler_angles<TYPE> orientation_filter<TYPE>::get_orientation(const sensor_data<T
 }
 
 template <typename TYPE>
-rotation_matrix<TYPE> orientation_filter<TYPE>::get_rotation_matrix(const sensor_data<TYPE> accel,
-               const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
+rotation_matrix<TYPE> orientation_filter<TYPE>::get_rotation_matrix(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_rot_matrix;
 }

diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h
index a04f3f7..97d372f 100644 (file)
--- a/src/sensor_fusion/orientation_filter.h
+++ b/src/sensor_fusion/orientation_filter.h
@@ -74,6 +74,8 @@ public:
        orientation_filter();
        ~orientation_filter();
 
+       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_mag_data(const sensor_data<TYPE> accel,
                        const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic);
        inline void init_accel_mag_data(const sensor_data<TYPE> accel,
@@ -89,14 +91,16 @@ public:
 
        euler_angles<TYPE> get_orientation(const sensor_data<TYPE> accel,
                        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);
+       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_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);
+       euler_angles<TYPE> get_device_rotation(const sensor_data<TYPE> *accel,
+                       const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
 };
 
 #include "orientation_filter.cpp"

diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp
index 70746f5..30c74c7 100644 (file)
--- a/src/sensor_fusion/test/orientation_sensor.cpp
+++ b/src/sensor_fusion/test/orientation_sensor.cpp
@@ -34,23 +34,23 @@ int roll_phase_compensation = -1;
 int azimuth_phase_compensation = -1;
 int magnetic_alignment_factor = -1;
 
-void pre_process_data(sensor_data<float> &data_out, sensor_data<float> &data_in, float *bias, int *sign, float scale)
+void pre_process_data(sensor_data<float> *data_out, sensor_data<float> *data_in, float *bias, int *sign, float scale)
 {
-       data_out.m_data.m_vec[0] = sign[0] * (data_in.m_data.m_vec[0] - bias[0]) / scale;
-       data_out.m_data.m_vec[1] = sign[1] * (data_in.m_data.m_vec[1] - bias[1]) / scale;
-       data_out.m_data.m_vec[2] = sign[2] * (data_in.m_data.m_vec[2] - bias[2]) / scale;
+       data_out->m_data.m_vec[0] = sign[0] * (data_in->m_data.m_vec[0] - bias[0]) / scale;
+       data_out->m_data.m_vec[1] = sign[1] * (data_in->m_data.m_vec[1] - bias[1]) / scale;
+       data_out->m_data.m_vec[2] = sign[2] * (data_in->m_data.m_vec[2] - bias[2]) / scale;
 
-       data_out.m_time_stamp = data_in.m_time_stamp;
+       data_out->m_time_stamp = data_in->m_time_stamp;
 }
 
 euler_angles<float> orientation_sensor::get_orientation(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);
+       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);
+       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;
@@ -61,14 +61,14 @@ euler_angles<float> orientation_sensor::get_orientation(sensor_data<float> accel
        return orien_filter.get_orientation(accel_data, gyro_data, magnetic_data);
 }
 
-rotation_matrix<float> orientation_sensor::get_rotation_matrix(sensor_data<float> accel_data,
-               sensor_data<float> gyro_data, sensor_data<float> magnetic_data)
+rotation_matrix<float> orientation_sensor::get_rotation_matrix(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);
+       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);
+       normalize(*magnetic_data);
 
        orien_filter.m_pitch_phase_compensation = pitch_phase_compensation;
        orien_filter.m_roll_phase_compensation = roll_phase_compensation;
@@ -81,10 +81,10 @@ rotation_matrix<float> orientation_sensor::get_rotation_matrix(sensor_data<float
 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);
+       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);
+       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;
@@ -98,9 +98,9 @@ quaternion<float> orientation_sensor::get_9axis_quaternion(sensor_data<float> ac
 quaternion<float> orientation_sensor::get_geomagnetic_quaternion(sensor_data<float> accel_data,
                sensor_data<float> magnetic_data)
 {
-       pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel);
+       pre_process_data(&accel_data, &accel_data, bias_accel, sign_accel, scale_accel);
        normalize(accel_data);
-       pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
+       pre_process_data(&magnetic_data, &magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
        normalize(magnetic_data);
 
        return orien_filter.get_geomagnetic_quaternion(accel_data, magnetic_data);
@@ -110,9 +110,9 @@ 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)
 {
-       pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel);
+       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(&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 358c476..5afbff4 100644 (file)
--- a/src/sensor_fusion/test/orientation_sensor.h
+++ b/src/sensor_fusion/test/orientation_sensor.h
@@ -29,8 +29,8 @@ public:
 
        euler_angles<float> get_orientation(sensor_data<float> accel,
                        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);
+       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_geomagnetic_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 c5593e9..8e6830b 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
@@ -85,7 +85,7 @@ int main()
 
                cout << "Orientation angles\t" << orientation.m_ang << "\n\n";
 
-               orientation_mat = orien_sensor2.get_rotation_matrix(accel_data, gyro_data, magnetic_data);
+               orientation_mat = orien_sensor2.get_rotation_matrix(&accel_data, &gyro_data, &magnetic_data);
 
                cout << "Orientation matrix\t" << orientation_mat.m_rot_mat << "\n\n";