#include "average_filter.h"
+#include <memory>
#include <stdlib.h>
static double mean(double *array, int size)
#ifndef __AVERAGE_FILTER_H__
#define __AVERAGE_FILTER_H__
-#include <memory>
-#include "common.h"
+#include "timestamp.h"
class average_filter {
public:
+
average_filter(int size);
+
~average_filter();
/************************************************************************
m_acceleration_compensator.reset();
}
-bool pedometer::get_pedometer(pedometer_info *info, timestamp_t timestamp, double acc[])
+bool pedometer::new_acceleration(pedometer_info *info, timestamp_t timestamp, double acc[])
{
bool result = false;
m_acceleration_compensator.add(timestamp, acc);
while (m_acceleration_compensator.has_next()) {
double acceleration[3];
m_acceleration_compensator.get_next(acceleration);
- if (m_step_detection.get_step(timestamp,
+ if (m_step_detection.new_acceleration(timestamp,
sqrt(acceleration[0] * acceleration[0]
+ acceleration[1] * acceleration[1]
+ acceleration[2] * acceleration[2]),
if (event.m_timestamp != UNKNOWN_TIMESTAMP) {
m_step_count++;
m_total_length += event.m_step_length;
- m_pedometer_filter.get_step(timestamp, event.m_step_length);
+ m_pedometer_filter.new_step(timestamp, event.m_step_length);
double speed = m_pedometer_filter.get_speed(timestamp);
info->timestamp = timestamp;
info->is_step_detected = true;
#ifndef __PEDOMETER_H__
#define __PEDOMETER_H__
-#include "common.h"
#include "step_detection.h"
#include "pedometer_info.h"
#include "pedometer_speed_filter.h"
#include "sensor_frequency_compensator.h"
+#include "timestamp.h"
/************************************************************************
* stores pedometer engine state.
~pedometer();
/************************************************************************
- * enables/disables savitzky filter.
+ * enables/disables Savitzky filter.
*/
void set_savitzky_filter(bool enable);
* @param info
* result of pedometer algorithm. valid only it method returns true.
* @param timestamp
- * timestamp of acceleration event in ns.
+ * timestamp of acceleration event in [ns].
* @param acc
- * global acceleration.
+ * global acceleration vector in [m/s^2].
*
* @result
* true if new step event was detected.
*/
- bool get_pedometer(pedometer_info *info, timestamp_t timestamp, double acc[]);
+ bool new_acceleration(pedometer_info *info, timestamp_t timestamp, double acc[]);
private:
/** detects step and estimates step length. */
step_detection m_step_detection;
- /** sum of lengths all steps from start. */
+ /** sum of lengths all steps from start in [m]. */
double m_total_length;
/** number of steps from start. */
bool m_some_speed;
sensor_frequency_compensator m_acceleration_compensator;
+
};
#endif /* __PEDOMETER_H__ */
#ifndef __PEDOMETER_INFO_H__
#define __PEDOMETER_INFO_H__
-#include "common.h"
+#include "timestamp.h"
/************************************************************************
* stores information about pedometer event detected.
*/
class pedometer_info {
public:
- /** timestamp this event was detected in ns. */
+ /** timestamp this event was detected in [ns]. */
timestamp_t timestamp;
/** is step detected. */
/** step count from scanner start. */
long long step_count;
- /** step length in meters. */
+ /** step length in [m]. */
double step_length;
- /** total length of all steps detected from scanner start in meters. */
+ /** total length of all steps detected from scanner start in [m]. */
double total_step_length;
- /** current mean speed in m/s. */
+ /** current mean speed in [m/s]. */
double step_speed;
};
pedometer_info info;
double acc[] = {data->values[0], data->values[1], data->values[2]};
- if (!m_pedometer.get_pedometer(&info, US_TO_NS(data->timestamp), acc))
+ if (!m_pedometer.new_acceleration(&info, US_TO_NS(data->timestamp), acc))
return OP_ERROR;
m_step_count = info.step_count;
#include "pedometer_speed_filter.h"
-/** default for maximum step duration in ns. currently 2s. */
-#define STEP_MAX_DURATION 2000000000L
+/** default for maximum step duration in [ns]. currently 2s. */
+static const long long STEP_MAX_DURATION = 2000000000L;
pedometer_speed_filter::pedometer_speed_filter()
: m_last_timestamp(UNKNOWN_TIMESTAMP)
}
/************************************************************************
- * sets new maximum step duration in ns.
+ * sets new maximum step duration in [ns].
* if there is no new speed during this time current speed is cleared.
* 0 disables this feature.
*
* @param step_max_duration
- * maximum step duration in ns.
+ * maximum step duration in [ns].
* 0 to disable step duration checking.
*/
void pedometer_speed_filter::set_step_max_duration(long long step_max_duration)
* called when new step detection event occurs.
*
* @param timestamp
- * timestamp of step detection event in ns.
+ * timestamp of step detection event in [ns].
* @param steplength
- * length of detected step in m.
+ * length of detected step in [m].
*/
-void pedometer_speed_filter::get_step(timestamp_t timestamp, double step_length)
+void pedometer_speed_filter::new_step(timestamp_t timestamp, double step_length)
{
if (m_last_timestamp == UNKNOWN_TIMESTAMP || timestamp == UNKNOWN_TIMESTAMP) {
clear_speed();
* reports new speed.
*
* @param timestamp
- * timestamp of speed event.
+ * timestamp of speed event in [ns].
* @param speed
- * current speed in m/s.
+ * current speed in [m/s].
*/
void pedometer_speed_filter::new_speed(timestamp_t timestamp, double speed)
{
*
* @param timestamp
* timestamp for which speed should be calculated.
- * @return speed for given timestamp in m/s.
+ * @return speed for given timestamp in [m/s].
*/
double pedometer_speed_filter::get_speed(timestamp_t timestamp)
{
* changes current speed.
*
* @param speed
- * current speed in m/s.
+ * current speed in [m/s].
*/
void pedometer_speed_filter::set_current_speed(double speed)
{
}
/************************************************************************
- * @return estimated current speed in m/s.
+ * @return estimated current speed in [m/s].
*/
double pedometer_speed_filter::get_current_speed(void)
{
#ifndef __PEDOMETER_SPEED_FILTER_H__
#define __PEDOMETER_SPEED_FILTER_H__
-#include "common.h"
+#include "timestamp.h"
/************************************************************************
* stores pedometer speed filter state.
*/
class pedometer_speed_filter {
public:
+
pedometer_speed_filter();
+
~pedometer_speed_filter();
void clear_speed(void);
* called when new step detection event occurs.
*
* @param timestamp
- * timestamp of step detection event in ns.
+ * timestamp of step detection event in [ns].
* @param steplength
- * length of detected step in m.
+ * length of detected step in [m].
*/
- void get_step(timestamp_t timestamp, double step_length);
+ void new_step(timestamp_t timestamp, double step_length);
/************************************************************************
* reports new speed.
*
* @param timestamp
- * timestamp of speed event.
+ * timestamp of speed event in [ns].
* @param speed
- * current speed in m/s.
+ * current speed in [m/s].
*/
void new_speed(timestamp_t timestamp, double speed);
* returns current speed.
*
* @param timestamp
- * timestamp for which speed should be calculated.
- * @return speed for given timestamp in m/s.
+ * timestamp for which speed should be calculated in [ns].
+ * @return speed for given timestamp in [m/s].
*/
double get_speed(timestamp_t timestamp);
* changes current speed.
*
* @param speed
- * current speed in m/s.
+ * current speed in [m/s].
*/
void set_current_speed(double speed);
/************************************************************************
- * @return estimated current speed in m/s.
+ * @return estimated current speed in [m/s].
*/
double get_current_speed(void);
bool is_known_timestamp(void);
/************************************************************************
- * @return timestamp of last step detection event in ns.
+ * @return timestamp of last step detection event in [ns].
*/
timestamp_t get_timestamp(void);
void reset(void);
private:
- /** timestamp of last step detection event in ns. */
+ /** timestamp of last step detection event in [ns]. */
timestamp_t m_last_timestamp;
- /** estimated current speed in m/s. */
+ /** estimated current speed in [m/s]. */
double m_current_speed;
- /** length of last step in m. */
+ /** length of last step in [m]. */
double m_last_step_length;
- /** duration of last step in s. */
+ /** duration of last step in [s]. */
double m_last_step_duration;
- /** maximum step duration in ns. 0 to disable step duration checking. */
+ /** maximum step duration in [ns]. 0 to disable step duration checking. */
long long m_step_max_duration;
};
#include "savitzky_golay_filter15.h"
+#include <memory>
#include <stdlib.h>
/* length of filter. changing it requires changing coef_array! */
#ifndef __SAVITZKYGOLAYFILTER15_H__
#define __SAVITZKYGOLAYFILTER15_H__
-#include <memory>
-#include "common.h"
+#include "timestamp.h"
/************************************************************************
- * stores savitzky-golay filter state.
+ * stores Savitzky-Golay filter state.
*/
class savitzky_golay_filter15 {
public:
+
savitzky_golay_filter15();
+
~savitzky_golay_filter15();
/************************************************************************
double filter(double value);
/************************************************************************
- * resets savitzky-golay filter to initial state.
+ * resets Savitzky-Golay filter to initial state.
*/
void reset(void);
/************************************************************************
*/
sensor_frequency_compensator::sensor_frequency_compensator(double desired_rate)
-: m_desired_frequency(desired_rate)
-, m_t1(0)
+: m_desired_rate(desired_rate)
+, m_t1(UNKNOWN_TIMESTAMP)
, m_v1{0.0, 0.0, 0.0}
-, m_t2(0)
+, m_t2(UNKNOWN_TIMESTAMP)
, m_v2{0.0, 0.0, 0.0}
-, m_timestamp(0)
+, m_timestamp(UNKNOWN_TIMESTAMP)
{
}
if (m_t1 == UNKNOWN_TIMESTAMP || m_t2 == UNKNOWN_TIMESTAMP) {
return false;
}
- return m_timestamp + m_desired_frequency < m_t2;
+ return m_timestamp + m_desired_rate < m_t2;
}
/************************************************************************
if (t3 < m_t1) {
t3 = m_t1;
}
- m_timestamp += m_desired_frequency;
- double t = (t3 - m_t1) / ((double) (m_t2 - m_t1));
- int i;
- for (i = 0; i < 3; i++) {
+ m_timestamp += m_desired_rate;
+ double t = ((double) (t3 - m_t1)) / (m_t2 - m_t1);
+ for (int i = 0; i < 3; i++) {
v[i] = (1.0 - t) * m_v1[i] + t * m_v2[i];
}
}
* limitations under the License.
*/
-#ifndef __SENSOR_FREQUENCY_COMPENSATOR_H_
-#define __SENSOR_FREQUENCY_COMPENSATOR_H_
+#ifndef __SENSOR_FREQUENCY_COMPENSATOR_H__
+#define __SENSOR_FREQUENCY_COMPENSATOR_H__
-#include "common.h"
+#include "timestamp.h"
#include <memory>
*/
class sensor_frequency_compensator {
public:
+
sensor_frequency_compensator(double desired_rate);
+
~sensor_frequency_compensator();
/************************************************************************
void get_next(double *value);
private:
- long long m_desired_frequency;
+
+ /** desired sensor rate in [ns]. */
+ long long m_desired_rate;
timestamp_t m_t1;
timestamp_t m_timestamp;
};
-#endif /* __SENSOR_FREQUENCY_COMPENSATOR_H_ */
+#endif /* __SENSOR_FREQUENCY_COMPENSATOR_H__ */
#include "step_detection.h"
#include <math.h>
-#include <sensor_log.h>
/* Size of average filter. */
#define AV_FILTER_SIZE 7
, m_zero_crossing_down(false)
, m_zc_filter()
, m_peak_threshold(FAST_PEAK_THRESHOLD)
-, m_use_savitzky(false)
+, m_use_savitzky(true)
, m_last_step_timestamp(UNKNOWN_TIMESTAMP)
, m_zero_crossing_up_detected(false)
, m_zero_crossing_down_detected(false)
}
double peak_threshold;
- if (m_zero_crossing_up.m_time_sum / 1E9 < 1.2) {
+ if (m_zero_crossing_up.get_time_sum() < 1.2) {
peak_threshold = m_peak_threshold;
m_is_slow_step_detected = false;
} else {
}
}
- if (m_zero_crossing_up.m_time_sum / 1E9 < 0.3)
+ if (m_zero_crossing_up.get_time_sum() < 0.3)
is_step_detected = false;
if (is_step_detected) {
double time = (timestamp - m_last_step_timestamp) / 1E9;
m_last_step_timestamp = timestamp;
- m_zero_crossing_up.m_time_sum = 0;
+ m_zero_crossing_up.clr_time_sum();
step->m_timestamp = timestamp;
step->m_step_length = cal_step_length(time, sqrt4peak_valley_diff);
}
}
- bool zup = m_zero_crossing_up.m_time_sum / 1E9 > 1.2;
+ bool zup = m_zero_crossing_up.get_time_sum() > 1.2;
if (n_zero_down) {
is_step_detected = false;
amplitude = abs(m_maximum_acceleration - m_minimum_acceleration);
}
}
- if (m_zero_crossing_up.m_time_sum / 1E9 < 0.3)
+ if (m_zero_crossing_up.get_time_sum() < 0.3)
is_step_detected = false;
if (is_step_detected) {
if (time > 1.0 || amplitude < 3) {
is_step_detected = false;
}
- m_zero_crossing_up.m_time_sum = 0;
+ m_zero_crossing_up.clr_time_sum();
if (is_step_detected) {
step->m_timestamp = timestamp;
step->m_step_length = cal_step_length(time, sqrt4peak_valley_diff);
/************************************************************************
*/
-bool step_detection::get_step(timestamp_t timestamp, double acc, step_event* step)
+bool step_detection::new_acceleration(timestamp_t timestamp, double acc, step_event* step)
{
return m_use_savitzky
? add_acc_sensor_values_savitzky(timestamp, acc, step)
#include "zero_crossing_step_detection.h"
#include "savitzky_golay_filter15.h"
#include "step_event.h"
-#include "common.h"
+#include "timestamp.h"
/************************************************************************
* step detection engine state.
*/
class step_detection {
public:
+
step_detection();
+
~step_detection();
/************************************************************************
/************************************************************************
*/
- bool is_slow_step(void);
-
- /************************************************************************
- */
- bool add_acc_sensor_values_average(
- timestamp_t timestamp, double acc, step_event* step);
-
- /************************************************************************
- */
- bool add_acc_sensor_values_savitzky(
- timestamp_t timestamp, double acc, step_event* step);
-
- /************************************************************************
- */
- bool get_step(timestamp_t timestamp, double acc, step_event* step);
+ bool new_acceleration(timestamp_t timestamp, double acc, step_event* step);
/************************************************************************
* resets step_detection object to initial state.
double m_minimum_acceleration;
double m_maximum_acceleration;
bool m_is_slow_step_detected;
+
+ /************************************************************************
+ */
+ bool add_acc_sensor_values_average(
+ timestamp_t timestamp, double acc, step_event* step);
+
+ /************************************************************************
+ */
+ bool add_acc_sensor_values_savitzky(
+ timestamp_t timestamp, double acc, step_event* step);
+
+ /************************************************************************
+ */
+ bool is_slow_step(void);
+
};
#endif /* __STEP_DETECTION_H__ */
#ifndef __STEP_EVENT_H__
#define __STEP_EVENT_H__
-#include "common.h"
+#include "timestamp.h"
class step_event {
public:
, m_step_length(0)
{}
+ /** step timestamp in [ns]. */
timestamp_t m_timestamp;
+
+ /** estimated step length in [m]. */
double m_step_length;
};
#ifndef __PEDOMETER_COMMON_H__
#define __PEDOMETER_COMMON_H__
-#include <stdio.h>
-
typedef long long timestamp_t;
-#define UNKNOWN_TIMESTAMP ((long long)0x8000000000000000)
+
+static const timestamp_t UNKNOWN_TIMESTAMP = (timestamp_t) 0x8000000000000000;
#endif /* __PEDOMETER_COMMON_H__ */
}
zero_crossing_step_detection::zero_crossing_step_detection(bool up)
-: m_time_sum(0)
-, m_up(up)
+: m_up(up)
, m_last_acceleration(0)
, m_last_timestamp(UNKNOWN_TIMESTAMP)
, m_last_zero_crossing_time(UNKNOWN_TIMESTAMP)
+, m_time_sum(0)
{
}
return step_detected;
}
+void zero_crossing_step_detection::clr_time_sum() {
+ m_time_sum = 0.0;
+}
+
+double zero_crossing_step_detection::get_time_sum() {
+ return m_time_sum / 1E9;
+}
+
void zero_crossing_step_detection::reset(void)
{
m_last_acceleration = 0;
#ifndef __ZERO_CROSSING_STEP_DETECTION_H__
#define __ZERO_CROSSING_STEP_DETECTION_H__
-#include "common.h"
+#include "timestamp.h"
/************************************************************************
* zero crossing detection engine state.
*/
class zero_crossing_step_detection {
public:
+
zero_crossing_step_detection(bool up);
+
~zero_crossing_step_detection();
/************************************************************************
*/
void reset(void);
- timestamp_t m_time_sum;
+ /************************************************************************
+ */
+ void clr_time_sum();
+
+ /************************************************************************
+ */
+ double get_time_sum();
private:
/**
/** acceleration in previous detect step. */
double m_last_acceleration;
- /** timestamp of last acc event. unknown time if no one. */
+ /** timestamp of last acc event in [ns]. unknown time if no one. */
timestamp_t m_last_timestamp;
- /** timestamp of last detected zero crossing. unknown time if not yet detected. */
+ /** timestamp of last detected zero crossing in [ns]. unknown time if not yet detected. */
timestamp_t m_last_zero_crossing_time;
+
+ timestamp_t m_time_sum;
};
#endif /* __ZERO_CROSSING_STEP_DETECTION_H__ */
projects / platform / core / system / sensord.git / commitdiff