From ccaf12750ba39fe0bc1fa6d2425ef2d1831ded9a Mon Sep 17 00:00:00 2001
From: Daniel Thompson <daniel@redfelineninja.org.uk>
Date: Tue, 9 Jun 2020 21:29:00 +0100
Subject: [PATCH] wasp: apps: Step counter application

Currently there's no fancy algorithms to estimate stride length. Just
pure simple step counting directly from the hardware's "intelligence
engine".

Signed-off-by: Daniel Thompson <daniel@redfelineninja.org.uk>
---
 .gitmodules                      |   3 ++
 Makefile                         |   7 +--
 res/feet.png                     | Bin 0 -> 388 bytes
 wasp/apps/steps.py               |  82 +++++++++++++++++++++++++++++++
 wasp/boards/pinetime/manifest.py |   2 +
 wasp/boards/pinetime/watch.py.in |   2 +
 wasp/boards/simulator/watch.py   |  23 +++++++++
 wasp/drivers/bma421.py           |  53 ++++++++++++++++++++
 wasp/modules/bma42x-upy          |   1 +
 wasp/wasp.py                     |   2 +
 10 files changed, 170 insertions(+), 5 deletions(-)
 create mode 100644 res/feet.png
 create mode 100644 wasp/apps/steps.py
 create mode 100644 wasp/drivers/bma421.py
 create mode 160000 wasp/modules/bma42x-upy

diff --git a/.gitmodules b/.gitmodules
index 7543a23..ec5319a 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -25,3 +25,6 @@
 [submodule "reloader"]
 	path = reloader
 	url = https://github.com/daniel-thompson/wasp-reloader
+[submodule "wasp/modules/bma42x-upy"]
+	path = wasp/modules/bma42x-upy
+	url = https://github.com/daniel-thompson/bma42x-upy
diff --git a/Makefile b/Makefile
index 5cdd4df..39fb23e 100644
--- a/Makefile
+++ b/Makefile
@@ -16,10 +16,6 @@ submodules :
 
 bootloader:
 	$(MAKE) -C bootloader/ BOARD=$(BOARD)_nrf52832 all genhex
-	python3 -m nordicsemi dfu genpkg \
-		--bootloader bootloader/_build-$(BOARD)_nrf52832/$(BOARD)_nrf52832_bootloader-*-nosd.hex \
-		--softdevice bootloader/lib/softdevice/s132_nrf52_6.1.1/s132_nrf52_6.1.1_softdevice.hex \
-		bootloader.zip
 	python3 tools/hexmerge.py \
 		bootloader/_build-$(BOARD)_nrf52832/$(BOARD)_nrf52832_bootloader-*-nosd.hex \
 		bootloader/lib/softdevice/s132_nrf52_6.1.1/s132_nrf52_6.1.1_softdevice.hex \
@@ -40,7 +36,8 @@ micropython: wasp/boards/pinetime/watch.py
 	$(MAKE) -C micropython/ports/nrf \
 		BOARD=$(BOARD) SD=s132 \
 		MICROPY_VFS_LFS2=1 \
-		FROZEN_MANIFEST=$(PWD)/wasp/boards/$(BOARD)/manifest.py
+		FROZEN_MANIFEST=$(PWD)/wasp/boards/$(BOARD)/manifest.py \
+		USER_C_MODULES=$(PWD)/wasp/modules
 	python3 -m nordicsemi dfu genpkg \
 		--dev-type 0x0052 \
 		--application micropython/ports/nrf/build-$(BOARD)-s132/firmware.hex \
