/*
* This file is part of Espruino, a JavaScript interpreter for Microcontrollers
*
* Copyright (C) 2021 Gordon Williams <gw@pur3.co.uk>
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
*
* ----------------------------------------------------------------------------
* Step Counter
* ----------------------------------------------------------------------------
*/
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
//#include "stepcount.h"
// a1bc34f9a9f5c54b9d68c3c26e973dba195e2105 HughB-walk-1834.csv 1446
// oxford filter 1584
// peak detection 1752
// state machine 1751
// STEPCOUNT_CONFIGURABLE is for use with https://github.com/gfwilliams/step-count
// to test/configure the step counter offline
/*
==========================================================
FIR filter designed with http://t-filter.engineerjs.com/
AccelFilter_get modified to return 8 bits of fractional
data.
==========================================================
*/
#define ACCELFILTER_TAP_NUM 7
typedef struct {
int8_t history[ACCELFILTER_TAP_NUM];
unsigned int last_index;
} AccelFilter;
const static int8_t filter_taps[ACCELFILTER_TAP_NUM] = {
-11, -15, 44, 68, 44, -15, -11
};
static void AccelFilter_init(AccelFilter* f) {
int i;
for(i = 0; i < ACCELFILTER_TAP_NUM; ++i)
f->history[i] = 0;
f->last_index = 0;
}
static void AccelFilter_put(AccelFilter* f, int8_t input) {
f->history[f->last_index++] = input;
if(f->last_index == ACCELFILTER_TAP_NUM)
f->last_index = 0;
}
static int AccelFilter_get(AccelFilter* f) {
int acc = 0;
int index = f->last_index, i;
for(i = 0; i < ACCELFILTER_TAP_NUM; ++i) {
index = index != 0 ? index-1 : ACCELFILTER_TAP_NUM-1;
acc += (int)f->history[index] * (int)filter_taps[i];
};
return acc >> 2;
}
AccelFilter accelFilter;
/* =================================
* DC Filter
*/
#define NSAMPLE 12 //Exponential Moving Average DC removal filter alpha = 1/NSAMPLE
int DCFilter_sample_avg_total = 8192*NSAMPLE;
int DCFilter(int sample) {
DCFilter_sample_avg_total += (sample - DCFilter_sample_avg_total/NSAMPLE);
return sample - DCFilter_sample_avg_total/NSAMPLE;
}
// ===============================================================
// These were calculated based on contributed data
// using https://github.com/gfwilliams/step-count
#define STEPCOUNTERTHRESHOLD_DEFAULT 600
// These are the ranges of values we test over
// when calculating the best data offline
#define STEPCOUNTERTHRESHOLD_MIN 0
#define STEPCOUNTERTHRESHOLD_MAX 2000
#define STEPCOUNTERTHRESHOLD_STEP 100
// This is a bit of a hack to allow us to configure
// variables which would otherwise have been compiler defines
#ifdef STEPCOUNT_CONFIGURABLE
int stepCounterThreshold = STEPCOUNTERTHRESHOLD_DEFAULT;
// These are handy values used for graphing
int accScaled;
#else
#define stepCounterThreshold STEPCOUNTERTHRESHOLD_DEFAULT
#endif
// ===============================================================
/** stepHistory allows us to check for repeated step counts.
Rather than registering each instantaneous step, we now only
measure steps if there were at least 3 steps (including the
current one) in 3 seconds
For each step it contains the number of iterations ago it occurred. 255 is the maximum
*/
int16_t accFiltered; // last accel reading, after running through filter
int16_t accFilteredHist[2]; // last 2 accel readings, 1=newest
// ===============================================================
/**
* (4) State Machine
*
* The state machine ensure all steps are checked that they fall
* between T_MIN_STEP and T_MAX_STEP. The 2v9.90 firmare uses X steps
* in Y seconds but this just enforces that the step X steps ago was
* within 6 seconds (75 samples). It is possible to have 4 steps
* within 1 second and then not get the 5th until T5 seconds. This
* could mean that the F/W would would be letting through 2 batches
* of steps that actually would not meet the threshold as the step at
* T5 could be the last. The F/W version also does not give back the
* X steps detected whilst it is waiting for X steps in Y seconds.
* After 100 cycles of the algorithm this would amount to 500 steps
* which is a 5% error over 10K steps. In practice the number of
* passes through the step machine from STEP_1 state to STEPPING
* state can be as high as 500 events. So using the state machine
* approach avoids this source of error.
*
*/
typedef enum {
S_STILL = 0, // just created state m/c no steps yet
S_STEP_1 = 1, // first step recorded
S_STEP_22N = 2, // counting 2-X steps
S_STEPPING = 3, // we've had X steps in X seconds
} StepState;
// In periods of 12.5Hz
#define T_MIN_STEP 4 // ~333ms
#define T_MAX_STEP 16 // ~1300ms
#define X_STEPS 6 // steps in a row needed
#define RAW_THRESHOLD 15
#define N_ACTIVE_SAMPLES 3
StepState stepState;
unsigned char holdSteps; // how many steps are we holding back?
unsigned char stepLength; // how many poll intervals since the last step?
int active_sample_count = 0;
bool gate_open = false; // start closed
// ===============================================================
// quick integer square root
// https://stackoverflow.com/questions/31117497/fastest-integer-square-root-in-the-least-amount-of-instructions
unsigned short int int_sqrt32(unsigned int x) {
unsigned short int res=0;
unsigned short int add= 0x8000;
int i;
for(i=0;i<16;i++) {
unsigned short int temp=res | add;
unsigned int g2=temp*temp;
if (x>=g2)
res=temp;
add>>=1;
}
return res;
}
// Init step count
void stepcount_init() {
AccelFilter_init(&accelFilter);
DCFilter_sample_avg_total = 8192*NSAMPLE;
accFiltered = 0;
accFilteredHist[0] = 0;
accFilteredHist[1] = 0;
stepState = S_STILL;
holdSteps = 0;
stepLength = 0;
}
int stepcount_had_step() {
StepState st = stepState;
switch (st) {
case S_STILL:
stepState = S_STEP_1;
holdSteps = 1;
return 0;
case S_STEP_1:
holdSteps = 1;
// we got a step within expected period
if (stepLength <= T_MAX_STEP && stepLength >= T_MIN_STEP) {
//stepDebug(" S_STEP_1 -> S_STEP_22N");
stepState = S_STEP_22N;
holdSteps = 2;
} else {
// we stay in STEP_1 state
//stepDebug(" S_STEP_1 -> S_STEP_1");
//reject_count++;
}
return 0;
case S_STEP_22N:
// we got a step within expected time range
if (stepLength <= T_MAX_STEP && stepLength >= T_MIN_STEP) {
holdSteps++;
if (holdSteps >= X_STEPS) {
stepState = S_STEPPING;
//pass_count++; // we are going to STEPPING STATE
//stepDebug(" S_STEP_22N -> S_STEPPING");
return X_STEPS;
}
//stepDebug(" S_STEP_22N -> S_STEP_22N");
} else {
// we did not get the step in time, back to STEP_1
stepState = S_STEP_1;
//stepDebug(" S_STEP_22N -> S_STEP_1");
//reject_count++;
}
return 0;
case S_STEPPING:
// we got a step within the expected window
if (stepLength <= T_MAX_STEP && stepLength >= T_MIN_STEP) {
stepState = S_STEPPING;
//stepDebug(" S_STEPPING -> S_STEPPING");
return 1;
} else {
// we did not get the step in time, back to STEP_1
stepState = S_STEP_1;
//stepDebug(" S_STEPPING -> S_STEP_1");
//reject_count++;
}
return 0;
}
// should never get here
//stepDebug(" ERROR");
return 0;
}
// do calculations
int stepcount_new(int accMagSquared) {
// square root accelerometer data
int accMag = int_sqrt32(accMagSquared);
// scale to fit and clip
//int v = (accMag-8192)>>5;
int v = DCFilter(accMag)>>5;
//printf("v %d\n",v);
//if (v>127 || v<-128) printf("Out of range %d\n", v);
if (v>127) v = 127;
if (v<-128) v = -128;
#ifdef STEPCOUNT_CONFIGURABLE
accScaled = v;
#endif
// do filtering
AccelFilter_put(&accelFilter, v);
accFilteredHist[0] = accFilteredHist[1];
accFilteredHist[1] = accFiltered;
int a = AccelFilter_get(&accelFilter);
if (a>32767) a=32767;
if (a<-32768) a=32768;
accFiltered = a;
if (v > RAW_THRESHOLD || v < -1*RAW_THRESHOLD) {
if (active_sample_count < N_ACTIVE_SAMPLES)
active_sample_count++;
if (active_sample_count == N_ACTIVE_SAMPLES)
gate_open = true;
} else {
if (active_sample_count > 0)
active_sample_count--;
if (active_sample_count == 0)
gate_open = false;
}
// increment step count history counters
if (stepLength<255)
stepLength++;
int stepsCounted = 0;
// check for a peak in the last sample - in which case call the state machine
if (gate_open == true && accFilteredHist[1] > accFilteredHist[0] && accFilteredHist[1] > accFiltered) {
// We now have something resembling a step!
stepsCounted = stepcount_had_step();
stepLength = 0;
}
return stepsCounted;
}