diff --git a/ChangeLog b/ChangeLog index 412a47d17..bdf9bb332 100644 --- a/ChangeLog +++ b/ChangeLog @@ -1,3 +1,5 @@ + Bangle.js: Significantly improved step counting algorithm using bandpass filter (fix #1846) + 2v09 : Bangle.js: increase default advertising interval from 375 to 200ms to ease connections Fix Math.acos for negative values (fix #1950) nRF5x: Add callback param to 'NRF.restart', allowing code to be called with softdevice disabled diff --git a/boards/BANGLEJS.py b/boards/BANGLEJS.py index 8e5bb37e7..e592afe99 100644 --- a/boards/BANGLEJS.py +++ b/boards/BANGLEJS.py @@ -51,6 +51,7 @@ info = { 'INCLUDE += -I$(ROOT)/libs/banglejs -I$(ROOT)/libs/misc', 'WRAPPERSOURCES += libs/banglejs/jswrap_bangle.c', 'SOURCES += libs/misc/nmea.c', + 'SOURCES += libs/misc/stepcount.c', 'JSMODULESOURCES += libs/js/banglejs/locale.min.js', 'NRF_BL_DFU_INSECURE=1', 'LINKER_BOOTLOADER=targetlibs/nrf5x_12/nrf5x_linkers/banglejs_dfu.ld', diff --git a/boards/DTNO1_F5.py b/boards/DTNO1_F5.py index ea1ec00ad..f2e6758bd 100644 --- a/boards/DTNO1_F5.py +++ b/boards/DTNO1_F5.py @@ -45,6 +45,7 @@ info = { 'INCLUDE += -I$(ROOT)/libs/banglejs -I$(ROOT)/libs/misc', 'WRAPPERSOURCES += libs/banglejs/jswrap_bangle.c', 'SOURCES += libs/misc/nmea.c', + 'SOURCES += libs/misc/stepcount.c', 'JSMODULESOURCES += libs/js/banglejs/locale.min.js', 'DEFINES += -DBANGLEJS', 'NRF_BL_DFU_INSECURE=1', diff --git a/boards/EMSCRIPTEN.py b/boards/EMSCRIPTEN.py index b4556c4ab..5b5de5ccc 100644 --- a/boards/EMSCRIPTEN.py +++ b/boards/EMSCRIPTEN.py @@ -42,6 +42,7 @@ info = { 'INCLUDE += -I$(ROOT)/libs/banglejs -I$(ROOT)/libs/misc', 'WRAPPERSOURCES += libs/banglejs/jswrap_bangle.c', 'SOURCES += libs/misc/nmea.c', + 'SOURCES += libs/misc/stepcount.c', 'JSMODULESOURCES += libs/js/banglejs/locale.min.js' ] } diff --git a/boards/SMAQ3.py b/boards/SMAQ3.py index 4bca3d642..abb877b06 100644 --- a/boards/SMAQ3.py +++ b/boards/SMAQ3.py @@ -50,6 +50,7 @@ info = { 'INCLUDE += -I$(ROOT)/libs/banglejs -I$(ROOT)/libs/misc', 'WRAPPERSOURCES += libs/banglejs/jswrap_bangle.c', 'SOURCES += libs/misc/nmea.c', + 'SOURCES += libs/misc/stepcount.c', 'JSMODULESOURCES += libs/js/banglejs/locale.min.js', 'DEFINES += -DBANGLEJS', 'DEFINES += -D\'IS_PIN_A_BUTTON(PIN)=((PIN==17)||(PIN==40)||(PIN==41))\'', diff --git a/libs/banglejs/jswrap_bangle.c b/libs/banglejs/jswrap_bangle.c index 729ac1efc..754da7ad2 100644 --- a/libs/banglejs/jswrap_bangle.c +++ b/libs/banglejs/jswrap_bangle.c @@ -55,6 +55,7 @@ #include "lcd_spilcd.h" #endif +#include "stepcount.h" #ifdef GPS_PIN_RX #include "nmea.h" #endif @@ -498,11 +499,6 @@ int accelGestureEndThresh = 2000*2000; int accelGestureInactiveCount = 4; /// how many samples must a gesture have before we notify about it? int accelGestureMinLength = 10; -// Step data -/// How low must acceleration magnitude squared get before we consider the next rise a step? -int stepCounterThresholdLow = (8192-80)*(8192-80); -/// How high must acceleration magnitude squared get before we consider it a step? -int stepCounterThresholdHigh = (8192+80)*(8192+80); /// How much acceleration to register a twist of the watch strap? int twistThreshold = 800; /// Maximum acceleration in Y to trigger a twist (low Y means watch is facing the right way up) @@ -512,8 +508,6 @@ int twistTimeout = 1000; /// Current steps since reset uint32_t stepCounter; -/// has acceleration counter passed stepCounterThresholdLow? -bool stepWasLow; /// What state was the touchscreen last in typedef enum { TS_NONE = 0, @@ -917,14 +911,18 @@ void peripheralPollHandler() { bangleTasks |= JSBT_FACE_UP; jshHadEvent(); } - // check for step counter - if (accMagSquared < stepCounterThresholdLow) - stepWasLow = true; - else if ((accMagSquared > stepCounterThresholdHigh) && stepWasLow) { - stepWasLow = false; - stepCounter++; - bangleTasks |= JSBT_STEP_EVENT; - jshHadEvent(); + // Step counter + if (bangleTasks & JSBT_ACCEL_INTERVAL_DEFAULT) { + // we've come out of powersave, reset the algorithm + stepcount_init(); + } + if (powerSaveTimer < POWER_SAVE_TIMEOUT) { + // only do step counting if power save is off (otherwise accel interval is too low - also wastes power) + if (stepcount_new(accMagSquared)) { + stepCounter++; + bangleTasks |= JSBT_STEP_EVENT; + jshHadEvent(); + } } // check for twist action if (twistTimer < TIMER_MAX) @@ -1594,7 +1592,7 @@ void jswrap_banglejs_setPollInterval(JsVarFloat interval) { ], "ifdef" : "BANGLEJS" } -Set internal options used for gestures, step counting, etc... +Set internal options used for gestures, etc... * `wakeOnBTN1` should the LCD turn on when BTN1 is pressed? default = `true` * `wakeOnBTN2` should the LCD turn on when BTN2 is pressed? default = `true` @@ -1605,15 +1603,10 @@ Set internal options used for gestures, step counting, etc... * `twistThreshold` How much acceleration to register a twist of the watch strap? Can be negative for oppsite direction. default = `800` * `twistMaxY` Maximum acceleration in Y to trigger a twist (low Y means watch is facing the right way up). default = `-800` * `twistTimeout` How little time (in ms) must a twist take from low->high acceleration? default = `1000` - * `gestureStartThresh` how big a difference before we consider a gesture started? default = `sqr(800)` * `gestureEndThresh` how small a difference before we consider a gesture ended? default = `sqr(2000)` * `gestureInactiveCount` how many samples do we keep after a gesture has ended? default = `4` * `gestureMinLength` how many samples must a gesture have before we notify about it? default = `10` -* -* `stepCounterThresholdLow` How low must acceleration magnitude squared get before we consider the next rise a step? default = `sqr(8192-80)` -* `stepCounterThresholdHigh` How high must acceleration magnitude squared get before we consider it a step? default = `sqr(8192+80)` - * `powerSave` after a minute of not being moved, Bangle.js will change the accelerometer poll interval down to 800ms (10x accelerometer samples). On movement it'll be raised to the default 80ms. If `Bangle.setPollInterval` is used this is disabled, and for it to work the poll interval must be either 80ms or 800ms. default = `true` @@ -1629,6 +1622,7 @@ void jswrap_banglejs_setOptions(JsVar *options) { bool wakeOnTouch = bangleFlags&JSBF_WAKEON_TOUCH; bool wakeOnTwist = bangleFlags&JSBF_WAKEON_TWIST; bool powerSave = bangleFlags&JSBF_POWER_SAVE; + int stepCounterThresholdLow, stepCounterThresholdHigh; // ignore these with new step counter jsvConfigObject configs[] = { {"gestureStartThresh", JSV_INTEGER, &accelGestureStartThresh}, {"gestureEndThresh", JSV_INTEGER, &accelGestureEndThresh}, @@ -2350,8 +2344,8 @@ void jswrap_banglejs_init() { #endif // Accelerometer variables init + stepcount_init(); stepCounter = 0; - stepWasLow = false; #ifdef MAG_I2C #ifdef MAG_DEVICE_GMC303 // compass init diff --git a/libs/misc/stepcount.c b/libs/misc/stepcount.c new file mode 100644 index 000000000..7c17040d7 --- /dev/null +++ b/libs/misc/stepcount.c @@ -0,0 +1,284 @@ +/* + * 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" + +/* + +========================================================== +FIR filter designed with http://t-filter.engineerjs.com/ + +AccelFilter_get modified to return 8 bits of fractional +data. +========================================================== + +Source Code Tab: + +Accel +Integer +10 +int8_t +int + +Actual Filter details: + +FIR filter designed with + http://t-filter.appspot.com + +sampling frequency: 12.5 Hz + +fixed point precision: 10 bits + +* 0 Hz - 1.4 Hz + gain = 0 + desired attenuation = -40 dB + actual attenuation = n/a + +* 1.5 Hz - 2.5 Hz + gain = 1 + desired ripple = 5 dB + actual ripple = n/a + +* 2.6 Hz - 6.25 Hz + gain = 0 + desired attenuation = -40 dB + actual attenuation = n/a + +*/ + +#define ACCELFILTER_TAP_NUM 127 + +typedef struct { + int8_t history[ACCELFILTER_TAP_NUM]; + unsigned int last_index; +} AccelFilter; + +static int8_t filter_taps[ACCELFILTER_TAP_NUM] = { + 3, + 5, + -1, + -5, + -7, + -1, + 6, + 9, + 2, + -6, + -8, + -2, + 3, + 3, + 0, + 0, + 3, + 4, + -2, + -9, + -9, + 2, + 12, + 11, + 0, + -9, + -8, + -1, + 2, + 1, + 1, + 4, + 6, + 1, + -6, + -8, + -2, + 4, + 3, + -1, + 1, + 7, + 8, + -4, + -17, + -15, + 3, + 19, + 15, + 0, + -9, + -4, + 0, + -11, + -21, + -6, + 31, + 51, + 19, + -45, + -76, + -35, + 46, + 86, + 46, + -35, + -76, + -45, + 19, + 51, + 31, + -6, + -21, + -11, + 0, + -4, + -9, + 0, + 15, + 19, + 3, + -15, + -17, + -4, + 8, + 7, + 1, + -1, + 3, + 4, + -2, + -8, + -6, + 1, + 6, + 4, + 1, + 1, + 2, + -1, + -8, + -9, + 0, + 11, + 12, + 2, + -9, + -9, + -2, + 4, + 3, + 0, + 0, + 3, + 3, + -2, + -8, + -6, + 2, + 9, + 6, + -1, + -7, + -5, + -1, + 5, + 3 +}; + +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; // MODIFIED - was 10. Now returns 8 bits of fractional data +} + +AccelFilter accelFilter; + +#define stepCounterThresholdMin 512 +int stepCounterThreshold; +/// has filtered acceleration passed stepCounterThresholdLow? +bool stepWasLow; + +// 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() { + stepWasLow = false; + stepCounterThreshold = stepCounterThresholdMin; + AccelFilter_init(&accelFilter); +} + +// do calculations +bool stepcount_new(int accMagSquared) { + // square root accelerometer data + int accMag = int_sqrt32(accMagSquared); + // scale to fit and clip + int v = (accMag-8192)>>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; + // do filtering + AccelFilter_put(&accelFilter, v); + int accFiltered = AccelFilter_get(&accelFilter); + + // check for step counter + bool hadStep = false; + if (accFiltered < -stepCounterThreshold) + stepWasLow = true; + else if ((accFiltered > stepCounterThreshold) && stepWasLow) { + stepWasLow = false; + + hadStep = true; + } + + int a = accFiltered; + if (a<0) a=-a; + stepCounterThreshold = (stepCounterThreshold*7 + a) >> 3; + if (stepCounterThreshold < stepCounterThresholdMin) + stepCounterThreshold = stepCounterThresholdMin; + + return hadStep; +} diff --git a/libs/misc/stepcount.h b/libs/misc/stepcount.h new file mode 100644 index 000000000..844b27845 --- /dev/null +++ b/libs/misc/stepcount.h @@ -0,0 +1,23 @@ +/* + * 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 + * ---------------------------------------------------------------------------- + */ + +/// Initialise step counting +void stepcount_init(); + +/* Registers a new data point for step counting. Data is expected + * as 12.5Hz, 8192=1g, and accMagSquared = x*x + y*y + z*z + * + * Returns true if a step was counted + */ +bool stepcount_new(int accMagSquared);