From: TaeminYeom Date: Fri, 7 Jan 2022 03:56:33 +0000 (+0900) Subject: Improve accuracy of orientation sensor calculation X-Git-Tag: submit/tizen/20220127.011830~3 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=1b6a103386ddae9f40dc084c31c0b6864ac2e3a8;p=platform%2Fcore%2Fsystem%2Fsensord.git 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 --- diff --git a/include/sensor_types.h b/include/sensor_types.h index 6c6315ef..c61dbf1d 100644 --- 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 9200b7f3..c3f0f88d 100644 --- a/src/fusion-sensor/orientation/gyro_orientation_sensor.cpp +++ b/src/fusion-sensor/orientation/gyro_orientation_sensor.cpp @@ -23,10 +23,12 @@ #include #include -#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 87522d57..328a67b5 100644 --- a/src/fusion-sensor/orientation/magnetic_orientation_sensor.cpp +++ b/src/fusion-sensor/orientation/magnetic_orientation_sensor.cpp @@ -23,11 +23,13 @@ #include #include -#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 cf14ae9c..30f0e10c 100644 --- a/src/fusion-sensor/orientation/orientation_sensor.cpp +++ b/src/fusion-sensor/orientation/orientation_sensor.cpp @@ -23,11 +23,13 @@ #include #include -#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 dab8fe57..8ae0c97f 100644 --- 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 d64d279d..9f280ad3 100644 --- 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 #include -#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 6c4606a9..45127243 100644 --- 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 #include -#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 a7a6049a..8f4007e7 100644 --- 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 2e3c6837..1336a09b 100644 --- a/src/server/fusion_sensor_handler.cpp +++ b/src/server/fusion_sensor_handler.cpp @@ -24,6 +24,8 @@ #include #include +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 9a7e4187..57411148 100644 --- 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 6808c6b2..9799b688 100644 --- a/src/server/physical_sensor_handler.cpp +++ b/src/server/physical_sensor_handler.cpp @@ -24,6 +24,8 @@ #include #include +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 67436040..fa02c660 100644 --- 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 5a875397..f0600834 100644 --- 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 5a1f791c..cbd54066 100644 --- 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;