diff --git a/res/feet.png b/res/feet.png
new file mode 100644
index 0000000000000000000000000000000000000000..03d43d57b9cca2340b72fc00af566619c247da29
GIT binary patch
literal 388
zcmV-~0ek+5P)<h;3K|Lk000e1NJLTq001xm001xu0ssI2*kEqZ0003?Nkl<ZSi{v>
zQ4WP541>n|Uzzt|9w8vujSY}b6Gka*nHqw2q?Cfgh?7uajGF<q#Moj&IBXmT=n@^1
z^mOitqFG{w`09BZB0pk{)%l4U4V5JI2vb9cxd<F{sM2yuNg<$;OU4}Vkh50~4fG+E
z2)`NdOQ!!|WZNPj_i&LrQ@G|9O^a;I7R-~435bepmZ5AeGfNXnLN9o%JSBAi#t|~8
z#UL&Zoou8a3b%Q-GL)=`YL(Uf&@26DY4lFyG4aq%n*?3n*;T2{Eo`eKELcyJx~DYx
z8Tw8@k0Jb>_p+63tAU1`zBGL8ReF4Cibi$?L*|%Z<=#brOztdEaYs?>n0p7LBk2`Q
zuPn~tMgT}e5`)I(-G0p_gUMB^=kpP;;p!MOI`y6Uv9#J>N<bcAc=T=W(uMU%V*KdU
i7OH=VIQFT!pGH5!5Y;vz=lOR40000<MNUMnLSTaIA*}8I

literal 0
HcmV?d00001

