Improve accuracy of orientation sensor calculation

-limit dT value range in rotation vector calculation
-fix timestamp reversal
-check max interval of sensor

Change-Id: I558a4af25aacbf903698244c57e34ffd1d5562e8
Signed-off-by: TaeminYeom <taemin.yeom@samsung.com>
(cherry picked from commit 1b6a103386ddae9f40dc084c31c0b6864ac2e3a8)

diff --git a/include/sensor_types.h b/include/sensor_types.h
index 6c6315e..c61dbf1 100644 (file)
--- a/include/sensor_types.h
+++ b/include/sensor_types.h
@@ -200,6 +200,7 @@ typedef struct sensor_info2_t {
        float max_range;
        float resolution;
        int min_interval;
+       int max_interval;
        int max_batch_count;
        bool wakeup_supported;
        const char *privilege;

diff --git a/src/fusion-sensor/orientation/gyro_orientation_sensor.cpp b/src/fusion-sensor/orientation/gyro_orientation_sensor.cpp
index 9200b7f..c3f0f88 100644 (file)
--- a/src/fusion-sensor/orientation/gyro_orientation_sensor.cpp
+++ b/src/fusion-sensor/orientation/gyro_orientation_sensor.cpp
@@ -23,10 +23,12 @@
 #include <sensor_types.h>
 #include <fusion_util.h>
 
-#define NAME_SENSOR "http://tizen.org/sensor/general/gyroscope_orientation/tizen_default"
-#define NAME_VENDOR "tizen.org"
-#define SRC_ID_RV   0x1
-#define SRC_STR_RV  "http://tizen.org/sensor/general/gyroscope_rotation_vector"
+#define NAME_SENSOR       "http://tizen.org/sensor/general/gyroscope_orientation/tizen_default"
+#define NAME_VENDOR       "tizen.org"
+#define SRC_ID_RV         0x1
+#define SRC_STR_RV        "http://tizen.org/sensor/general/gyroscope_rotation_vector"
+
+#define GYRO_MAX_INTERVAL 50
 
 static sensor_info2_t sensor_info = {
        id: 0x1,
@@ -37,6 +39,7 @@ static sensor_info2_t sensor_info = {
        max_range: 360,
        resolution: 0.01,
        min_interval: 10,
+       max_interval: (int) (GYRO_MAX_INTERVAL * 0.8),
        max_batch_count: 0,
        wakeup_supported: false,
        privilege:"",

diff --git a/src/fusion-sensor/orientation/magnetic_orientation_sensor.cpp b/src/fusion-sensor/orientation/magnetic_orientation_sensor.cpp
index 87522d5..328a67b 100644 (file)
--- a/src/fusion-sensor/orientation/magnetic_orientation_sensor.cpp
+++ b/src/fusion-sensor/orientation/magnetic_orientation_sensor.cpp
@@ -23,11 +23,13 @@
 #include <sensor_types.h>
 #include <fusion_util.h>
 
-#define NAME_SENSOR "http://tizen.org/sensor/general/geomagnetic_orientation/tizen_default"
-#define NAME_VENDOR "tizen.org"
+#define NAME_SENSOR      "http://tizen.org/sensor/general/geomagnetic_orientation/tizen_default"
+#define NAME_VENDOR      "tizen.org"
 
-#define SRC_ID_RV   0x1
-#define SRC_STR_RV  "http://tizen.org/sensor/general/geomagnetic_rotation_vector"
+#define SRC_ID_RV        0x1
+#define SRC_STR_RV       "http://tizen.org/sensor/general/geomagnetic_rotation_vector"
+
+#define MAG_MAX_INTERVAL 100
 
 static sensor_info2_t sensor_info = {
        id: 0x1,
@@ -38,6 +40,7 @@ static sensor_info2_t sensor_info = {
        max_range: 360,
        resolution: 0.01,
        min_interval: 10,
+       max_interval: (int) (MAG_MAX_INTERVAL * 0.8),
        max_batch_count: 0,
        wakeup_supported: false,
        privilege:"",

diff --git a/src/fusion-sensor/orientation/orientation_sensor.cpp b/src/fusion-sensor/orientation/orientation_sensor.cpp
index cf14ae9..30f0e10 100644 (file)
--- a/src/fusion-sensor/orientation/orientation_sensor.cpp
+++ b/src/fusion-sensor/orientation/orientation_sensor.cpp
@@ -23,11 +23,13 @@
 #include <sensor_types.h>
 #include <fusion_util.h>
 
-#define NAME_SENSOR "http://tizen.org/sensor/general/orientation/tizen_default"
-#define NAME_VENDOR "tizen.org"
+#define NAME_SENSOR       "http://tizen.org/sensor/general/orientation/tizen_default"
+#define NAME_VENDOR       "tizen.org"
 
-#define SRC_ID_RV   0x1
-#define SRC_STR_RV  "http://tizen.org/sensor/general/rotation_vector"
+#define SRC_ID_RV         0x1
+#define SRC_STR_RV        "http://tizen.org/sensor/general/rotation_vector"
+
+#define GYRO_MAX_INTERVAL 50
 
 static sensor_info2_t sensor_info = {
        id: 0x1,
@@ -38,6 +40,7 @@ static sensor_info2_t sensor_info = {
        max_range: 360,
        resolution: 0.01,
        min_interval: 10,
+       max_interval: (int) (GYRO_MAX_INTERVAL * 0.8),
        max_batch_count: 0,
        wakeup_supported: false,
        privilege:"",

diff --git a/src/fusion-sensor/rotation_vector/fusion_base.cpp b/src/fusion-sensor/rotation_vector/fusion_base.cpp
index dab8fe5..8ae0c97 100644 (file)
--- a/src/fusion-sensor/rotation_vector/fusion_base.cpp
+++ b/src/fusion-sensor/rotation_vector/fusion_base.cpp
@@ -28,6 +28,10 @@
 const float RAD2DEG = 57.29577951;
 const float US2S = 1000000.0f;
 
+const float ACCEL_MAX_INTERVAL = 0.1f; // 100ms
+const float GYRO_MAX_INTERVAL = 0.05f; // 50ms
+const float MAG_MAX_INTERVAL = 0.1f;   // 100ms
+
 fusion_base::fusion_base()
 : m_enable_accel(false)
 , m_enable_gyro(false)
@@ -61,10 +65,13 @@ void fusion_base::push_accel(sensor_data_t &data)
 
        float dT = (data.timestamp - m_timestamp_accel) / US2S;
        m_timestamp_accel = data.timestamp;
-       m_timestamp = data.timestamp;
+       if (m_timestamp < data.timestamp)
+               m_timestamp = data.timestamp;
 
        m_enable_accel = true;
-       m_orientation_filter.handleAcc(v, dT);
+
+       if (0 < dT && dT < ACCEL_MAX_INTERVAL)
+               m_orientation_filter.handleAcc(v, dT);
        store_orientation();
 }
 
@@ -78,10 +85,13 @@ void fusion_base::push_gyro(sensor_data_t &data)
 
        float dT = (data.timestamp - m_timestamp_gyro) / US2S;
        m_timestamp_gyro = data.timestamp;
-       m_timestamp = data.timestamp;
+       if (m_timestamp < data.timestamp)
+               m_timestamp = data.timestamp;
 
        m_enable_gyro = true;
-       m_orientation_filter.handleGyro(v, dT);
+
+       if (0 < dT && dT < GYRO_MAX_INTERVAL)
+               m_orientation_filter.handleGyro(v, dT);
        store_orientation();
 }
 
@@ -90,11 +100,15 @@ void fusion_base::push_mag(sensor_data_t &data)
        //_I("[fusion_sensor] : Pushing mag");
        android::vec3_t v(data.values);
 
+       float dT = (data.timestamp - m_timestamp_mag) / US2S;
        m_timestamp_mag = data.timestamp;
-       m_timestamp = data.timestamp;
+       if (m_timestamp < data.timestamp)
+               m_timestamp = data.timestamp;
 
        m_enable_magnetic = true;
-       m_orientation_filter.handleMag(v);
+
+       if (0 < dT && dT < MAG_MAX_INTERVAL)
+               m_orientation_filter.handleMag(v);
        store_orientation();
 }
 

diff --git a/src/fusion-sensor/rotation_vector/gyro_rv_sensor.cpp b/src/fusion-sensor/rotation_vector/gyro_rv_sensor.cpp
index d64d279..9f280ad 100644 (file)
--- a/src/fusion-sensor/rotation_vector/gyro_rv_sensor.cpp
+++ b/src/fusion-sensor/rotation_vector/gyro_rv_sensor.cpp
@@ -22,14 +22,16 @@
 #include <sensor_log.h>
 #include <sensor_types.h>
 
-#define NAME_SENSOR  "http://tizen.org/sensor/general/gyroscope_rotation_vector/tizen_default"
-#define NAME_VENDOR  "tizen.org"
+#define NAME_SENSOR       "http://tizen.org/sensor/general/gyroscope_rotation_vector/tizen_default"
+#define NAME_VENDOR       "tizen.org"
 
-#define SRC_ID_ACC   0x1
-#define SRC_STR_ACC  "http://tizen.org/sensor/general/accelerometer"
+#define SRC_ID_ACC        0x1
+#define SRC_STR_ACC       "http://tizen.org/sensor/general/accelerometer"
 
-#define SRC_ID_GYRO  0x2
-#define SRC_STR_GYRO "http://tizen.org/sensor/general/gyroscope"
+#define SRC_ID_GYRO       0x2
+#define SRC_STR_GYRO      "http://tizen.org/sensor/general/gyroscope"
+
+#define GYRO_MAX_INTERVAL 50
 
 static sensor_info2_t sensor_info = {
        id: 0x1,
@@ -40,6 +42,7 @@ static sensor_info2_t sensor_info = {
        max_range: 1,
        resolution: 1,
        min_interval: 10,
+       max_interval: (int) (GYRO_MAX_INTERVAL * 0.8),
        max_batch_count: 0,
        wakeup_supported: false,
        privilege:"",

diff --git a/src/fusion-sensor/rotation_vector/magnetic_rv_sensor.cpp b/src/fusion-sensor/rotation_vector/magnetic_rv_sensor.cpp
index 6c4606a..4512724 100644 (file)
--- a/src/fusion-sensor/rotation_vector/magnetic_rv_sensor.cpp
+++ b/src/fusion-sensor/rotation_vector/magnetic_rv_sensor.cpp
@@ -22,14 +22,16 @@
 #include <sensor_log.h>
 #include <sensor_types.h>
 
-#define NAME_SENSOR  "http://tizen.org/sensor/general/geomagnetic_rotation_vector/tizen_default"
-#define NAME_VENDOR  "tizen.org"
+#define NAME_SENSOR      "http://tizen.org/sensor/general/geomagnetic_rotation_vector/tizen_default"
+#define NAME_VENDOR      "tizen.org"
 
-#define SRC_ID_ACC   0x1
-#define SRC_STR_ACC  "http://tizen.org/sensor/general/accelerometer"
+#define SRC_ID_ACC       0x1
+#define SRC_STR_ACC      "http://tizen.org/sensor/general/accelerometer"
 
-#define SRC_ID_MAG   0x3
-#define SRC_STR_MAG  "http://tizen.org/sensor/general/magnetic"
+#define SRC_ID_MAG       0x3
+#define SRC_STR_MAG      "http://tizen.org/sensor/general/magnetic"
+
+#define MAG_MAX_INTERVAL 100
 
 static sensor_info2_t sensor_info = {
        id: 0x1,
@@ -40,6 +42,7 @@ static sensor_info2_t sensor_info = {
        max_range: 1,
        resolution: 1,
        min_interval: 10,
+       max_interval: (int) (MAG_MAX_INTERVAL * 0.8),
        max_batch_count: 0,
        wakeup_supported: false,
        privilege:"",

diff --git a/src/fusion-sensor/rotation_vector/rv_sensor.cpp b/src/fusion-sensor/rotation_vector/rv_sensor.cpp
index a7a6049..8f4007e 100644 (file)
--- a/src/fusion-sensor/rotation_vector/rv_sensor.cpp
+++ b/src/fusion-sensor/rotation_vector/rv_sensor.cpp
@@ -34,6 +34,8 @@
 #define SRC_ID_MAG   0x3
 #define SRC_STR_MAG  "http://tizen.org/sensor/general/magnetic"
 
+#define GYRO_MAX_INTERVAL 50
+
 static sensor_info2_t sensor_info = {
        id: 0x1,
        type: ROTATION_VECTOR_SENSOR,
@@ -43,6 +45,7 @@ static sensor_info2_t sensor_info = {
        max_range: 1,
        resolution: 1,
        min_interval: 10,
+       max_interval: (int) (GYRO_MAX_INTERVAL * 0.8),
        max_batch_count: 0,
        wakeup_supported: false,
        privilege:"",

diff --git a/src/server/fusion_sensor_handler.cpp b/src/server/fusion_sensor_handler.cpp
index 2e3c683..1336a09 100644 (file)
--- a/src/server/fusion_sensor_handler.cpp
+++ b/src/server/fusion_sensor_handler.cpp
@@ -24,6 +24,8 @@
 #include <sensor_log.h>
 #include <algorithm>
 
+const int MAX_INTERVAL = 255000;
+
 using namespace sensor;
 
 fusion_sensor_handler::fusion_sensor_handler(const sensor_info &info,
@@ -124,17 +126,33 @@ int fusion_sensor_handler::get_min_interval(void)
 
        interval = *std::min_element(temp.begin(), temp.end());
 
-       if (interval < m_info.get_min_interval())
+       if (interval > m_info.get_min_interval())
                return m_info.get_min_interval();
 
        return interval;
 }
 
+int fusion_sensor_handler::get_max_interval(void)
+{
+       int ret = m_info.get_max_interval();
+       if (ret)
+               return ret;
+       return MAX_INTERVAL;
+}
+
 int fusion_sensor_handler::set_interval(sensor_observer *ob, int32_t interval)
 {
        retv_if(!m_sensor, -EINVAL);
 
        int _interval = interval;
+       int max_interval = get_max_interval();
+       int min_interval = get_min_interval();
+
+       if (max_interval > 0 && _interval > max_interval)
+               _interval = max_interval;
+       else if (min_interval > 0 && _interval < min_interval)
+               _interval = min_interval;
+
        int policy = OP_DEFAULT;
 
        policy = m_sensor->set_interval(ob, _interval);
@@ -142,9 +160,6 @@ int fusion_sensor_handler::set_interval(sensor_observer *ob, int32_t interval)
 
        m_interval_map[ob] = _interval;
 
-       if (policy == OP_DEFAULT)
-               _interval = get_min_interval();
-
        update_prev_interval(_interval);
 
        return set_interval_internal(_interval);

diff --git a/src/server/fusion_sensor_handler.h b/src/server/fusion_sensor_handler.h
index 9a7e418..5741114 100644 (file)
--- a/src/server/fusion_sensor_handler.h
+++ b/src/server/fusion_sensor_handler.h
@@ -79,6 +79,7 @@ private:
        int set_attribute_internal(int32_t attr, const char *value, int len);
 
        int get_min_interval(void);
+       int get_max_interval(void);
        int get_min_batch_latency(void);
 
        fusion_sensor *m_sensor;

diff --git a/src/server/physical_sensor_handler.cpp b/src/server/physical_sensor_handler.cpp
index 6808c6b..9799b68 100644 (file)
--- a/src/server/physical_sensor_handler.cpp
+++ b/src/server/physical_sensor_handler.cpp
@@ -24,6 +24,8 @@
 #include <message.h>
 #include <algorithm>
 
+const int MAX_INTERVAL = 255000;
+
 using namespace sensor;
 
 physical_sensor_handler::physical_sensor_handler(const sensor_info &info,
@@ -149,35 +151,48 @@ int physical_sensor_handler::get_min_interval(void)
 
        interval = *std::min_element(temp.begin(), temp.end());
 
-       if (interval < m_info.get_min_interval())
+       if (interval > m_info.get_min_interval())
                return m_info.get_min_interval();
 
        return interval;
 }
 
+int physical_sensor_handler::get_max_interval(void)
+{
+       int ret = m_info.get_max_interval();
+       if (ret)
+               return ret;
+       return MAX_INTERVAL;
+}
+
 int physical_sensor_handler::set_interval(sensor_observer *ob, int32_t interval)
 {
        retv_if(!m_device, -EINVAL);
 
-       bool ret = false;
-       int32_t cur_interval = interval;
+       int _interval = interval;
+       int max_interval = get_max_interval();
+       int min_interval = get_min_interval();
+
+       if (_interval > max_interval)
+               _interval = max_interval;
+       else if (_interval < min_interval)
+               _interval = min_interval;
+
        int policy = OP_DEFAULT;
 
        if (m_sensor) {
-               policy = m_sensor->set_interval(ob, cur_interval);
+               policy = m_sensor->set_interval(ob, _interval);
                retv_if(policy <= OP_ERROR, policy);
        }
 
-       m_interval_map[ob] = cur_interval;
+       m_interval_map[ob] = _interval;
 
-       if (policy == OP_DEFAULT)
-               cur_interval = get_min_interval();
+       retv_if(m_prev_interval == _interval, OP_SUCCESS);
 
-       retv_if(m_prev_interval == cur_interval, OP_SUCCESS);
-
-       ret = m_device->set_interval(m_hal_id, cur_interval);
+       bool ret;
+       ret = m_device->set_interval(m_hal_id, _interval);
 
-       update_prev_interval(cur_interval);
+       update_prev_interval(_interval);
 
        return (ret ? OP_SUCCESS : OP_ERROR);
 }

diff --git a/src/server/physical_sensor_handler.h b/src/server/physical_sensor_handler.h
index 6743604..fa02c66 100644 (file)
--- a/src/server/physical_sensor_handler.h
+++ b/src/server/physical_sensor_handler.h
@@ -64,6 +64,7 @@ public:
 
 private:
        int get_min_interval(void);
+       int get_max_interval(void);
        int get_min_batch_latency(void);
 
        sensor_device *m_device;

diff --git a/src/shared/sensor_info.cpp b/src/shared/sensor_info.cpp
index 5a87539..f060083 100644 (file)
--- a/src/shared/sensor_info.cpp
+++ b/src/shared/sensor_info.cpp
@@ -43,6 +43,7 @@ sensor_info::sensor_info()
 , m_max_range(0)
 , m_resolution(0)
 , m_min_interval(0)
+, m_max_interval(0)
 , m_max_batch_count(0)
 , m_wakeup_supported(false)
 , m_privilege("")
@@ -58,6 +59,7 @@ sensor_info::sensor_info(const sensor_info &info)
 , m_max_range(info.m_max_range)
 , m_resolution(info.m_resolution)
 , m_min_interval(info.m_min_interval)
+, m_max_interval(info.m_max_interval)
 , m_max_batch_count(info.m_max_batch_count)
 , m_wakeup_supported(info.m_wakeup_supported)
 , m_privilege(info.m_privilege)
@@ -79,6 +81,7 @@ sensor_info::sensor_info(const sensor_info_t &info)
        set_max_range(info.max_range);
        set_resolution(info.resolution);
        set_min_interval(info.min_interval);
+       set_max_interval(info.max_interval);
        set_max_batch_count(info.max_batch_count);
        set_wakeup_supported(info.wakeup_supported);
        /* TODO: sensor_info_t should have privilege string */
@@ -98,6 +101,7 @@ sensor_info::sensor_info(const sensor_info2_t &info)
        set_max_range(info.max_range);
        set_resolution(info.resolution);
        set_min_interval(info.min_interval);
+       set_max_interval(info.max_interval);
        set_max_batch_count(info.max_batch_count);
        set_wakeup_supported(info.wakeup_supported);
        set_privilege(info.privilege);
@@ -143,6 +147,11 @@ int sensor_info::get_min_interval(void)
        return m_min_interval;
 }
 
+int sensor_info::get_max_interval(void)
+{
+       return m_max_interval;
+}
+
 int sensor_info::get_max_batch_count(void)
 {
        return m_max_batch_count;
@@ -212,6 +221,11 @@ void sensor_info::set_min_interval(int min_interval)
        m_min_interval = min_interval;
 }
 
+void sensor_info::set_max_interval(int max_interval)
+{
+       m_max_interval = max_interval;
+}
+
 void sensor_info::set_max_batch_count(int max_batch_count)
 {
        m_max_batch_count = max_batch_count;
@@ -244,6 +258,7 @@ void sensor_info::serialize(raw_data_t &data)
        put(data, m_max_range);
        put(data, m_resolution);
        put(data, m_min_interval);
+       //put(data, m_max_interval); to do
        put(data, m_max_batch_count);
        put(data, m_wakeup_supported);
        put(data, m_privilege);
@@ -265,6 +280,7 @@ void sensor_info::deserialize(const char *data, int data_len)
        it = get(it, m_max_range);
        it = get(it, m_resolution);
        it = get(it, m_min_interval);
+       //it = get(it, m_max_interval); to do
        it = get(it, m_max_batch_count);
        it = get(it, m_wakeup_supported);
        it = get(it, m_privilege);
@@ -294,6 +310,7 @@ void sensor_info::clear(void)
        m_max_range = 0.0f;
        m_resolution = 0.0f;
        m_min_interval = 0;
+       m_max_interval = 0;
        m_max_batch_count = 0;
        m_wakeup_supported = false;
        m_privilege.clear();

diff --git a/src/shared/sensor_info.h b/src/shared/sensor_info.h
index 5a1f791..cbd5406 100644 (file)
--- a/src/shared/sensor_info.h
+++ b/src/shared/sensor_info.h
@@ -48,6 +48,7 @@ public:
        float get_max_range(void);
        float get_resolution(void);
        int get_min_interval(void);
+       int get_max_interval(void);
        int get_max_batch_count(void);
        bool is_wakeup_supported(void);
        std::string &get_privilege(void);
@@ -60,6 +61,7 @@ public:
        void set_max_range(float max_range);
        void set_resolution(float resolution);
        void set_min_interval(int min_interval);
+       void set_max_interval(int max_interval);
        void set_max_batch_count(int max_batch_count);
        void set_wakeup_supported(bool supported);
        void set_privilege(const char *privilege);
@@ -80,6 +82,7 @@ private:
        float m_max_range;
        float m_resolution;
        int m_min_interval;
+       int m_max_interval;
        int m_max_batch_count;
        bool m_wakeup_supported;
        std::string m_privilege;