From ffddb3be7c4e0fb0935a8a52e0bd9f0f8736a3be Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 16 Aug 2017 10:52:20 +0900 Subject: [PATCH 01/16] sensorctl: apply ASLR to sensorctl Change-Id: I92770df7a1e80753b618a380f775385d47763571 Signed-off-by: kibak.yoon --- src/sensorctl/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/sensorctl/CMakeLists.txt b/src/sensorctl/CMakeLists.txt index 7b44303..cfbc071 100644 --- a/src/sensorctl/CMakeLists.txt +++ b/src/sensorctl/CMakeLists.txt @@ -19,7 +19,8 @@ FOREACH(flag ${PKGS_CFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") ENDFOREACH(flag) -SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${EXTRA_CFLAGS}") +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${EXTRA_CFLAGS} -fPIE") +SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie") # Installing files FILE(GLOB_RECURSE SRCS *.cpp) -- 2.7.4 From 7c79a93dbf8d32d91820ea00aaf2640cc2708544 Mon Sep 17 00:00:00 2001 From: Marcin Masternak Date: Fri, 8 Sep 2017 20:16:22 +0200 Subject: [PATCH 02/16] sensord: samsung pedometer sensor for fused location fix/update. Change-Id: I007bf4564c426e897ab273824231782fa65054ee Signed-off-by: Marcin Masternak --- src/sensor/pedometer/pedometer.cpp | 46 +++++++----- src/sensor/pedometer/pedometer.h | 8 ++- src/sensor/pedometer/pedometer_sensor.cpp | 6 +- .../pedometer/sensor_frequency_compensator.cpp | 83 ++++++++++++++++++++++ .../pedometer/sensor_frequency_compensator.h | 68 ++++++++++++++++++ src/sensor/pedometer/step_detection.cpp | 14 ++-- 6 files changed, 199 insertions(+), 26 deletions(-) create mode 100644 src/sensor/pedometer/sensor_frequency_compensator.cpp create mode 100644 src/sensor/pedometer/sensor_frequency_compensator.h diff --git a/src/sensor/pedometer/pedometer.cpp b/src/sensor/pedometer/pedometer.cpp index b642746..f076c16 100644 --- a/src/sensor/pedometer/pedometer.cpp +++ b/src/sensor/pedometer/pedometer.cpp @@ -19,12 +19,16 @@ #include +/** Desired sensors rate. Currently 33ms. */ +#define DESIRED_RATE ((1000 / 30) * 1000000) + pedometer::pedometer() : m_step_detection() , m_total_length(0) , m_step_count(0) , m_pedometer_filter() , m_some_speed(false) +, m_acceleration_compensator(DESIRED_RATE) { } @@ -44,30 +48,40 @@ void pedometer::reset(void) m_step_detection.reset(); m_pedometer_filter.reset(); m_some_speed = false; + m_acceleration_compensator.reset(); } -bool pedometer::get_pedometer(timestamp_t timestamp, double acc, pedometer_info *info) +bool pedometer::get_pedometer(pedometer_info *info, timestamp_t timestamp, double acc[]) { bool result = false; + m_acceleration_compensator.add(timestamp, acc); step_event event; - if (m_step_detection.get_step(timestamp, acc, &event)) { - if (event.m_timestamp != UNKNOWN_TIMESTAMP) { - m_step_count++; - m_total_length += event.m_step_length; - m_pedometer_filter.get_step(timestamp, event.m_step_length); - double speed = m_pedometer_filter.get_speed(timestamp); - info->timestamp = timestamp; - info->is_step_detected = true; - info->step_count = m_step_count; - info->step_length = event.m_step_length; - info->total_step_length = m_total_length; - info->step_speed = speed; - result = true; - m_some_speed = speed != 0; + while (m_acceleration_compensator.has_next()) { + double acceleration[3]; + m_acceleration_compensator.get_next(acceleration); + if (m_step_detection.get_step(timestamp, + sqrt(acceleration[0] * acceleration[0] + + acceleration[1] * acceleration[1] + + acceleration[2] * acceleration[2]), + &event)) { + if (event.m_timestamp != UNKNOWN_TIMESTAMP) { + m_step_count++; + m_total_length += event.m_step_length; + m_pedometer_filter.get_step(timestamp, event.m_step_length); + double speed = m_pedometer_filter.get_speed(timestamp); + info->timestamp = timestamp; + info->is_step_detected = true; + info->step_count = m_step_count; + info->step_length = event.m_step_length; + info->total_step_length = m_total_length; + info->step_speed = speed; + result = true; + m_some_speed = speed != 0; + } } } - if (m_some_speed) { + if ((!result) && m_some_speed) { double speed = m_pedometer_filter.get_speed(timestamp); if (speed == 0) { m_some_speed = false; diff --git a/src/sensor/pedometer/pedometer.h b/src/sensor/pedometer/pedometer.h index 3f904d7..edf3638 100644 --- a/src/sensor/pedometer/pedometer.h +++ b/src/sensor/pedometer/pedometer.h @@ -21,6 +21,7 @@ #include "step_detection.h" #include "pedometer_info.h" #include "pedometer_speed_filter.h" +#include "sensor_frequency_compensator.h" /************************************************************************ * stores pedometer engine state. @@ -48,12 +49,12 @@ public: * @param timestamp * timestamp of acceleration event in ns. * @param acc - * vertical component of global acceleration. + * global acceleration. * * @result * true if new step event was detected. */ - bool get_pedometer(timestamp_t timestamp, double acc, pedometer_info *info); + bool get_pedometer(pedometer_info *info, timestamp_t timestamp, double acc[]); private: /** detects step and estimates step length. */ @@ -70,6 +71,9 @@ private: /** some non zero speed was detected. */ bool m_some_speed; + + sensor_frequency_compensator m_acceleration_compensator; + }; #endif /* __PEDOMETER_H__ */ diff --git a/src/sensor/pedometer/pedometer_sensor.cpp b/src/sensor/pedometer/pedometer_sensor.cpp index d16bdcc..e60ec43 100644 --- a/src/sensor/pedometer/pedometer_sensor.cpp +++ b/src/sensor/pedometer/pedometer_sensor.cpp @@ -28,8 +28,6 @@ #define SRC_ID_ACC 0x1 #define SRC_STR_ACC "http://tizen.org/sensor/general/accelerometer" -#define NORM(x, y, z) sqrt((x)*(x) + (y)*(y) + (z)*(z)) - #define US_TO_NS(x) (x * 1000) /* Sensor information */ @@ -82,9 +80,9 @@ int pedometer_sensor::get_required_sensors(const required_sensor_s **sensors) int pedometer_sensor::update(uint32_t id, sensor_data_t *data, int len) { pedometer_info info; - double acc = NORM(data->values[0], data->values[1], data->values[2]); + double acc[] = {data->values[0], data->values[1], data->values[2]}; - if (!m_pedometer.get_pedometer(US_TO_NS(data->timestamp), acc, &info)) + if (!m_pedometer.get_pedometer(&info, US_TO_NS(data->timestamp), acc)) return OP_ERROR; m_step_count = info.step_count; diff --git a/src/sensor/pedometer/sensor_frequency_compensator.cpp b/src/sensor/pedometer/sensor_frequency_compensator.cpp new file mode 100644 index 0000000..9475bdc --- /dev/null +++ b/src/sensor/pedometer/sensor_frequency_compensator.cpp @@ -0,0 +1,83 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "sensor_frequency_compensator.h" + +#include +#include +#include + +/************************************************************************ + */ +sensor_frequency_compensator::sensor_frequency_compensator(double desired_rate) +: m_desired_frequency(desired_rate) +, m_t1(0) +, m_v1{0.0, 0.0, 0.0} +, m_t2(0) +, m_v2{0.0, 0.0, 0.0} +, m_timestamp(0) +{ +} + +/************************************************************************ + */ +sensor_frequency_compensator::~sensor_frequency_compensator() +{ +} + +/************************************************************************ + */ +void sensor_frequency_compensator::reset() { + m_t1 = UNKNOWN_TIMESTAMP; + m_t2 = UNKNOWN_TIMESTAMP; + m_timestamp = UNKNOWN_TIMESTAMP; +} + +/************************************************************************ + */ +void sensor_frequency_compensator::add(timestamp_t t, double *v) { + if (m_timestamp == UNKNOWN_TIMESTAMP) { + m_timestamp = t; + } + m_t1 = m_t2; + memcpy(m_v1, m_v2, sizeof(m_v1)); + m_t2 = t; + memcpy(m_v2, v, sizeof(m_v2)); +} + +/************************************************************************ + */ +bool sensor_frequency_compensator::has_next() { + if (m_t1 == UNKNOWN_TIMESTAMP || m_t2 == UNKNOWN_TIMESTAMP) { + return false; + } + return m_timestamp + m_desired_frequency < m_t2; +} + +/************************************************************************ + */ +void sensor_frequency_compensator::get_next(double *v) { + timestamp_t t3 = m_timestamp; + if (t3 < m_t1) { + t3 = m_t1; + } + m_timestamp += m_desired_frequency; + double t = (t3 - m_t1) / ((double) (m_t2 - m_t1)); + int i; + for (i = 0; i < 3; i++) { + v[i] = (1.0 - t) * m_v1[i] + t * m_v2[i]; + } +} diff --git a/src/sensor/pedometer/sensor_frequency_compensator.h b/src/sensor/pedometer/sensor_frequency_compensator.h new file mode 100644 index 0000000..664fedb --- /dev/null +++ b/src/sensor/pedometer/sensor_frequency_compensator.h @@ -0,0 +1,68 @@ +/* + * Copyright (c) 2016-2017 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef __SENSOR_FREQUENCY_COMPENSATOR_H_ +#define __SENSOR_FREQUENCY_COMPENSATOR_H_ + +#include "common.h" + +#include + +/************************************************************************ + * Stores frequency compensator state. + */ +class sensor_frequency_compensator { +public: + sensor_frequency_compensator(double desired_rate); + ~sensor_frequency_compensator(); + + /************************************************************************ + * Resets frequency compensator to initial state. + */ + void reset(); + + /************************************************************************ + * Filters input data. + * + * @param value + * Data to filter. + * @result Filtered data. + */ + void add(timestamp_t t, double *value); + + /************************************************************************ + */ + bool has_next(); + + /************************************************************************ + */ + void get_next(double *value); + +private: + long long m_desired_frequency; + + timestamp_t m_t1; + + double m_v1[3]; + + timestamp_t m_t2; + + double m_v2[3]; + + timestamp_t m_timestamp; +}; + +#endif /* __SENSOR_FREQUENCY_COMPENSATOR_H_ */ diff --git a/src/sensor/pedometer/step_detection.cpp b/src/sensor/pedometer/step_detection.cpp index afabc84..ad977f2 100644 --- a/src/sensor/pedometer/step_detection.cpp +++ b/src/sensor/pedometer/step_detection.cpp @@ -189,6 +189,7 @@ bool step_detection::add_acc_sensor_values_savitzky( timestamp_t timestamp, double acc, step_event* step) { double acceleration = m_zc_filter.filter(acc - m_average_gfilter.filter(acc)); + double amplitude = 0; bool n_zero_up = m_zero_crossing_up.detect_step(timestamp, acceleration); bool n_zero_down = m_zero_crossing_down.detect_step(timestamp, acceleration); @@ -217,6 +218,7 @@ bool step_detection::add_acc_sensor_values_savitzky( bool zup = m_zero_crossing_up.m_time_sum / 1E9 > 1.2; if (n_zero_down) { is_step_detected = false; + amplitude = abs(m_maximum_acceleration - m_minimum_acceleration); if ((m_maximum_acceleration > 0.6 && m_minimum_acceleration < -0.852) @@ -251,11 +253,15 @@ bool step_detection::add_acc_sensor_values_savitzky( double time = (timestamp - m_last_step_timestamp) / 1E9; m_last_step_timestamp = timestamp; + if (time > 1.0 || amplitude < 3) { + is_step_detected = false; + } m_zero_crossing_up.m_time_sum = 0; - step->m_timestamp = timestamp; - step->m_step_length = cal_step_length(time, sqrt4peak_valley_diff); - - return true; + if (is_step_detected) { + step->m_timestamp = timestamp; + step->m_step_length = cal_step_length(time, sqrt4peak_valley_diff); + return true; + } } return false; -- 2.7.4 From f3d5bac2ce31553cecd88eb6aff0860241484584 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Mon, 11 Sep 2017 21:37:20 +0900 Subject: [PATCH 03/16] sensord: fix coding rule violations Change-Id: I19591605e855fdbd1b44aae8fed8fe9ded23ee16 Signed-off-by: kibak.yoon --- src/sensor/gesture/face_down_alg_impl.h | 1 - src/sensor/pedometer/pedometer.h | 1 - src/sensor/rotation_vector/fusion_base.cpp | 1 - src/sensor/rotation_vector/fusion_utils/matrix.cpp | 8 +++----- src/sensor/rotation_vector/fusion_utils/vector.cpp | 8 +++----- src/sensorctl/loopback.cpp | 4 ++-- src/sensorctl/test_bench.cpp | 1 - src/sensorctl/testcase/unit_ipc.cpp | 6 +++--- 8 files changed, 11 insertions(+), 19 deletions(-) diff --git a/src/sensor/gesture/face_down_alg_impl.h b/src/sensor/gesture/face_down_alg_impl.h index 4afa3bb..997719d 100644 --- a/src/sensor/gesture/face_down_alg_impl.h +++ b/src/sensor/gesture/face_down_alg_impl.h @@ -38,7 +38,6 @@ private: void remove_old_up_time(void); unsigned long long is_facing_down(); unsigned long long was_facing_up(); - }; #endif /* __FACE_DOWN_ALG_IMPL_H__ */ diff --git a/src/sensor/pedometer/pedometer.h b/src/sensor/pedometer/pedometer.h index edf3638..f80c315 100644 --- a/src/sensor/pedometer/pedometer.h +++ b/src/sensor/pedometer/pedometer.h @@ -73,7 +73,6 @@ private: bool m_some_speed; sensor_frequency_compensator m_acceleration_compensator; - }; #endif /* __PEDOMETER_H__ */ diff --git a/src/sensor/rotation_vector/fusion_base.cpp b/src/sensor/rotation_vector/fusion_base.cpp index ad92d2a..fbd887d 100644 --- a/src/sensor/rotation_vector/fusion_base.cpp +++ b/src/sensor/rotation_vector/fusion_base.cpp @@ -80,7 +80,6 @@ void fusion_base::push_mag(sensor_data_t &data) m_enable_magnetic = true; if (get_orientation()) store_orientation(); - } bool fusion_base::get_rv(unsigned long long ×tamp, float &x, float &y, float &z, float &w) diff --git a/src/sensor/rotation_vector/fusion_utils/matrix.cpp b/src/sensor/rotation_vector/fusion_utils/matrix.cpp index 62f0555..2b1c177 100644 --- a/src/sensor/rotation_vector/fusion_utils/matrix.cpp +++ b/src/sensor/rotation_vector/fusion_utils/matrix.cpp @@ -156,17 +156,15 @@ T_R_C matrix operator /(const matrix m1, const T val) T_R1_C1_R2_C2 bool operator ==(const matrix m1, const matrix m2) { - if ((R1 == R2) && (C1 == C2)) - { + if ((R1 == R2) && (C1 == C2)) { for (int i = 0; i < R1; i++) for (int j = 0; j < C2; j++) if (m1.m_mat[i][j] != m2.m_mat[i][j]) return false; + return true; } - else - return false; - return true; + return false; } T_R1_C1_R2_C2 bool operator !=(const matrix m1, const matrix m2) diff --git a/src/sensor/rotation_vector/fusion_utils/vector.cpp b/src/sensor/rotation_vector/fusion_utils/vector.cpp index 432cb66..d06ad7d 100644 --- a/src/sensor/rotation_vector/fusion_utils/vector.cpp +++ b/src/sensor/rotation_vector/fusion_utils/vector.cpp @@ -155,16 +155,14 @@ T_S vect operator /(const vect v, const T val) T_S1_S2 bool operator ==(const vect v1, const vect v2) { - if (S1 == S2) - { + if (S1 == S2) { for (int i = 0; i < S1; i++) if (v1.m_vec[i] != v2.m_vec[i]) return false; + return true; } - else - return false; - return true; + return false; } T_S1_S2 bool operator !=(const vect v1, const vect v2) diff --git a/src/sensorctl/loopback.cpp b/src/sensorctl/loopback.cpp index ee177b8..b890f47 100644 --- a/src/sensorctl/loopback.cpp +++ b/src/sensorctl/loopback.cpp @@ -58,7 +58,7 @@ bool loopback_manager::run(int argc, char *argv[]) * sensorhub (bytes) command [0~1] delaytime [2~5] data[6~83] * shell (argv) command [0~2] delaytime [3] data[4~81] */ - char test_data[MAX_DATA_SIZE] = {SHUB_INST_LIB_ADD, SHUB_LOOP_BACK_LIB,}; + char test_data[MAX_DATA_SIZE] = {SHUB_INST_LIB_ADD, SHUB_LOOP_BACK_LIB, }; int_to_bytes(atoi(argv[3]), sizeof(int), &(test_data[2])); for (int i = 4; i < argc; i++) @@ -75,7 +75,7 @@ bool loopback_manager::run(int argc, char *argv[]) sensor = sensord_get_sensor(CONTEXT_SENSOR); handle = sensord_connect(sensor); - char test_data[4] = {SHUB_INST_LIB_REMOVE, SHUB_LOOP_BACK_LIB,}; + char test_data[4] = {SHUB_INST_LIB_REMOVE, SHUB_LOOP_BACK_LIB, }; sensord_set_attribute_str(handle, 0, test_data, sizeof(test_data)); sensord_disconnect(handle); diff --git a/src/sensorctl/test_bench.cpp b/src/sensorctl/test_bench.cpp index db0b974..8e5b5d8 100644 --- a/src/sensorctl/test_bench.cpp +++ b/src/sensorctl/test_bench.cpp @@ -237,7 +237,6 @@ void test_bench::show(void) for (auto it = testcases.begin(); it != testcases.end(); it = testcases.upper_bound(it->first)) { - auto range = testcases.equal_range(it->first); _I("[%s]\n", it->first.c_str()); diff --git a/src/sensorctl/testcase/unit_ipc.cpp b/src/sensorctl/testcase/unit_ipc.cpp index 799924a..c11d439 100644 --- a/src/sensorctl/testcase/unit_ipc.cpp +++ b/src/sensorctl/testcase/unit_ipc.cpp @@ -112,7 +112,7 @@ static bool run_ipc_client_sleep_1s(const char *str, int size, int count) message msg; message reply; - char buf[MAX_BUF_SIZE] = {'1', '1', '1',}; + char buf[MAX_BUF_SIZE] = {'1', '1', '1', }; msg.enclose(buf, MAX_BUF_SIZE); @@ -147,7 +147,7 @@ static bool run_ipc_client_small_buffer(const char *str, int size, int count) int ret; message msg; message reply; - char buf[MAX_BUF_SIZE] = {'1', '1', '1',}; + char buf[MAX_BUF_SIZE] = {'1', '1', '1', }; msg.enclose(buf, MAX_BUF_SIZE); @@ -190,7 +190,7 @@ static bool run_ipc_client_1M(const char *str, int size, int count) int ret; message msg; message reply; - char buf[MAX_BUF_SIZE] = {'1', '1', '1',}; + char buf[MAX_BUF_SIZE] = {'1', '1', '1', }; msg.enclose(buf, MAX_BUF_SIZE); -- 2.7.4 From 1777765d033aa5853cec24c0d621b8e82ae262e0 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Thu, 14 Sep 2017 14:45:33 +0900 Subject: [PATCH 04/16] sensorctl: fix bug which senosr type was not set Change-Id: Ie441084d2b95f0ea99c70ca0c70b6a2d0b5ba50b Signed-off-by: kibak.yoon --- src/sensorctl/tester_manual.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/sensorctl/tester_manual.cpp b/src/sensorctl/tester_manual.cpp index f2270eb..2e40851 100644 --- a/src/sensorctl/tester_manual.cpp +++ b/src/sensorctl/tester_manual.cpp @@ -32,7 +32,7 @@ #define TEST_MANUAL "manual" -static sensor_type_t type; +static sensor_type_t stype; static int interval; static int latency; static int powersave; @@ -61,6 +61,7 @@ tester_manual::~tester_manual() bool tester_manual::setup(sensor_type_t type, int argc, char *argv[]) { + stype = type; interval = TEST_DEFAULT_INTERVAL; latency = TEST_DEFAULT_LATENCY; powersave = TEST_DEFAULT_POWERSAVE_OPTION; @@ -127,18 +128,18 @@ TESTCASE(manual_test, sensor) int index = 0; sensor_data_t data; - if (sensor_adapter::get_count(type) > 1) { + if (sensor_adapter::get_count(stype) > 1) { _N("There are more than 2 sensors. please enter the index : "); if (scanf("%d", &index) != 1) return false; } - sensor_info info(type, index, interval, latency, powersave, test_cb, NULL); + sensor_info info(stype, index, interval, latency, powersave, test_cb, NULL); ret = sensor_adapter::start(info, handle); ASSERT_TRUE(ret); - ret = sensor_adapter::get_data(handle, type, data); + ret = sensor_adapter::get_data(handle, stype, data); EXPECT_TRUE(ret); mainloop::run(); -- 2.7.4 From adffc586d24bfd6a5ebcd8db13d8ad3091f2d55a Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Mon, 18 Sep 2017 11:09:16 +0900 Subject: [PATCH 05/16] sensorctl: change scanf to cin for security issue Change-Id: Id5760c1fe643729d67716323360cb43d0147aa7b Signed-off-by: kibak.yoon --- src/sensorctl/tester_manual.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/sensorctl/tester_manual.cpp b/src/sensorctl/tester_manual.cpp index 2e40851..cc10bd1 100644 --- a/src/sensorctl/tester_manual.cpp +++ b/src/sensorctl/tester_manual.cpp @@ -17,6 +17,7 @@ * */ +#include #include #include "tester.h" @@ -130,8 +131,7 @@ TESTCASE(manual_test, sensor) if (sensor_adapter::get_count(stype) > 1) { _N("There are more than 2 sensors. please enter the index : "); - if (scanf("%d", &index) != 1) - return false; + std::cin >> index; } sensor_info info(stype, index, interval, latency, powersave, test_cb, NULL); -- 2.7.4 From e39e5f796690ed08d1210a29779c0f02f04c8781 Mon Sep 17 00:00:00 2001 From: Marcin Masternak Date: Fri, 15 Sep 2017 12:31:31 +0200 Subject: [PATCH 06/16] sensord: samsung pedometer sensor for fused location fix/update. Change-Id: I297e0a9e6eb09afb0791fd7a64f608900dc62893 Signed-off-by: Marcin Masternak --- src/sensor/pedometer/average_filter.cpp | 1 + src/sensor/pedometer/average_filter.h | 5 ++-- src/sensor/pedometer/pedometer.cpp | 6 ++-- src/sensor/pedometer/pedometer.h | 13 ++++---- src/sensor/pedometer/pedometer_info.h | 10 +++---- src/sensor/pedometer/pedometer_sensor.cpp | 2 +- src/sensor/pedometer/pedometer_speed_filter.cpp | 24 +++++++-------- src/sensor/pedometer/pedometer_speed_filter.h | 34 +++++++++++---------- src/sensor/pedometer/savitzky_golay_filter15.cpp | 1 + src/sensor/pedometer/savitzky_golay_filter15.h | 9 +++--- .../pedometer/sensor_frequency_compensator.cpp | 17 +++++------ .../pedometer/sensor_frequency_compensator.h | 14 +++++---- src/sensor/pedometer/step_detection.cpp | 17 +++++------ src/sensor/pedometer/step_detection.h | 35 ++++++++++++---------- src/sensor/pedometer/step_event.h | 5 +++- src/sensor/pedometer/{common.h => timestamp.h} | 5 ++-- .../pedometer/zero_crossing_step_detection.cpp | 12 ++++++-- .../pedometer/zero_crossing_step_detection.h | 18 ++++++++--- 18 files changed, 130 insertions(+), 98 deletions(-) rename src/sensor/pedometer/{common.h => timestamp.h} (90%) diff --git a/src/sensor/pedometer/average_filter.cpp b/src/sensor/pedometer/average_filter.cpp index 81ca075..b03f6ef 100644 --- a/src/sensor/pedometer/average_filter.cpp +++ b/src/sensor/pedometer/average_filter.cpp @@ -16,6 +16,7 @@ #include "average_filter.h" +#include #include static double mean(double *array, int size) diff --git a/src/sensor/pedometer/average_filter.h b/src/sensor/pedometer/average_filter.h index e083ce0..ba89741 100644 --- a/src/sensor/pedometer/average_filter.h +++ b/src/sensor/pedometer/average_filter.h @@ -17,12 +17,13 @@ #ifndef __AVERAGE_FILTER_H__ #define __AVERAGE_FILTER_H__ -#include -#include "common.h" +#include "timestamp.h" class average_filter { public: + average_filter(int size); + ~average_filter(); /************************************************************************ diff --git a/src/sensor/pedometer/pedometer.cpp b/src/sensor/pedometer/pedometer.cpp index f076c16..2d2fe8a 100644 --- a/src/sensor/pedometer/pedometer.cpp +++ b/src/sensor/pedometer/pedometer.cpp @@ -51,7 +51,7 @@ void pedometer::reset(void) m_acceleration_compensator.reset(); } -bool pedometer::get_pedometer(pedometer_info *info, timestamp_t timestamp, double acc[]) +bool pedometer::new_acceleration(pedometer_info *info, timestamp_t timestamp, double acc[]) { bool result = false; m_acceleration_compensator.add(timestamp, acc); @@ -60,7 +60,7 @@ bool pedometer::get_pedometer(pedometer_info *info, timestamp_t timestamp, doubl while (m_acceleration_compensator.has_next()) { double acceleration[3]; m_acceleration_compensator.get_next(acceleration); - if (m_step_detection.get_step(timestamp, + if (m_step_detection.new_acceleration(timestamp, sqrt(acceleration[0] * acceleration[0] + acceleration[1] * acceleration[1] + acceleration[2] * acceleration[2]), @@ -68,7 +68,7 @@ bool pedometer::get_pedometer(pedometer_info *info, timestamp_t timestamp, doubl if (event.m_timestamp != UNKNOWN_TIMESTAMP) { m_step_count++; m_total_length += event.m_step_length; - m_pedometer_filter.get_step(timestamp, event.m_step_length); + m_pedometer_filter.new_step(timestamp, event.m_step_length); double speed = m_pedometer_filter.get_speed(timestamp); info->timestamp = timestamp; info->is_step_detected = true; diff --git a/src/sensor/pedometer/pedometer.h b/src/sensor/pedometer/pedometer.h index f80c315..f990906 100644 --- a/src/sensor/pedometer/pedometer.h +++ b/src/sensor/pedometer/pedometer.h @@ -17,11 +17,11 @@ #ifndef __PEDOMETER_H__ #define __PEDOMETER_H__ -#include "common.h" #include "step_detection.h" #include "pedometer_info.h" #include "pedometer_speed_filter.h" #include "sensor_frequency_compensator.h" +#include "timestamp.h" /************************************************************************ * stores pedometer engine state. @@ -32,7 +32,7 @@ public: ~pedometer(); /************************************************************************ - * enables/disables savitzky filter. + * enables/disables Savitzky filter. */ void set_savitzky_filter(bool enable); @@ -47,20 +47,20 @@ public: * @param info * result of pedometer algorithm. valid only it method returns true. * @param timestamp - * timestamp of acceleration event in ns. + * timestamp of acceleration event in [ns]. * @param acc - * global acceleration. + * global acceleration vector in [m/s^2]. * * @result * true if new step event was detected. */ - bool get_pedometer(pedometer_info *info, timestamp_t timestamp, double acc[]); + bool new_acceleration(pedometer_info *info, timestamp_t timestamp, double acc[]); private: /** detects step and estimates step length. */ step_detection m_step_detection; - /** sum of lengths all steps from start. */ + /** sum of lengths all steps from start in [m]. */ double m_total_length; /** number of steps from start. */ @@ -73,6 +73,7 @@ private: bool m_some_speed; sensor_frequency_compensator m_acceleration_compensator; + }; #endif /* __PEDOMETER_H__ */ diff --git a/src/sensor/pedometer/pedometer_info.h b/src/sensor/pedometer/pedometer_info.h index ac521db..21f51e3 100644 --- a/src/sensor/pedometer/pedometer_info.h +++ b/src/sensor/pedometer/pedometer_info.h @@ -17,14 +17,14 @@ #ifndef __PEDOMETER_INFO_H__ #define __PEDOMETER_INFO_H__ -#include "common.h" +#include "timestamp.h" /************************************************************************ * stores information about pedometer event detected. */ class pedometer_info { public: - /** timestamp this event was detected in ns. */ + /** timestamp this event was detected in [ns]. */ timestamp_t timestamp; /** is step detected. */ @@ -33,13 +33,13 @@ public: /** step count from scanner start. */ long long step_count; - /** step length in meters. */ + /** step length in [m]. */ double step_length; - /** total length of all steps detected from scanner start in meters. */ + /** total length of all steps detected from scanner start in [m]. */ double total_step_length; - /** current mean speed in m/s. */ + /** current mean speed in [m/s]. */ double step_speed; }; diff --git a/src/sensor/pedometer/pedometer_sensor.cpp b/src/sensor/pedometer/pedometer_sensor.cpp index e60ec43..5f46dff 100644 --- a/src/sensor/pedometer/pedometer_sensor.cpp +++ b/src/sensor/pedometer/pedometer_sensor.cpp @@ -82,7 +82,7 @@ int pedometer_sensor::update(uint32_t id, sensor_data_t *data, int len) pedometer_info info; double acc[] = {data->values[0], data->values[1], data->values[2]}; - if (!m_pedometer.get_pedometer(&info, US_TO_NS(data->timestamp), acc)) + if (!m_pedometer.new_acceleration(&info, US_TO_NS(data->timestamp), acc)) return OP_ERROR; m_step_count = info.step_count; diff --git a/src/sensor/pedometer/pedometer_speed_filter.cpp b/src/sensor/pedometer/pedometer_speed_filter.cpp index d6ac774..372745e 100644 --- a/src/sensor/pedometer/pedometer_speed_filter.cpp +++ b/src/sensor/pedometer/pedometer_speed_filter.cpp @@ -16,8 +16,8 @@ #include "pedometer_speed_filter.h" -/** default for maximum step duration in ns. currently 2s. */ -#define STEP_MAX_DURATION 2000000000L +/** default for maximum step duration in [ns]. currently 2s. */ +static const long long STEP_MAX_DURATION = 2000000000L; pedometer_speed_filter::pedometer_speed_filter() : m_last_timestamp(UNKNOWN_TIMESTAMP) @@ -43,12 +43,12 @@ void pedometer_speed_filter::clear_speed(void) } /************************************************************************ - * sets new maximum step duration in ns. + * sets new maximum step duration in [ns]. * if there is no new speed during this time current speed is cleared. * 0 disables this feature. * * @param step_max_duration - * maximum step duration in ns. + * maximum step duration in [ns]. * 0 to disable step duration checking. */ void pedometer_speed_filter::set_step_max_duration(long long step_max_duration) @@ -60,11 +60,11 @@ void pedometer_speed_filter::set_step_max_duration(long long step_max_duration) * called when new step detection event occurs. * * @param timestamp - * timestamp of step detection event in ns. + * timestamp of step detection event in [ns]. * @param steplength - * length of detected step in m. + * length of detected step in [m]. */ -void pedometer_speed_filter::get_step(timestamp_t timestamp, double step_length) +void pedometer_speed_filter::new_step(timestamp_t timestamp, double step_length) { if (m_last_timestamp == UNKNOWN_TIMESTAMP || timestamp == UNKNOWN_TIMESTAMP) { clear_speed(); @@ -86,9 +86,9 @@ void pedometer_speed_filter::get_step(timestamp_t timestamp, double step_length) * reports new speed. * * @param timestamp - * timestamp of speed event. + * timestamp of speed event in [ns]. * @param speed - * current speed in m/s. + * current speed in [m/s]. */ void pedometer_speed_filter::new_speed(timestamp_t timestamp, double speed) { @@ -106,7 +106,7 @@ void pedometer_speed_filter::new_speed(timestamp_t timestamp, double speed) * * @param timestamp * timestamp for which speed should be calculated. - * @return speed for given timestamp in m/s. + * @return speed for given timestamp in [m/s]. */ double pedometer_speed_filter::get_speed(timestamp_t timestamp) { @@ -122,7 +122,7 @@ double pedometer_speed_filter::get_speed(timestamp_t timestamp) * changes current speed. * * @param speed - * current speed in m/s. + * current speed in [m/s]. */ void pedometer_speed_filter::set_current_speed(double speed) { @@ -130,7 +130,7 @@ void pedometer_speed_filter::set_current_speed(double speed) } /************************************************************************ - * @return estimated current speed in m/s. + * @return estimated current speed in [m/s]. */ double pedometer_speed_filter::get_current_speed(void) { diff --git a/src/sensor/pedometer/pedometer_speed_filter.h b/src/sensor/pedometer/pedometer_speed_filter.h index 4c6fde1..d9e0b7f 100644 --- a/src/sensor/pedometer/pedometer_speed_filter.h +++ b/src/sensor/pedometer/pedometer_speed_filter.h @@ -17,14 +17,16 @@ #ifndef __PEDOMETER_SPEED_FILTER_H__ #define __PEDOMETER_SPEED_FILTER_H__ -#include "common.h" +#include "timestamp.h" /************************************************************************ * stores pedometer speed filter state. */ class pedometer_speed_filter { public: + pedometer_speed_filter(); + ~pedometer_speed_filter(); void clear_speed(void); @@ -44,19 +46,19 @@ public: * called when new step detection event occurs. * * @param timestamp - * timestamp of step detection event in ns. + * timestamp of step detection event in [ns]. * @param steplength - * length of detected step in m. + * length of detected step in [m]. */ - void get_step(timestamp_t timestamp, double step_length); + void new_step(timestamp_t timestamp, double step_length); /************************************************************************ * reports new speed. * * @param timestamp - * timestamp of speed event. + * timestamp of speed event in [ns]. * @param speed - * current speed in m/s. + * current speed in [m/s]. */ void new_speed(timestamp_t timestamp, double speed); @@ -64,8 +66,8 @@ public: * returns current speed. * * @param timestamp - * timestamp for which speed should be calculated. - * @return speed for given timestamp in m/s. + * timestamp for which speed should be calculated in [ns]. + * @return speed for given timestamp in [m/s]. */ double get_speed(timestamp_t timestamp); @@ -73,12 +75,12 @@ public: * changes current speed. * * @param speed - * current speed in m/s. + * current speed in [m/s]. */ void set_current_speed(double speed); /************************************************************************ - * @return estimated current speed in m/s. + * @return estimated current speed in [m/s]. */ double get_current_speed(void); @@ -87,7 +89,7 @@ public: bool is_known_timestamp(void); /************************************************************************ - * @return timestamp of last step detection event in ns. + * @return timestamp of last step detection event in [ns]. */ timestamp_t get_timestamp(void); @@ -97,19 +99,19 @@ public: void reset(void); private: - /** timestamp of last step detection event in ns. */ + /** timestamp of last step detection event in [ns]. */ timestamp_t m_last_timestamp; - /** estimated current speed in m/s. */ + /** estimated current speed in [m/s]. */ double m_current_speed; - /** length of last step in m. */ + /** length of last step in [m]. */ double m_last_step_length; - /** duration of last step in s. */ + /** duration of last step in [s]. */ double m_last_step_duration; - /** maximum step duration in ns. 0 to disable step duration checking. */ + /** maximum step duration in [ns]. 0 to disable step duration checking. */ long long m_step_max_duration; }; diff --git a/src/sensor/pedometer/savitzky_golay_filter15.cpp b/src/sensor/pedometer/savitzky_golay_filter15.cpp index ced5d8f..549204b 100644 --- a/src/sensor/pedometer/savitzky_golay_filter15.cpp +++ b/src/sensor/pedometer/savitzky_golay_filter15.cpp @@ -16,6 +16,7 @@ #include "savitzky_golay_filter15.h" +#include #include /* length of filter. changing it requires changing coef_array! */ diff --git a/src/sensor/pedometer/savitzky_golay_filter15.h b/src/sensor/pedometer/savitzky_golay_filter15.h index a9a8899..77eef64 100644 --- a/src/sensor/pedometer/savitzky_golay_filter15.h +++ b/src/sensor/pedometer/savitzky_golay_filter15.h @@ -17,15 +17,16 @@ #ifndef __SAVITZKYGOLAYFILTER15_H__ #define __SAVITZKYGOLAYFILTER15_H__ -#include -#include "common.h" +#include "timestamp.h" /************************************************************************ - * stores savitzky-golay filter state. + * stores Savitzky-Golay filter state. */ class savitzky_golay_filter15 { public: + savitzky_golay_filter15(); + ~savitzky_golay_filter15(); /************************************************************************ @@ -38,7 +39,7 @@ public: double filter(double value); /************************************************************************ - * resets savitzky-golay filter to initial state. + * resets Savitzky-Golay filter to initial state. */ void reset(void); diff --git a/src/sensor/pedometer/sensor_frequency_compensator.cpp b/src/sensor/pedometer/sensor_frequency_compensator.cpp index 9475bdc..855f2ca 100644 --- a/src/sensor/pedometer/sensor_frequency_compensator.cpp +++ b/src/sensor/pedometer/sensor_frequency_compensator.cpp @@ -23,12 +23,12 @@ /************************************************************************ */ sensor_frequency_compensator::sensor_frequency_compensator(double desired_rate) -: m_desired_frequency(desired_rate) -, m_t1(0) +: m_desired_rate(desired_rate) +, m_t1(UNKNOWN_TIMESTAMP) , m_v1{0.0, 0.0, 0.0} -, m_t2(0) +, m_t2(UNKNOWN_TIMESTAMP) , m_v2{0.0, 0.0, 0.0} -, m_timestamp(0) +, m_timestamp(UNKNOWN_TIMESTAMP) { } @@ -64,7 +64,7 @@ bool sensor_frequency_compensator::has_next() { if (m_t1 == UNKNOWN_TIMESTAMP || m_t2 == UNKNOWN_TIMESTAMP) { return false; } - return m_timestamp + m_desired_frequency < m_t2; + return m_timestamp + m_desired_rate < m_t2; } /************************************************************************ @@ -74,10 +74,9 @@ void sensor_frequency_compensator::get_next(double *v) { if (t3 < m_t1) { t3 = m_t1; } - m_timestamp += m_desired_frequency; - double t = (t3 - m_t1) / ((double) (m_t2 - m_t1)); - int i; - for (i = 0; i < 3; i++) { + m_timestamp += m_desired_rate; + double t = ((double) (t3 - m_t1)) / (m_t2 - m_t1); + for (int i = 0; i < 3; i++) { v[i] = (1.0 - t) * m_v1[i] + t * m_v2[i]; } } diff --git a/src/sensor/pedometer/sensor_frequency_compensator.h b/src/sensor/pedometer/sensor_frequency_compensator.h index 664fedb..d22f57c 100644 --- a/src/sensor/pedometer/sensor_frequency_compensator.h +++ b/src/sensor/pedometer/sensor_frequency_compensator.h @@ -14,10 +14,10 @@ * limitations under the License. */ -#ifndef __SENSOR_FREQUENCY_COMPENSATOR_H_ -#define __SENSOR_FREQUENCY_COMPENSATOR_H_ +#ifndef __SENSOR_FREQUENCY_COMPENSATOR_H__ +#define __SENSOR_FREQUENCY_COMPENSATOR_H__ -#include "common.h" +#include "timestamp.h" #include @@ -26,7 +26,9 @@ */ class sensor_frequency_compensator { public: + sensor_frequency_compensator(double desired_rate); + ~sensor_frequency_compensator(); /************************************************************************ @@ -52,7 +54,9 @@ public: void get_next(double *value); private: - long long m_desired_frequency; + + /** desired sensor rate in [ns]. */ + long long m_desired_rate; timestamp_t m_t1; @@ -65,4 +69,4 @@ private: timestamp_t m_timestamp; }; -#endif /* __SENSOR_FREQUENCY_COMPENSATOR_H_ */ +#endif /* __SENSOR_FREQUENCY_COMPENSATOR_H__ */ diff --git a/src/sensor/pedometer/step_detection.cpp b/src/sensor/pedometer/step_detection.cpp index ad977f2..701add5 100644 --- a/src/sensor/pedometer/step_detection.cpp +++ b/src/sensor/pedometer/step_detection.cpp @@ -17,7 +17,6 @@ #include "step_detection.h" #include -#include /* Size of average filter. */ #define AV_FILTER_SIZE 7 @@ -41,7 +40,7 @@ step_detection::step_detection() , m_zero_crossing_down(false) , m_zc_filter() , m_peak_threshold(FAST_PEAK_THRESHOLD) -, m_use_savitzky(false) +, m_use_savitzky(true) , m_last_step_timestamp(UNKNOWN_TIMESTAMP) , m_zero_crossing_up_detected(false) , m_zero_crossing_down_detected(false) @@ -147,7 +146,7 @@ bool step_detection::add_acc_sensor_values_average( } double peak_threshold; - if (m_zero_crossing_up.m_time_sum / 1E9 < 1.2) { + if (m_zero_crossing_up.get_time_sum() < 1.2) { peak_threshold = m_peak_threshold; m_is_slow_step_detected = false; } else { @@ -165,7 +164,7 @@ bool step_detection::add_acc_sensor_values_average( } } - if (m_zero_crossing_up.m_time_sum / 1E9 < 0.3) + if (m_zero_crossing_up.get_time_sum() < 0.3) is_step_detected = false; if (is_step_detected) { @@ -174,7 +173,7 @@ bool step_detection::add_acc_sensor_values_average( double time = (timestamp - m_last_step_timestamp) / 1E9; m_last_step_timestamp = timestamp; - m_zero_crossing_up.m_time_sum = 0; + m_zero_crossing_up.clr_time_sum(); step->m_timestamp = timestamp; step->m_step_length = cal_step_length(time, sqrt4peak_valley_diff); @@ -215,7 +214,7 @@ bool step_detection::add_acc_sensor_values_savitzky( } } - bool zup = m_zero_crossing_up.m_time_sum / 1E9 > 1.2; + bool zup = m_zero_crossing_up.get_time_sum() > 1.2; if (n_zero_down) { is_step_detected = false; amplitude = abs(m_maximum_acceleration - m_minimum_acceleration); @@ -243,7 +242,7 @@ bool step_detection::add_acc_sensor_values_savitzky( } } - if (m_zero_crossing_up.m_time_sum / 1E9 < 0.3) + if (m_zero_crossing_up.get_time_sum() < 0.3) is_step_detected = false; if (is_step_detected) { @@ -256,7 +255,7 @@ bool step_detection::add_acc_sensor_values_savitzky( if (time > 1.0 || amplitude < 3) { is_step_detected = false; } - m_zero_crossing_up.m_time_sum = 0; + m_zero_crossing_up.clr_time_sum(); if (is_step_detected) { step->m_timestamp = timestamp; step->m_step_length = cal_step_length(time, sqrt4peak_valley_diff); @@ -269,7 +268,7 @@ bool step_detection::add_acc_sensor_values_savitzky( /************************************************************************ */ -bool step_detection::get_step(timestamp_t timestamp, double acc, step_event* step) +bool step_detection::new_acceleration(timestamp_t timestamp, double acc, step_event* step) { return m_use_savitzky ? add_acc_sensor_values_savitzky(timestamp, acc, step) diff --git a/src/sensor/pedometer/step_detection.h b/src/sensor/pedometer/step_detection.h index addd786..613751c 100644 --- a/src/sensor/pedometer/step_detection.h +++ b/src/sensor/pedometer/step_detection.h @@ -21,14 +21,16 @@ #include "zero_crossing_step_detection.h" #include "savitzky_golay_filter15.h" #include "step_event.h" -#include "common.h" +#include "timestamp.h" /************************************************************************ * step detection engine state. */ class step_detection { public: + step_detection(); + ~step_detection(); /************************************************************************ @@ -41,21 +43,7 @@ public: /************************************************************************ */ - bool is_slow_step(void); - - /************************************************************************ - */ - bool add_acc_sensor_values_average( - timestamp_t timestamp, double acc, step_event* step); - - /************************************************************************ - */ - bool add_acc_sensor_values_savitzky( - timestamp_t timestamp, double acc, step_event* step); - - /************************************************************************ - */ - bool get_step(timestamp_t timestamp, double acc, step_event* step); + bool new_acceleration(timestamp_t timestamp, double acc, step_event* step); /************************************************************************ * resets step_detection object to initial state. @@ -77,6 +65,21 @@ private: double m_minimum_acceleration; double m_maximum_acceleration; bool m_is_slow_step_detected; + + /************************************************************************ + */ + bool add_acc_sensor_values_average( + timestamp_t timestamp, double acc, step_event* step); + + /************************************************************************ + */ + bool add_acc_sensor_values_savitzky( + timestamp_t timestamp, double acc, step_event* step); + + /************************************************************************ + */ + bool is_slow_step(void); + }; #endif /* __STEP_DETECTION_H__ */ diff --git a/src/sensor/pedometer/step_event.h b/src/sensor/pedometer/step_event.h index 724f262..f571c45 100644 --- a/src/sensor/pedometer/step_event.h +++ b/src/sensor/pedometer/step_event.h @@ -17,7 +17,7 @@ #ifndef __STEP_EVENT_H__ #define __STEP_EVENT_H__ -#include "common.h" +#include "timestamp.h" class step_event { public: @@ -26,7 +26,10 @@ public: , m_step_length(0) {} + /** step timestamp in [ns]. */ timestamp_t m_timestamp; + + /** estimated step length in [m]. */ double m_step_length; }; diff --git a/src/sensor/pedometer/common.h b/src/sensor/pedometer/timestamp.h similarity index 90% rename from src/sensor/pedometer/common.h rename to src/sensor/pedometer/timestamp.h index b1a0fef..371c9a8 100644 --- a/src/sensor/pedometer/common.h +++ b/src/sensor/pedometer/timestamp.h @@ -17,9 +17,8 @@ #ifndef __PEDOMETER_COMMON_H__ #define __PEDOMETER_COMMON_H__ -#include - typedef long long timestamp_t; -#define UNKNOWN_TIMESTAMP ((long long)0x8000000000000000) + +static const timestamp_t UNKNOWN_TIMESTAMP = (timestamp_t) 0x8000000000000000; #endif /* __PEDOMETER_COMMON_H__ */ diff --git a/src/sensor/pedometer/zero_crossing_step_detection.cpp b/src/sensor/pedometer/zero_crossing_step_detection.cpp index aff2b3c..f94647c 100644 --- a/src/sensor/pedometer/zero_crossing_step_detection.cpp +++ b/src/sensor/pedometer/zero_crossing_step_detection.cpp @@ -25,11 +25,11 @@ static bool detect_zero_crossing(double last_accel, double new_accel, bool up) } zero_crossing_step_detection::zero_crossing_step_detection(bool up) -: m_time_sum(0) -, m_up(up) +: m_up(up) , m_last_acceleration(0) , m_last_timestamp(UNKNOWN_TIMESTAMP) , m_last_zero_crossing_time(UNKNOWN_TIMESTAMP) +, m_time_sum(0) { } @@ -56,6 +56,14 @@ bool zero_crossing_step_detection::detect_step(timestamp_t timestamp, double acc return step_detected; } +void zero_crossing_step_detection::clr_time_sum() { + m_time_sum = 0.0; +} + +double zero_crossing_step_detection::get_time_sum() { + return m_time_sum / 1E9; +} + void zero_crossing_step_detection::reset(void) { m_last_acceleration = 0; diff --git a/src/sensor/pedometer/zero_crossing_step_detection.h b/src/sensor/pedometer/zero_crossing_step_detection.h index 79ca518..76a155f 100644 --- a/src/sensor/pedometer/zero_crossing_step_detection.h +++ b/src/sensor/pedometer/zero_crossing_step_detection.h @@ -17,14 +17,16 @@ #ifndef __ZERO_CROSSING_STEP_DETECTION_H__ #define __ZERO_CROSSING_STEP_DETECTION_H__ -#include "common.h" +#include "timestamp.h" /************************************************************************ * zero crossing detection engine state. */ class zero_crossing_step_detection { public: + zero_crossing_step_detection(bool up); + ~zero_crossing_step_detection(); /************************************************************************ @@ -35,7 +37,13 @@ public: */ void reset(void); - timestamp_t m_time_sum; + /************************************************************************ + */ + void clr_time_sum(); + + /************************************************************************ + */ + double get_time_sum(); private: /** @@ -46,11 +54,13 @@ private: /** acceleration in previous detect step. */ double m_last_acceleration; - /** timestamp of last acc event. unknown time if no one. */ + /** timestamp of last acc event in [ns]. unknown time if no one. */ timestamp_t m_last_timestamp; - /** timestamp of last detected zero crossing. unknown time if not yet detected. */ + /** timestamp of last detected zero crossing in [ns]. unknown time if not yet detected. */ timestamp_t m_last_zero_crossing_time; + + timestamp_t m_time_sum; }; #endif /* __ZERO_CROSSING_STEP_DETECTION_H__ */ -- 2.7.4 From 698da8a0ba27729d4ee469eb5789650aae4639c6 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Fri, 22 Sep 2017 14:12:17 +0900 Subject: [PATCH 07/16] sensord: limit the maximum number of listeners per client - MAX_LISTENER = 100 Change-Id: Ib736ef66f2c50a9a218244a4bc5a41bca52ef0d3 Signed-off-by: kibak.yoon --- src/client/sensor_internal.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/client/sensor_internal.cpp b/src/client/sensor_internal.cpp index 22d8840..5a9f042 100644 --- a/src/client/sensor_internal.cpp +++ b/src/client/sensor_internal.cpp @@ -32,6 +32,7 @@ #include #define CONVERT_OPTION_PAUSE_POLICY(option) ((option) ^ 0b11) +#define MAX_LISTENER 100 using namespace sensor; @@ -238,6 +239,7 @@ API int sensord_connect(sensor_t sensor) retvm_if(!manager.connect(), -EIO, "Failed to connect"); retvm_if(!manager.is_supported(sensor), -EINVAL, "Invalid sensor[%p]", sensor); + retvm_if(listeners.size() > MAX_LISTENER, -EPERM, "Exceeded the maximum listener"); sensor::sensor_listener *listener; -- 2.7.4 From 1ee7ae20a3447a8a89283f0656a87c5272edf1b1 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Fri, 22 Sep 2017 14:15:32 +0900 Subject: [PATCH 08/16] sensord: check the range of values provided from the client Change-Id: Ib6102f684dd1b5ee877b28aa871f7b6b169adf23 Signed-off-by: kibak.yoon --- src/client/sensor_provider.cpp | 17 ++++++++++++++--- src/shared/sensor_info.cpp | 14 ++++++++++++++ 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/src/client/sensor_provider.cpp b/src/client/sensor_provider.cpp index 50772d5..aec9c22 100644 --- a/src/client/sensor_provider.cpp +++ b/src/client/sensor_provider.cpp @@ -26,9 +26,12 @@ #include #include #include +#include #include "sensor_provider_channel_handler.h" +#define DEFAULT_RESOLUTION 0.1 + using namespace sensor; sensor_provider::sensor_provider(const char *uri) @@ -57,9 +60,9 @@ bool sensor_provider::init(const char *uri) } m_sensor.set_uri(uri); - m_sensor.set_min_range(0); - m_sensor.set_max_range(1); - m_sensor.set_resolution(1); + m_sensor.set_min_range(-FLT_MAX); + m_sensor.set_max_range(FLT_MAX); + m_sensor.set_resolution(DEFAULT_RESOLUTION); /* TODO: temporary walkaround */ const char *priv = sensor::utils::get_privilege(uri); m_sensor.set_privilege(priv); @@ -166,6 +169,14 @@ void sensor_provider::restore(void) int sensor_provider::publish(sensor_data_t *data, int len) { + for (int i = 0; i < data->value_count; ++i) { + if (data->values[i] < m_sensor.get_min_range() || + data->values[i] > m_sensor.get_max_range()) { + _E("Out of range"); + return OP_ERROR; + } + } + ipc::message msg; msg.set_type(CMD_PROVIDER_PUBLISH); msg.enclose((const char *)data, len); diff --git a/src/shared/sensor_info.cpp b/src/shared/sensor_info.cpp index e56be7e..bcb1930 100644 --- a/src/shared/sensor_info.cpp +++ b/src/shared/sensor_info.cpp @@ -22,11 +22,15 @@ #include #include #include +#include #include #include #include "sensor_utils.h" +#define MIN_RANGE -FLT_MAX +#define MAX_RANGE FLT_MAX + using namespace sensor; sensor_info::sensor_info() @@ -176,11 +180,21 @@ void sensor_info::set_vendor(const char *vendor) void sensor_info::set_min_range(float min_range) { m_min_range = min_range; + + if (m_min_range < MIN_RANGE) + m_min_range = MIN_RANGE; + if (m_min_range > MAX_RANGE) + m_min_range = MAX_RANGE; } void sensor_info::set_max_range(float max_range) { m_max_range = max_range; + + if (m_max_range < MIN_RANGE) + m_max_range = MIN_RANGE; + if (m_max_range > MAX_RANGE) + m_max_range = MAX_RANGE; } void sensor_info::set_resolution(float resolution) -- 2.7.4 From 303c2fcef5f3582cc756d949db1e749553cc31b1 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Fri, 22 Sep 2017 17:08:56 +0900 Subject: [PATCH 09/16] sensord: version up 4.0.3 Change-Id: I25f0435ab579d3e541f9a3ed1308879c7388868a Signed-off-by: kibak.yoon --- packaging/sensord.spec | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index b96b6fe..2cf492e 100644 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -1,6 +1,6 @@ Name: sensord Summary: Sensor daemon -Version: 4.0.2 +Version: 4.0.3 Release: 1 Group: System/Sensor Framework License: Apache-2.0 -- 2.7.4 From 29caec272fffe2792f82d872c13d8f4c9e80b3fb Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 11 Oct 2017 10:49:38 +0900 Subject: [PATCH 10/16] sensor: fix problem that gyroscope rotation vector does not work Change-Id: I7a6e4305bb982d36c87cc4b2f3789c0b09f02c55 Signed-off-by: kibak.yoon --- .../rotation_vector/fusion_utils/orientation_filter.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/sensor/rotation_vector/fusion_utils/orientation_filter.cpp b/src/sensor/rotation_vector/fusion_utils/orientation_filter.cpp index e352f5b..43ea867 100644 --- a/src/sensor/rotation_vector/fusion_utils/orientation_filter.cpp +++ b/src/sensor/rotation_vector/fusion_utils/orientation_filter.cpp @@ -285,15 +285,9 @@ inline void orientation_filter::time_update_gaming_rv() euler_aid = quat2euler(m_quat_aid); euler_driv = quat2euler(m_quat_output); - if ((SQUARE(m_accel.m_data.m_vec[1]) < ACCEL_THRESHOLD) && (SQUARE(m_gyro.m_data.m_vec[0]) < GYRO_THRESHOLD)) { - if ((SQUARE(m_accel.m_data.m_vec[0]) < ACCEL_THRESHOLD) && (SQUARE(m_gyro.m_data.m_vec[1]) < GYRO_THRESHOLD)) { - if (SQUARE(m_gyro.m_data.m_vec[2]) < GYRO_THRESHOLD) { - euler_angles euler_gaming_rv(euler_aid.m_ang.m_vec[0], euler_aid.m_ang.m_vec[1], - euler_driv.m_ang.m_vec[2]); - m_quat_gaming_rv = euler2quat(euler_gaming_rv); - } - } - } + euler_angles euler_gaming_rv(euler_aid.m_ang.m_vec[0], euler_aid.m_ang.m_vec[1], + euler_driv.m_ang.m_vec[2]); + m_quat_gaming_rv = euler2quat(euler_gaming_rv); if (is_initialized(m_state_new)) { m_state_error.m_vec[0] = m_euler_error.m_ang.m_vec[0]; -- 2.7.4 From e9e7d98e4a3c68fca14e28ab23a1e53bf2bd4cd2 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Fri, 13 Oct 2017 23:03:59 +0900 Subject: [PATCH 11/16] sensor: check if the event value is NaN Change-Id: I97615392d32d7435e74788cc28f13b56f8e72549 Signed-off-by: kibak.yoon --- src/client/sensor_provider.cpp | 6 +++++- src/shared/sensor_info.cpp | 5 +++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/client/sensor_provider.cpp b/src/client/sensor_provider.cpp index aec9c22..2855b1f 100644 --- a/src/client/sensor_provider.cpp +++ b/src/client/sensor_provider.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include "sensor_provider_channel_handler.h" @@ -170,7 +171,10 @@ void sensor_provider::restore(void) int sensor_provider::publish(sensor_data_t *data, int len) { for (int i = 0; i < data->value_count; ++i) { - if (data->values[i] < m_sensor.get_min_range() || + if (std::isnan(data->values[i]) || + std::isnan(m_sensor.get_min_range()) || + std::isnan(m_sensor.get_max_range()) || + data->values[i] < m_sensor.get_min_range() || data->values[i] > m_sensor.get_max_range()) { _E("Out of range"); return OP_ERROR; diff --git a/src/shared/sensor_info.cpp b/src/shared/sensor_info.cpp index bcb1930..855695c 100644 --- a/src/shared/sensor_info.cpp +++ b/src/shared/sensor_info.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include "sensor_utils.h" @@ -181,6 +182,8 @@ void sensor_info::set_min_range(float min_range) { m_min_range = min_range; + if (std::isnan(m_min_range)) + m_min_range = 0; /* set value to 0 when the value is NaN */ if (m_min_range < MIN_RANGE) m_min_range = MIN_RANGE; if (m_min_range > MAX_RANGE) @@ -191,6 +194,8 @@ void sensor_info::set_max_range(float max_range) { m_max_range = max_range; + if (std::isnan(m_max_range)) + m_max_range = 1; /* set value to 1 when the value is NaN */ if (m_max_range < MIN_RANGE) m_max_range = MIN_RANGE; if (m_max_range > MAX_RANGE) -- 2.7.4 From 2b50eaf74210901c1c8f817b31f8c58621ba4243 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Mon, 16 Oct 2017 11:47:22 +0900 Subject: [PATCH 12/16] sensord: check if the event value is a valid number Change-Id: I414f8a6f71baad87ec3dd9538d5ee60b8c3a5edf Signed-off-by: kibak.yoon --- src/client/sensor_provider.cpp | 7 ++----- src/shared/sensor_info.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/client/sensor_provider.cpp b/src/client/sensor_provider.cpp index 2855b1f..e742309 100644 --- a/src/client/sensor_provider.cpp +++ b/src/client/sensor_provider.cpp @@ -171,11 +171,8 @@ void sensor_provider::restore(void) int sensor_provider::publish(sensor_data_t *data, int len) { for (int i = 0; i < data->value_count; ++i) { - if (std::isnan(data->values[i]) || - std::isnan(m_sensor.get_min_range()) || - std::isnan(m_sensor.get_max_range()) || - data->values[i] < m_sensor.get_min_range() || - data->values[i] > m_sensor.get_max_range()) { + if (!(data->values[i] >= m_sensor.get_min_range() && + data->values[i] <= m_sensor.get_max_range())) { _E("Out of range"); return OP_ERROR; } diff --git a/src/shared/sensor_info.cpp b/src/shared/sensor_info.cpp index 855695c..5a87539 100644 --- a/src/shared/sensor_info.cpp +++ b/src/shared/sensor_info.cpp @@ -182,8 +182,8 @@ void sensor_info::set_min_range(float min_range) { m_min_range = min_range; - if (std::isnan(m_min_range)) - m_min_range = 0; /* set value to 0 when the value is NaN */ + if (!std::isnormal(m_min_range)) + m_min_range = 0; /* set value to 0 when the value is NaN, infinity, zero or subnormal */ if (m_min_range < MIN_RANGE) m_min_range = MIN_RANGE; if (m_min_range > MAX_RANGE) @@ -194,8 +194,8 @@ void sensor_info::set_max_range(float max_range) { m_max_range = max_range; - if (std::isnan(m_max_range)) - m_max_range = 1; /* set value to 1 when the value is NaN */ + if (!std::isnormal(m_max_range)) + m_max_range = 0; /* set value to 0 when the value is NaN, infinity, zero or subnormal */ if (m_max_range < MIN_RANGE) m_max_range = MIN_RANGE; if (m_max_range > MAX_RANGE) -- 2.7.4 From aaf347d3a48a5b313fcba57cb85c30a7cec773b2 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 25 Oct 2017 00:27:04 +0900 Subject: [PATCH 13/16] sensord: caching data to get previous data - Svace/Coverity : no issue - tct is passed : utc/itc 100% Change-Id: If6e023acf3d189869183fc33220562f2013534da Signed-off-by: kibak.yoon --- src/server/sensor_handler.cpp | 28 ++++++++++++++++++++++++++++ src/server/sensor_handler.h | 6 ++++++ src/server/sensor_listener_proxy.cpp | 3 +-- 3 files changed, 35 insertions(+), 2 deletions(-) diff --git a/src/server/sensor_handler.cpp b/src/server/sensor_handler.cpp index 2a2eb6a..37c4dc2 100644 --- a/src/server/sensor_handler.cpp +++ b/src/server/sensor_handler.cpp @@ -28,6 +28,8 @@ using namespace sensor; sensor_handler::sensor_handler(const sensor_info &info) : m_info(info) +, m_last_data(NULL) +, m_last_data_size(0) { const char *priv = sensor::utils::get_privilege(m_info.get_uri()); m_info.set_privilege(priv); @@ -84,6 +86,8 @@ int sensor_handler::notify(const char *uri, sensor_data_t *data, int len) if (msg->ref_count() == 0) msg->unref(); + set_cache(data, len); + return OP_SUCCESS; } @@ -91,3 +95,27 @@ uint32_t sensor_handler::observer_count(void) { return m_observers.size(); } + +void sensor_handler::set_cache(sensor_data_t *data, int size) +{ + if (m_last_data == NULL) { + m_last_data = (sensor_data_t*)malloc(size); + retm_if(m_last_data == NULL, "Memory allocation failed"); + } + + m_last_data_size = size; + memcpy(m_last_data, data, size); +} + +int sensor_handler::get_cache(sensor_data_t **data, int *len) +{ + retv_if(m_last_data == NULL, -ENODATA); + + *data = (sensor_data_t *)malloc(m_last_data_size); + retvm_if(*data == NULL, -ENOMEM, "Memory allocation failed"); + + memcpy(*data, m_last_data, m_last_data_size); + *len = m_last_data_size; + + return 0; +} diff --git a/src/server/sensor_handler.h b/src/server/sensor_handler.h index 37774e9..db237e4 100644 --- a/src/server/sensor_handler.h +++ b/src/server/sensor_handler.h @@ -52,11 +52,17 @@ public: virtual int flush(sensor_observer *ob) = 0; virtual int get_data(sensor_data_t **data, int *len) = 0; + void set_cache(sensor_data_t *data, int size); + int get_cache(sensor_data_t **data, int *len); + protected: sensor_info m_info; private: std::list m_observers; + + sensor_data_t *m_last_data; + int m_last_data_size; }; } diff --git a/src/server/sensor_listener_proxy.cpp b/src/server/sensor_listener_proxy.cpp index a850018..2a704cd 100644 --- a/src/server/sensor_listener_proxy.cpp +++ b/src/server/sensor_listener_proxy.cpp @@ -191,8 +191,7 @@ int sensor_listener_proxy::get_data(sensor_data_t **data, int *len) sensor_handler *sensor = m_manager->get_sensor(m_uri); retv_if(!sensor, -EINVAL); - /* TODO : caching the last data & retry logic if there is no data */ - return sensor->get_data(data, len); + return sensor->get_cache(data, len); } std::string sensor_listener_proxy::get_required_privileges(void) -- 2.7.4 From ac8515b9e3ac4678dfda3b73f5b2a63d5cd6ddb4 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Mon, 13 Nov 2017 12:09:55 +0900 Subject: [PATCH 14/16] sensor: gyro-rv: fix invalid order of sensor data Change-Id: I7554129c77434aca6eb5871dc4d0ad28597d91a1 Signed-off-by: kibak.yoon --- src/sensor/rotation_vector/gyro_rv_sensor.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/sensor/rotation_vector/gyro_rv_sensor.cpp b/src/sensor/rotation_vector/gyro_rv_sensor.cpp index 1a0aaaa..8f2d9bb 100644 --- a/src/sensor/rotation_vector/gyro_rv_sensor.cpp +++ b/src/sensor/rotation_vector/gyro_rv_sensor.cpp @@ -109,10 +109,10 @@ int gyro_rv_sensor::get_data(sensor_data_t **data, int *length) sensor_data->accuracy = m_accuracy; sensor_data->timestamp = m_time; sensor_data->value_count = 4; - sensor_data->values[0] = m_w; - sensor_data->values[1] = m_x; - sensor_data->values[2] = m_y; - sensor_data->values[3] = m_z; + sensor_data->values[0] = m_x; + sensor_data->values[1] = m_y; + sensor_data->values[2] = m_z; + sensor_data->values[3] = m_w; *data = sensor_data; *length = sizeof(sensor_data_t); -- 2.7.4 From 8ac01686ee36a4159e44e3538f8b5e9f33ecf612 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Thu, 16 Nov 2017 21:06:48 +0900 Subject: [PATCH 15/16] sensord: resize message size - if there are so many sensors in the device, memory size 4096 is too small to contain all of that sensor information. for example, Gear S3 has 43 sensors including virtual sensors. it needs 7000 byte for information of all sensors. - so message size should be resized from 4096(4kB) to 10240(10kB). Change-Id: Ied553e260216258295d2337a6f0c468e4a2490cc Signed-off-by: kibak.yoon --- src/shared/command_types.h | 2 +- src/shared/message.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/shared/command_types.h b/src/shared/command_types.h index a994f29..34cb5fc 100644 --- a/src/shared/command_types.h +++ b/src/shared/command_types.h @@ -24,7 +24,7 @@ #include "sensor_info.h" #define SENSOR_CHANNEL_PATH "/run/.sensord.socket" -#define MAX_BUF_SIZE 4096 +#define MAX_BUF_SIZE 10240 /* TODO: OOP - create serializer interface */ enum cmd_type_e { diff --git a/src/shared/message.h b/src/shared/message.h index 111cd9b..3b8fb67 100644 --- a/src/shared/message.h +++ b/src/shared/message.h @@ -23,7 +23,7 @@ #include /* size_t */ #include -#define MAX_MSG_CAPACITY 4096 +#define MAX_MSG_CAPACITY 10240 #define MAX_HEADER_RESERVED 3 namespace ipc { -- 2.7.4 From 90027075c1edab60bb22f525fdcede09e6e39707 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Thu, 16 Nov 2017 21:14:03 +0900 Subject: [PATCH 16/16] sensord: change the style to URI if it is unknown type - Bug Fix Change-Id: Id56d8bad087a13db8dda4e441b75e0969235604b Signed-off-by: kibak.yoon --- src/shared/sensor_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/shared/sensor_utils.cpp b/src/shared/sensor_utils.cpp index 9cd8141..9bae0e6 100644 --- a/src/shared/sensor_utils.cpp +++ b/src/shared/sensor_utils.cpp @@ -128,7 +128,7 @@ const char *sensor::utils::get_uri(sensor_type_t type) { auto it = types.find(type); if (it == types.end()) - return "Unknown Type"; + return types[UNKNOWN_SENSOR]; return it->second; } -- 2.7.4