#include <math.h>
+/** 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)
{
}
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;
#include "step_detection.h"
#include "pedometer_info.h"
#include "pedometer_speed_filter.h"
+#include "sensor_frequency_compensator.h"
/************************************************************************
* stores pedometer engine state.
* @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. */
/** some non zero speed was detected. */
bool m_some_speed;
+
+ sensor_frequency_compensator m_acceleration_compensator;
+
};
#endif /* __PEDOMETER_H__ */
#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 */
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;
--- /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 "sensor_frequency_compensator.h"
+
+#include <stdlib.h>
+#include <math.h>
+#include <string.h>
+
+/************************************************************************
+ */
+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];
+ }
+}
--- /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 __SENSOR_FREQUENCY_COMPENSATOR_H_
+#define __SENSOR_FREQUENCY_COMPENSATOR_H_
+
+#include "common.h"
+
+#include <memory>
+
+/************************************************************************
+ * 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_ */
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);
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)
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;
projects / platform / core / system / sensord.git / commitdiff