SET(LINEAR_ACCEL "ON")
SET(RV "ON")
SET(ORIENTATION "ON")
+SET(PEDOMETER "ON")
INCLUDE_DIRECTORIES(
${CMAKE_SOURCE_DIR}/src/shared
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})
#ifdef ENABLE_ORIENTATION
#include "orientation/orientation_sensor.h"
#endif
+#ifdef ENABLE_PEDOMETER
+#include "pedometer/pedometer_sensor.h"
+#endif
static std::vector<fusion_sensor_t> sensors;
create_sensor<orientation_sensor>("Orientation Sensor");
#endif
+#ifdef ENABLE_PEDOMETER
+ create_sensor<pedometer_sensor>("Pedometer");
+#endif
+
*fsensors = &sensors[0];
return sensors.size();
}
--- /dev/null
+/*
+ * 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 <stdlib.h>
+
+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;
+}
--- /dev/null
+/*
+ * 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 <memory>
+#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__ */
--- /dev/null
+/*
+ * 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 <stdio.h>
+
+typedef long long timestamp_t;
+#define UNKNOWN_TIMESTAMP ((long long)0x8000000000000000)
+
+#endif /* __PEDOMETER_COMMON_H__ */
--- /dev/null
+/*
+ * 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 <math.h>
+
+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;
+}
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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 <sensor_log.h>
+#include <sensor_types.h>
+#include <fusion_util.h>
+#include <cmath>
+
+#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;
+}
--- /dev/null
+/*
+ * 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 <fusion_sensor.h>
+#include <sensor_types.h>
+
+#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__ */
--- /dev/null
+/*
+ * 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();
+}
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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 <stdlib.h>
+
+/* 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;
+}
--- /dev/null
+/*
+ * 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 <memory>
+#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__ */
--- /dev/null
+/*
+ * 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 <math.h>
+#include <sensor_log.h>
+
+/* 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);
+}
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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;
+}
--- /dev/null
+/*
+ * 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 <code>true</code> detects zero up crossing, for <code>false</code> 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__ */
projects / platform / core / system / sensord.git / commitdiff