Adding geomagnetic_rv implementation to orientation_filter class

- updated orientation_filter class for quaternion computation based
only on accel and geomagnetic input sensors.
- updated standalone test code for supporting geomagenetic rotation
vector computation.

Change-Id: Ie9b2ea60f3815adacff9fc7ee3c8e545f27b0816

diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp
index bd1a991..c08325e 100644 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -119,6 +119,17 @@ inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<T
 }
 
 template <typename TYPE>
+inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<TYPE> accel,
+               const sensor_data<TYPE> magnetic)
+{
+       m_accel.m_data = accel.m_data;
+       m_magnetic.m_data = magnetic.m_data;
+
+       m_accel.m_time_stamp = accel.m_time_stamp;
+       m_magnetic.m_time_stamp = magnetic.m_time_stamp;
+}
+
+template <typename TYPE>
 inline void orientation_filter<TYPE>::orientation_triad_algorithm()
 {
        TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
@@ -329,4 +340,17 @@ quaternion<TYPE> orientation_filter<TYPE>::get_9axis_quaternion(const sensor_dat
        return m_quat_9axis;
 }
 
+template <typename TYPE>
+quaternion<TYPE> orientation_filter<TYPE>::get_geomagnetic_quaternion(const sensor_data<TYPE> accel,
+               const sensor_data<TYPE> magnetic)
+{
+       initialize_sensor_data(accel, magnetic);
+
+       normalize(m_accel);
+       normalize(m_magnetic);
+
+       orientation_triad_algorithm();
+
+       return m_quat_aid;
+}
 #endif  //_ORIENTATION_FILTER_H_

diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h
index 200bf19..09f5b90 100644 (file)
--- a/src/sensor_fusion/orientation_filter.h
+++ b/src/sensor_fusion/orientation_filter.h
@@ -65,6 +65,8 @@ public:
 
        inline void initialize_sensor_data(const sensor_data<TYPE> accel,
                        const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic);
+       inline void initialize_sensor_data(const sensor_data<TYPE> accel,
+                       const sensor_data<TYPE> magnetic);
        inline void orientation_triad_algorithm();
        inline void compute_covariance();
        inline void time_update();
@@ -76,6 +78,8 @@ public:
                        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);
 };
 
 #include "orientation_filter.cpp"

diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp
index 39ee81b..06ebf5a 100644 (file)
--- a/src/sensor_fusion/test/orientation_sensor.cpp
+++ b/src/sensor_fusion/test/orientation_sensor.cpp
@@ -95,4 +95,14 @@ quaternion<float> orientation_sensor::get_9axis_quaternion(sensor_data<float> ac
        return orien_filter.get_9axis_quaternion(accel_data, gyro_data, magnetic_data);
 }
 
+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);
+       normalize(accel_data);
+       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);
+}
 #endif

diff --git a/src/sensor_fusion/test/orientation_sensor.h b/src/sensor_fusion/test/orientation_sensor.h
index 4ab0fb3..e9fd2b8 100644 (file)
--- a/src/sensor_fusion/test/orientation_sensor.h
+++ b/src/sensor_fusion/test/orientation_sensor.h
@@ -33,6 +33,8 @@ public:
                        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);
 };
 
 #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 a67cc82..a30f8ee 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
@@ -38,7 +38,8 @@ int main()
        euler_angles<float> orientation;
        rotation_matrix<float> orientation_mat;
        quaternion<float> orientation_9axis_quat;
-       orientation_sensor orien_sensor1, orien_sensor2, orien_sensor3;
+       quaternion<float> orientation_geomagnetic_quat;
+       orientation_sensor orien_sensor1, orien_sensor2, orien_sensor3, orien_sensor4;
 
        accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str());
        gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str());
@@ -90,6 +91,10 @@ int main()
                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";
+
+               orientation_geomagnetic_quat = orien_sensor4.get_geomagnetic_quaternion(accel_data, magnetic_data);
+
+               cout << "Orientation geomagnetic quaternion\t" << orientation_geomagnetic_quat.m_quat << "\n\n";
        }
 
        accel_in.close();