archive-gh-me/Espruino
1
0
Fork 0
mirror of https://github.com/espruino/Espruino.git synced 2025-12-08 19:06:15 +00:00
Code Issues Projects Releases Wiki Activity

Bangle.js: Significantly improved step counting algorithm using bandpass filter (fix #1846)

Browse Source
This commit is contained in:
Gordon Williams 2021-04-29 11:00:05 +01:00
parent 88f666cb08
commit d338bfbf8a
8 changed files with 329 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ChangeLog
View File

@ -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

1
boards/BANGLEJS.py
View File

@ -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',

1
boards/DTNO1_F5.py
View File

@ -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',

1
boards/EMSCRIPTEN.py
View File

@ -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'
]
}

1
boards/SMAQ3.py
View File

@ -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))\'',

38
libs/banglejs/jswrap_bangle.c
View File

@ -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

284
libs/misc/stepcount.c Normal file
View File

@ -0,0 +1,284 @@
/*
* 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"
/*
==========================================================
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;
}

23
libs/misc/stepcount.h Normal file
View File

@ -0,0 +1,23 @@
/*
* 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
* ----------------------------------------------------------------------------
*/
/// 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);