/* * This file is part of Espruino, a JavaScript interpreter for Microcontrollers * * Copyright (C) 2021 Gordon Williams * * 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 #include #include //#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; }