Changing orientation sensor to follow single sensor fusion flow

- Updating orientation sensor to work on common single sensor fusion
code flow.

Change-Id: I5581e833e33757a0e24d56a90717473bded3e172

diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp
index 0c8bed3..900a7c0 100755 (executable)
--- a/src/orientation/orientation_sensor.cpp
+++ b/src/orientation/orientation_sensor.cpp
@@ -339,7 +339,7 @@ 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);
+               euler_orientation = m_orientation_filter.get_orientation(&m_accel, &m_gyro, &m_magnetic);
 
                if(m_raw_data_unit == "DEGREES") {
                        euler_orientation = rad2deg(euler_orientation);
@@ -400,7 +400,7 @@ 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);
+       euler_orientation = m_orientation_filter_poll.get_orientation(&m_accel, &m_gyro, &m_magnetic);
 
        if(m_raw_data_unit == "DEGREES") {
                euler_orientation = rad2deg(euler_orientation);

diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp
index 03e50d6..298e9aa 100644 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -420,9 +420,6 @@ euler_angles<TYPE> orientation_filter<TYPE>::get_device_rotation(const sensor_da
 {
        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();
 
@@ -441,24 +438,10 @@ euler_angles<TYPE> orientation_filter<TYPE>::get_device_rotation(const sensor_da
 }
 
 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)
+euler_angles<TYPE> orientation_filter<TYPE>::get_orientation(const sensor_data<TYPE> *accel,
+               const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic)
 {
-       euler_angles<TYPE> cor_euler_ang;
-
-       init_accel_gyro_mag_data(accel, gyro, magnetic);
-
-       normalize(m_accel);
-       m_gyro.m_data = m_gyro.m_data * (TYPE) PI;
-       normalize(m_magnetic);
-
-       orientation_triad_algorithm();
-
-       compute_covariance();
-
-       time_update();
-
-       measurement_update();
+       get_device_rotation(accel, gyro, magnetic);
 
        return m_orientation;
 }
@@ -477,7 +460,7 @@ quaternion<TYPE> orientation_filter<TYPE>::get_9axis_quaternion(const sensor_dat
                const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
 {
 
-       get_orientation(accel, gyro, magnetic);
+       get_orientation(&accel, &gyro, &magnetic);
 
        return m_quat_9axis;
 }

diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h
index 97d372f..08add8a 100644 (file)
--- a/src/sensor_fusion/orientation_filter.h
+++ b/src/sensor_fusion/orientation_filter.h
@@ -89,8 +89,8 @@ 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);
+       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,

diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp
index 30c74c7..57b6393 100644 (file)
--- a/src/sensor_fusion/test/orientation_sensor.cpp
+++ b/src/sensor_fusion/test/orientation_sensor.cpp
@@ -43,15 +43,15 @@ 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,
-               sensor_data<float> gyro_data, sensor_data<float> magnetic_data)
+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);
-       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);
+       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;

diff --git a/src/sensor_fusion/test/orientation_sensor.h b/src/sensor_fusion/test/orientation_sensor.h
index 5afbff4..dab0677 100644 (file)
--- a/src/sensor_fusion/test/orientation_sensor.h
+++ b/src/sensor_fusion/test/orientation_sensor.h
@@ -27,8 +27,8 @@ class orientation_sensor
 public:
        orientation_filter<float> orien_filter;
 
-       euler_angles<float> get_orientation(sensor_data<float> accel,
-                       sensor_data<float> gyro, sensor_data<float> magnetic);
+       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);
        quaternion<float> get_9axis_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 8e6830b..a4e7da1 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
@@ -79,7 +79,7 @@ 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);
+               orientation = orien_sensor1.get_orientation(&accel_data, &gyro_data, &magnetic_data);
 
                orien_file << orientation.m_ang;