From 7c79a93dbf8d32d91820ea00aaf2640cc2708544 Mon Sep 17 00:00:00 2001 From: Marcin Masternak Date: Fri, 8 Sep 2017 20:16:22 +0200 Subject: [PATCH] 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