Adding lock in virtual sensor layer for sensor fusion library

- Adding mutex lock in virtual sensor layer to restrict usage and
access to sensor fusion library.
- adding lock for event driven and polling based calls

Change-Id: I2202d046544de4d8474606179a516a5e38b77548

diff --git a/src/orientation/orientation_sensor.cpp b/src/orientation/orientation_sensor.cpp
index 7653ef3..02afcd1 100755 (executable)
--- a/src/orientation/orientation_sensor.cpp
+++ b/src/orientation/orientation_sensor.cpp
@@ -343,7 +343,10 @@ void orientation_sensor::synthesize(const sensor_event_t &event, vector<sensor_e
                m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
                m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 
-               euler_orientation = m_orientation.get_orientation(m_accel, m_gyro, m_magnetic);
+               {
+                       AUTOLOCK(m_fusion_mutex);
+                       euler_orientation = m_orientation.get_orientation(m_accel, m_gyro, m_magnetic);
+               }
 
                if(m_raw_data_unit == "DEGREES") {
                        euler_orientation = rad2deg(euler_orientation);
@@ -403,7 +406,10 @@ int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_da
        m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
        m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
 
-       euler_orientation = m_orientation.get_orientation(accel, gyro, magnetic);
+       {
+               AUTOLOCK(m_fusion_mutex);
+               euler_orientation = m_orientation.get_orientation(m_accel, m_gyro, m_magnetic);
+       }
 
        if(m_raw_data_unit == "DEGREES") {
                euler_orientation = rad2deg(euler_orientation);

diff --git a/src/rotation_vector/rv/rv_sensor.cpp b/src/rotation_vector/rv/rv_sensor.cpp
index cee3a94..e37ff33 100755 (executable)
--- a/src/rotation_vector/rv/rv_sensor.cpp
+++ b/src/rotation_vector/rv/rv_sensor.cpp
@@ -341,18 +341,44 @@ void rv_sensor::synthesize(const sensor_event_t& event, vector<sensor_event_t> &
 
 int rv_sensor::get_sensor_data(unsigned int data_id, sensor_data_t &data)
 {
-       if (data_id != ROTATION_VECTOR_BASE_DATA_SET)
-               return -1;
+       sensor_data<float> accel;
+       sensor_data<float> gyro;
+       sensor_data<float> magnetic;
 
-       data.accuracy = SENSOR_ACCURACY_GOOD;
+       sensor_data_t accel_data;
+       sensor_data_t gyro_data;
+       sensor_data_t magnetic_data;
+
+       quaternion<float> quaternion_orientation;
 
-       AUTOLOCK(m_value_mutex);
+       m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data);
+       m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data);
+       m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data);
+
+       pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
+       pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
+       pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
+       accel.m_time_stamp = accel_data.timestamp;
+       gyro.m_time_stamp = gyro_data.timestamp;
+       magnetic.m_time_stamp = magnetic_data.timestamp;
+
+       m_orientation.m_pitch_phase_compensation = m_pitch_rotation_compensation;
+       m_orientation.m_roll_phase_compensation = m_roll_rotation_compensation;
+       m_orientation.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
+       m_orientation.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
+
+       {
+               AUTOLOCK(m_fusion_mutex);
+               quaternion_orientation = m_orientation.get_quaternion(m_accel, m_gyro, m_magnetic);
+       }
+
+       data.accuracy = SENSOR_ACCURACY_GOOD;
        data.timestamp = m_time;
-       data.values[0] = m_x;
-       data.values[1] = m_y;
-       data.values[2] = m_z;
-       data.values[3] = m_w;
        data.value_count = 4;
+       data.values[0] = quaternion_orientation.m_quat.m_vec[1];
+       data.values[1] = quaternion_orientation.m_quat.m_vec[2];
+       data.values[2] = quaternion_orientation.m_quat.m_vec[3];
+       data.values[3] = quaternion_orientation.m_quat.m_vec[0];
 
        return 0;
 }

diff --git a/src/shared/virtual_sensor.h b/src/shared/virtual_sensor.h
index d592ce6..3cb79fb 100755 (executable)
--- a/src/shared/virtual_sensor.h
+++ b/src/shared/virtual_sensor.h
@@ -33,6 +33,8 @@ public:
        bool is_virtual(void);
 
 protected:
+       cmutex m_fusion_mutex;
+
        bool activate(void);
        bool deactivate(void);