Restructuring all virtual sensors and sensor fusion for single flow

- Removed separate methods for computing orientation/RVs and replaced
with single method
- Updated virtual sensors to use the single sensor fusion method
- Updated test files for using single method
- Cleanup.

Change-Id: Iacd555215cf1c146e2aa75451baa92751e67a2e1

diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp
index 900a7c0..d5fe72d 100755 (executable)
--- a/src/orientation/orientation_sensor.cpp
+++ b/src/orientation/orientation_sensor.cpp
@@ -339,7 +339,9 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector<sensor_e
                m_orientation_filter.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
                m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 
-               euler_orientation = m_orientation_filter.get_orientation(&m_accel, &m_gyro, &m_magnetic);
+               m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, &m_magnetic);
+
+               euler_orientation = m_orientation_filter.m_orientation;
 
                if(m_raw_data_unit == "DEGREES") {
                        euler_orientation = rad2deg(euler_orientation);
@@ -400,7 +402,9 @@ int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_da
        m_orientation_filter_poll.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
        m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 
-       euler_orientation = m_orientation_filter_poll.get_orientation(&m_accel, &m_gyro, &m_magnetic);
+       m_orientation_filter_poll.get_device_orientation(&m_accel, &m_gyro, &m_magnetic);
+
+       euler_orientation = m_orientation_filter_poll.m_orientation;
 
        if(m_raw_data_unit == "DEGREES") {
                euler_orientation = rad2deg(euler_orientation);

diff --git a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp
index 86ec74f..7e8b005 100755 (executable)
--- a/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp
+++ b/src/rotation_vector/gaming_rv/gaming_rv_sensor.cpp
@@ -244,7 +244,9 @@ void gaming_rv_sensor::synthesize(const sensor_event_t& event, vector<sensor_eve
        if (m_enable_gaming_rv == GAMING_RV_ENABLED) {
                m_enable_gaming_rv = 0;
 
-               quaternion_gaming_rv = m_orientation_filter.get_gaming_quaternion(&m_accel, &m_gyro);
+               m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, NULL);
+
+               quaternion_gaming_rv = m_orientation_filter.m_quat_gaming_rv;
 
                m_time = get_timestamp();
                rv_event.sensor_id = get_id();
@@ -284,7 +286,9 @@ int gaming_rv_sensor::get_sensor_data(unsigned int event_type, sensor_data_t &da
        accel.m_time_stamp = accel_data.timestamp;
        gyro.m_time_stamp = gyro_data.timestamp;
 
-       quaternion_gaming_rv = m_orientation_filter_poll.get_gaming_quaternion(&m_accel, &m_gyro);
+       m_orientation_filter_poll.get_device_orientation(&m_accel, &m_gyro, NULL);
+
+       quaternion_gaming_rv = m_orientation_filter_poll.m_quat_gaming_rv;
 
        data.accuracy = SENSOR_ACCURACY_GOOD;
        data.timestamp = get_timestamp();

diff --git a/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp b/src/rotation_vector/geomagnetic_rv/geomagnetic_rv_sensor.cpp
index 4a77526..ef56350 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,9 @@ 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);
+               m_orientation_filter.get_device_orientation(&m_accel, NULL, &m_magnetic);
+
+               quaternion_geo_rv = m_orientation_filter.m_quat_aid;
 
                m_time = get_timestamp();
                rv_event.sensor_id = get_id();
@@ -297,7 +299,9 @@ 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);
+       m_orientation_filter_poll.get_device_orientation(&m_accel, NULL, &m_magnetic);
+
+       quaternion_geo_rv = m_orientation_filter_poll.m_quat_aid;
 
        data.accuracy = SENSOR_ACCURACY_GOOD;
        data.timestamp = get_timestamp();

diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp
index 2aacf39..7abe036 100755 (executable)
--- a/src/rotation_vector/rv/rv_sensor.cpp
+++ b/src/rotation_vector/rv/rv_sensor.cpp
@@ -305,7 +305,9 @@ 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);
+               m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, &m_magnetic);
+
+               quaternion_orientation = m_orientation_filter.m_quat_9axis;
 
                m_time = get_timestamp();
                rv_event.sensor_id = get_id();
