Updating geomagnetic_rv sensor to follow single sensor fusion flow

- The gyro input pointer is passed a value NULL
- Cleaning up unused initialization code and old flow code

Change-Id: I438d8dc1615d2e9dcc3188c9f06ca7d8aa042d4c

diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp
index 6649333..4a77526 100755 (executable)
--- a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp
+++ b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp
@@ -255,7 +255,7 @@ void geomagnetic_rv_sensor::synthesize(const sensor_event_t& event, vector<senso
 
                m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 
-               quaternion_geo_rv = m_orientation_filter.get_geomagnetic_quaternion(m_accel, m_magnetic);
+               quaternion_geo_rv = m_orientation_filter.get_geomagnetic_quaternion(&m_accel, &m_magnetic);
 
                m_time = get_timestamp();
                rv_event.sensor_id = get_id();
@@ -297,7 +297,7 @@ int geomagnetic_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_
 
        m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 
-       quaternion_geo_rv = m_orientation_filter_poll.get_geomagnetic_quaternion(m_accel, m_magnetic);
+       quaternion_geo_rv = m_orientation_filter_poll.get_geomagnetic_quaternion(&m_accel, &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 840bd09..c74ae4c 100644 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -107,19 +107,6 @@ inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<T
 }
 
 template <typename TYPE>
-inline void orientation_filter<TYPE>::init_accel_mag_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;
-
-       normalize(m_magnetic);
-}
-
-template <typename TYPE>
 inline void orientation_filter<TYPE>::init_accel_gyro_data(const sensor_data<TYPE> accel,
                const sensor_data<TYPE> gyro)
 {
@@ -445,15 +432,10 @@ quaternion<TYPE> orientation_filter<TYPE>::get_9axis_quaternion(const sensor_dat
 }
 
 template <typename TYPE>
-quaternion<TYPE> orientation_filter<TYPE>::get_geomagnetic_quaternion(const sensor_data<TYPE> accel,
-               const sensor_data<TYPE> magnetic)
+quaternion<TYPE> orientation_filter<TYPE>::get_geomagnetic_quaternion(const sensor_data<TYPE> *accel,
+               const sensor_data<TYPE> *magnetic)
 {
-       init_accel_mag_data(accel, magnetic);
-
-       normalize(m_accel);
-       normalize(m_magnetic);
-
-       orientation_triad_algorithm();
+       get_device_rotation(accel, NULL, magnetic);
 
        return m_quat_aid;
 }

diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h
index ea4abc2..c36fb7c 100644 (file)
--- a/src/sensor_fusion/orientation_filter.h
+++ b/src/sensor_fusion/orientation_filter.h
@@ -76,10 +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_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,
-                       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();
@@ -95,8 +91,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);
+       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,

diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp
index 6c417ce..0d8d17e 100644 (file)
--- a/src/sensor_fusion/test/orientation_sensor.cpp
+++ b/src/sensor_fusion/test/orientation_sensor.cpp
@@ -95,13 +95,13 @@ quaternion<float> orientation_sensor::get_9axis_quaternion(sensor_data<float> *a
        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)
+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);
+       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);
 }

diff --git a/src/sensor_fusion/test/orientation_sensor.h b/src/sensor_fusion/test/orientation_sensor.h
index 80a12e4..09e383b 100644 (file)
--- a/src/sensor_fusion/test/orientation_sensor.h
+++ b/src/sensor_fusion/test/orientation_sensor.h
@@ -33,8 +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);
+       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);
 };

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 b1fef99..ebeb7b4 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
@@ -93,7 +93,7 @@ int main()
 
                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);
+               orientation_geomagnetic_quat = orien_sensor4.get_geomagnetic_quaternion(&accel_data, &magnetic_data);
 
                cout << "Orientation geomagnetic quaternion\t" << orientation_geomagnetic_quat.m_quat << "\n\n";