Adding orientation filter to support quaternion output

- orientation_filter would output device orientation as quaternion
to support rotation_vector virtual sensor.

Change-Id: I5e0cdcb1b218c59d32965deac76629f04c728899

diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp
index a615217..05537a3 100644 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -218,8 +218,11 @@ inline void orientation_filter<TYPE>::time_update()
 
        m_quat_driv = m_quat_driv + (quat_diff * (TYPE) m_gyro_dt * (TYPE) PI);
        m_quat_driv.quat_normalize();
+
        quat_output = phase_correction(m_quat_driv, m_quat_aid);
 
+       m_quaternion = quat_output;
+
        orientation = quat2euler(quat_output);
 
        m_orientation.m_ang.m_vec[0] = orientation.m_ang.m_vec[0] * m_pitch_phase_compensation;
@@ -313,4 +316,14 @@ rotation_matrix<TYPE> orientation_filter<TYPE>::get_rotation_matrix(const sensor
        return m_rot_matrix;
 }
 
+template <typename TYPE>
+quaternion<TYPE> orientation_filter<TYPE>::get_quaternion(const sensor_data<TYPE> accel,
+               const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
+{
+
+       get_orientation(accel, gyro, magnetic);
+
+       return m_quaternion;
+}
+
 #endif  //_ORIENTATION_FILTER_H_

diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h
index f7bc4d7..40e2c50 100644 (file)
--- a/src/sensor_fusion/orientation_filter.h
+++ b/src/sensor_fusion/orientation_filter.h
@@ -52,6 +52,7 @@ public:
        quaternion<TYPE> m_quat_driv;
        rotation_matrix<TYPE> m_rot_matrix;
        euler_angles<TYPE> m_orientation;
+       quaternion<TYPE> m_quaternion;
        TYPE m_gyro_dt;
 
        int m_pitch_phase_compensation;
@@ -73,6 +74,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_quaternion(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/standalone/orientation_sensor.cpp b/src/sensor_fusion/standalone/orientation_sensor.cpp
index c006922..82c227f 100644 (file)
--- a/src/sensor_fusion/standalone/orientation_sensor.cpp
+++ b/src/sensor_fusion/standalone/orientation_sensor.cpp
@@ -64,7 +64,6 @@ euler_angles<float> orientation_sensor::get_orientation(sensor_data<float> accel
 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);
        pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro);
@@ -79,4 +78,21 @@ 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_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);
+
+       orien_filter.m_pitch_phase_compensation = pitch_phase_compensation;
+       orien_filter.m_roll_phase_compensation = roll_phase_compensation;
+       orien_filter.m_azimuth_phase_compensation = azimuth_phase_compensation;
+       orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor;
+
+       return orien_filter.get_quaternion(accel_data, gyro_data, magnetic_data);
+}
+
 #endif

diff --git a/src/sensor_fusion/standalone/orientation_sensor.h b/src/sensor_fusion/standalone/orientation_sensor.h
index a02e2af..1ead53e 100644 (file)
--- a/src/sensor_fusion/standalone/orientation_sensor.h
+++ b/src/sensor_fusion/standalone/orientation_sensor.h
@@ -31,6 +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_quaternion(sensor_data<float> accel,
+                       sensor_data<float> gyro, sensor_data<float> magnetic);
 };
 
 #include "orientation_sensor.cpp"

diff --git a/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp b/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp
index b3ef1f5..12058e0 100644 (file)
--- a/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp
+++ b/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp
@@ -37,7 +37,8 @@ int main()
        unsigned long long time_stamp;
        euler_angles<float> orientation;
        rotation_matrix<float> orientation_mat;
-       orientation_sensor orien_sensor1, orien_sensor2;
+       quaternion<float> orientation_quat;
+       orientation_sensor orien_sensor1, orien_sensor2, orien_sensor3;
 
        accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str());
        gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str());
@@ -82,9 +83,13 @@ int main()
 
                cout << "Orientation angles\t" << orientation.m_ang << "\n\n";
 
-               orientation_mat = orien_sensor1.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";
+
+               orientation_quat = orien_sensor3.get_quaternion(accel_data, gyro_data, magnetic_data);
+
+               cout << "Orientation quaternion\t" << orientation_quat.m_quat << "\n\n";
        }
 
        accel_in.close();