diff --git a/wasp/apps/steps.py b/wasp/apps/steps.py
new file mode 100644
index 0000000..d114eff
--- /dev/null
+++ b/wasp/apps/steps.py
@@ -0,0 +1,82 @@
+# SPDX-License-Identifier: LGPL-3.0-or-later
+# Copyright (C) 2020 Daniel Thompson
+
+import wasp
+
+import fonts
+import icons
+import time
+import watch
+
+# 2-bit RLE, generated from res/feet.png, 240 bytes
+feet = (
+    b'\x02'
+    b'00'
+    b'\x13\xc1-\xc4+\xc6*\xc6*\xc6&\xc3\x01\xc6\t\xc2'
+    b'\x1b\xc3\x02\xc5\x08\xc4\x1a\xc4\x01\xc5\x08\xc5\x19\xc4\x02\xc3'
+    b'\x08\xc6\x17\xc1\x02\xc3\x02\xc3\x08\xc6\x16\xc3\x02\xc1\x0e\xc6'
+    b'\x01\xc3\x12\xc3\x11\xc6\x01\xc3\x13\xc2\x05\xc2\n\xc5\x02\xc3'
+    b'\x10\xc2\x01\xc2\x02\xc6\n\xc4\x01\xc4\x10\xc2\x04\xc7\x0b\xc1'
+    b'\x03\xc3\x11\xc3\x02\xc8\x10\xc2\x01\xc3\r\xc2\x02\xc9\x13\xc3'
+    b'\x0b\xc1\x05\xc9\x0c\xc2\x05\xc3\x0b\xc2\x03\xc9\x0c\xc5\x03\xc2'
+    b'\x0c\xc2\x02\xca\x0c\xc6\x05\xc2\t\xc2\x02\xca\x0c\xc7\x03\xc3'
+    b'\r\xca\x0c\xc8\x02\xc3\x0c\xca\r\xc9\x02\xc1\r\xca\r\xc9'
+    b'\x04\xc2\n\xca\x0e\xc9\x02\xc3\n\xca\x0e\xc9\x02\xc2\x0b\xca'
+    b'\x0e\xca\x0e\xca\x0e\xca\x0f\xc9\x0e\xca\x0f\xca\r\xca\x0f\xca'
+    b'\r\xca\x10\xcb\x0b\xca\x10\xcc\n\xca\x10\xcd\t\xca\x11\xcc'
+    b'\x08\xca\x12\xcc\x07\xcb\x13\xcb\x06\xcb\x14\xcb\x05\xcc\x15\xca'
+    b'\x04\xcc\x16\xc9\x05\xcc\x17\xc7\x05\xcd\x17\xc7\x05\xcc\x1a\xc4'
+    b"\x07\xcb%\xca&\xca'\xc8)\xc6+\xc4\x0e"
+)
+
+class StepCounterApp():
+    NAME = 'Steps'
+    ICON = icons.app
+
+    def __init__(self):
+        watch.accel.reset()
+        self._meter = wasp.widgets.BatteryMeter()
+        self._count = 0
+
+    def foreground(self):
+        """Activate the application."""
+        self._last_clock = ( -1, -1, -1, -1, -1, -1 )
+
+        self._draw()
+        wasp.system.request_tick(1000)
+
+    def tick(self, ticks):
+        self._count += 686;
+        self._update()
+
+    def _draw(self):
+        """Draw the display from scratch."""
+        draw = wasp.watch.drawable
+        draw.fill()
+        draw.blit(feet, 12, 132-24)
+
+        self._last_count = -1
+        self._update()
+        self._meter.draw()
+
+    def _update(self):
+        draw = wasp.watch.drawable
+
+        # Lazy update of the clock and battery meter
+        now = wasp.watch.rtc.get_localtime()
+        if now[4] != self._last_clock[4]:
+            t1 = '{:02}:{:02}'.format(now[3], now[4])
+            draw.set_font(fonts.sans24)
+            draw.string(t1, 48, 16, 240-96)
+            self._last_clock = now
+            self._meter.update()
+
+            if now[2] != self._last_clock[2]:
+                watch.accel.steps = 0
+                draw.fill(60, 132-18, 180, 36)
+
+        count = watch.accel.steps
+        t = str(count)
+        w = fonts.width(fonts.sans36, t)
+        draw.set_font(fonts.sans36)
+        draw.string(t, 228-w, 132-18)
diff --git a/wasp/boards/pinetime/manifest.py b/wasp/boards/pinetime/manifest.py
index f7cb9d9..61cc24a 100644
--- a/wasp/boards/pinetime/manifest.py
+++ b/wasp/boards/pinetime/manifest.py
@@ -9,10 +9,12 @@ freeze('../..',
         'apps/launcher.py',
         'apps/pager.py',
         'apps/settings.py',
+        'apps/steps.py',
         'apps/stopwatch.py',
         'apps/testapp.py',
         'boot.py',
         'draw565.py',
+        'drivers/bma421.py',
         'drivers/battery.py',
         'drivers/cst816s.py',
         'drivers/nrf_rtc.py',
diff --git a/wasp/boards/pinetime/watch.py.in b/wasp/boards/pinetime/watch.py.in
index 46fc6f6..d064d8f 100644
--- a/wasp/boards/pinetime/watch.py.in
+++ b/wasp/boards/pinetime/watch.py.in
@@ -18,6 +18,7 @@ from machine import Pin
 from machine import SPI
 
 from drivers.battery import Battery
+from drivers.bma421 import BMA421
 from drivers.cst816s import CST816S
 from drivers.signal import Signal
 from drivers.st7789 import ST7789_SPI
@@ -65,6 +66,7 @@ battery = Battery(
         Signal(Pin('USB_PWR', Pin.IN), invert=True))
 button = Pin('BUTTON', Pin.IN)
 i2c = I2C(1, scl='I2C_SCL', sda='I2C_SDA')
+accel = BMA421(i2c)
 touch = CST816S(i2c)
 vibrator = Vibrator(Pin('MOTOR', Pin.OUT, value=0), active_low=True)
 
diff --git a/wasp/boards/simulator/watch.py b/wasp/boards/simulator/watch.py
index 178b1eb..b9de73d 100644
--- a/wasp/boards/simulator/watch.py
+++ b/wasp/boards/simulator/watch.py
@@ -22,6 +22,28 @@ from drivers.cst816s import CST816S
 from drivers.st7789 import ST7789_SPI
 from drivers.vibrator import Vibrator
 
+class Accelerometer:
+    """Simulated accelerometer.
+
+    Accelerometers such as BMA421 are complex and most of the driver
+    is written in C. For that reason we simulate the accelerometer
+    rather than emulate (by comparison we emulate the ST7789).
+    """
+    def reset(self):
+        self._steps = 3
+
+    @property
+    def steps(self):
+        """Report the number of steps counted."""
+        if self._steps < 10000:
+            self._steps = int(self._steps * 1.34)
+        else:
+            self._steps += 1
+        return self._steps
+
+    @steps.setter
+    def steps(self, value):
+        self.reset()
 
 class Backlight(object):
     def __init__(self, level=1):
@@ -105,6 +127,7 @@ display = ST7789_SPI(240, 240, spi,
         res=Pin("DISP_RST", Pin.OUT, quiet=True))
 drawable = draw565.Draw565(display)
 
+accel = Accelerometer()
 battery = Battery()
 button = Pin('BUTTON', Pin.IN, quiet=True)
 rtc = RTC()
diff --git a/wasp/drivers/bma421.py b/wasp/drivers/bma421.py
new file mode 100644
index 0000000..aae4b59
--- /dev/null
+++ b/wasp/drivers/bma421.py
@@ -0,0 +1,53 @@
+# SPDX-License-Identifier: LGPL-3.0-or-later
+# Copyright (C) 2020 Daniel Thompson
+
+"""Bosch BMA421 accelerometer driver
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+"""
+
+import bma42x
+import time
+
+class BMA421:
+    """BMA421 driver
+
+    .. automethod:: __init__
+    """
+    def __init__(self, i2c):
+        """Configure the driver.
+
+        :param machine.I2C i2c: I2C bus used to access the sensor.
+        """
+        self._dev = bma42x.BMA42X(i2c)
+
+    def reset(self):
+        """Reset and reinitialize the sensor."""
+        dev = self._dev
+
+        # Init, reset, wait for reset, enable I2C watchdog
+        dev.init()
+        dev.set_command_register(0xb6)
+        time.sleep(0.20)
+        dev.set_reg(bma42x.NV_CONFIG_ADDR, 6);
+
+        # Configure the sensor for basic step counting
+        dev.write_config_file()
+        dev.set_accel_enable(True)
+        dev.set_accel_config(odr=bma42x.OUTPUT_DATA_RATE_100HZ,
+                               range=bma42x.ACCEL_RANGE_2G,
+                               bandwidth=bma42x.ACCEL_NORMAL_AVG4,
+                               perf_mode=bma42x.CIC_AVG_MODE)
+        dev.feature_enable(bma42x.STEP_CNTR, True)
+
+    @property
+    def steps(self):
+        """Report the number of steps counted."""
+        return self._dev.step_counter_output()
+
+    @steps.setter
+    def steps(self, value):
+        if value != 0:
+            raise ValueError()
+        # TODO: There is a more efficient way to reset the step counter
+        #       but I haven't looked it up yet!
+        self.reset()
diff --git a/wasp/modules/bma42x-upy b/wasp/modules/bma42x-upy
new file mode 160000
index 0000000..55c16a0
--- /dev/null
+++ b/wasp/modules/bma42x-upy
@@ -0,0 +1 @@
+Subproject commit 55c16a0d0407b1f15efe0d2118a4cca13102fe7e
diff --git a/wasp/wasp.py b/wasp/wasp.py
index 8f09a9a..b94bef5 100644
--- a/wasp/wasp.py
+++ b/wasp/wasp.py
@@ -25,6 +25,7 @@ from apps.flashlight import FlashlightApp
 from apps.launcher import LauncherApp
 from apps.pager import PagerApp, CrashApp
 from apps.settings import SettingsApp
+from apps.steps import StepCounterApp
 from apps.stopwatch import StopwatchApp
 from apps.testapp import TestApp
 
@@ -108,6 +109,7 @@ class Manager():
         # TODO: Eventually these should move to main.py
         self.register(ClockApp(), True)
         self.register(StopwatchApp(), True)
+        self.register(StepCounterApp(), True)
         self.register(FlashlightApp(), False)
         self.register(SettingsApp(), False)
         self.register(TestApp(), False)