@@ -355,7 +357,9 @@ 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);
+       m_orientation_filter_poll.get_device_orientation(&m_accel, &m_gyro, &m_magnetic);
+
+       quaternion_orientation = m_orientation_filter_poll.m_quat_9axis;
 
        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 e5b4f72..ae43948 100644 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -78,12 +78,9 @@ 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);
-       }
+       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;
@@ -103,7 +100,6 @@ inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<T
                m_magnetic.m_data = magnetic->m_data;
                m_magnetic.m_time_stamp = magnetic->m_time_stamp;
        }
-
 }
 
 template <typename TYPE>
@@ -231,6 +227,7 @@ inline void orientation_filter<TYPE>::time_update()
        quat_output = phase_correction(m_quat_driv, m_quat_aid);
 
        m_quat_9axis = quat_output;
+       m_quat_gaming_rv = m_quat_9axis;
 
        orientation = quat2euler(quat_output);
 
@@ -362,73 +359,26 @@ inline void orientation_filter<TYPE>::measurement_update()
 }
 
 template <typename TYPE>
-euler_angles<TYPE> orientation_filter<TYPE>::get_device_rotation(const sensor_data<TYPE> *accel,
+void orientation_filter<TYPE>::get_device_orientation(const sensor_data<TYPE> *accel,
                const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic)
 {
        initialize_sensor_data(accel, gyro, magnetic);
 
        if (magnetic != NULL)
                orientation_triad_algorithm();
-       else if(gyro != NULL)
+       else if (gyro != NULL)
                compute_accel_orientation();
 
        if (gyro != NULL) {
                compute_covariance();
 
-               if(magnetic != NULL)
+               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)
-{
-       get_device_rotation(accel, gyro, magnetic);
-
-       return m_orientation;
-}
-
-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)
-{
-       get_device_rotation(accel, gyro, magnetic);
-
-       return m_rot_matrix;
 }
 
-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)
-{
-
-       get_device_rotation(accel, gyro, magnetic);
-
-       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)
-{
-       get_device_rotation(accel, NULL, magnetic);
-
-       return m_quat_aid;
-}
-
-template <typename TYPE>
-quaternion<TYPE> orientation_filter<TYPE>::get_gaming_quaternion(const sensor_data<TYPE> *accel,
-               const sensor_data<TYPE> *gyro)
-{
-       get_device_rotation(accel, gyro, NULL);
-
-       return m_quat_gaming_rv;
-}
 #endif  //_ORIENTATION_FILTER_H_

diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h
index 0b8af39..8a1351b 100644 (file)
--- a/src/sensor_fusion/orientation_filter.h
+++ b/src/sensor_fusion/orientation_filter.h
@@ -83,17 +83,7 @@ public:
        inline void time_update_gaming_rv();
        inline void measurement_update();
 
-       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);
-       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,
+       void get_device_orientation(const sensor_data<TYPE> *accel,
                        const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
 };
 

diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp
index c8c8950..b9ff4bb 100644 (file)
--- a/src/sensor_fusion/test/orientation_sensor.cpp
+++ b/src/sensor_fusion/test/orientation_sensor.cpp
@@ -26,7 +26,7 @@ int sign_accel[] = {+1, +1, +1};
 int sign_gyro[] = {+1, +1, +1};
 int sign_magnetic[] = {+1, +1, +1};
 float scale_accel = 1;
-float scale_gyro = 575;
+float scale_gyro = 1150;
 float scale_magnetic = 1;
 
 int pitch_phase_compensation = -1;
@@ -43,7 +43,7 @@ void pre_process_data(sensor_data<float> *data_out, sensor_data<float> *data_in,
        data_out->m_time_stamp = data_in->m_time_stamp;
 }
 
