From 17238ab0c43b389fb66869f938e61d071e791d37 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Wed, 17 May 2017 13:48:38 +0900 Subject: [PATCH] sensord: enable samsung pedometer sensor for fused location - URI = http://samsung.com/sensor/healthinfo/pedometer/samsung_pedometer - Model = samsung_pedometer - Vendor = samsung.com Change-Id: Ic6220e0001bc5923e392a7a0dec99860ba47227d Signed-off-by: kibak.yoon --- src/sensor/CMakeLists.txt | 6 + src/sensor/create.cpp | 7 + src/sensor/pedometer/average_filter.cpp | 64 +++++ src/sensor/pedometer/average_filter.h | 48 ++++ src/sensor/pedometer/common.h | 25 ++ src/sensor/pedometer/pedometer.cpp | 84 +++++++ src/sensor/pedometer/pedometer.h | 75 ++++++ src/sensor/pedometer/pedometer_info.h | 46 ++++ src/sensor/pedometer/pedometer_sensor.cpp | 134 ++++++++++ src/sensor/pedometer/pedometer_sensor.h | 50 ++++ src/sensor/pedometer/pedometer_speed_filter.cpp | 161 ++++++++++++ src/sensor/pedometer/pedometer_speed_filter.h | 116 +++++++++ src/sensor/pedometer/savitzky_golay_filter15.cpp | 71 ++++++ src/sensor/pedometer/savitzky_golay_filter15.h | 50 ++++ src/sensor/pedometer/step_detection.cpp | 271 +++++++++++++++++++++ src/sensor/pedometer/step_detection.h | 82 +++++++ src/sensor/pedometer/step_event.h | 33 +++ .../pedometer/zero_crossing_step_detection.cpp | 65 +++++ .../pedometer/zero_crossing_step_detection.h | 56 +++++ 19 files changed, 1444 insertions(+) create mode 100644 src/sensor/pedometer/average_filter.cpp create mode 100644 src/sensor/pedometer/average_filter.h create mode 100644 src/sensor/pedometer/common.h create mode 100644 src/sensor/pedometer/pedometer.cpp create mode 100644 src/sensor/pedometer/pedometer.h create mode 100644 src/sensor/pedometer/pedometer_info.h create mode 100644 src/sensor/pedometer/pedometer_sensor.cpp create mode 100644 src/sensor/pedometer/pedometer_sensor.h create mode 100644 src/sensor/pedometer/pedometer_speed_filter.cpp create mode 100644 src/sensor/pedometer/pedometer_speed_filter.h create mode 100644 src/sensor/pedometer/savitzky_golay_filter15.cpp create mode 100644 src/sensor/pedometer/savitzky_golay_filter15.h create mode 100644 src/sensor/pedometer/step_detection.cpp create mode 100644 src/sensor/pedometer/step_detection.h create mode 100644 src/sensor/pedometer/step_event.h create mode 100644 src/sensor/pedometer/zero_crossing_step_detection.cpp create mode 100644 src/sensor/pedometer/zero_crossing_step_detection.h diff --git a/src/sensor/CMakeLists.txt b/src/sensor/CMakeLists.txt index 9735d8d..388cb3a 100644 --- a/src/sensor/CMakeLists.txt +++ b/src/sensor/CMakeLists.txt @@ -7,6 +7,7 @@ SET(GRAVITY "ON") SET(LINEAR_ACCEL "ON") SET(RV "ON") SET(ORIENTATION "ON") +SET(PEDOMETER "ON") INCLUDE_DIRECTORIES( ${CMAKE_SOURCE_DIR}/src/shared @@ -59,6 +60,11 @@ FILE(GLOB_RECURSE SRCS ${SRCS} orientation/*.cpp) ADD_DEFINITIONS(-DENABLE_ORIENTATION) ENDIF() +IF("${PEDOMETER}" STREQUAL "ON") +FILE(GLOB_RECURSE SRCS ${SRCS} pedometer/*.cpp) +ADD_DEFINITIONS(-DENABLE_PEDOMETER) +ENDIF() + MESSAGE("Sources: ${SRCS}") ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS}) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PLUGINS_PKGS_LDFLAGS}) diff --git a/src/sensor/create.cpp b/src/sensor/create.cpp index 0dd39b5..3c9b372 100644 --- a/src/sensor/create.cpp +++ b/src/sensor/create.cpp @@ -37,6 +37,9 @@ #ifdef ENABLE_ORIENTATION #include "orientation/orientation_sensor.h" #endif +#ifdef ENABLE_PEDOMETER +#include "pedometer/pedometer_sensor.h" +#endif static std::vector sensors; @@ -82,6 +85,10 @@ extern "C" int create(fusion_sensor_t **fsensors) create_sensor("Orientation Sensor"); #endif +#ifdef ENABLE_PEDOMETER + create_sensor("Pedometer"); +#endif + *fsensors = &sensors[0]; return sensors.size(); } diff --git a/src/sensor/pedometer/average_filter.cpp b/src/sensor/pedometer/average_filter.cpp new file mode 100644 index 0000000..81ca075 --- /dev/null +++ b/src/sensor/pedometer/average_filter.cpp @@ -0,0 +1,64 @@ +/* + * 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 "average_filter.h" + +#include + +static double mean(double *array, int size) +{ + double avrg = 0; + + for (int i = 0; i < size; ++i) + avrg = avrg + array[i]; + + return avrg / size; +} + +average_filter::average_filter(int sz) +: m_size(sz) +, m_index(0) +, m_ready(false) +{ + m_array = (double *)calloc(sz, sizeof(double)); +} + +average_filter::~average_filter() +{ + if (m_array == NULL) + return; + + free(m_array); + m_array = NULL; + m_size = 0; +} + +double average_filter::filter(double value) +{ + m_array[m_index++] = value; + + if (m_index >= m_size) { + m_ready = true; + m_index = 0; + } + return mean(m_array, (m_ready ? m_size : m_index)); +} + +void average_filter::reset(void) +{ + m_index = 0; + m_ready = false; +} diff --git a/src/sensor/pedometer/average_filter.h b/src/sensor/pedometer/average_filter.h new file mode 100644 index 0000000..e083ce0 --- /dev/null +++ b/src/sensor/pedometer/average_filter.h @@ -0,0 +1,48 @@ +/* + * 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 __AVERAGE_FILTER_H__ +#define __AVERAGE_FILTER_H__ + +#include +#include "common.h" + +class average_filter { +public: + average_filter(int size); + ~average_filter(); + + /************************************************************************ + * Filters input data. + */ + double filter(double value); + + /************************************************************************ + * Resets average filter to initial state. + */ + void reset(void); + +private: + /************************************************************************ + * Average filter state. + */ + double *m_array; + int m_size; + int m_index; + bool m_ready; +}; + +#endif /* __AVERAGE_FILTER_H__ */ diff --git a/src/sensor/pedometer/common.h b/src/sensor/pedometer/common.h new file mode 100644 index 0000000..b1a0fef --- /dev/null +++ b/src/sensor/pedometer/common.h @@ -0,0 +1,25 @@ +/* + * 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 __PEDOMETER_COMMON_H__ +#define __PEDOMETER_COMMON_H__ + +#include + +typedef long long timestamp_t; +#define UNKNOWN_TIMESTAMP ((long long)0x8000000000000000) + +#endif /* __PEDOMETER_COMMON_H__ */ diff --git a/src/sensor/pedometer/pedometer.cpp b/src/sensor/pedometer/pedometer.cpp new file mode 100644 index 0000000..b642746 --- /dev/null +++ b/src/sensor/pedometer/pedometer.cpp @@ -0,0 +1,84 @@ +/* + * 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 "pedometer.h" +#include "step_event.h" + +#include + +pedometer::pedometer() +: m_step_detection() +, m_total_length(0) +, m_step_count(0) +, m_pedometer_filter() +, m_some_speed(false) +{ +} + +pedometer::~pedometer() +{ +} + +void pedometer::set_savitzky_filter(bool enable) +{ + m_step_detection.set_use_savitzky(enable); +} + +void pedometer::reset(void) +{ + m_total_length = 0; + m_step_count = 0; + m_step_detection.reset(); + m_pedometer_filter.reset(); + m_some_speed = false; +} + +bool pedometer::get_pedometer(timestamp_t timestamp, double acc, pedometer_info *info) +{ + bool result = false; + 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; + } + } + if (m_some_speed) { + double speed = m_pedometer_filter.get_speed(timestamp); + if (speed == 0) { + m_some_speed = false; + info->timestamp = timestamp; + info->is_step_detected = true; + info->step_count = m_step_count; + info->step_length = 0; + info->total_step_length = m_total_length; + info->step_speed = 0; + result = true; + } + } + return result; +} diff --git a/src/sensor/pedometer/pedometer.h b/src/sensor/pedometer/pedometer.h new file mode 100644 index 0000000..3f904d7 --- /dev/null +++ b/src/sensor/pedometer/pedometer.h @@ -0,0 +1,75 @@ +/* + * 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 __PEDOMETER_H__ +#define __PEDOMETER_H__ + +#include "common.h" +#include "step_detection.h" +#include "pedometer_info.h" +#include "pedometer_speed_filter.h" + +/************************************************************************ + * stores pedometer engine state. + */ +class pedometer { +public: + pedometer(); + ~pedometer(); + + /************************************************************************ + * enables/disables savitzky filter. + */ + void set_savitzky_filter(bool enable); + + /************************************************************************ + * resets {@link pedometer} object to initial state. + */ + void reset(void); + + /************************************************************************ + * called on new acceleration event. + * + * @param info + * result of pedometer algorithm. valid only it method returns true. + * @param timestamp + * timestamp of acceleration event in ns. + * @param acc + * vertical component of global acceleration. + * + * @result + * true if new step event was detected. + */ + bool get_pedometer(timestamp_t timestamp, double acc, pedometer_info *info); + +private: + /** detects step and estimates step length. */ + step_detection m_step_detection; + + /** sum of lengths all steps from start. */ + double m_total_length; + + /** number of steps from start. */ + long long m_step_count; + + /** estimates current speed from step length. */ + pedometer_speed_filter m_pedometer_filter; + + /** some non zero speed was detected. */ + bool m_some_speed; +}; + +#endif /* __PEDOMETER_H__ */ diff --git a/src/sensor/pedometer/pedometer_info.h b/src/sensor/pedometer/pedometer_info.h new file mode 100644 index 0000000..ac521db --- /dev/null +++ b/src/sensor/pedometer/pedometer_info.h @@ -0,0 +1,46 @@ +/* + * 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 __PEDOMETER_INFO_H__ +#define __PEDOMETER_INFO_H__ + +#include "common.h" + +/************************************************************************ + * stores information about pedometer event detected. + */ +class pedometer_info { +public: + /** timestamp this event was detected in ns. */ + timestamp_t timestamp; + + /** is step detected. */ + bool is_step_detected; + + /** step count from scanner start. */ + long long step_count; + + /** step length in meters. */ + double step_length; + + /** total length of all steps detected from scanner start in meters. */ + double total_step_length; + + /** current mean speed in m/s. */ + double step_speed; +}; + +#endif /* __PEDOMETER_INFO_H__ */ diff --git a/src/sensor/pedometer/pedometer_sensor.cpp b/src/sensor/pedometer/pedometer_sensor.cpp new file mode 100644 index 0000000..bbd9b67 --- /dev/null +++ b/src/sensor/pedometer/pedometer_sensor.cpp @@ -0,0 +1,134 @@ +/* + * Copyright (c) 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 "pedometer_sensor.h" + +#include +#include +#include +#include + +#define NAME_SENSOR "http://samsung.com/sensor/healthinfo/pedometer/samsung_pedometer" +#define NAME_VENDOR "samsung.com" + +#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 */ +static sensor_info2_t sensor_info = { + id: 0x1, + type: HUMAN_PEDOMETER_SENSOR, + uri: NAME_SENSOR, + vendor: NAME_VENDOR, + min_range: 0, + max_range: 1, + resolution: 1, + min_interval: 0, + max_batch_count: 0, + wakeup_supported: false, + privilege: "http://tizen.org/privilege/healthinfo", +}; + +/* Required sensor list */ +static required_sensor_s required_sensors[] = { + {SRC_ID_ACC, SRC_STR_ACC} +}; + +pedometer_sensor::pedometer_sensor() +: m_step_count(-1) +, m_step_length(-1) +, m_step_total_length(-1) +, m_step_speed(-1) +, m_time(0) +{ +} + +pedometer_sensor::~pedometer_sensor() +{ +} + +int pedometer_sensor::get_sensor_info(const sensor_info2_t **info) +{ + *info = &sensor_info; + return OP_SUCCESS; +} + +int pedometer_sensor::get_required_sensors(const required_sensor_s **sensors) +{ + *sensors = required_sensors; + + /* You should return the number of required sensor */ + return 1; +} + +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]); + + if (!m_pedometer.get_pedometer(US_TO_NS(data->timestamp), acc, &info)) + return OP_ERROR; + + m_step_count = info.step_count; + m_step_length = info.step_length; + m_step_total_length = info.total_step_length; + m_step_speed = info.step_speed; + m_time = data->timestamp; + + _D("[%lld] %lld %f %f %f", data->timestamp, + info.step_count, info.step_length, info.total_step_length, info.step_speed); + + return OP_SUCCESS; +} + +int pedometer_sensor::get_data(sensor_data_t **data, int *len) +{ + sensor_data_t *sensor_data; + sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + + sensor_data->accuracy = SENSOR_ACCURACY_GOOD; + sensor_data->timestamp = m_time; + sensor_data->value_count = 8; + sensor_data->values[0] = (float)m_step_count; + sensor_data->values[1] = (float)m_step_count; + sensor_data->values[2] = 0; + sensor_data->values[3] = m_step_total_length; + sensor_data->values[4] = 0; + sensor_data->values[5] = m_step_speed; + sensor_data->values[6] = 0; + sensor_data->values[7] = 0; + + *data = sensor_data; + *len = sizeof(sensor_data_t); + + return 0; +} + +int pedometer_sensor::start(observer_h ob) +{ + m_pedometer.reset(); + return OP_DEFAULT; +} + +int pedometer_sensor::stop(observer_h ob) +{ + return OP_DEFAULT; +} diff --git a/src/sensor/pedometer/pedometer_sensor.h b/src/sensor/pedometer/pedometer_sensor.h new file mode 100644 index 0000000..32153b3 --- /dev/null +++ b/src/sensor/pedometer/pedometer_sensor.h @@ -0,0 +1,50 @@ +/* + * Copyright (c) 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 __PEDOMETER_SENSOR_H__ +#define __PEDOMETER_SENSOR_H__ + +#include +#include + +#include "pedometer.h" + +class pedometer_sensor : public fusion_sensor { +public: + pedometer_sensor(); + virtual ~pedometer_sensor(); + + int get_sensor_info(const sensor_info2_t **info); + int get_required_sensors(const required_sensor_s **sensors); + + int update(uint32_t id, sensor_data_t *data, int len); + int get_data(sensor_data_t **data, int *len); + + int start(observer_h ob); + int stop(observer_h ob); + +private: + long long m_step_count; + float m_step_length; + float m_step_total_length; + float m_step_speed; + unsigned long long m_time; + + pedometer m_pedometer; +}; + +#endif /* __PEDOMETER_SENSOR_H__ */ diff --git a/src/sensor/pedometer/pedometer_speed_filter.cpp b/src/sensor/pedometer/pedometer_speed_filter.cpp new file mode 100644 index 0000000..d6ac774 --- /dev/null +++ b/src/sensor/pedometer/pedometer_speed_filter.cpp @@ -0,0 +1,161 @@ +/* + * 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 "pedometer_speed_filter.h" + +/** default for maximum step duration in ns. currently 2s. */ +#define STEP_MAX_DURATION 2000000000L + +pedometer_speed_filter::pedometer_speed_filter() +: m_last_timestamp(UNKNOWN_TIMESTAMP) +, m_current_speed(0) +, m_last_step_length(0) +, m_last_step_duration(0) +, m_step_max_duration(STEP_MAX_DURATION) +{ +} + +pedometer_speed_filter::~pedometer_speed_filter() +{ +} + +/************************************************************************ + * clears current speed, last step length and last step duration. + */ +void pedometer_speed_filter::clear_speed(void) +{ + m_current_speed = 0; + m_last_step_length = 0; + m_last_step_duration = 0; +} + +/************************************************************************ + * 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. + * 0 to disable step duration checking. + */ +void pedometer_speed_filter::set_step_max_duration(long long step_max_duration) +{ + m_step_max_duration = step_max_duration; +} + +/************************************************************************ + * called when new step detection event occurs. + * + * @param timestamp + * timestamp of step detection event in ns. + * @param steplength + * length of detected step in m. + */ +void pedometer_speed_filter::get_step(timestamp_t timestamp, double step_length) +{ + if (m_last_timestamp == UNKNOWN_TIMESTAMP || timestamp == UNKNOWN_TIMESTAMP) { + clear_speed(); + } else if (m_step_max_duration != 0 && timestamp - m_last_timestamp > m_step_max_duration) { + clear_speed(); + } else if (m_last_timestamp < timestamp) { + double step_duration = (timestamp - m_last_timestamp) / 1e9; + m_current_speed = + (step_length + m_last_step_length) / (step_duration + m_last_step_duration); + m_last_step_length = step_length; + m_last_step_duration = step_duration; + } else { + return; + } + m_last_timestamp = timestamp; +} + +/************************************************************************ + * reports new speed. + * + * @param timestamp + * timestamp of speed event. + * @param speed + * current speed in m/s. + */ +void pedometer_speed_filter::new_speed(timestamp_t timestamp, double speed) +{ + if (m_last_timestamp == UNKNOWN_TIMESTAMP || timestamp == UNKNOWN_TIMESTAMP + || timestamp > m_last_timestamp) { + m_last_timestamp = timestamp; + m_current_speed = speed; + m_last_step_length = 0; + m_last_step_duration = 0; + } +} + +/************************************************************************ + * returns current speed. + * + * @param timestamp + * timestamp for which speed should be calculated. + * @return speed for given timestamp in m/s. + */ +double pedometer_speed_filter::get_speed(timestamp_t timestamp) +{ + if (m_step_max_duration != 0 && (m_last_timestamp == UNKNOWN_TIMESTAMP || + timestamp == UNKNOWN_TIMESTAMP || + timestamp - m_last_timestamp > m_step_max_duration)) { + clear_speed(); + } + return m_current_speed; +} + +/************************************************************************ + * changes current speed. + * + * @param speed + * current speed in m/s. + */ +void pedometer_speed_filter::set_current_speed(double speed) +{ + m_current_speed = speed; +} + +/************************************************************************ + * @return estimated current speed in m/s. + */ +double pedometer_speed_filter::get_current_speed(void) +{ + return m_current_speed; +} + +/************************************************************************ + */ +bool pedometer_speed_filter::is_known_timestamp(void) +{ + return m_last_timestamp != UNKNOWN_TIMESTAMP; +} + +/************************************************************************ + */ +timestamp_t pedometer_speed_filter::get_timestamp(void) +{ + return m_last_timestamp; +} + +/************************************************************************ + * resets filter to initial state. + */ +void pedometer_speed_filter::reset(void) +{ + m_last_timestamp = UNKNOWN_TIMESTAMP; + clear_speed(); +} diff --git a/src/sensor/pedometer/pedometer_speed_filter.h b/src/sensor/pedometer/pedometer_speed_filter.h new file mode 100644 index 0000000..4c6fde1 --- /dev/null +++ b/src/sensor/pedometer/pedometer_speed_filter.h @@ -0,0 +1,116 @@ +/* + * 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 __PEDOMETER_SPEED_FILTER_H__ +#define __PEDOMETER_SPEED_FILTER_H__ + +#include "common.h" + +/************************************************************************ + * stores pedometer speed filter state. + */ +class pedometer_speed_filter { +public: + pedometer_speed_filter(); + ~pedometer_speed_filter(); + + void clear_speed(void); + + /************************************************************************ + * 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. + * 0 to disable step duration checking. + */ + void 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. + * @param steplength + * length of detected step in m. + */ + void get_step(timestamp_t timestamp, double step_length); + + /************************************************************************ + * reports new speed. + * + * @param timestamp + * timestamp of speed event. + * @param speed + * current speed in m/s. + */ + void new_speed(timestamp_t timestamp, double speed); + + /************************************************************************ + * returns current speed. + * + * @param timestamp + * timestamp for which speed should be calculated. + * @return speed for given timestamp in m/s. + */ + double get_speed(timestamp_t timestamp); + + /************************************************************************ + * changes current speed. + * + * @param speed + * current speed in m/s. + */ + void set_current_speed(double speed); + + /************************************************************************ + * @return estimated current speed in m/s. + */ + double get_current_speed(void); + + /************************************************************************ + */ + bool is_known_timestamp(void); + + /************************************************************************ + * @return timestamp of last step detection event in ns. + */ + timestamp_t get_timestamp(void); + + /************************************************************************ + * resets filter to initial state. + */ + void reset(void); + +private: + /** timestamp of last step detection event in ns. */ + timestamp_t m_last_timestamp; + + /** estimated current speed in m/s. */ + double m_current_speed; + + /** length of last step in m. */ + double m_last_step_length; + + /** duration of last step in s. */ + double m_last_step_duration; + + /** maximum step duration in ns. 0 to disable step duration checking. */ + long long m_step_max_duration; +}; + +#endif /* __PEDOMETER_SPEED_FILTER_H__ */ diff --git a/src/sensor/pedometer/savitzky_golay_filter15.cpp b/src/sensor/pedometer/savitzky_golay_filter15.cpp new file mode 100644 index 0000000..ced5d8f --- /dev/null +++ b/src/sensor/pedometer/savitzky_golay_filter15.cpp @@ -0,0 +1,71 @@ +/* + * 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 "savitzky_golay_filter15.h" + +#include + +/* length of filter. changing it requires changing coef_array! */ +#define ARRAY_SIZE 15 + +/* sum of numerators of elements in coef_array. */ +#define SUM_COEF 1105.0 + +/* array with coefficients for savitzky-golay filter. must be length of n! */ +static double coef_array[] = { + -78 / SUM_COEF, -13 / SUM_COEF, 42 / SUM_COEF, 87 / SUM_COEF, + 122 / SUM_COEF, 147 / SUM_COEF, 162 / SUM_COEF, 167 / SUM_COEF, + 162 / SUM_COEF, 147 / SUM_COEF, 122 / SUM_COEF, 87 / SUM_COEF, + 42 / SUM_COEF, -13 / SUM_COEF, -78 / SUM_COEF }; + +savitzky_golay_filter15::savitzky_golay_filter15() +: m_empty(true) +{ + m_array = (double *)calloc(ARRAY_SIZE, sizeof(double)); +} + +savitzky_golay_filter15::~savitzky_golay_filter15() +{ + if (m_array == NULL) + return; + + free(m_array); + m_array = NULL; +} + +double savitzky_golay_filter15::filter(double value) +{ + if (m_empty) { + for (int i = 0; i < ARRAY_SIZE; i++) + m_array[i] = value; + m_empty = false; + return value; + } + + for (int i = 1; i < ARRAY_SIZE; i++) + m_array[i - 1] = m_array[i]; + m_array[ARRAY_SIZE - 1] = value; + + double avrg = 0; + for (int i = 0; i < ARRAY_SIZE; i++) + avrg += m_array[i] * coef_array[i]; + return avrg; +} + +void savitzky_golay_filter15::reset(void) +{ + m_empty = true; +} diff --git a/src/sensor/pedometer/savitzky_golay_filter15.h b/src/sensor/pedometer/savitzky_golay_filter15.h new file mode 100644 index 0000000..a9a8899 --- /dev/null +++ b/src/sensor/pedometer/savitzky_golay_filter15.h @@ -0,0 +1,50 @@ +/* + * 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 __SAVITZKYGOLAYFILTER15_H__ +#define __SAVITZKYGOLAYFILTER15_H__ + +#include +#include "common.h" + +/************************************************************************ + * stores savitzky-golay filter state. + */ +class savitzky_golay_filter15 { +public: + savitzky_golay_filter15(); + ~savitzky_golay_filter15(); + + /************************************************************************ + * filters input data. + * + * @param value + * data to filter. + * @result filtered data. + */ + double filter(double value); + + /************************************************************************ + * resets savitzky-golay filter to initial state. + */ + void reset(void); + +private: + double *m_array; + bool m_empty; +}; + +#endif /* __SAVITZKYGOLAYFILTER15_H__ */ diff --git a/src/sensor/pedometer/step_detection.cpp b/src/sensor/pedometer/step_detection.cpp new file mode 100644 index 0000000..afabc84 --- /dev/null +++ b/src/sensor/pedometer/step_detection.cpp @@ -0,0 +1,271 @@ +/* + * 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 "step_detection.h" + +#include +#include + +/* Size of average filter. */ +#define AV_FILTER_SIZE 7 + +#define AV_GFILTER_SIZE 500 + +#define FAST_PEAK_THRESHOLD 0.77 + +#define SLOW_PEAK_THRESHOLD 0.3 + +#define PEAK_THRESHOLD_FOR_MEAN 2.0 + +#define B_COEF 1.08 + +#define SLOPE_PARAM -0.75 + +step_detection::step_detection() +: m_average_filter(AV_FILTER_SIZE) +, m_average_gfilter(AV_GFILTER_SIZE) +, m_zero_crossing_up(true) +, m_zero_crossing_down(false) +, m_zc_filter() +, m_peak_threshold(FAST_PEAK_THRESHOLD) +, m_use_savitzky(false) +, m_last_step_timestamp(UNKNOWN_TIMESTAMP) +, m_zero_crossing_up_detected(false) +, m_zero_crossing_down_detected(false) +, m_minimum_acceleration(0) +, m_maximum_acceleration(0) +, m_is_slow_step_detected(false) +{ +} + +step_detection::~step_detection() +{ +} + +/************************************************************************ + */ +void step_detection::reset(void) +{ + m_average_filter.reset(); + m_average_gfilter.reset(); + m_zero_crossing_up.reset(); + m_zero_crossing_down.reset(); + + m_average_gfilter.filter(9.81); + m_zc_filter.reset(); + + m_zero_crossing_up_detected = false; + m_zero_crossing_down_detected = false; + m_minimum_acceleration = 0; + m_maximum_acceleration = 0; + m_is_slow_step_detected = false; +} + +/************************************************************************ + */ +void step_detection::set_peak_threshold(double threshold) +{ + m_peak_threshold = threshold; +} + +/************************************************************************ + */ +void step_detection::set_use_savitzky(bool use_savitzky) +{ + m_use_savitzky = use_savitzky; +} + +/************************************************************************ + */ +bool step_detection::is_slow_step(void) +{ + return m_is_slow_step_detected; +} + +/************************************************************************ + */ +static double cal_step_length(double time, double sqrt4peak_valley_diff) +{ + double step_length = 0; + if (time <= 0 || time > 1.00) { + step_length = 0.50 * sqrt4peak_valley_diff; + } else if (time < 0.3) { + step_length = 0; + } else { + step_length = B_COEF + SLOPE_PARAM * time; + } + + if (step_length > 1.5) + step_length = 0; + + return step_length; +} + +/************************************************************************ + */ +bool step_detection::add_acc_sensor_values_average( + timestamp_t timestamp, double acc, step_event* step) +{ + double acceleration = m_average_filter.filter(acc - 9.8066f); + + bool n_zero_up = m_zero_crossing_up.detect_step(timestamp, acceleration); + bool n_zero_down = m_zero_crossing_down.detect_step(timestamp, acceleration); + + double sqrt4peak_valley_diff = 0; + bool is_step_detected = false; + + if (n_zero_up) { + m_zero_crossing_up_detected = true; + m_zero_crossing_down_detected = false; + } + if (n_zero_down) { + m_zero_crossing_up_detected = false; + m_zero_crossing_down_detected = true; + } + if (m_zero_crossing_up_detected) { + if (m_maximum_acceleration < acceleration) { + m_maximum_acceleration = acceleration; + } + } + if (m_zero_crossing_down_detected) { + if (m_minimum_acceleration > acceleration) { + m_minimum_acceleration = acceleration; + } + } + + double peak_threshold; + if (m_zero_crossing_up.m_time_sum / 1E9 < 1.2) { + peak_threshold = m_peak_threshold; + m_is_slow_step_detected = false; + } else { + peak_threshold = SLOW_PEAK_THRESHOLD; + m_is_slow_step_detected = true; + } + + if (n_zero_down) { + if (m_maximum_acceleration > peak_threshold + || (m_maximum_acceleration - m_minimum_acceleration > PEAK_THRESHOLD_FOR_MEAN)) { + sqrt4peak_valley_diff = pow(m_maximum_acceleration - m_minimum_acceleration, 0.25); + m_minimum_acceleration = 0; + m_maximum_acceleration = 0; + is_step_detected = true; + } + } + + if (m_zero_crossing_up.m_time_sum / 1E9 < 0.3) + is_step_detected = false; + + if (is_step_detected) { + if (m_last_step_timestamp == UNKNOWN_TIMESTAMP) + m_last_step_timestamp = timestamp; + + double time = (timestamp - m_last_step_timestamp) / 1E9; + m_last_step_timestamp = timestamp; + 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; + } + return false; +} + +/************************************************************************ + */ +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)); + bool n_zero_up = m_zero_crossing_up.detect_step(timestamp, acceleration); + bool n_zero_down = m_zero_crossing_down.detect_step(timestamp, acceleration); + + double sqrt4peak_valley_diff = 0; + bool is_step_detected = false; + + if (n_zero_up) { + m_zero_crossing_up_detected = true; + m_zero_crossing_down_detected = false; + } + if (n_zero_down) { + m_zero_crossing_up_detected = false; + m_zero_crossing_down_detected = true; + } + if (m_zero_crossing_up_detected) { + if (m_maximum_acceleration < acceleration) { + m_maximum_acceleration = acceleration; + } + } + if (m_zero_crossing_down_detected) { + if (m_minimum_acceleration > acceleration) { + m_minimum_acceleration = acceleration; + } + } + + bool zup = m_zero_crossing_up.m_time_sum / 1E9 > 1.2; + if (n_zero_down) { + is_step_detected = false; + + if ((m_maximum_acceleration > 0.6 && + m_minimum_acceleration < -0.852) + || (m_maximum_acceleration > 0.714 + && m_minimum_acceleration < -0.455)) { + is_step_detected = true; + m_is_slow_step_detected = false; + } + if (m_maximum_acceleration - m_minimum_acceleration > 3.32) { + is_step_detected = true; + m_is_slow_step_detected = false; + } + if (zup && m_maximum_acceleration > 0.764 && m_minimum_acceleration < -0.0235) {// slow steps + is_step_detected = true; + m_is_slow_step_detected = true; + } + + if (is_step_detected) { + sqrt4peak_valley_diff = pow(m_maximum_acceleration - m_minimum_acceleration, 0.25); + m_minimum_acceleration = 0.0; + m_maximum_acceleration = 0.0; + } + } + + if (m_zero_crossing_up.m_time_sum / 1E9 < 0.3) + is_step_detected = false; + + if (is_step_detected) { + if (m_last_step_timestamp == UNKNOWN_TIMESTAMP) + m_last_step_timestamp = timestamp; + + double time = (timestamp - m_last_step_timestamp) / 1E9; + + m_last_step_timestamp = timestamp; + 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; + } + + return false; +} + +/************************************************************************ + */ +bool step_detection::get_step(timestamp_t timestamp, double acc, step_event* step) +{ + return m_use_savitzky + ? add_acc_sensor_values_savitzky(timestamp, acc, step) + : add_acc_sensor_values_average(timestamp, acc, step); +} diff --git a/src/sensor/pedometer/step_detection.h b/src/sensor/pedometer/step_detection.h new file mode 100644 index 0000000..addd786 --- /dev/null +++ b/src/sensor/pedometer/step_detection.h @@ -0,0 +1,82 @@ +/* + * 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 __STEP_DETECTION_H__ +#define __STEP_DETECTION_H__ + +#include "average_filter.h" +#include "zero_crossing_step_detection.h" +#include "savitzky_golay_filter15.h" +#include "step_event.h" +#include "common.h" + +/************************************************************************ + * step detection engine state. + */ +class step_detection { +public: + step_detection(); + ~step_detection(); + + /************************************************************************ + */ + void set_peak_threshold(double threshold); + + /************************************************************************ + */ + void set_use_savitzky(bool use_savitzky); + + /************************************************************************ + */ + 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); + + /************************************************************************ + * resets step_detection object to initial state. + */ + void reset(void); + +private: + average_filter m_average_filter; + average_filter m_average_gfilter; + zero_crossing_step_detection m_zero_crossing_up; + zero_crossing_step_detection m_zero_crossing_down; + + savitzky_golay_filter15 m_zc_filter; + double m_peak_threshold; + bool m_use_savitzky; + timestamp_t m_last_step_timestamp; + bool m_zero_crossing_up_detected; + bool m_zero_crossing_down_detected; + double m_minimum_acceleration; + double m_maximum_acceleration; + bool m_is_slow_step_detected; +}; + +#endif /* __STEP_DETECTION_H__ */ diff --git a/src/sensor/pedometer/step_event.h b/src/sensor/pedometer/step_event.h new file mode 100644 index 0000000..724f262 --- /dev/null +++ b/src/sensor/pedometer/step_event.h @@ -0,0 +1,33 @@ +/* + * 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 __STEP_EVENT_H__ +#define __STEP_EVENT_H__ + +#include "common.h" + +class step_event { +public: + step_event() + : m_timestamp(UNKNOWN_TIMESTAMP) + , m_step_length(0) + {} + + timestamp_t m_timestamp; + double m_step_length; +}; + +#endif /* __STEP_EVENT_H__ */ diff --git a/src/sensor/pedometer/zero_crossing_step_detection.cpp b/src/sensor/pedometer/zero_crossing_step_detection.cpp new file mode 100644 index 0000000..aff2b3c --- /dev/null +++ b/src/sensor/pedometer/zero_crossing_step_detection.cpp @@ -0,0 +1,65 @@ +/* + * 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 "zero_crossing_step_detection.h" + +static bool detect_zero_crossing(double last_accel, double new_accel, bool up) +{ + if (up) + return (last_accel < 0 && new_accel >= 0); + + return (last_accel > 0 && new_accel <= 0); +} + +zero_crossing_step_detection::zero_crossing_step_detection(bool up) +: m_time_sum(0) +, m_up(up) +, m_last_acceleration(0) +, m_last_timestamp(UNKNOWN_TIMESTAMP) +, m_last_zero_crossing_time(UNKNOWN_TIMESTAMP) +{ +} + +zero_crossing_step_detection::~zero_crossing_step_detection() +{ +} + +bool zero_crossing_step_detection::detect_step(timestamp_t timestamp, double acceleration) +{ + bool step_detected = false; + + if (m_last_timestamp != UNKNOWN_TIMESTAMP) { + // zero crossing detected + if (detect_zero_crossing(m_last_acceleration, acceleration, m_up)) { + m_time_sum += timestamp - m_last_timestamp; + m_last_timestamp = timestamp; + m_last_zero_crossing_time = timestamp; + step_detected = true; + } + } else { + m_last_timestamp = timestamp; + } + m_last_acceleration = acceleration; + return step_detected; +} + +void zero_crossing_step_detection::reset(void) +{ + m_last_acceleration = 0; + m_time_sum = 0; + m_last_timestamp = UNKNOWN_TIMESTAMP; + m_last_zero_crossing_time = UNKNOWN_TIMESTAMP; +} diff --git a/src/sensor/pedometer/zero_crossing_step_detection.h b/src/sensor/pedometer/zero_crossing_step_detection.h new file mode 100644 index 0000000..79ca518 --- /dev/null +++ b/src/sensor/pedometer/zero_crossing_step_detection.h @@ -0,0 +1,56 @@ +/* + * 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 __ZERO_CROSSING_STEP_DETECTION_H__ +#define __ZERO_CROSSING_STEP_DETECTION_H__ + +#include "common.h" + +/************************************************************************ + * zero crossing detection engine state. + */ +class zero_crossing_step_detection { +public: + zero_crossing_step_detection(bool up); + ~zero_crossing_step_detection(); + + /************************************************************************ + */ + bool detect_step(timestamp_t timestamp, double acceleration); + + /************************************************************************ + */ + void reset(void); + + timestamp_t m_time_sum; + +private: + /** + * for true detects zero up crossing, for false detects zero down crossing. + */ + bool m_up; + + /** acceleration in previous detect step. */ + double m_last_acceleration; + + /** timestamp of last acc event. unknown time if no one. */ + timestamp_t m_last_timestamp; + + /** timestamp of last detected zero crossing. unknown time if not yet detected. */ + timestamp_t m_last_zero_crossing_time; +}; + +#endif /* __ZERO_CROSSING_STEP_DETECTION_H__ */ -- 2.7.4