From f5c62e1096393da4ea88a603705a22e4e77a09a0 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 2 Mar 2015 17:26:40 +0530 Subject: [PATCH] Changing orientation sensor to follow single sensor fusion flow - Updating orientation sensor to work on common single sensor fusion code flow. Change-Id: I5581e833e33757a0e24d56a90717473bded3e172 --- src/orientation/orientation_sensor.cpp | 4 ++-- src/sensor_fusion/orientation_filter.cpp | 25 ++++------------------ src/sensor_fusion/orientation_filter.h | 4 ++-- src/sensor_fusion/test/orientation_sensor.cpp | 14 ++++++------ src/sensor_fusion/test/orientation_sensor.h | 4 ++-- .../orientation_sensor_main.cpp | 2 +- 6 files changed, 18 insertions(+), 35 deletions(-) diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp index 0c8bed3..900a7c0 100755 --- 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 orientation_filter::get_device_rotation(const sensor_da { initialize_sensor_data(accel, gyro, magnetic); - const sensor_data accel_in, gyro_in, magnetic_in; - euler_angles cor_euler_ang; - if (magnetic != NULL) orientation_triad_algorithm(); @@ -441,24 +438,10 @@ euler_angles orientation_filter::get_device_rotation(const sensor_da } template -euler_angles orientation_filter::get_orientation(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic) +euler_angles orientation_filter::get_orientation(const sensor_data *accel, + const sensor_data *gyro, const sensor_data *magnetic) { - euler_angles 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 orientation_filter::get_9axis_quaternion(const sensor_dat const sensor_data gyro, const sensor_data 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 --- 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 get_orientation(const sensor_data accel, - const sensor_data gyro, const sensor_data magnetic); + euler_angles get_orientation(const sensor_data *accel, + const sensor_data *gyro, const sensor_data *magnetic); rotation_matrix get_rotation_matrix(const sensor_data *accel, const sensor_data *gyro, const sensor_data *magnetic); quaternion get_9axis_quaternion(const sensor_data accel, diff --git a/src/sensor_fusion/test/orientation_sensor.cpp b/src/sensor_fusion/test/orientation_sensor.cpp index 30c74c7..57b6393 100644 --- 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 *data_out, sensor_data *data_in, data_out->m_time_stamp = data_in->m_time_stamp; } -euler_angles orientation_sensor::get_orientation(sensor_data accel_data, - sensor_data gyro_data, sensor_data magnetic_data) +euler_angles orientation_sensor::get_orientation(sensor_data *accel_data, + sensor_data *gyro_data, sensor_data *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 --- 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 orien_filter; - euler_angles get_orientation(sensor_data accel, - sensor_data gyro, sensor_data magnetic); + euler_angles get_orientation(sensor_data *accel, + sensor_data *gyro, sensor_data *magnetic); rotation_matrix get_rotation_matrix(sensor_data *accel, sensor_data *gyro, sensor_data *magnetic); quaternion get_9axis_quaternion(sensor_data 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 --- 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; -- 2.7.4