-euler_angles<float> orientation_sensor::get_orientation(sensor_data<float> *accel_data,
+void orientation_sensor::get_device_orientation(sensor_data<float> *accel_data,
                sensor_data<float> *gyro_data, sensor_data<float> *magnetic_data)
 {
 
@@ -58,62 +58,7 @@ euler_angles<float> orientation_sensor::get_orientation(sensor_data<float> *acce
        orien_filter.m_azimuth_phase_compensation = azimuth_phase_compensation;
        orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor;
 
-       return orien_filter.get_orientation(accel_data, gyro_data, magnetic_data);
+       orien_filter.get_device_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)
-{
-       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_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)
-{
-       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_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);
-}
-
-
-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);
-       normalize(*accel_data);
-       pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro);
-
-       return orien_filter.get_gaming_quaternion(accel_data, gyro_data);
-}
 #endif

diff --git a/src/sensor_fusion/test/orientation_sensor.h b/src/sensor_fusion/test/orientation_sensor.h
index ebf9aea..c7aff2f 100644 (file)
--- a/src/sensor_fusion/test/orientation_sensor.h
+++ b/src/sensor_fusion/test/orientation_sensor.h
@@ -27,16 +27,8 @@ class orientation_sensor
 public:
        orientation_filter<float> orien_filter;
 
-       euler_angles<float> get_orientation(sensor_data<float> *accel,
+       void get_device_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);
-       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,
-                       sensor_data<float> *gyro);
 };
 
 #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 6f6ebb3..53599d2 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
@@ -40,7 +40,7 @@ int main()
        quaternion<float> orientation_9axis_quat;
        quaternion<float> orientation_geomagnetic_quat;
        quaternion<float> orientation_gaming_quat;
-       orientation_sensor orien_sensor1, orien_sensor2, orien_sensor3, orien_sensor4, orien_sensor5;
+       orientation_sensor orien_sensor;
 
        accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str());
        gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str());
@@ -79,27 +79,19 @@ int main()
 
                cout << "Magnetic Data\t" << magnetic_data.m_data << "\t Time Stamp\t" << magnetic_data.m_time_stamp << "\n\n";
 
-               orientation = orien_sensor1.get_orientation(&accel_data, &gyro_data, &magnetic_data);
+               orien_sensor.get_device_orientation(&accel_data, &gyro_data, &magnetic_data);
 
-               orien_file << orientation.m_ang;
+               orien_file << orien_sensor.orien_filter.m_orientation.m_ang;
 
-               cout << "Orientation angles\t" << orientation.m_ang << "\n\n";
+               cout << "Orientation angles\t" << orien_sensor.orien_filter.m_orientation.m_ang << "\n\n";
 
-               orientation_mat = orien_sensor2.get_rotation_matrix(&accel_data, &gyro_data, &magnetic_data);
+               cout << "Orientation matrix\t" << orien_sensor.orien_filter.m_rot_matrix.m_rot_mat << "\n\n";
 
-               cout << "Orientation matrix\t" << orientation_mat.m_rot_mat << "\n\n";
+               cout << "Orientation 9-axis quaternion\t" << orien_sensor.orien_filter.m_quat_9axis.m_quat << "\n\n";
 
-               orientation_9axis_quat = orien_sensor3.get_9axis_quaternion(&accel_data, &gyro_data, &magnetic_data);
+               cout << "Orientation geomagnetic quaternion\t" << orien_sensor.orien_filter.m_quat_aid.m_quat << "\n\n";
 
-               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";
-
-               orientation_gaming_quat = orien_sensor5.get_gaming_quaternion(&accel_data, &gyro_data);
-
-               cout << "Orientation gaming quaternion\t" << orientation_gaming_quat.m_quat << "\n\n";
+               cout << "Orientation gaming quaternion\t" << orien_sensor.orien_filter.m_quat_gaming_rv.m_quat << "\n\n";
        }
 
        accel_in.close();